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

以你之姓@ 2024-03-08 08:14 44阅读 0赞

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

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

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

![a4869e64f6170ef1b5fa5cd4f69bd71d.png][]

红色的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误差方程：

![5b25634baff6cab19de6965f68720d03.png][]

其中，

![b7aaf30b1d6b8fa45cd4f89a80a62979.png][]

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

![e72d15b8c60f171d0a9d9a4899e10283.png][]

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

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

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

![46212e469a449e2b53cf7acce526be37.png][]

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

![3809ee24033635e447917e1b95406cc8.png][]

其中，

![863df794b1a7429c964cdd9dcda0a111.png][]

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

![fa064744712f7e587b7b56431e605bd3.png][]

其中：

![3f5343cb34fc77490be96fa8a758d749.png][]

C b n C\_\{b\}^\{n\} Cbn是当前t时刻，IMU body frame到导航系n的变换

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

![f4945afef5e6fcefaa7c8be3beaf32bf.png][]

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

而 B t B\_t Bt的完整形式如下：

![2847226fc2181ece5479aa76ac175faf.png][]

#### 2.2 观测方程的推导 ####

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

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

![da85c2e6f4af3b52e9189aef9af6328e.png][]

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

![c4428dbe6d7879dadd514e1d2319826a.png][]

由（5）式 X X X状态量的形式，可以推算出 G t G\_t Gt和 C t C\_t Ct分别如下：

![94aab4d38dd7c0fd46cb1f6b5035a34b.png][]

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

![d4bea512d060381fd73697eb9f33ef7d.png][]

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

#### 2.3 构建ESKF滤波器 ####

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

![26d423eb3622059cb9b06aa6bbd45b6b.png][]

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

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

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

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

![1b98c152c60e9eb1feb709b8c1cd2fe5.png][]

公式（9）的实现过程：[点此进入][Link 2]

而 B t B\_t Bt 的离散化形式为：

![b2ec3a338bd4ac8baf841e82b728b065.png][]

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

公式（10）的实现过程：[点此进入][Link 3]

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

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

![9558148d40227c77bc52d6f0d3a5d63c.png][]

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

公式组（11）的五个公式的代码实现过程分别是：[①][Link 4]、[②][Link 5]、[③][Link 6]、[④][Link 7]、[⑤][Link 8]

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

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

对于公式（11）中的  Y k Y\_k Yk 它对应的就是测量误差，对于GPS和IMU的系统，它的计算方法是：GPS的测量值减去IMU的预测值，[代码在此][Link 10]

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

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

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

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

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

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

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

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

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

#### 2.5 总结 ####

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

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

*  上面的IMU误差方程是基于高精度的IMU推导的，考虑了地球的转速了，如果你的IMU精度一般，你可以忽略地球的转速，也就是上式（6）中的  F 33 F\_\{33\} F33设置为0矩阵，并不会对精度产生很大的影响；
 *  另外有一点你一定要格外的注意，上面方程的推导都是基于导航坐标系的，而你的IMU结果是针对body坐标系的，切记不要弄混了坐标系；
 *  矩阵 F 33 F\_\{33\} F33是加速度计敏感到的加速度在导航系下的表示，代码中不要忘记了这一步，另外还要注意这里的加速度不需要去掉重力加速度；
 *  还要格外的注意单位的统一，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][]，我写过一篇介绍它的使用方法的博客：[点击入口][Link 13]，如果你不会使用，可以参考一下。
    
    安装好evo之后，使用如下命令可以显示运行之后的轨迹，

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

*  对于滤波器误差噪声的调节，是在`config`文件夹下的`config.yaml`文件中。
    
    当你运行起我的代码之后，你可以或者如下的这样融合轨迹：

![b87b7141e5a58bee5739b44e3cf2839a.png][]

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

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

[Link 1]: https://download.csdn.net/download/sheziqiong/88304498
[a4869e64f6170ef1b5fa5cd4f69bd71d.png]: https://image.dandelioncloud.cn/pgy_files/images/2024/03/08/d75c48b568d54eaebea4ab262f9a2cd7.png
[5b25634baff6cab19de6965f68720d03.png]: https://image.dandelioncloud.cn/pgy_files/images/2024/03/08/bd3e39c9241e4499856769969329790b.png
[b7aaf30b1d6b8fa45cd4f89a80a62979.png]: https://image.dandelioncloud.cn/pgy_files/images/2024/03/08/3605423665aa4c87a561154f688e6c66.png
[e72d15b8c60f171d0a9d9a4899e10283.png]: https://image.dandelioncloud.cn/pgy_files/images/2024/03/08/bac437153e9940018b7cc32d18cfbbe0.png
[46212e469a449e2b53cf7acce526be37.png]: https://image.dandelioncloud.cn/pgy_files/images/2024/03/08/d2344021d8ab4674805b38bbbdcc70d5.png
[3809ee24033635e447917e1b95406cc8.png]: https://image.dandelioncloud.cn/pgy_files/images/2024/03/08/81b1d4a6f4504bc69e0a76f124442c3e.png
[863df794b1a7429c964cdd9dcda0a111.png]: https://image.dandelioncloud.cn/pgy_files/images/2024/03/08/681684ab1a7c4faa90761968676c474d.png
[fa064744712f7e587b7b56431e605bd3.png]: https://image.dandelioncloud.cn/pgy_files/images/2024/03/08/1274c6899e8b4fdfae3f1fdd5e626df0.png
[3f5343cb34fc77490be96fa8a758d749.png]: https://image.dandelioncloud.cn/pgy_files/images/2024/03/08/5b3eff18e83f4660a69e430ca9088781.png
[f4945afef5e6fcefaa7c8be3beaf32bf.png]: https://image.dandelioncloud.cn/pgy_files/images/2024/03/08/ad0120d2c1c649fa8e9187fc2c47d7b7.png
[2847226fc2181ece5479aa76ac175faf.png]: https://image.dandelioncloud.cn/pgy_files/images/2024/03/08/15e3fd8183af4b90ad99e047cae4cbc0.png
[da85c2e6f4af3b52e9189aef9af6328e.png]: https://image.dandelioncloud.cn/pgy_files/images/2024/03/08/26a0807a41624ea69fe86e934ac4f031.png
[c4428dbe6d7879dadd514e1d2319826a.png]: https://image.dandelioncloud.cn/pgy_files/images/2024/03/08/8e9903a5a3b94e21b8a0a890503a7433.png
[94aab4d38dd7c0fd46cb1f6b5035a34b.png]: https://image.dandelioncloud.cn/pgy_files/images/2024/03/08/436e42bbb62e444e8b12d12971c15f72.png
[d4bea512d060381fd73697eb9f33ef7d.png]: https://image.dandelioncloud.cn/pgy_files/images/2024/03/08/75d689379d0d432f9d0a2ae762b383ab.png
[26d423eb3622059cb9b06aa6bbd45b6b.png]: https://image.dandelioncloud.cn/pgy_files/images/2024/03/08/d347aac3a7b645f1a9d9ec64d3883e52.png
[1b98c152c60e9eb1feb709b8c1cd2fe5.png]: https://image.dandelioncloud.cn/pgy_files/images/2024/03/08/31550dcaced14d08b7a1ff7637b5f326.png
[Link 2]: https://github.com/zm0612/eskf-gps-imu-fusion/blob/cda303dae87bb1e11f79268e3c39257e7a25211f/src/eskf.cpp#L134
[b2ec3a338bd4ac8baf841e82b728b065.png]: https://image.dandelioncloud.cn/pgy_files/images/2024/03/08/529d891ab4434530ab2f595661a80d64.png
[Link 3]: https://github.com/zm0612/eskf-gps-imu-fusion/blob/cda303dae87bb1e11f79268e3c39257e7a25211f/src/eskf.cpp#L135
[9558148d40227c77bc52d6f0d3a5d63c.png]: https://image.dandelioncloud.cn/pgy_files/images/2024/03/08/d644024b7eab41fdb006e1d9d52d755e.png
[Link 4]: https://github.com/zm0612/eskf-gps-imu-fusion/blob/cda303dae87bb1e11f79268e3c39257e7a25211f/src/eskf.cpp#L139
[Link 5]: https://github.com/zm0612/eskf-gps-imu-fusion/blob/cda303dae87bb1e11f79268e3c39257e7a25211f/src/eskf.cpp#L140
[Link 6]: https://github.com/zm0612/eskf-gps-imu-fusion/blob/cda303dae87bb1e11f79268e3c39257e7a25211f/src/eskf.cpp#L98
[Link 7]: https://github.com/zm0612/eskf-gps-imu-fusion/blob/cda303dae87bb1e11f79268e3c39257e7a25211f/src/eskf.cpp#L100
[Link 8]: https://github.com/zm0612/eskf-gps-imu-fusion/blob/cda303dae87bb1e11f79268e3c39257e7a25211f/src/eskf.cpp#L101
[Link 9]: https://github.com/zm0612/eskf-gps-imu-fusion/blob/aa92472323157b326a2cee6c28a190237b61cd6a/src/eskf.cpp#L105
[Link 10]: https://github.com/zm0612/eskf-gps-imu-fusion/blob/aa92472323157b326a2cee6c28a190237b61cd6a/src/eskf.cpp#L96
[Link 11]: https://github.com/zm0612/eskf-gps-imu-fusion/blob/aa92472323157b326a2cee6c28a190237b61cd6a/src/eskf.cpp#L103
[Link 12]: https://github.com/zm0612/eskf-gps-imu-fusion/blob/cda303dae87bb1e11f79268e3c39257e7a25211f/src/observability_analysis.cpp#L34
[evo]: https://github.com/MichaelGrupp/evo
[Link 13]: https://blog.csdn.net/u011341856/article/details/104594392
[b87b7141e5a58bee5739b44e3cf2839a.png]: https://image.dandelioncloud.cn/pgy_files/images/2024/03/08/9cabd149a4e341499f9d06d909d2bee0.png