最近在研究传感器融合，看到一个很好的开源项目，适合小白学习，为以后做传感器融合、SLAM、自动驾驶和室内定位等方向打下基础。
算法概述
题目：基于改进扩展卡尔曼滤波（Error State-EKF）的LiDAR/GNSS/IMU的传感器融合轨迹估计
关键词：改进扩展卡尔曼滤波（Error State Extended Kalman Filter，ES-EFK）、传感器融合、轨迹估计、激光雷达（LiDAR）、卫星导航（GNSS）、惯性测量元件（IMU）
算法的overview如下图所示：
整体而言，就是使用LiDAR、GNSS和IMU的数据根据ES-EKF进行融合迭代估计轨迹。在本项目中，IMU的采样频率较高，而GNSS和LiDAR的采样频率较低。该算法可以分为两个部分：预测（Prediction）和改正（Correction）。下面让我们一起看看这个算法。
Prediction
Prediction阶段是基于小车IMU测量的运动模型进行预测轨迹，然后再结合GNSS或LiDAR的数据用EKF融合改正预测的轨迹。
下面的小车的参数，包括位置pk、速度vk和姿态qk。
运动方程以IMU的数据作为输入，包括四元数转旋转矩阵Cns，比力加速度fk（由加速度计测量），角速度Wk（由陀螺仪测量）。这里需要注意的是，处理IMU数据的时候一般都用四元数，避免用欧拉角带来的死锁问题。
Correction
Correction阶段就是用GNSS或者LiDAR观测到的数据对估计的位置进行改正。
GNSS的观测方程：
LiDAR的观测方程：
ES-EKF算法
Error-state Extended Kalman Filter（ES-EKF）是一种改进的扩展卡尔曼滤波算法，基本思想就是将State分为两部分Nominal State和Error State。它用于状态估计问题，特别是对于非线性系统的状态估计问题，例如在机器人定位、导航和控制方面的应用。
x代表真值， x_hat代表Nominal State，占比较大， delta_x代表Error State，占比较小。
线性化如下：
ES-EKF试图通过引入误差状态来改进EKF的性能。它的核心思想是，通过对状态误差进行线性化而不是对状态本身进行线性化，可以更好地处理非线性性质，并提高滤波器的稳健性和准确性。相比起一般的EKF算法，优势在于：（1）Error State的线性化比Nominal State更好；（2）对于三维空间的数据处理较好，使用旋转的情况，因此很适合本项目的3D LiDAR数据。
详细推导过程可以参考该文章：ES-EKF算法推导
融合算法实现轨迹估计
1.使用IMU数据更新运动模型
2.不确定度计算和传播
3. 使用GNSS或LiDAR改正
3.1 计算卡尔曼增益
3.2 更新Error State
3.3 改正状态预测
3.4 计算改正方差
实验结果
该项目中，已经提供了预处理好的IMU、GNSS和LiDAR数据，参考轨迹和预测轨迹的对比实验结果如下：
位置和姿态矩阵误差：（蓝色实线是误差，红色虚线是不确定度）
以上就是这个小项目的主要算法介绍和实验结果，非常适合小白学习。最后，附上原作者的项目链接文章来源：https://www.toymoban.com/news/detail-856980.html
我个人改进后的项目链接文章来源地址https://www.toymoban.com/news/detail-856980.html
到了这里，关于基于ES-EKF的LiDAR/GNSS/IMU传感器融合轨迹估计（附项目源码）的文章就介绍完了。如果您还想了解更多内容，请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章，希望大家以后多多支持TOY模板网！
