基于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数据,参考轨迹和预测轨迹的对比实验结果如下:

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

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

我个人改进后的项目链接

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:/a/540245.html

如若内容造成侵权/违法违规/事实不符,请联系我们进行投诉反馈qq邮箱809451989@qq.com,一经查实,立即删除!

相关文章

再谈C语言——理解指针(一)

内存和地址 内存 在讲内存和地址之前,我们想有个⽣活中的案例: 假设有⼀栋宿舍楼,把你放在楼⾥,楼上有100个房间,但是房间没有编号,你的⼀个朋友来找你玩, 如果想找到你,就得挨个房…

Vue 移动端(H5)项目怎么实现页面缓存(即列表页面进入详情返回后列表页面缓存且还原页面滚动条位置)keep-alive缓存及清除keep-alive缓存

一、需求 产品要求:Vue移动端项目进入列表页,列表页需要刷新,而从详情页返回列表页,列表页则需要缓存并且还原页面滚动条位置 二、实现思路 1、使用Vue中的keep-alive组件,keep-alive提供了路由缓存功能 2、因为我项…

《黑马点评》Redis高并发项目实战笔记(上)P1~P43

P1 Redis企业实战课程介绍 P2 短信登录 导入黑马点评项目 首先在数据库连接下新建一个数据库hmdp,然后右键hmdp下的表,选择运行SQL文件,然后指定运行文件hmdp.sql即可(建议MySQL的版本在5.7及以上): 下面这…

冲刺2024年思维100春季线上比赛:做做历年思维100真题(附答案)

今天是2024年4月13日,距离2024年春季思维100活动第一阶段的线上比赛4月20日还有7天。今年思维100活动的考试重点是什么呢?虽然主办方未公布,我们可以从历年的思维100真题中来分析和推测,把历年真题和背后的知识点吃透了&#xff0…

宠物领养|基于springboot的宠物领养系统设计与实现(附项目源码+论文)

基于springboot的宠物领养系统设计与实现 目录 一、摘要 二、相关技术 三、系统设计 3.1 整体功能设计图 3.2 功能具体细节设计 四、数据库设计 五、核心代码 六、论文参考 七、源码获取 一、摘要 如今社会上各行各业,都在用属于自己专用的软件来进行…

记录一下如何腾讯云服务器用客户端连MySQL

我一般喜欢用IDEA连数据库,别问我为啥(就喜欢用一个软件解决所有问题) 当然写SQL语句个人还是觉得sqlyog体验最佳!

llama-factory SFT系列教程 (二),大模型在自定义数据集 lora 训练与部署

文章目录 简介支持的模型列表2. 添加自定义数据集3. lora 微调4. 大模型 lora 权重,部署问题 参考资料 简介 llama-factory SFT系列教程 (一),大模型 API 部署与使用本文为 llama-factory SFT系列教程的第二篇; 支持的模型列表 模型名模型…

服务器配置环境步骤

1、创建虚拟环境 conda create --name 名字 pythonpython版本号2、进入虚拟环境 conda activate 名字3、确认自己要安装的torch版本和torchvision版本,进入https://pytorch.org/get-started/previous-versions/ 复制相应的命令,运行即可 注&#xff1a…

Visual Studio Code SSH 连接远程服务器

Visual Studio Code通过 SSH 连接远程服务器并实现免密登录,你可以按照以下步骤进行操作: 1. **安装插件**:首先,在 VS Code 中安装 "Remote - SSH" 插件。打开 VS Code,点击左侧的扩展图标,搜索…

C++如何排查并发编程死锁问题?

C如何排查并发编程死锁问题? 最近在Apache arrow里面写一个支持并行的算子:nested loop join,然后既然涉及到并行,这里就会遇到大家常说的死锁问题,假设你碰到了死锁问题,如何调试与定位呢? 那这…

最小均方(LMS)自适应滤波算法

1 LMS函数实现 % ----------------------------LMS(Least Mean Squre)算法------------------------------ % % parm: % xn 输入的信号序列 (列向量) % dn 所期望的响应序列 (列向量) % M 滤波器的阶数 (标量) % mu 收敛因子(步长) …

统信UOS(Linux)安装nvm node管理工具

整篇看完再操作,有坑!! 官网 nvm官网 按照官网方式安装,一直报 错 经过不断研究,正确步骤如下 1、下载安装包 可能因为网络安全不能访问github,我是链接热点下载的 wget https://github.com/nvm-sh/…

Java Maven项目推送到 Maven 中央仓库

准备阶段 namespace 域名认证 当需要在 sonatype 认证 com.xxx命名空间时,需要将 .xxx.com 配置域名解析。 记录类型:TXT 文本内容:验证的 key。 GPG 公私钥生成 GPG 下载地址:https://www.gnupg.org/download/index.html M…

RestTemplate—微服务远程调用—案例解析

简介:总结来说,微服务之间的调用方式有多种,选择哪种方式取决于具体的业务需求、技术栈和架构设计。RESTful API和HTTP客户端是常见的选择,而Feign和Ribbon等辅助库可以简化调用过程。RPC和消息队列适用于特定的场景,如…

单片机方案 发声毛绒小黄鸭

随着科技的不断进步,智能早教已经成为了新时代儿童教育的趋势。智能早教玩具,一款集互动陪伴、启蒙教育、情感培养于一身的高科技产品。它不仅能陪伴孩子成长,还能在游戏中启迪智慧,是家长和孩子的理想选择。 酷得电子方案开发特…

OV通配符证书:安全、便捷的网络认证新选择

OV通配符证书,即组织验证型通配符证书,其最大特点在于其通配符功能。这意味着,一个OV通配符证书可以覆盖同一主域名下的多个子域名,大大简化了证书管理和维护的复杂性。无论是大型企业还是个人网站,都可以通过OV通配符…

[面向对象] 单例模式与工厂模式

单例模式 是一种创建模式,保证一个类只有一个实例,且提供访问实例的全局节点。 工厂模式 面向对象其中的三大原则: 单一职责:一个类只有一个职责(Game类负责什么时候创建英雄机,而不需要知道创建英雄机要…

多因子模型的数据处理

优质博文:IT-BLOG-CN 数据处理的基本目的是从多量的、可能是杂乱无章的、难以理解的数据中抽取并推导出有价值、有意义的数据。特别是金融数据,存在数据缺失,不完整以及极端异常值等问题,对于我们的分析和建模影响很多。 对于我…

Git分布式版本控制系统——Git常用命令(二)

五、Git常用命令————分支操作 同一个仓库可以有多个分支,各个分支相互独立,互不干扰 分支的相关命令,具体如下: git branch 查看分支 git branch [name] 创建分支&#x…

20240409在全志H3平台的Nano Pi NEO CORE开发板上运行Ubuntu Core16.04时跑通4G模块EC200A-CN【PPP模式】

20240409在全志H3平台的Nano Pi NEO CORE开发板上运行Ubuntu Core16.04时跑通4G模块EC200A-CN【PPP模式】 2024/4/9 14:25 【不建议使用ppp模式,功耗大,貌似更过分的!网速还低!】 【唯一的优点:ppp模式下是通过脚本配置…