# 误差状态卡尔曼滤波(error-state Kalman Filter)，扩展卡尔曼滤波，实现GPS+IMU融合，EKF ESKF GPS+IMU
> 发现了一个讲[卡尔曼](https://so.csdn.net/so/search?q=卡尔曼&spm=1001.2101.3001.7020)滤波特别好的视频，但是需要科学上网。[卡尔曼滤波视频](https://www.youtube.com/watch?v=CaCcOwJPytQ&list=PLX2gX-ftPVXU3oUFNATxGXY90AULiqnWT&index=1)
最近在学习卡尔曼滤波器，今天抽出点儿时间总结一下！
在这篇博客中，我将会向你解释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 Y*Y*的完整形式如下：
由（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）的实现过程：[点此进入](https://github.com/zm0612/eskf-gps-imu-fusion/blob/cda303dae87bb1e11f79268e3c39257e7a25211f/src/eskf.cpp#L134)
而$B_t$ 的离散化形式为�
C++和Python使用误差状态卡尔曼滤波器融合GPS和IMU.zip
版权申诉
5星 · 超过95%的资源 42 浏览量
2023-09-06
08:31:02
上传
评论
收藏 4.71MB ZIP 举报 
C++和Python使用误差状态卡尔曼滤波器融合GPS和IMU.zip （56个子文件） 
C++和Python使用误差状态卡尔曼滤波器融合GPS和IMU eskf-gps-imu-fusion include 
global_defination.h.in 185B tool.h 590B observability_analysis.h 1KB imu_flow.h 600B imu_data.h 535B eskf.h 5KB eskf_flow.h 1KB gps_data.h 607B gps_flow.h 701B
CMakeLists.txt 989B src eskf_flow.cpp 5KB gps_flow.cpp 6KB observability_analysis.cpp 3KB eskf.cpp 9KB imu_flow.cpp 7KB app gps_imu_fusion.cpp 359B data raw_data ref_gyro.csv 3.49MB ref_accel.csv 3.45MB accel-0.csv 3.47MB gps_time.csv 117KB time.csv 1.14MB gps-0.csv 713KB ref_gps.csv 708KB gyro-0.csv 3.49MB LICENSE 1KB cmake global_defination.cmake 219B YAML.cmake 178B sophus.cmake 129B eigen.cmake 73B 3rd GeographicLib include Geocentric Math.hpp 34KB Constants.hpp 16KB Config.h 408B Geocentric.hpp 11KB LocalCartesian.hpp 9KB CMakeLists.txt 1KB src Math.cpp 2KB LocalCartesian.cpp 2KB Geocentric.cpp 7KB sophus rxso3.hpp 22KB so3.hpp 21KB test_macros.hpp 5KB se3.hpp 21KB sim3.hpp 23KB types.hpp 2KB common.hpp 4KB so2.hpp 16KB example_ensure_handler.cpp 1KB se2.hpp 19KB test test_gps.cpp 904B test_imu.cpp 816B .gitignore 815B README.md 15KB gnss-inss-sim my_test.csv 346B readme.md 1KB my_test.py 1KB config config.yaml 306B资源评论
朱紫冬相识是缘！2023-11-10资源有一定的参考价值，与资源描述一致，很实用，能够借鉴的部分挺多的，值得下载。
449631802023-12-25资源有一定的参考价值，与资源描述一致，很实用，能够借鉴的部分挺多的，值得下载。
