Covariance Index Internals

Type System

The type system that the ov_msckf leverages was inspired by graph-based optimization frameworks such as Georgia Tech Smoothing and Mapping library (GTSAM). As compared to manually knowing where in the covariance state elements are, the interaction is abstracted away from the user and is replaced by a straight forward way to access the desired elements. The ov_msckf::State is owner of these types and thus after creation one should not delete these externally.

class Type {
protected:
    // Current best estimate
    Eigen::MatrixXd _value;
    // Location of error state in covariance
    int _id = -1;
    // Dimension of error state
    int _size = -1;
};

A type is defined by its location in its covariance, its current estimate and its error state size. The current value does not have to be a vector, but could be a matrix in the case of an SO(3) rotation group type object. The error state needs to be a vector and thus a type will need to define the mapping between this error state and its manifold representation.

Type-based EKF Update

To show the power of this type-based indexing system, we will go through how we compute the EKF update. The specific method we will be looking at is the ov_msckf::StateHelper::EKFUpdate() which takes in the state, vector of types, Jacobian, residual, and measurement covariance. As compared to passing a "big" Jacobian matrix that is the full size of state wide, we instead leverage this type system by passing a "small" Jacobian that has only the state elements that the measurements are a function of.

For example, if we have a global 3D SLAM feature update (implemented in ov_msckf::UpdaterSLAM) our Jacobian will only be a function of the newest clone and the feature. This means that our Jacobian is only of size 9 as compared to the size our state. In addition to the matrix containing the Jacobian elements, we need to know what order this Jacobian is in, thus we pass a vector of types which correspond to the state elements our Jacobian is a function of. Thus in our example, it would contain two types: our newest clone ov_type::PoseJPL and current landmark feature ov_type::Landmark with our Jacobian being the type size of the pose plus the type size of the landmark in width. In the above figure the orientation and position would correspond to the x1 and x2 states, while the feature is the x5 state.