Following the implementation of ESKF by Joan Sola in the previous article, I wanted to draw inferences from one example and also implement the EKF algorithm. Therefore, I studied the filtering algorithm based on navigation information in the "Multi-Sensor Fusion Positioning" algorithm of Deep Blue Academy, and also reviewed the algorithm. Implement the code. Attached are two renderings (they look similar to those of ESKF in the previous article). The github address is: GitHub - Shelfcol/gps_imu_fusion: gps_imu_fusion with eskf,ekf,ukf,etc

The following is a detailed derivation of the formula and some of my own understanding.

1 Introduction

The main equation of Kalman filter is the construction of prediction equation and observation equation. The two equations are as follows:

$x_k =F(x_{k-1},v_k)+B_{k-1}\omega_k$

$y_k=G(x_k)+C_kn_k$

Some models can easily derive the equation of motion, while some models cannot directly construct the relationship between the two states and cannot derive the equation of motion. Therefore, the differential equation of the state quantity can also be used to indirectly derive the equation of motion.

2. Filtering based on navigation information

1) Differential equation of navigation information

The state quantity used here is $X=[P^T \ V^T \ q^T \ \epsilon^T \bigtriangledown^T]_{16 \times 1}$ , where $\epsilon$ is the bias of angular velocity and $\bigtriangledown$ is the bias of acceleration. It is worth noting that the order of the quaternions here is real part first, imaginary step last. This order can also be seen when solving the F matrix later.

The differential equation of navigation information is as follows:

Therefore, the state equation can be written in the following differential form:

$\dot X =f(X,v,w,g^n)$

It is nonlinear, so if it needs to be linearized, we can get

$\dot X =f(X,v,w,g^n)$

2) Jacobian matrix solution

Linearize differential equations

$\dot X = F_tX+B_tW+\overline f(g^n)$

Jacobian matrix derivation:

Here we need to explain the quaternion product:

3) Discretization of differential equations of continuous systems

Because what is obtained is a continuous time differential equation, discretization is required here. The reference here is: Robot Dynamics Simulation - Discretization of State Space Model

3-1) Accurate discretization

3-2) Approximate discretization

3-3) Discretization results of this system

3. Code description

The code is deduced strictly according to the formula. Only the key codes for predicting and updating equations are posted here.

Prediction :

// IMU数据预测
bool EKF::Predict(const IMUData &curr_imu_data) {
    imu_data_buff_.push_back(curr_imu_data);

    double delta_t = curr_imu_data.time - imu_data_buff_.front().time; // dt

    Eigen::Vector3d curr_accel = curr_imu_data.linear_accel; // 局部坐标系下加速度
    Eigen::Vector3d curr_angle_velocity = curr_imu_data.angle_velocity; // 局部坐标系下的角速度
    Eigen::Quaterniond curr_ori = Eigen::Quaterniond(pose_.block<3, 3>(0, 0));
    UpdateEkfState(delta_t, curr_accel,curr_angle_velocity,curr_ori);
    UpdateState(); //! 每次都需要更新位姿
    imu_data_buff_.pop_front();
    return true;
}

bool EKF::UpdateEkfState(const double t, const Eigen::Vector3d &accel, const Eigen::Vector3d& curr_angle_velocity,
                        const Eigen::Quaterniond& curr_ori ) {
    F_.setZero(); // 初始化为零矩阵
    F_.block<3,3>(INDEX_STATE_POSI, INDEX_STATE_VEL) = Eigen::Matrix3d::Identity(); 
    
    double q0 = curr_ori.w();
    double q1 = curr_ori.x();
    double q2 = curr_ori.y();
    double q3 = curr_ori.z();
    double FVq0 = 2 * Eigen::Vector3d(q0,-q3,q2).transpose()*accel;
    double FVq1 = 2 * Eigen::Vector3d(q1,q2,q3).transpose()*accel;
    double FVq2 = 2 * Eigen::Vector3d(-q2,q1,q0).transpose()*accel;
    double FVq3 = 2 * Eigen::Vector3d(-q3,-q0,q1).transpose()*accel;
    Eigen::Matrix<double,3,4> FVq = (Eigen::Matrix<double,3,4>()<<  FVq0,FVq1,FVq2,FVq3,
                                                                    -FVq3,-FVq2,FVq1,FVq0,
                                                                    FVq2,-FVq3,-FVq0,FVq1).finished();

    F_.block<3,4>(INDEX_STATE_VEL, INDEX_STATE_ORI) = FVq;
    F_.block<3,3>(INDEX_STATE_VEL, INDEX_STATE_ACC_BIAS) = pose_.block<3,3>(0,0);

    Eigen::Vector3d w = curr_angle_velocity;
    Eigen::Matrix<double,4,4> Fqq = 0.5* (Eigen::Matrix<double,4,4>()<<0,-w.x(),-w.y(),-w.z(),
                                                                        w.x(),0,w.z(),-w.y(),
                                                                        w.y(),-w.z(),0,w.x(),
                                                                        w.z(),w.y(),-w.x(),0).finished();
    F_.block<4,4>(INDEX_STATE_ORI,INDEX_STATE_ORI) = Fqq;

    Eigen::Matrix<double,4,3> Fqkesi  = 0.5 * (Eigen::Matrix<double,4,3>()<<-q1,-q2,-q3,
                                                                        q0,-q3,q2,
                                                                        q3,q0,-q1,
                                                                        -q2,q1,q0).finished();
    F_.block<4,3>(INDEX_STATE_ORI,INDEX_STATE_GYRO_BIAS) = Fqkesi;

    B_.setZero();
    B_.block<3,3>(INDEX_STATE_VEL, 3) = pose_.block<3,3>(0,0);
    B_.block<4,3>(INDEX_STATE_ORI, 0) = Fqkesi;

    TypeMatrixF Fk = TypeMatrixF::Identity() + F_ * t;
    TypeMatrixB Bk = B_ * t;

    X_ = Fk * X_ + Bk * W_+ gt_*t ; 
    P_ = Fk * P_ * Fk.transpose() + Bk * Q_ * Bk.transpose();

    return true;
}

Update :

bool EKF::Correct(const GPSData &curr_gps_data) {
    curr_gps_data_ = curr_gps_data;

    C_.setIdentity(); // 单位矩阵
    G_.setZero();
    G_.block<3,3>(INDEX_MEASUREMENT_POSI, INDEX_MEASUREMENT_POSI) = Eigen::Matrix3d::Identity();

    Y_ = curr_gps_data.position_ned; //Y_measure
    K_ = P_ * G_.transpose() * (G_ * P_ * G_.transpose() + C_ * R_ * C_.transpose()).inverse(); // kalman增益

    P_ = (TypeMatrixP::Identity() - K_ * G_) * P_;
    // P_ = (TypeMatrixP::Identity() - K_ * G_) * P_ * (TypeMatrixP::Identity() - K_ * G_).transpose()+K_*C_*K_.transpose();
    X_ = X_ + K_ * (Y_ - G_ * X_);

    UpdateState();
    return true;
}

4. Summary

The difference between EKF and ESKF:

1) ESKF needs to calculate the odometer based on the equation of motion every time, and EKF’s prediction equation replaces the odometer calculation.

2) The state quantity of ESKF is more compact and has no redundancy, while EKF represents a rotated quaternion with redundancy.

5. References

Robot dynamics simulation - discretization of state space model - Gu Yueju

[With source code + code comments] Error-state Kalman Filter, extended Kalman filter, realizes GPS+IMU fusion, EKF ESKF GPS+IMU_Not cute at all Mengmeng's blog-CSDN blog _Error Kalman

Quaternion kinematics for error state kalman filter realizes GPS+IMU fusion, (with source code)_qq_38650944's blog-CSDN blog

Multi-sensor fusion positioning Chapter 4

Implementation of EKF filtering algorithm based on navigation information (source code attached)