基于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。