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