【附源码+代码注释】误差状态卡尔曼滤波(error-state Kalman Filter)，扩展卡尔曼滤波，实现GPS+IMU融合，EKF ESKF GPS+IMU

大家好，又见面了，我是你们的朋友全栈君。

2021年6月23日更新:发现了一个讲卡尔曼滤波特别好的视频，但是需要科学上网。卡尔曼滤波视频

最近在学习卡尔曼滤波器，今天抽出点儿时间总结一下！

我的所有源码都放在Github的仓库里面了：eskf-gps-imu-fusion（记得要给我点star呀，哈哈）

在这篇博客中，我将会向你解释GPS融合IMU的扩展卡尔曼的推导过程，并且还会提供完整的源代码和数据。

首先来看一张实验结果，下图是我通过仿真gps和imu数据得到的融合结果，GPS的误差大约在5米，IMU的是中等精度，可以看到通过卡尔曼融合之后，误差降低非常多。

红色的GPS的测量数据，蓝色是轨迹的真值，绿色是融合之后的轨迹，可以看到融合了imu之后的轨迹，相比于只有GPS的情况提升了很多，甚至与真实值都非常接近了。

1.前言

卡尔曼滤波器在1958年被卡尔曼等人提出之后，经历了60多年，这期间有各种变体被提出来，但是核心的思想并没有变化。这其中比较突出的工作有，EKF、IEKF、ESKF、UKF等等。它们的方程有一些差异，但是用法基本区别不大，只要学会应用其中的一种，别的就问题不大了。

卡尔曼滤波器的公式推导，并不重要!
卡尔曼滤波器的公式推导，并不重要!
卡尔曼滤波器的公式推导，并不重要!
　　
如果你初学卡尔曼滤波器，千万不要在公式推导和理解上花费太多的精力，这将会得不偿失，根据我的学习经验，想直接从卡尔曼的公式推到上去理解用法是比较难的，但是先不管原因，边应用边学习，将会是一个非常正确的做法，对于理解卡尔曼滤波器将会事半功倍！

2. 状态误差卡尔曼（Error-State Kalman Filter，ESKF）

本篇博客我们将探索一下状态误差卡尔曼(ESKF)的应用，它是卡尔曼滤波器的变种中应用最为广泛的一种，与EKF一样，它也是一种针对时变系统的非线性滤波器。但是与EKF不同的是，它的线性化是总是在0附近，因此线性化更准确。

根据我的经验，绝大部分的场景，ESKF就足够使用了。如果对于滤波有更高的要求，可以选择UKF，甚至PF(例子滤波)。

2.1 状态方程的推导

在融合IMU和GPS的数据时，因为IMU的频率更高，所以常常用IMU的姿态解算作为轨迹增量的预测，如果使用EKF滤波器，那么就是这种做法。由于我们这里介绍的是更为复杂的ESKF，所以这里并不是对导航信息做滤波，而是对导航信息中的误差进行滤波，因为误差是小量，线性化时更精确。

这里直接给出IMU的误差方程，由于误差方程的推导比较复杂，需要的知识比较多，而且这里主要强调的是ESKF的用法，所以就忽略IMU误差方程的推导。

如果你想系统学习IMU误差方程的推导，推荐你看《捷联惯导算法与组合导航原理》（严恭敏）

你也可以直接记住下面的结论，先继续往后学习，当你把整个流程都走一遍之后，你就明白了！

IMU误差方程：
$$\begin{array}{rl}\dot{\varphi }& =\varphi \times {\omega }_{ie}^n\\ \delta \dot{V}& =f^n\times \varphi +\delta f^n\\ \delta \dot{P}& =\delta V\end{array}\text{\hspace{1em}(0)}$$其中，
　　$\varphi =[{\varphi }_E,{\varphi }_N,{\varphi }_U{]}^T$称为失准角
　　
　　${\omega }_{ie}^n=[0,\omega \cos L,\omega \sin L{]}^T$地球自转角速度的分量,$\omega =7.292115e(-5)rad/s$
　　
　　$\delta w_{ib}^n=C_b^n\ast [{\epsilon }_x{\epsilon }_y{\epsilon }_z{]}^T$，$\epsilon$是陀螺仪的bias在body frame坐标系下的分量，$C_b^n$是当前IMU body frame到导航系的变换矩阵
　　
　　$f^n=[f_E{f}_N{f}_U{]}^T$ 是IMU测量的加速度在导航系下的表示
　　
　　$\delta f^n=C_b^n\ast [{\nabla }_x{\nabla }_y{\nabla }_z{]}^T$，$\nabla$是加速度计的bias在body frame下的分量

然后，将以上变量全部带入公式（0），得到如下式子：
$$\left[\begin{array}{c}{\dot{\varphi }}_E\\ {\dot{\varphi }}_E\\ {\dot{\varphi }}_E\end{array}\right]=\left[\begin{array}{ccc}0& -{\varphi }_U& {\varphi }_N\\ {\varphi }_U& 0& -{\varphi }_E\\ -{\varphi }_N& {\varphi }_E& 0\end{array}\right]\left[\begin{array}{c}0\\ \omega \cos L\\ \omega \sin L\end{array}\right]-C_b^n\left[\begin{array}{c}{\epsilon }_x\\ {\epsilon }_y\\ {\epsilon }_z\end{array}\right]\text{\hspace{1em}(1)}$$

$$\left[\begin{array}{c}\delta {\dot{V}}_E\\ \delta {\dot{V}}_N\\ \delta {\dot{V}}_U\end{array}\right]=\left[\begin{array}{ccc}0& -f_U& f_N\\ f_U& 0& -f_E\\ -f_N& f_E& 0\end{array}\right]\left[\begin{array}{c}{\varphi }_E\\ {\varphi }_N\\ {\varphi }_U\end{array}\right]+C_b^n\left[\begin{array}{c}{\nabla }_x\\ {\nabla }_y\\ {\nabla }_z\end{array}\right]\text{\hspace{1em}(2)}$$

$$\left[\begin{array}{c}\delta {\dot{P}}_E\\ \delta {\dot{P}}_N\\ \delta {\dot{P}}_U\end{array}\right]=\left[\begin{array}{c}\delta V_E\\ \delta V_N\\ \delta V_U\end{array}\right]\text{\hspace{1em}(3)}$$

这里稍微解释一下误差方程，由于在做IMU的解算时，做了一些近似，而那些舍掉的东西就给IMU解算带来了误差。ESKF要做的就是对误差进行滤波，获得当前情况下误差的最优估计，然后将计算出来的位移、速度、姿态等量减去这个误差，修正由于近似带来的不准确。

IMU误差方程的推导可以参考《捷联惯导算法与组合导航原理》(严恭敏)

在滤波器中，状态方程一般都是写为如下形式：
$$\dot{X}=F_{t}X+B_{t}W\text{\hspace{1em}(4)}$$其中，$X$就是我们要估计的状态量，对于IMU+GPS的融合系统，需要估计的状态量可以有多种选择，这里我们选：位移误差、速度误差、姿态误差、陀螺仪的bias误差、加速度计的bias误差，将其写成向量如下：
$$X={\left[\begin{array}{ccccc}\delta P^T& \delta V^T& \delta {\varphi }^T& {\epsilon }^T& {\nabla }^T\end{array}\right]}^T\text{\hspace{1em}(5)}$$其中：
　　$\delta P^T={\left[\begin{array}{ccc}\delta P_E& \delta P_N& \delta P_U\end{array}\right]}^T$
　　$\delta V^T={\left[\begin{array}{ccc}\delta V_E& \delta V_N& \delta V_U\end{array}\right]}^T$
　　$\delta {\varphi }^T={\left[\begin{array}{ccc}\delta {\varphi }_E& \delta {\varphi }_N& \delta {\varphi }_U\end{array}\right]}^T$
　　${\epsilon }^T={\left[\begin{array}{ccc}\delta {\epsilon }_x& {\epsilon }_y& {\epsilon }_z\end{array}\right]}^T$
　　${\nabla }^T={\left[\begin{array}{ccc}{\nabla }_x& {\nabla }_y& {\nabla }_z\end{array}\right]}^T$

根据（1）（2）（3）等式，将状态量$X$带入方程（4），则可以得到一个完整的$F_t$矩阵：
$$F_t=\left[\begin{array}{cccccc}{0}_{3\times 3}& I_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& \\ 0_{3\times 3}& 0_{3\times 3}& F_{23}& 0_{3\times 3}& C_b^n& \\ 0_{3\times 3}& 0_{3\times 3}& F_{33}& -C_b^n& 0_{3\times 3}& \\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& \\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& \end{array}\right]\text{\hspace{1em}(6)}$$

其中：
$$F_{23}=\left[\begin{array}{ccc}0& -f_U& f_N\\ f_U& 0& -f_E\\ -f_N& f_E& 0\end{array}\right]$$$$F_{33}=\left[\begin{array}{ccc}0& \omega \sin L& -\omega \cos L\\ -\omega \sin L& 0& 0\\ \omega \cos L& 0& 0\end{array}\right]$$
　　　$C_b^n$是当前t时刻，IMU body frame到导航系$n$的变换

对于（4）式已经给出了$F_t$的完整形式，对于$B_t$和$W$，它们分别代表了噪声转移矩阵和IMU中陀螺仪与加速度计的bias噪声，它们的完整形式如下：

$$W=\left[\begin{array}{cccccc}{\omega }_{gx}& {\omega }_{gy}& {\omega }_{gz}& {\omega }_{ax}& {\omega }_{ay}& {\omega }_{az}\end{array}\right]$$其中，前三个是陀螺仪在三个轴上的bias噪声，后三个是加速度计在三个轴上的bias噪声。

而$B_t$的完整形式如下：
$$B_t=\left[\begin{array}{cc}{0}_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& C_b^n\\ -C_b^n& 0_{3\times 3}\\ 0_{6\times 3}& 0_{6\times 3}\end{array}\right]$$

2.2 观测方程的推导

前面说的预测值是来自于IMU，而对于观测值我们采用的是GPS的观测。

在滤波器中，观测方程的形式比较统一，一般写为：
$$Y=G_{t}X+C_{t}N\text{\hspace{1em}(7)}$$

在IMU和GPS的融合中，GPS观测量一般之后位置，所以$Y$的完整形式如下：
$$Y={\left[\begin{array}{ccc}\delta P_E& \delta P_N& \delta P_U\end{array}\right]}^T$$

由（5）式$X$状态量的形式，可以推算出$G_t$和$C_t$分别如下：
$$G_t=\left[\begin{array}{cc}{I}_{3\times 3}& I_{3\times 12}\end{array}\right]$$$$C_t=\left[\begin{array}{c}{I}_{3\times 3}\end{array}\right]$$

等式（7）中的$N$是观测噪声，它是由于GPS传感器不准确带来的，它的完整形式如下：
$$C_t=\left[\begin{array}{ccc}{n}_{P_E}& n_{P_N}& n_{P_U}\end{array}\right]$$

等式（4）和（7）就是IMU+GPS的系统下的状态方程和测量方程。

2.3 构建ESKF滤波器

首先我们先写出ESKF滤波器的五个经典公式：
$$\begin{array}{rl}{\check{x}}_k& =f({\hat{x}}_{k-1},v_k,0)\\ {\check{P}}_k& =F_{k-1}{\hat{P}}_{k-1}{F}_{k-1}^T+B_{k-1}{Q}_k{B}_{k-1}^T\\ K_k& ={\check{P}}_k{G}_k^T(G_k{\check{P}}_K{G}_k^T+C_k{R}_k{C}_k^T{)}^{-1}\\ {\hat{P}}_k& =(I-K_k{G}_k){\check{P}}_k\\ {\hat{x}}_k& ={\check{x}}_k+K_k(y_k-g({\check{x}}_k,0))\end{array}\text{\hspace{1em}(8)}$$

ESKF和EKF本身没有区别，只不过ESKF是使用的EKF滤波器对误差进行滤波

这五个公式就是ESKF滤波的基础。

根据2.1节和2.2节的推导，我们已经获得了IMU+GPS系统的状态方程和测量方程，现在我们要做的就是将状态方程和测量方程，应用到卡尔曼滤波器的五个公式中。

对于状态方程和测量方程的推导，都是在连续时间下完成的，想要应用到滤波器中，必须要离散化，离散化时按照采样时间进行离散化。对于（4）式中的$F_t$采用一阶泰勒近似，可以得到如下形式：
$$F_{k-1}=I_{15\times 15}+F_t({\hat{X}}_{k-1},v_k,0)T\text{\hspace{1em}(9)}$$ 公式（9）的实现过程：点此进入

而$B_t$的离散化形式为：
$$B_{k-1}=B_t({\hat{X}}_{k-1},v_k,0)T\text{\hspace{1em}(10)}$$其中，$T$为卡尔曼的滤波周期
公式（10）的实现过程：点此进入

对于观测方程，本生就是离散化的，所以可以直接使用。

将式子（9）（10）带入到（8）式中，得到最终的卡尔曼滤波器方程为：
$$\begin{array}{rl}{\check{X}}_k& =F_{k-1}{\hat{X}}_{k-1}+B_{k-1}{W}_k\\ {\check{P}}_k& =F_{k-1}{\hat{P}}_{k-1}{F}_{k-1}^T+B_{k-1}{Q}_k{B}_{k-1}^T\\ K_k& ={\check{P}}_k{G}_k^T(G_k{\check{P}}_K{G}_k^T+C_k{R}_k{C}_k^T{)}^{-1}\\ {\hat{P}}_k& =(I-K_k{G}_k){\check{P}}_k\\ {\hat{X}}_k& ={\check{X}}_k+K_k(Y_k-G_k{\check{X}}_k)\end{array}\text{\hspace{1em}(11)}$$等式组(11)中的前两个等式对应的是预测过程，它主要用来预测状态量和状态量对应的协方差，第三个等式的$K_k$是卡尔曼增益，它是用来决策当前次的预测和测量中，更应该相信谁。第四个和第五个等式则是矫正前两个方程预测时的误差。
公式组（11）的五个公式的代码实现过程分别是：①、②、③、④、⑤

至此，整个基于IMU和GPS的状态误差卡尔曼滤波（ESKF）推导完成。

ESKF是对状态的误差进行矫正，所以ESKF每次迭代完成之后，必须要将状态量$X$重新设置为零，但是协方差矩阵$P$中的数值需要保留，代码在此。

对于公式（11）中的$Y_K$它对应的就是测量误差，对于GPS和IMU的系统，它的计算方法是：GPS的测量值减去IMU的预测值，代码在此

每次使用卡尔曼迭代完成之后，都需要将最终得到的误差，重新带回到预测的位置、速度、方向中消除掉这个误差，代码在此

上面只是把结论给出来了，如果确实需要了解其中卡尔曼滤波器的推导过程，可以参考《机器人学中的状态估计》或者《Quaternion kinematics for the error-state Kalman filter》

2.4 可观测性和可观测度分析

可观测性分析：在设计滤波器时，有些状态量是不可观，如果此时继续将这些状态量带入滤波器进行滤波，可能导致滤波器不收敛，或者收敛到一个错误的值。可观测度就是用来判断状态量是否都会收敛。

可观测度分析：可观测分析是为了评估状态量在滤波时收敛的快慢和收敛的精度，对于一些可观测度非常小的状态量可以考虑进行限制或者直接扔掉。

本文所讲的IMU+GPS滤波系统是一种时变系统，对于时变系统它的可观测性和可观测度分析，可以参考论文《Observability Analysis of Piece-Wise Constant Systems》

这里我不再展示详细的推导过程，直接给出结论，如果你对这部分内容有兴趣，请阅读上面这篇论文，并且在我的代码中也给出了实现过程，点此进入，你可以参考。

IMU+GPS的扩展卡尔曼滤波器系统，可观测度和可观测性分析结论：

• 载体静止或着匀速运动时：航向角， x 轴加速度bias和 y 轴加速度bias均不可观，而且z 轴角速度bias虽然收敛，但是收敛较慢；
• 载体加减速时： 所有状态量基本都可观，z轴角速度零偏，x 轴加速度bias，y轴加速度bias收敛较慢，需要几十秒，但是比静止和匀速运动时提升了很多；
• 载体转向时： 所有状态量基本都可观，但是航向角和加速度计bias以及角速度计bias参数收敛速度有一些慢，需要几十秒才能收敛，但是比静止和匀速运动时提升了很多。

2.5 总结

上面展示的这些流程，就是从IMU的误差方程到扩展卡尔曼滤波器的所有流程，其中很多的推导比较难理解，我能省略的就尽量省略了。

但是有以下的一些点，你需要注意：

• 上面的IMU误差方程是基于高精度的IMU推导的，考虑了地球的转速了，如果你的IMU精度一般，你可以忽略地球的转速，也就是上式（6）中的$F_{33}$设置为0矩阵，并不会对精度产生很大的影响；
• 另外有一点你一定要格外的注意，上面方程的推导都是基于导航坐标系的，而你的IMU结果是针对body坐标系的，切记不要弄混了坐标系；
• 矩阵$F_{33}$是加速度计敏感到的加速度在导航系下的表示，代码中不要忘记了这一步，另外还要注意这里的加速度不需要去掉重力加速度；
• 还要格外的注意单位的统一，GPS的测量值是经纬高，要转换到导航系下，表示成$x,y,z$单位是米，并且IMU的角速度是以弧度为基础的；
• 卡尔曼滤波器公式（11）中的第一个等式是状态的传递方程，后面有一个噪声项，你可以加上，也可以忽略，根据你的实际情况去决定是否需要。我的实验结果是，加不加影响不大。

3.代码实现

a. 我的所有源码都放在Github的仓库里面了：eskf-gps-imu-fusion

b. 在代码的文件夹/data/raw_data中我提供了方针的IMU和GPS数据，它们的生成是通过gnss-ins-sim，关于gnss-ins-sim的使用方法，作者提供了比较详细的使用教程，你按照教程就能学会它的使用方法，另外你需要注意的是，不要把坐标系弄混了。我在eskf-gps-imu-fusion文件夹下的gnss-ins-sim文件中放了我所使用的脚本和运动控制文件，你按照readme中的方法操作，就能实现数据仿真了。

c. 融合过程中产生的轨迹和融合数据都保存在data文件夹下，它们都是以kitti的姿态格式保存的，如果你想可视化它们，我推荐你使用evo，我写过一篇介绍它的使用方法的博客：点击入口，如果你不会使用，可以参考一下。

安装好evo之后，使用如下命令可以显示运行之后的轨迹，

evo_traj kitti fused.txt gt.txt measured.txt -p

d. 对于滤波器误差噪声的调节，是在config文件夹下的config.yaml文件中。

当你运行起我的代码之后，你可以或者如下的这样融合轨迹：

一定要把上面的公式和代码结合起来看，如果实在难以理解，可以先把这个流程和实现过程弄明白，然后再边用变理解，不然你可能很难学会！