Learning VINS-Mono/Fusion source code from scratch (4): Error Kalman filtering

This section derives the error Kalman filter formula to calculate the confidence of the error state quantity.

VINS-Mono/Fusion code learning series:
Learning VINS-Mono/Fusion source code from scratch (1): Main function
learning VINS-Mono/Fusion source code from scratch (2): Front-end image tracking
learning VINS-Mono/Fusion from scratch Source code (3): IMU pre-integration formula derivation and
learning VINS-Mono/Fusion from scratch source code (4): Error Kalman filter
learning VINS-Mono/Fusion from scratch source code (5): VIO initialization
learning VINS- from scratch Mono/Fusion source code (6): back-end optimization

The error Kalman filter uses the error value as the filter state quantity. The error Kalman filter is used in IMU pre-integration to calculate the confidence and obtain the covariance matrix of the error. (Mainly due to the over-parameterization problem caused by rotating quaternions
)

1 Why use error Kalman filter?

• Quaternions are used to represent rotation in IMU pre-integration, but quaternions are over-parameterized and a 4x4 matrix is ​​needed to describe the confidence; in error Kalman filtering, the rotation vector is used as a parameter, so that a 3x3 matrix can be used Covariance matrix to measure the confidence of the rotation.
• The error is usually very small, close to 0, thus avoiding the periodicity problem of the rotation vector.
• The amount of error is so small that the calculation of the second derivative can be ignored, and it is faster to only calculate the first derivative.
• The error changes very slowly, and when using Kalman filtering for state correction, the observation frequency can be reduced.

2 Continuous time IMU error state transfer

vins paper formula:

derivation speed:

Derive posture:

3 Discrete time pre-integration error transfer

Similarly, using the median integral calculation, the zero bias is considered unchanged between two frames of IMU time.

4 IMU zero bias modeling

In the above derivation, it is considered that the IMU zero bias is unchanged, but during the VINS optimization process, the zero bias will also be optimized, resulting in the corresponding IMU pre-integration may need to be recalculated. Therefore, the error Jacobian is used to correct the IMU pre-integration.

In order to facilitate maintenance, a 15x15 Jacobian matrix is ​​established in the VINS code, which is the unit matrix I in the initial state:

5 code reading

Find the class IntegrationBase in the integration_base.h header file and see its constructor: it relies on the accelerometer and gyro bias at the initial moment; maintains a 15x15 Jacobian matrix with the initial value being the unit matrix; constructs the error covariance matrix covariance , initially 0.
noise corresponds to the noise covariance matrix V.

IntegrationBase(const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0,
                    const Eigen::Vector3d &_linearized_ba, const Eigen::Vector3d &_linearized_bg)
        : acc_0{
    
    _acc_0}, gyr_0{
    
    _gyr_0}, linearized_acc{
    
    _acc_0}, linearized_gyr{
    
    _gyr_0},
          linearized_ba{
    
    _linearized_ba}, linearized_bg{
    
    _linearized_bg},
            jacobian{
    
    Eigen::Matrix<double, 15, 15>::Identity()}, covariance{
    
    Eigen::Matrix<double, 15, 15>::Zero()},
          sum_dt{
    
    0.0}, delta_p{
    
    Eigen::Vector3d::Zero()}, delta_q{
    
    Eigen::Quaterniond::Identity()}, delta_v{
    
    Eigen::Vector3d::Zero()}

    {
    
    
        noise = Eigen::Matrix<double, 18, 18>::Zero();
        noise.block<3, 3>(0, 0) =  (ACC_N * ACC_N) * Eigen::Matrix3d::Identity();
        noise.block<3, 3>(3, 3) =  (GYR_N * GYR_N) * Eigen::Matrix3d::Identity();
        noise.block<3, 3>(6, 6) =  (ACC_N * ACC_N) * Eigen::Matrix3d::Identity();
        noise.block<3, 3>(9, 9) =  (GYR_N * GYR_N) * Eigen::Matrix3d::Identity();
        noise.block<3, 3>(12, 12) =  (ACC_W * ACC_W) * Eigen::Matrix3d::Identity();
        noise.block<3, 3>(15, 15) =  (GYR_W * GYR_W) * Eigen::Matrix3d::Identity();
    }

In midPointIntegration(), after the median integral updates the state quantity, the covariance matrix and Jacobian matrix are updated, and the discrete-time pre-integration error transfer formula based on the median integral is applied:

		if(update_jacobian)
        {
    
    
            Vector3d w_x = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
            Vector3d a_0_x = _acc_0 - linearized_ba;
            Vector3d a_1_x = _acc_1 - linearized_ba;
            Matrix3d R_w_x, R_a_0_x, R_a_1_x;

            R_w_x<<0, -w_x(2), w_x(1),
                w_x(2), 0, -w_x(0),
                -w_x(1), w_x(0), 0;
            R_a_0_x<<0, -a_0_x(2), a_0_x(1),
                a_0_x(2), 0, -a_0_x(0),
                -a_0_x(1), a_0_x(0), 0;
            R_a_1_x<<0, -a_1_x(2), a_1_x(1),
                a_1_x(2), 0, -a_1_x(0),
                -a_1_x(1), a_1_x(0), 0;

            MatrixXd F = MatrixXd::Zero(15, 15);
            F.block<3, 3>(0, 0) = Matrix3d::Identity();
            F.block<3, 3>(0, 3) = -0.25 * delta_q.toRotationMatrix() * R_a_0_x * _dt * _dt + 
                                  -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt * _dt;
            F.block<3, 3>(0, 6) = MatrixXd::Identity(3,3) * _dt;
            F.block<3, 3>(0, 9) = -0.25 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt * _dt;
            F.block<3, 3>(0, 12) = -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * -_dt;
            F.block<3, 3>(3, 3) = Matrix3d::Identity() - R_w_x * _dt;
            F.block<3, 3>(3, 12) = -1.0 * MatrixXd::Identity(3,3) * _dt;
            F.block<3, 3>(6, 3) = -0.5 * delta_q.toRotationMatrix() * R_a_0_x * _dt + 
                                  -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt;
            F.block<3, 3>(6, 6) = Matrix3d::Identity();
            F.block<3, 3>(6, 9) = -0.5 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt;
            F.block<3, 3>(6, 12) = -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * -_dt;
            F.block<3, 3>(9, 9) = Matrix3d::Identity();
            F.block<3, 3>(12, 12) = Matrix3d::Identity();
            //cout<<"A"<<endl<<A<<endl;

            MatrixXd V = MatrixXd::Zero(15,18);
            V.block<3, 3>(0, 0) =  0.25 * delta_q.toRotationMatrix() * _dt * _dt;
            V.block<3, 3>(0, 3) =  0.25 * -result_delta_q.toRotationMatrix() * R_a_1_x  * _dt * _dt * 0.5 * _dt;
            V.block<3, 3>(0, 6) =  0.25 * result_delta_q.toRotationMatrix() * _dt * _dt;
            V.block<3, 3>(0, 9) =  V.block<3, 3>(0, 3);
            V.block<3, 3>(3, 3) =  0.5 * MatrixXd::Identity(3,3) * _dt;
            V.block<3, 3>(3, 9) =  0.5 * MatrixXd::Identity(3,3) * _dt;
            V.block<3, 3>(6, 0) =  0.5 * delta_q.toRotationMatrix() * _dt;
            V.block<3, 3>(6, 3) =  0.5 * -result_delta_q.toRotationMatrix() * R_a_1_x  * _dt * 0.5 * _dt;
            V.block<3, 3>(6, 6) =  0.5 * result_delta_q.toRotationMatrix() * _dt;
            V.block<3, 3>(6, 9) =  V.block<3, 3>(6, 3);
            V.block<3, 3>(9, 12) = MatrixXd::Identity(3,3) * _dt;
            V.block<3, 3>(12, 15) = MatrixXd::Identity(3,3) * _dt;

            //更新雅克比和协方差
            jacobian = F * jacobian;
            covariance = F * covariance * F.transpose() + V * noise * V.transpose();
        }