继上一篇实现了Joan Sola大神的ESKF之后，就想着举一反三，也实现一下EKF算法，因此就研究了一下深蓝学院的《多传感器融合定位》算法中基于导航信息的滤波算法，同时对算法进行代码实现。先附上两张效果图（看起来跟前一篇文章ESKF的差不多）。github地址为：GitHub - Shelfcol/gps_imu_fusion: gps_imu_fusion with eskf,ekf,ukf,etc
下面对公式进行详细的推导，也给出我自己的一些理解。
1. 前言
卡尔曼滤波的主要方程就是预测方程和观测方程的构建。两个方程如下：
有的模型可以很容易推出运动方程，而有的模型不好直接构建两个状态之间的关系，没法推导出运动方程，因此这里也可以使用状态量的微分方程来间接推导出运动方程。
2. 基于导航信息的滤波
1）导航信息的微分方程
这里使用的状态量为，其中
为角速度的bias，
为加速度的bias。值得注意的是，这里的四元数的顺序是实部在前，虚步在后，这个顺序也可以从后面求解F矩阵时可以看出。
导航信息的微分方程如下：
因此可以将状态方程写成如下的微分形式：
它具有非线性，所以需要进行线形话，可以得到
2） 雅可比矩阵求解
将微分方程进行线性化
雅可比矩阵推导：
这里需要说明一下四元数乘积：
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.参考文献
Quaternion kinematics for error state kalman filter实现GPS+IMU融合，（附源码）_qq_38650944的博客-CSDN博客
多传感器融合定位第四章