程序员经验分享-hwhale

无人驾驶算法学习（六）：多传感器融合MSF算法

2023-05-16

文章目录

  • 1.引言
  • 2. 算法理论
    • 2.1 MSF基本模型
    • 2.2 预测
    • 2.3 测量与更新
  • 3. 核心代码分析
  • 4. 代码实战

1.引言

本文的多传感器融合是建立在读懂《Quaternion kinematics for the error-state Kalman filter》基础上的 ，是一种相机和IMU融合的理论，里面讲解了IMU的误差状态运动方程构建。误差状态四元数，是有开源的程序的，但是它是集成在rtslam里面的，不方便提取出来使用。
但还有另外一个开源的程序，ETH的MSF，可以比较方便地用在自己的工程里面，并且它的理论与误差状态四元数很接近，稍微有点不同，所以MSF开源程序就成了一个不错的选择。
所以本人研究了ETH的两篇文章：《Vision Based Navigation for Micro Helicopters》和《A Robust and Modular Multi-Sensor Fusion Approach Applied to MAV Navigation》
其核心就是扩展卡尔曼滤波EKF，本人把MSF的核心代码阅读了一遍，推导了论文中的算法，并最后做了实验。

2. 算法理论

MSF关键点：

  • 我们使用IMU含噪声的测量值和bias去计算估计状态量X。并且没有考虑测量值的噪声项和扰动，从而产生了误差。我们把所有不确定性引来的误差放在误差变量δx中考虑，并推导了误差状态运动模型，以此求解Fd状态转移矩阵和P过程协方差矩阵
  • 利用其它来源的数据如GPS,视觉等来作为矫正（使误差可观）。通过估计状态量X计算Hj，利用观测量和估计量计算残差，最后计算K，来更新P和误差变量δx，最后更新状态量X。

2.1 MSF基本模型

基本模型如下图所示。MSF的可扩展性很好，程序里可以接入新的传感器，比如GPS，激光雷达，码盘。

在这里插入图片描述下图是msf的原理示意图：

在这里插入图片描述
下图是eskf中所有变量解释表
在这里插入图片描述

2.2 预测

状态表示：
在这里插入图片描述
理想运动模型：
在这里插入图片描述
实际运动模型：
在这里插入图片描述
误差状态表示：

在这里插入图片描述
误差运动状态模型：
在这里插入图片描述
注意：在eskf中有详细推导和解释
在这里插入图片描述
在这里插入图片描述

误差状态导数的转移矩阵：
在这里插入图片描述
二阶泰勒展开（忽略二阶以上微小量）：
在这里插入图片描述
误差状态的转移矩阵：
在这里插入图片描述
预测误差状态和预测误差差协方差矩阵：
在这里插入图片描述

2.3 测量与更新

p测量模型：
在这里插入图片描述

q测量模型：

在这里插入图片描述

测量更新：

在这里插入图片描述

3. 核心代码分析

核心算法的代码分析如下图所示：
在这里插入图片描述
预测与更新算法调用关系图：
在这里插入图片描述

4. 代码实战

msf-ethsal编译及使用
相机IMU融合四部曲（三）:MSF详细解读与使用
基本操作

roslaunch msf_updates position_pose_sensor.launch 
roslaunch msf_updates viconpos_sensor.launch
rosrun rqt_reconfigure rqt_reconfigure#初始化卡尔曼滤波器,很重要!!
如下界面,需要点击filter

在这里插入图片描述

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)

无人驾驶算法学习（六）：多传感器融合MSF算法 的相关文章

  • 树莓派安装宝塔面板后无法连接VNC

    解决方法 xff1a 登陆宝塔面板后台 xff0c 在 安全 中放行端口5900即可

  • Python并行处理视频帧

    参考链接 xff1a Speedy Computer Vision Pipelines using Parallelism 方案 xff1a 使用Python多进程编程 xff0c 将视频分成多个小段 xff0c 可按照CPU核数num p

  • 深度学习目标检测之SSD

    经典论文SSD笔记 论文链接 xff1a SSD Single Shot MultiBox Detector论文报告 xff1a ssd eccv2016 slide目标检测百页综述 xff0c 从传统方法到深度学习 xff1a Objec

  • C++编译报错fmt未定义的引用

    对 fmt v5 internal basic data POWERS OF 10 64 未定义的引用 1 最简单的方法 xff1a 把代码中printf的输出全部换成std cout或者其他的 2 安装fmt包 git clone htt

  • PyG/torch_geometric的一些坑

    安装PyG span class token keyword import span os span class token keyword import span torch os span class token punctuation

  • Pixhawk官网飞行模式介绍

    欢迎交流 个人 Gitter 交流平台 xff0c 点击直达 xff1a Flight Mode 飞行模式 原文地址 http dev px4 io concept flight modes html 飞行模式定义了系统在任何给定时间的状态

  • PX4中文维基汉化项目启动

    欢迎交流 个人 Gitter 交流平台 xff0c 点击直达 xff1a 敬启者 xff1a 打算进行PX4官网的汉化工作 GitBook 与官网的方式相同 xff0c 我们也是将网站以GitBook的方式呈现给大家 汉化后的版本先点点点点

  • Windows / Ubuntu操作系统下Pixhawk原生固件PX4的编译方法

    欢迎交流 个人 Gitter 交流平台 xff0c 点击直达 xff1a 更新于2017 3 13 FAQ 本文说明针对 PX4 Firmware 1 6 0 问题 1 xff1a 找不到python jinja2模块 CMake Erro

  • Sublime Text中文乱码的解决方法

    Sublime Text Sublime Text这款代码编译器相当不错 xff0c 自带高亮显示 xff0c 界面清新 但是Sublime Text默认是不支持中文显示的 xff0c 这种中文乱码的行为万万是不能够接受的 这里简单介绍一下

  • 自制Pixhawk飞控板烧写BootLoader教程

    对于自己制作的飞控板 xff0c 通过USB连接电脑之后 xff0c 开始电脑是无法检测到飞控板的端口存在的 检测不到端口 xff0c 就不能用控制台给飞控板烧写固件 xff0c 就不能用QGroundControl xff0c 就不能进行

  • Pixhawk原生固件PX4之常用函数解读

    欢迎交流 个人 Gitter 交流平台 xff0c 点击直达 xff1a PX4Firmware 经常有人将Pixhawk PX4 APM还有ArduPilot弄混 这里首先还是简要说明一下 xff1a Pixhawk是飞控硬件平台 xff

  • Pixhawk原生固件PX4之添加uORB主题

    欢迎交流 个人 Gitter 交流平台 xff0c 点击直达 xff1a 本说明针对 Firmware v1 5 4 1 添加流程说明 1 在Firmware msg下新建uORB的成员变量 xff0c eg xxx msg 2 在Firm

  • Pixhawk原生固件PX4之SITL软件在环仿真

    欢迎交流 个人 Gitter 交流平台 xff0c 点击直达 xff1a 故事开始之前 xff0c 先按照笔者的这一篇博客在Ubuntu上完成固件的编译 jMAVSim仿真 jMAVSim仿真不需要任何配置 xff0c 直接输入指令即可 s

  • Pixhawk原生固件PX4之串口读取信息

    欢迎交流 个人 Gitter 交流平台 xff0c 点击直达 xff1a 这篇博客纯粹出于对FreeApe这位先行者贡献的复现 xff0c 也是本人一直想要进行的一项操作 在此还是做一下记录 时代在改变 xff0c 代码在更新 xff0c

  • Effective Modern C++ 条款21 比起直接使用new，更偏爱使用std::make_unique和std::make_shared

    比起直接使用new xff0c 更偏爱使用std make unique和std make shared 让我们从std make unique和std make shared之间的比较开始讲起吧 std make shared是C 43

  • Pixhawk原生固件PX4之串口添加读取传感器实现

    欢迎交流 个人 Gitter 交流平台 xff0c 点击直达 xff1a 本博客承接前一篇 xff0c 对FreeApe的串口添加超声波传感器博文后半部分进行学习 为什么叫前奏呢 xff0c 因为用了伪传感器 xff0c 把单片机用串口发送

  • Pixhawk原生固件PX4之MAVLink协议解析

    欢迎交流 个人 Gitter 交流平台 xff0c 点击直达 xff1a PX4 对Mavlink 协议提供了良好的原生支持 该协议既可以用于地面站 Ground ControlStation GCS 对无人机 UAV 的控制 xff0c

  • Pixhawk原生固件PX4之TAKEOFF的启动流程

    欢迎交流 个人 Gitter 交流平台 xff0c 点击直达 xff1a 以TAKEOFF为例说明PX4中一个飞行模式的启动流程 众所周知由遥控器或者地面站决定Main state作为用户期望到达的飞行模式然后有commander进行条件判

  • Pixhawk原生固件PX4之驱动ID

    欢迎交流 个人 Gitter 交流平台 xff0c 点击直达 xff1a 驱动ID PX4使用驱动ID将独立传感器贯穿于整个系统 这些ID存储于配置参数中 xff0c 用于匹配传感器校正值 xff0c 以及决定哪些传感器被记录到log中 传

  • Pixhawk原生固件PX4之SPI驱动注册过程

    欢迎交流 个人 Gitter 交流平台 xff0c 点击直达 xff1a 一切事出有因 xff0c 为了添加一个自定义SPI总线连接的传感器 xff0c 首先要弄清楚一个SPI设备的注册过程 xff0c 大致涉及以下的一些文件 接下来就该以

随机推荐

  • Pixhawk原生固件PX4之MPU6000驱动分析

    欢迎交流 个人 Gitter 交流平台 xff0c 点击直达 xff1a 要想自己添加一个传感器的话 xff0c 最好先搞明白已有的传感器的工作过程 这里记录一下PX4中MPU6000加速度计陀螺仪的解读过程 xff0c 从mpu6000

  • Pixhawk原生固件PX4之日期时间的确定

    欢迎交流 个人 Gitter 交流平台 xff0c 点击直达 xff1a 偶然注意到PX4日志中老是出现类似于2000 01 01 00 00 00这种日期 有兴趣的可以搜索一下千年虫问题 xff0c 于是结合代码进行了一波分析 最后定位到

  • Pixhawk原生固件PX4之添外置传感器MPU6500

    欢迎交流 个人 Gitter 交流平台 xff0c 点击直达 xff1a 成功的在Pixhawk上添加了一个自定义的传感器MPU6500 Pixhawk飞控板上空余出一个SPI4接口 提示 xff1a 多出来的GPIO EXT引脚可以作为片

  • 多旋翼无人机进阶教程

    无人机是一个系统的工程 xff0c 不可谓不庞大 开源飞控盛行 xff0c 重复造轮子的工作实在无需再做 但是若决定真正的去研究飞控 xff0c 必须从本质出发 xff0c 熟悉并了解其实现原理 纷繁复杂的资料 xff0c 让人无法分辨 笔

  • Pixhawk原生固件PX4之MAVLink外部通讯

    欢迎交流 个人 Gitter 交流平台 xff0c 点击直达 xff1a 目前的需求是 xff1a 一台电脑连数传 xff0c Pixhawk飞控上电连数传 xff0c 电脑向Pixhawk发送MAVlink消息 至少有5种方案 xff1a

  • rtsp流浏览器播放方案

    rtsp流在主流浏览器并不支持直接播放 比如大华的视频流 xff1a rtsp admin 123456 64 192 168 10 129 cam realmonitor channel 61 1 amp subtype 61 0 xff

  • PX4开发指南中文版维护说明

    PX4中文维基现已与PX4开发者官网合并 现在大家可以直接进入官网进行语言切换 PX4中文版的维护依然需要你的贡献 贡献说明 xff1a 官方的GitHub仓库为https github com PX4 Devguide 我将其Fork后的

  • OpenCV图像坐标系与行列宽高的关系

    刚开始接触图像处理 xff0c 关于图像坐标系与行列宽高的关系感到纠结 xff0c 但是似乎没有更好的处理方法了 xff0c 其对应关系大致如下 row 61 61 height 61 61 Point y col 61 61 width

  • Pixhawk原生固件PX4之位姿控制算法解读

    欢迎交流 个人 Gitter 交流平台 xff0c 点击直达 xff1a 参考文献 xff1a Minimum Snap Trajectory Generation and Control for Quadrotors PX4中多旋翼无人机

  • Pixhawk精准着陆之IRLock配置

    安装说明 下载Pixymon和pixy对应markone的固件 xff0c 在这里 固件必须是firmware IRLOCKpixy 1 0 1 hex irlock 61 markone 然后固件里 irlock 61 pixy 给Pix

  • Pixhawk原生固件PX4之offboard

    offboard PX4中的offboard 暂译作外部控制 是一个非常强大的功能 可以接受来自外部的控制指令 xff0c 按照目前的了解来看 xff0c offboard搭配上MAVROS以及类似于TX1 NUC板载计算器 xff0c 在

  • 相机标定原理

    cnblogs上的这篇讲相机标定的博文值得一看 csdn上这篇也可以参考 相机标定基础知识 相机标定技术涉及到一些数学原理和几何模型 xff0c 这些数学原理和几何模型是相机标定算法使用和进一步发展的基础 下面对相机标定技术中涉及到的齐次坐

  • VS Code的Git插件

    Visual Studio Code是微软公司推出的一款跨平台代码编辑 Edit 编译 Build 调试 Debug 工具 笔者认为其相当于是Sublime Text这款代码编辑器的升级版 集成了丰富的插件 xff0c 包括代码管理中极为常

  • Ubuntu缺少libncurses.so.5的解决办法

    执行arm none eabi gdb时候出错 xff1a arm none eabi gdb error while loading shared libraries libncurses so 5 cannot open shared

  • freeRTOS的任务抢占和时间片轮转

    实时操作系统的一个特点就是可以任务抢占 xff0c 高优先级的任务可以抢占比自己优先级低的任务 xff0c 如果新任务优先级和当前人任务优先级一样 xff0c 且在使能了时间片的方式的话 xff0c 二者以时间片的方式共享cpu xff0c

  • RH850 F1L freeRTOS 任务栈的切换

    pxCurrentTCB指向的任务块中 xff0c 有2个和栈相关的变量pxTopOfStack和pxStack pxTopOfStack指向当前堆栈栈顶 xff0c 随着进栈出栈 xff0c pxTopOfStack指向的位置是会变化的

  • opencv 3.0 图像去畸变 undistortion

    主要用到的是 initUndistortRectif yMap这个函数 在opencv中这个函数是用于 去除镜头畸变的图像拉伸 为了快速算法 xff1a 使用了坐标查找变和双线性差值的方法 先上结果图 原图 去畸变至全图 去畸变并保留最大图

  • 多传感器融合MSF算法源码阅读(三)

    文章目录 1 触发测量更新回调函数2 测量更新状态量3 总结 无人驾驶算法学习 xff08 六 xff09 xff1a 多传感器融合MSF算法 多传感器融合MSF算法源码阅读 一 多传感器融合MSF算法源码阅读 二 1 触发测量更新回调函数

  • 视觉VIO:S-MSCKF算法学习(一)

    文章目录 1 算法简介2 算法调试3 代码解读3 1 前端 ImageProcessor3 2 后端 Msckf Vio 1 算法简介 Mingyang Li博士于2007年提出MSCKF Multi State Constraint Ka

  • 无人驾驶算法学习（六）：多传感器融合MSF算法

    文章目录 1 引言2 算法理论2 1 MSF基本模型2 2 预测2 3 测量与更新 3 核心代码分析4 代码实战 1 引言 本文的多传感器融合是建立在读懂 Quaternion kinematics for the error state

热门标签