class
StateHelperHelper which manipulates the State and its covariance.
Contents
 Reference
In general, this class has all the core logic for an Extended Kalman Filter (EKF)based system. This has all functions that change the covariance along with addition and removing elements from the state. All functions here are static, and thus are selfcontained so that in the future multiple states could be tracked and updated. We recommend you look directly at the code for this class for clarity on what exactly we are doing in each and the matching documentation pages.
Public static functions

static void EKFPropagation(std::shared_ptr<State> state,
const std::vector<std::shared_ptr<ov_type::
Type >>& order_NEW, const std::vector<std::shared_ptr<ov_type::Type >>& order_OLD, const Eigen::MatrixXd& Phi, const Eigen::MatrixXd& Q)  Performs EKF propagation of the state covariance.

static void EKFUpdate(std::shared_ptr<State> state,
const std::vector<std::shared_ptr<ov_type::
Type >>& H_order, const Eigen::MatrixXd& H, const Eigen::VectorXd& res, const Eigen::MatrixXd& R)  Performs EKF update of the state (see Linear Measurement Update page)

static void set_initial_covariance(std::shared_ptr<State> state,
const Eigen::MatrixXd& covariance,
const std::vector<std::shared_ptr<ov_type::
Type >>& order)  This will set the initial covaraince of the specified state elements. Will also ensure that proper crosscovariances are inserted.

static auto get_marginal_covariance(std::shared_ptr<State> state,
const std::vector<std::shared_ptr<ov_type::
Type >>& small_variables) > Eigen::MatrixXd  For a given set of variables, this will this will calculate a smaller covariance.
 static auto get_full_covariance(std::shared_ptr<State> state) > Eigen::MatrixXd
 This gets the full covariance matrix.

static void marginalize(std::shared_ptr<State> state,
std::shared_ptr<ov_type::
Type > marg)  Marginalizes a variable, properly modifying the ordering/covariances in the state.

static auto clone(std::shared_ptr<State> state,
std::shared_ptr<ov_type::
Type > variable_to_clone) > std::shared_ptr<ov_type::Type >  Clones "variable to clone" and places it at end of covariance.

static auto initialize(std::shared_ptr<State> state,
std::shared_ptr<ov_type::
Type > new_variable, const std::vector<std::shared_ptr<ov_type::Type >>& H_order, Eigen::MatrixXd& H_R, Eigen::MatrixXd& H_L, Eigen::MatrixXd& R, Eigen::VectorXd& res, double chi_2_mult) > bool  Initializes new variable into covariance.

static void initialize_invertible(std::shared_ptr<State> state,
std::shared_ptr<ov_type::
Type > new_variable, const std::vector<std::shared_ptr<ov_type::Type >>& H_order, const Eigen::MatrixXd& H_R, const Eigen::MatrixXd& H_L, const Eigen::MatrixXd& R, const Eigen::VectorXd& res)  Initializes new variable into covariance (H_L must be invertible)
 static void augment_clone(std::shared_ptr<State> state, Eigen::Matrix<double, 3, 1> last_w)
 Augment the state with a stochastic copy of the current IMU pose.
 static void marginalize_old_clone(std::shared_ptr<State> state)
 Remove the oldest clone, if we have more then the max clone count!!
 static void marginalize_slam(std::shared_ptr<State> state)
 Marginalize bad SLAM features.
Function documentation
static void ov_msckf::StateHelper:: EKFPropagation(std::shared_ptr<State> state,
const std::vector<std::shared_ptr<ov_type::Type >>& order_NEW,
const std::vector<std::shared_ptr<ov_type::Type >>& order_OLD,
const Eigen::MatrixXd& Phi,
const Eigen::MatrixXd& Q)
Performs EKF propagation of the state covariance.
Parameters  

state  Pointer to state 
order_NEW  Contiguous variables that have evolved according to this state transition 
order_OLD  Variable ordering used in the state transition 
Phi  State transition matrix (size order_NEW by size order_OLD) 
Q  Additive state propagation noise matrix (size order_NEW by size order_NEW) 
The mean of the state should already have been propagated, thus just moves the covariance forward in time. The new states that we are propagating the old covariance into, should be contiguous in memory. The user only needs to specify the subvariables that this block is a function of.
static void ov_msckf::StateHelper:: EKFUpdate(std::shared_ptr<State> state,
const std::vector<std::shared_ptr<ov_type::Type >>& H_order,
const Eigen::MatrixXd& H,
const Eigen::VectorXd& res,
const Eigen::MatrixXd& R)
Performs EKF update of the state (see Linear Measurement Update page)
Parameters  

state  Pointer to state 
H_order  Variable ordering used in the compressed Jacobian 
H  Condensed Jacobian of updating measurement 
res  Residual of updating measurement 
R  Updating measurement covariance 
static void ov_msckf::StateHelper:: set_initial_covariance(std::shared_ptr<State> state,
const Eigen::MatrixXd& covariance,
const std::vector<std::shared_ptr<ov_type::Type >>& order)
This will set the initial covaraince of the specified state elements. Will also ensure that proper crosscovariances are inserted.
Parameters  

state  Pointer to state 
covariance  The covariance of the system state 
order  Order of the covariance matrix 
static Eigen::MatrixXd ov_msckf::StateHelper:: get_marginal_covariance(std::shared_ptr<State> state,
const std::vector<std::shared_ptr<ov_type::Type >>& small_variables)
For a given set of variables, this will this will calculate a smaller covariance.
Parameters  

state  Pointer to state 
small_variables  Vector of variables whose marginal covariance is desired 
Returns  Marginal covariance of the passed variables 
That only includes the ones specified with all crossterms. Thus the size of the return will be the summed dimension of all the passed variables. Normal use for this is a chisquared check before update (where you don't need the full covariance).
static Eigen::MatrixXd ov_msckf::StateHelper:: get_full_covariance(std::shared_ptr<State> state)
This gets the full covariance matrix.
Parameters  

state  Pointer to state 
Returns  Covariance of current state 
Should only be used during simulation as operations on this covariance will be slow. This will return a copy, so this cannot be used to change the covariance by design. Please use the other interface functions in the StateHelper to progamatically change to covariance.
static void ov_msckf::StateHelper:: marginalize(std::shared_ptr<State> state,
std::shared_ptr<ov_type::Type > marg)
Marginalizes a variable, properly modifying the ordering/covariances in the state.
Parameters  

state  Pointer to state 
marg  Pointer to variable to marginalize 
This function can support any Type variable out of the box. Right now the marginalization of a subvariable/type is not supported. For example if you wanted to just marginalize the orientation of a PoseJPL, that isn't supported. We will first remove the rows and columns corresponding to the type (i.e. do the marginalization). After we update all the type ids so that they take into account that the covariance has shrunk in parts of it.
static std::shared_ptr<ov_type::Type > ov_msckf::StateHelper:: clone(std::shared_ptr<State> state,
std::shared_ptr<ov_type::Type > variable_to_clone)
Clones "variable to clone" and places it at end of covariance.
Parameters  

state  Pointer to state 
variable_to_clone  Pointer to variable that will be cloned 
static bool ov_msckf::StateHelper:: initialize(std::shared_ptr<State> state,
std::shared_ptr<ov_type::Type > new_variable,
const std::vector<std::shared_ptr<ov_type::Type >>& H_order,
Eigen::MatrixXd& H_R,
Eigen::MatrixXd& H_L,
Eigen::MatrixXd& R,
Eigen::VectorXd& res,
double chi_2_mult)
Initializes new variable into covariance.
Parameters  

state  Pointer to state 
new_variable  Pointer to variable to be initialized 
H_order  Vector of pointers in order they are contained in the condensed state Jacobian 
H_R  Jacobian of initializing measurements wrt variables in H_order 
H_L  Jacobian of initializing measurements wrt new variable 
R  Covariance of initializing measurements (isotropic) 
res  Residual of initializing measurements 
chi_2_mult  Value we should multiply the chi2 threshold by (larger means it will be accepted more measurements) 
Uses Givens to separate into updating and initializing systems (therefore system must be fed as isotropic). If you are not isotropic first whiten your system (TODO: we should add a helper function to do this). If your H_L Jacobian is already directly invertable, the just call the initialize_
static void ov_msckf::StateHelper:: initialize_invertible(std::shared_ptr<State> state,
std::shared_ptr<ov_type::Type > new_variable,
const std::vector<std::shared_ptr<ov_type::Type >>& H_order,
const Eigen::MatrixXd& H_R,
const Eigen::MatrixXd& H_L,
const Eigen::MatrixXd& R,
const Eigen::VectorXd& res)
Initializes new variable into covariance (H_L must be invertible)
Parameters  

state  Pointer to state 
new_variable  Pointer to variable to be initialized 
H_order  Vector of pointers in order they are contained in the condensed state Jacobian 
H_R  Jacobian of initializing measurements wrt variables in H_order 
H_L  Jacobian of initializing measurements wrt new variable (needs to be invertible) 
R  Covariance of initializing measurements 
res  Residual of initializing measurements 
Please refer to Delayed Feature Initialization page for detailed derivation. This is just the update assuming that H_L is invertable (and thus square) and isotropic noise.
static void ov_msckf::StateHelper:: augment_clone(std::shared_ptr<State> state,
Eigen::Matrix<double, 3, 1> last_w)
Augment the state with a stochastic copy of the current IMU pose.
Parameters  

state  Pointer to state 
last_w  The estimated angular velocity at cloning time (used to estimate imucam time offset) 
After propagation, normally we augment the state with an new clone that is at the new update timestep. This augmentation clones the IMU pose and adds it to our state's clone map. If we are doing time offset calibration we also make our cloning a function of the time offset. Time offset logic is based on Li and Mourikis [28].
We can write the current clone at the true imu base clock time as the follow:
where we say that we have propagated our state up to the current estimated true imaging time for the current image, is the angular velocity at the end of propagation with biases removed. This is off by some smaller error, so to get to the true imaging time in the imu base clock, we can append some small timeoffset error. Thus the Jacobian in respect to our time offset during our cloning procedure is the following:
static void ov_msckf::StateHelper:: marginalize_old_clone(std::shared_ptr<State> state)
Remove the oldest clone, if we have more then the max clone count!!
Parameters  

state  Pointer to state 
This will marginalize the clone from our covariance, and remove it from our state. This is mainly a helper function that we can call after each update. It will marginalize the clone specified by State::
static void ov_msckf::StateHelper:: marginalize_slam(std::shared_ptr<State> state)
Marginalize bad SLAM features.
Parameters  

state  Pointer to state 