参考资料

https://zhuanlan.zhihu.com/p/441182819

惯性导航(三)-基于流型的ESKF及代码实现_eskf和ekf-CSDN博客

用ESKF实现IMU/GNSS组合导航（学习记录）_eskf imu-CSDN博客

0.ESKF与KF的区别以及总体流程

0.1卡尔曼滤波过程

卡尔曼滤波的流程按照1-5分为预测和更新两部分

预测部分：

首先通过运动方程抽象得到状态转移矩阵F和系统控制量U，计算预测状态

通过状态转移矩阵F，初始时刻（上一时刻）的协方差矩阵和Q，预测新的协方差矩阵

更新部分：

通过观测方程的状态转移矩阵H，协方差矩阵，观测噪声协方差R，计算卡尔曼滤波增益

  (0.3)

通过传感器观测量与观测方程的计算值之差可以得到观测值与预测值存在的偏差，最终的估计值结合观测值与预测值存在的偏差对预测值进行校准，卡尔曼增益可以理解为校准系数。如果卡尔曼增益越大，说明越信任观测值（更多使用观测值对预测值进行校准），如果卡尔曼增益越小，说明越信赖预测值（说明系统方程很准确）。

最后通过预测协方差矩阵得到估计协方差矩阵

0.2ESKF的流程

ESKF可分为两部分

1）首先是对误差状态的卡尔曼滤波进行预测更新（其步骤与传统KF差不多，只是状态变量，转移矩阵等存在差异）

使用误差状态的离散方程推导状态转移矩阵F，进而推导P，在更新中通过误差状态的观测方程Z，计算观测方程相对误差状态的雅可比矩阵，进而计算K，以及误差状态的更新

2）其次是使用ESKF估计后的误差状态=真值-名义状态，加上名义状态变量()，得到最终的状态变量估计真值（）。

名义状态X计算过程中，使用上一时刻的真值（卡尔曼滤波状态估计）作为输入，以X状态变量的离散方程抽象状态矩阵A（和误差状态的不一样）

1.IMU的运动方程推导

1.1旋转矩阵的导数

1.2运动方程的离散模式

后续ESKF中，名义状态的离散公式就是下图推导，下图的推导原理为物理中的牛二定理，如红色笔记所述。

2.求误差状态变量的运动方程

名义状态：相当于运动方程预测的变量

真实状态：KF更新后的状态（近似为真值）

误差状态：真值-名义=K*（Z-HX）

2.1连续方程误差变量的导数

最终的连续方式误差状态导数（用于后续在离散方程中用导数来计算dt内的增量，如v(t+dt)=v(t)+v't(t)dt）

2.2离散状态误差变量的导数

名义状态的离散方程使用了1.2中运动方程的离散模式进行计算

误差状态的离散方程，使用了2.1中的误差状态的导数，通过dt时间的积分得到。

3.ESKF过程

3.1ESKF的预测过程

预测过程类似于卡尔曼滤波，通过上述6个状态运动方程，得到对应状态转移矩阵F，并利用F和系统噪声Q，计算协方差矩阵P

3.2ESKF的更新过程

总体过程类似于卡尔曼滤波，但是要注意把观测方程的雅可比矩阵H线性化

4.实际应用与代码公式结合

代码参考链接：

GitHub - gaoxiang12/slam_in_autonomous_driving: 《自动驾驶中的SLAM技术》对应开源代码

利用ESKF进行RTK和IMU的组合导航

4.1状态和变量的初始化

对imu运动方程的6个状态变量进行初始化，该状态变量X为18*1

4.2误差状态ESKF预测过程

其主要作用为

1.递推名义状态变量

2.计算误差状态协方差,误差状态转移矩阵F

通过上面的公式

  （4.1）

 （4.2）

其中（4.1）公式由于在每次进行ESKF更新后重新置为0，可以省略。由3.1节中的F公式，可以通过（4.2）公式计算协方差矩阵。

首先利用IMU的离散运动方程，进行名义状态的推导，公式结构为：

名义状态=F*真值状态

其中输入的真值状态为上一时刻ESKF的状态估计值，F是IMU离散方程的状态转移矩阵（和误差状态转移矩阵不同），输出的名义状态，用于结合误差状态得到最终的ESKF估计（作为此次计算真值），该真值并用于下一次的名义状态计算真值输入。

  （4.3）

其中，就是名义状态变量（由IMU运动离散方程计算），就是此次ESKF的估计（亦为下一次名义状态递推的输入），这是更新时的误差状态变量（预测时，dx=0）

4.3误差状态ESKF预测

对误差状态进行预测，其中

4.4更新名义状态变量

使用误差状态变量加上预测状态计算的名义状态X（通过IMU离散方程4.2节），可以得到误差校正后的名义状态，作为实际的卡尔曼滤波估计输出。