基于ES-EKF的LiDAR/GNSS/IMU传感器融合轨迹估计（附项目源码）

最近在研究传感器融合，看到一个很好的开源项目，适合小白学习，为以后做传感器融合、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数据，参考轨迹和预测轨迹的对比实验结果如下：

位置和姿态矩阵误差：（蓝色实线是误差，红色虚线是不确定度）

以上就是这个小项目的主要算法介绍和实验结果，非常适合小白学习。最后，附上原作者的项目链接

我个人改进后的项目链接