首页 > 编程语言> > 基于导航信息的EKF滤波算法实现（附源码）

基于导航信息的EKF滤波算法实现（附源码）

2022-03-20 18:31:14 作者：互联网

继上一篇实现了Joan Sola大神的ESKF之后，就想着举一反三，也实现一下EKF算法，因此就研究了一下深蓝学院的《多传感器融合定位》算法中基于导航信息的滤波算法，同时对算法进行代码实现。先附上两张效果图（看起来跟前一篇文章ESKF的差不多）。

下面对公式进行详细的推导，也给出我自己的一些理解。

1. 前言

卡尔曼滤波的主要方程就是预测方程和观测方程的构建。两个方程如下：

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

$y_k=G(x_k)+C_kn_k$

有的模型可以很容易推出运动方程，而有的模型不好直接构建两个状态之间的关系，没法推导出运动方程，因此这里也可以使用状态量的微分方程来间接推导出运动方程。

2. 基于导航信息的滤波

1）导航信息的微分方程

这里使用的状态量为 $X=[P^T \ V^T \ q^T \ \epsilon^T \bigtriangledown^T]_{16 \times 1}$ ，其中 $\epsilon$ 为角速度的bias， $\bigtriangledown$ 为加速度的bias。值得注意的是，这里的四元数的顺序是实部在前，虚步在后，这个顺序也可以从后面求解F矩阵时可以看出。

导航信息的微分方程如下：

因此可以将状态方程写成如下的微分形式：

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

它具有非线性，所以需要进行线形话，可以得到

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

2）雅可比矩阵求解

将微分方程进行线性化

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

雅可比矩阵推导：

这里需要说明一下四元数乘积：

3）连续系统微分方程离散化

因为得到的是连续时间微分方程，所以这里需要进行离散化。这里参考的是：机器人动力学仿真——状态空间模型的离散化

3-1）精确离散化

3-2）近似离散化

3-3）本系统离散化结果

3. 代码说明

代码里面是严格按按照公式进行推导的，这里只贴出预测和更新方程的关键代码

预测：

// 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;
}

更新：

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. 总结

EKF和ESKF的区别：

1）ESKF每次需要根据运动方程算里程计，而EKF的预测方程就代替了里程计的计算

2）ESKF的状态量更紧凑，没有冗余，而EKF表示旋转的四元数有冗余

5.参考文献

机器人动力学仿真——状态空间模型的离散化 - 古月居

【附源码+代码注释】误差状态卡尔曼滤波(error-state Kalman Filter)，扩展卡尔曼滤波，实现GPS+IMU融合，EKF ESKF GPS+IMU_一点儿也不萌的萌萌的博客-CSDN博客_误差卡尔曼

Quaternion kinematics for error state kalman filter实现GPS+IMU融合，（附源码）_qq_38650944的博客-CSDN博客

多传感器融合定位第四章

标签：INDEX,curr,Eigen,EKF,滤波,_.,STATE,源码,data
来源： https://blog.csdn.net/qq_38650944/article/details/123594568