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

2024-04-10 09:36

本文主要是介绍基于ES-EKF的LiDAR/GNSS/IMU传感器融合轨迹估计(附项目源码),希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

基于改进EKF的LiDAR/GNSS/IMU传感器融合轨迹估计(附项目源码)

  • 算法概述
    • Prediction
    • Correction
    • ES-EKF算法
    • 融合算法实现轨迹估计
    • 实验结果

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

在这里插入图片描述
位置和姿态矩阵误差:(蓝色实线是误差,红色虚线是不确定度)
在这里插入图片描述

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

我个人改进后的项目链接

这篇关于基于ES-EKF的LiDAR/GNSS/IMU传感器融合轨迹估计(附项目源码)的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/890696

相关文章

Python实现无痛修改第三方库源码的方法详解

《Python实现无痛修改第三方库源码的方法详解》很多时候,我们下载的第三方库是不会有需求不满足的情况,但也有极少的情况,第三方库没有兼顾到需求,本文将介绍几个修改源码的操作,大家可以根据需求进行选择... 目录需求不符合模拟示例 1. 修改源文件2. 继承修改3. 猴子补丁4. 追踪局部变量需求不符合很

一文教你如何将maven项目转成web项目

《一文教你如何将maven项目转成web项目》在软件开发过程中,有时我们需要将一个普通的Maven项目转换为Web项目,以便能够部署到Web容器中运行,本文将详细介绍如何通过简单的步骤完成这一转换过程... 目录准备工作步骤一:修改​​pom.XML​​1.1 添加​​packaging​​标签1.2 添加

tomcat多实例部署的项目实践

《tomcat多实例部署的项目实践》Tomcat多实例是指在一台设备上运行多个Tomcat服务,这些Tomcat相互独立,本文主要介绍了tomcat多实例部署的项目实践,具有一定的参考价值,感兴趣的可... 目录1.创建项目目录,测试文China编程件2js.创建实例的安装目录3.准备实例的配置文件4.编辑实例的

springboot集成Deepseek4j的项目实践

《springboot集成Deepseek4j的项目实践》本文主要介绍了springboot集成Deepseek4j的项目实践,文中通过示例代码介绍的非常详细,对大家的学习或者工作具有一定的参考学习价... 目录Deepseek4j快速开始Maven 依js赖基础配置基础使用示例1. 流式返回示例2. 进阶

SpringBoot项目启动报错"找不到或无法加载主类"的解决方法

《SpringBoot项目启动报错找不到或无法加载主类的解决方法》在使用IntelliJIDEA开发基于SpringBoot框架的Java程序时,可能会出现找不到或无法加载主类com.example.... 目录一、问题描述二、排查过程三、解决方案一、问题描述在使用 IntelliJ IDEA 开发基于

SpringBoot项目使用MDC给日志增加唯一标识的实现步骤

《SpringBoot项目使用MDC给日志增加唯一标识的实现步骤》本文介绍了如何在SpringBoot项目中使用MDC(MappedDiagnosticContext)为日志增加唯一标识,以便于日... 目录【Java】SpringBoot项目使用MDC给日志增加唯一标识,方便日志追踪1.日志效果2.实现步

Spring 中 BeanFactoryPostProcessor 的作用和示例源码分析

《Spring中BeanFactoryPostProcessor的作用和示例源码分析》Spring的BeanFactoryPostProcessor是容器初始化的扩展接口,允许在Bean实例化前... 目录一、概览1. 核心定位2. 核心功能详解3. 关键特性二、Spring 内置的 BeanFactory

Ubuntu中Nginx虚拟主机设置的项目实践

《Ubuntu中Nginx虚拟主机设置的项目实践》通过配置虚拟主机,可以在同一台服务器上运行多个独立的网站,本文主要介绍了Ubuntu中Nginx虚拟主机设置的项目实践,具有一定的参考价值,感兴趣的可... 目录简介安装 Nginx创建虚拟主机1. 创建网站目录2. 创建默认索引文件3. 配置 Nginx4

SpringBoot项目启动错误:找不到或无法加载主类的几种解决方法

《SpringBoot项目启动错误:找不到或无法加载主类的几种解决方法》本文主要介绍了SpringBoot项目启动错误:找不到或无法加载主类的几种解决方法,具有一定的参考价值,感兴趣的可以了解一下... 目录方法1:更改IDE配置方法2:在Eclipse中清理项目方法3:使用Maven命令行在开发Sprin

Nginx实现高并发的项目实践

《Nginx实现高并发的项目实践》本文主要介绍了Nginx实现高并发的项目实践,文中通过示例代码介绍的非常详细,对大家的学习或者工作具有一定的参考学习价值,需要的朋友们下面随着小编来一起学习学习吧... 目录使用最新稳定版本的Nginx合理配置工作进程(workers)配置工作进程连接数(worker_co