误差状态卡尔曼滤波(error-state Kalman Filter)，扩展卡尔曼滤波，实现GPS+IMU融合，EKF ESKF GPS+IMU

转自：【附源码+代码注释】误差状态卡尔曼滤波(error-state Kalman Filter)，扩展卡尔曼滤波，实现GPS+IMU融合，EKF ESKF GPS+IMU_一点儿也不萌的萌萌的博客-CSDN博客_误差状态卡尔曼滤波

原作者功力深厚，请移步认真学习。

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误差方程的推导，推荐你看《捷联惯导算法与组合导航原理》（严恭敏）