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

相关文章

这15个Vue指令,让你的项目开发爽到爆

1. V-Hotkey 仓库地址: github.com/Dafrok/v-ho… Demo: 戳这里 https://dafrok.github.io/v-hotkey 安装: npm install --save v-hotkey 这个指令可以给组件绑定一个或多个快捷键。你想要通过按下 Escape 键后隐藏某个组件,按住 Control 和回车键再显示它吗?小菜一碟: <template

如何用Docker运行Django项目

本章教程,介绍如何用Docker创建一个Django,并运行能够访问。 一、拉取镜像 这里我们使用python3.11版本的docker镜像 docker pull python:3.11 二、运行容器 这里我们将容器内部的8080端口,映射到宿主机的80端口上。 docker run -itd --name python311 -p

JAVA智听未来一站式有声阅读平台听书系统小程序源码

智听未来,一站式有声阅读平台听书系统 🌟&nbsp;开篇:遇见未来,从“智听”开始 在这个快节奏的时代,你是否渴望在忙碌的间隙,找到一片属于自己的宁静角落?是否梦想着能随时随地,沉浸在知识的海洋,或是故事的奇幻世界里?今天,就让我带你一起探索“智听未来”——这一站式有声阅读平台听书系统,它正悄悄改变着我们的阅读方式,让未来触手可及! 📚&nbsp;第一站:海量资源,应有尽有 走进“智听

在cscode中通过maven创建java项目

在cscode中创建java项目 可以通过博客完成maven的导入 建立maven项目 使用快捷键 Ctrl + Shift + P 建立一个 Maven 项目 1 Ctrl + Shift + P 打开输入框2 输入 "> java create"3 选择 maven4 选择 No Archetype5 输入 域名6 输入项目名称7 建立一个文件目录存放项目,文件名一般为项目名8 确定

Java ArrayList扩容机制 (源码解读)

结论:初始长度为10,若所需长度小于1.5倍原长度,则按照1.5倍扩容。若不够用则按照所需长度扩容。 一. 明确类内部重要变量含义         1:数组默认长度         2:这是一个共享的空数组实例,用于明确创建长度为0时的ArrayList ,比如通过 new ArrayList<>(0),ArrayList 内部的数组 elementData 会指向这个 EMPTY_EL

如何在Visual Studio中调试.NET源码

今天偶然在看别人代码时,发现在他的代码里使用了Any判断List<T>是否为空。 我一般的做法是先判断是否为null,再判断Count。 看了一下Count的源码如下: 1 [__DynamicallyInvokable]2 public int Count3 {4 [__DynamicallyInvokable]5 get

Vue3项目开发——新闻发布管理系统(六)

文章目录 八、首页设计开发1、页面设计2、登录访问拦截实现3、用户基本信息显示①封装用户基本信息获取接口②用户基本信息存储③用户基本信息调用④用户基本信息动态渲染 4、退出功能实现①注册点击事件②添加退出功能③数据清理 5、代码下载 八、首页设计开发 登录成功后,系统就进入了首页。接下来,也就进行首页的开发了。 1、页面设计 系统页面主要分为三部分,左侧为系统的菜单栏,右侧

工厂ERP管理系统实现源码(JAVA)

工厂进销存管理系统是一个集采购管理、仓库管理、生产管理和销售管理于一体的综合解决方案。该系统旨在帮助企业优化流程、提高效率、降低成本,并实时掌握各环节的运营状况。 在采购管理方面,系统能够处理采购订单、供应商管理和采购入库等流程,确保采购过程的透明和高效。仓库管理方面,实现库存的精准管理,包括入库、出库、盘点等操作,确保库存数据的准确性和实时性。 生产管理模块则涵盖了生产计划制定、物料需求计划、

韦季李输入法_输入法和鼠标的深度融合

在数字化输入的新纪元,传统键盘输入方式正悄然进化。以往,面对实体键盘,我们常需目光游离于屏幕与键盘之间,以确认指尖下的精准位置。而屏幕键盘虽直观可见,却常因占据屏幕空间,迫使我们在操作与视野间做出妥协,频繁调整布局以兼顾输入与界面浏览。 幸而,韦季李输入法的横空出世,彻底颠覆了这一现状。它不仅对输入界面进行了革命性的重构,更巧妙地将鼠标这一传统外设融入其中,开创了一种前所未有的交互体验。 想象

SpringBoot项目是如何启动

启动步骤 概念 运行main方法,初始化SpringApplication 从spring.factories读取listener ApplicationContentInitializer运行run方法读取环境变量,配置信息创建SpringApplication上下文预初始化上下文,将启动类作为配置类进行读取调用 refresh 加载 IOC容器,加载所有的自动配置类,创建容器在这个过程