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

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

作者:互联网

        继上一篇实现了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