C++和Python使用误差状态卡尔曼滤波器融合GPS和IMU

资源下载地址：https://download.csdn.net/download/sheziqiong/88304498
资源下载地址：https://download.csdn.net/download/sheziqiong/88304498

在这篇博客中，我将会向你解释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误差方程：

其中，

然后，将以上变量全部带入公式（0），得到如下式子：

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

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

在滤波器中，状态方程一般都是写为如下形式：

其中，$X$就是我们要估计的状态量，对于IMU+GPS的融合系统，需要估计的状态量可以有多种选择，这里我们选：位移误差、速度误差、姿态误差、陀螺仪的bias误差、加速度计的bias误差，将其写成向量如下：

其中，

根据（1）（2）（3）等式，将状态量X XX带入方程（4），则可以得到一个完整的$F_t$矩阵：

其中：

$C_b^n$是当前t时刻，IMU body frame到导航系n的变换

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

其中，前三个是陀螺仪在三个轴上的bias噪声，后三个是加速度计在三个轴上的bias噪声。

而$B_t$的完整形式如下：

2.2 观测方程的推导

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

在滤波器中，观测方程的形式比较统一，一般写为：

在IMU和GPS的融合中，GPS观测量一般之后位置，所以Y YY的完整形式如下：

由（5）式$X$状态量的形式，可以推算出$G_t$和$C_t$分别如下：

等式（7）中的$N$是观测噪声，它是由于GPS传感器不准确带来的，它的完整形式如下：

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

2.3 构建ESKF滤波器

首先我们先写出ESKF滤波器的五个经典公式：

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

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

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

对于状态方程和测量方程的推导，都是在连续时间下完成的，想要应用到滤波器中，必须要离散化，离散化时按照采样时间进行离散化。对于（4）式中的$F_t$ 采用一阶泰勒近似，可以得到如下形式：

公式（9）的实现过程：点此进入

而$B_t$ 的离散化形式为：

其中，T为卡尔曼的滤波周期

公式（10）的实现过程：点此进入

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

将式子（9）（10）带入到（8）式中，得到最终的卡尔曼滤波器方程为：

等式组(11)中的前两个等式对应的是预测过程，它主要用来预测状态量和状态量对应的协方差，第三个等式的 $K_k$ 是卡尔曼增益，它是用来决策当前次的预测和测量中，更应该相信谁。第四个和第五个等式则是矫正前两个方程预测时的误差。

公式组（11）的五个公式的代码实现过程分别是：①、②、③、④、⑤

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

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

对于公式（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 x,y,zx,y,z单位是米，并且IMU的角速度是以弧度为基础的；
• 卡尔曼滤波器公式（11）中的第一个等式是状态的传递方程，后面有一个噪声项，你可以加上，也可以忽略，根据你的实际情况去决定是否需要。我的实验结果是，加不加影响不大。

3.代码实现

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

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

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

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

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

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

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

资源下载地址：https://download.csdn.net/download/sheziqiong/88304498
资源下载地址：https://download.csdn.net/download/sheziqiong/88304498