2023年9月4日更新：重构代码，修复代码BUG，修复公式错误，统一坐标系。 2021年6月23日更新：发现了一个讲卡尔曼滤波特别好的视频，但是需要科学上网。卡尔曼滤波视频 最近在学习卡尔曼滤波器，今天抽出点儿时间总结一下！ 我的所有源码都放在Github的仓库里面了：https://github.com/zm0612/eskf-gps-imu-fusion（记得要给我点star呀，哈哈

IMU&GNSS的误差状态卡尔曼滤波器（ESKF）---更新过程 ESKF的更新过程 ESKF的更新过程 前面介绍的是ESKF的运动过程，现在考虑更新过程。假设一个抽象的传感器能够对状态变量产生观测，其观测方程为抽象的h,那么可以写为 其中z为观测数据，v为观测噪声，V为该噪声的协方差矩阵。 在传统的EKF中，可以直观地对观测方程线性化，求出观测方程相对与状态变量的雅克比矩阵

IMU&GNSS 误差状态卡尔曼滤波器（ESKF）的离散时间的ESKF 运动方程与运动过程 离散时间的ESKF 运动方程ESKF的运动过程 离散时间的ESKF 运动方程 名义状态变量的离散时间运动方程可以写为：（不用考虑噪声，噪声在误差状态中） 误差状态变量的离散时间运动方程为： 式中右侧部分省略了括号里的（t）以简化公式 噪声项并不参与递推，需要把它们单独归入噪声部分。连

临近放假，老板要求回去之前找其汇报进展，无奈近几月忙于毕业论文的编写，实在是没有多少可以汇报的内容，想来自己弄得定位程序只能实现定位，要不自己再加一个路径规划，直接干！ 本文的文字量较大，请各位耐心阅读 首先，我自己的定位程序是参考网课写的，其主要实现的是基于ESKF的LIDAR/GNSS/IMU的多传感器融合定位系统，那么我自己的程序的TF树如下图所示。 可以看到，我的定位程序给出的是m