程序员经验分享-hwhale

MSCKF 公式推导

2023-05-16

看完这篇博客建议再看：http://www.xinliang-zhong.vip/msckf_notes/

 

目录

一、简介

二、符号说明

三、状态向量

3.1 真实状态向量 true state

3.2 误差状态向量 error state

四、IMU 预测

4.1 连续形式的误差运动方程

4.2 连续形式的协方差传播

五、相机位姿状态增广

5.1 状态向量增广

5.2 协方差矩阵增广

六、视觉测量模型

6.1 视觉测量残差

6.2 残差线性化近些

6.3 边缘化路标点的位置误差

七、更新

八、参考文献

一、简介

MSCKF 是明尼苏达州大学 Mourikis 等人提出的一种基于 EKF 的 VIO 紧耦 合的 SLAM 框架[1][2]。该框架的最大创新点在于并未将路标点的位置加入到状 态向量中（因为加进去会导致状态向量一直增加，效率会越来越慢），而是等某 个路标点不见或者太老时，先通过 GN 优化方法计算出该路标点的 3D 位置，然 后将多个相机对这个路标点的观测作为一种约束，整合到 EKF 更新中，这样既 不损失信息，又不增加状态向量，有点像边缘化 Marg 的思路。

MSCKF 算法流程如下图所示，分为预测、图像注册、更新：

 

二、符号说明

 

三、状态向量

3.1 真实状态向量 true state

当前帧即第 k 时刻的所有误差状态向量，包括两部分：当前第 k 时刻的 IMU 状态，和 N 个相机位姿：

3.2 误差状态向量 error state

四、IMU 预测

4.1 连续形式的误差运动方程

4.2 连续形式的协方差传播

第 k 时刻的系统协方差矩阵为：

下一个 IMU 来时，即 k+1 时刻的系统协方差可写为：

 

五、相机位姿状态增广

5.1 状态向量增广

当获得一张新的相机图像时，需要将相机位姿加入到当前状态向量中，相机位姿可以通过 IMU 估计出来 （ 相机和IMU建立的联系 ）：

5.2 协方差矩阵增广

当来一帧新图像后，新的协方差矩阵可写成：

 

六、视觉测量模型

6.1 视觉测量残差

6.2 残差线性化近些

6.3 边缘化路标点的位置误差

对于 EKF ，残差线性化需要满足如下形式，即残差与误差项成线性化关系， 且噪声项为与状态向量无关的零均值的高斯分布：

下图给出了直接将公式 (32) 用于 EKF 的结果，对比公式 (34) 的结果可发现， 当噪声项与状态向量相关时，EKF 估计出来的结果将偏移得更严重 [5] 。

 

七、更新

 

八、参考文献

[1] A. I. Mourikis. A multi-state constraint kalman filter for vision-aided inertial
navigation. Proc. ICRA, 2007.
[2] A. I. Mourikis. A multi-state constraint kalman filter for vision-aided inertial
navigation. 2006. Dept. of Computer Science and Engineering, University of Minnesota,
Tech. Rep.
[3] F. Gonzalez. Visual inertial odometry for mobile robotics. 2015.
[4] N. Trawny. Indirect kalman filter for 3D attitude estimation. 2005.
[5] L. Clement. The battle for filter supremacy: a comparative study of the multi-state
constraint kalman filter and the sliding window filter. CVR. 2015

 

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

MSCKF 公式推导 的相关文章

  • detecron2中的注册机制

    文章目录 一 为什么使用注册类二 注册类的实现 传送门 Detectron2 01 注册机制 Registry 实现 一 为什么使用注册类 以下转自知乎 https zhuanlan zhihu com p 93835858 对于detec

  • 单目视觉SLAM在无人机上的应用

    目录 写在前面 本文说明 论文链接和实验视频 源代码链接 一 实验环境 硬件 软件 算法 二 实验环境搭建过程 1 Intel NUC安装Ubuntu 16 04 2 安装ROS D435i驱动以及ORB SLAM2算法 3 ORB SLA

  • TypeError: Expected cv::UMat for argument ‘img‘

    研究Detectron2中的在线数据增强的源码 并显示数据增强后的图像时 xff0c 遇到 TypeError Expected cv UMat for argument 39 img 的问题 显然 xff0c 问题要求是输入一个uint8

  • Linux 桌面终于迎来了 Docker Desktop

    Docker 桌面是容器化应用程序的最简单方法 无需考虑在你选择的系统上设置环境即可开始使用 只需要安装 Docker 桌面 xff0c 就可以开始了 Docker 桌面应用程序附带容器工具 xff0c 如 Kubernetes Docke

  • Dockerfile制作镜像和常用Shell脚本语法记录

    一 常规命令和使用记录 1 定义变量 判断匹配 赋值等 定义变量 addr 61 a namespace 61 b 判断匹配自定义的 包括参数化构建设置的变量 环境变量 xff0c 并赋值 case 34 Env 34 in dev nam

  • [Holo_wo]-vscode常用配置项说明

    vscode 配置文件核心 settings json xff1a 整个vscode的配置 xff0c 是本地vscode的配置 xff0c 如果有ssh远程 xff0c 那么会在远程主机的 vscode server目录下有setting

  • 基于jQuery的ajax跨域请求，PHP作为服务器端代码

    ajax实现跨域请求有两种方式 xff1a 方法一 xff1a jsonp的方式 jsonp方式的关键点在客户请求以jsonp作为数据类型 xff0c 服务器端接收jsonp的回调函数 xff0c 并通过回调函数进行数据的传输 具体代码如下

  • 无人机位置信息计算无人机的瞬时速度的matlab仿真

    目录 1 算法描述 2 仿真效果预览 3 MATLAB核心程序 4 完整MATLAB 1 算法描述 无人机最早在20世纪20年代出现 xff0c 1914年第一次世界大战正进行得如火如荼 xff0c 英国的卡德尔和皮切尔两位将军 xff0c

  • 基于simulink的无人机姿态飞行控制仿真

    目录 1 算法描述 2 仿真效果预览 3 MATLAB核心程序 4 完整MATLAB 1 算法描述 无人机是无人驾驶飞机的简称 xff08 Unmanned Aerial Vehicle xff09 xff0c 是利用无线电遥控设备和自备的

  • 基于ADRC自抗扰控制器的simulink仿真,ESO和TD等模块使用S函数开发

    目录 1 算法仿真效果 2 MATLAB核心程序 3 算法涉及理论知识概要 4 完整MATLAB 1 算法仿真效果 matlab2022a仿真结果如下 xff1a 2 MATLAB核心程序 function sys x0 str ts 61

  • m基于模糊控制与遗传优化的自适应ADRC双闭环控制策略matlab仿真

    目录 1 算法仿真效果 2 MATLAB核心程序 3 算法涉及理论知识概要 4 完整MATLAB 1 算法仿真效果 matlab2013b仿真结果如下 xff1a 遗传优化的优化迭代过程仿真图 xff1a 这个是我们采用的优化算法的有过过程

  • VINS-mono在Ubuntu20.04上从零开始安装运行和环境配置（尝试）

    最近尝试在Ubuntu 20 04上安装运行港科大的VINS mono算法 详细记录一下安装过程以及遇到的问题 先记录一下结果 ROS opencv Eigen Ceres以及VINS mono都编译并安装成功了 但是用euroc数据集跑V

  • 数据结构-C++实现

    之前的2周一直在学数据结构 xff0c 头都大了 我是之前对数据结构一点认识都没有 xff0c 我是直接看书怼的 xff0c 我看的是 大话数据结构 xff0c 前面的讲解还不错 xff0c 到了树 图后 xff0c 就有点看不懂了 xff

  • 几款好看的css表格

    表格一 xff1a 代码 xff1a html代码段 xff1a 是用vs写的 表头 lt th gt 那是从数据库读取的数据段 lt td gt 那是我为测试效果加的代码 xff0c 大家可以自行更改 lt h1 gt 待处理订单 lt

  • 非线性优化 (曲线拟合) 问题：高斯牛顿、g2o 方法总结

    其实还有一个Ceres库可以进行优化 xff0c 但是之前的博客已经具体分析了 xff0c 所以这里就对其余两个进行了介绍 xff0c 相关的内容是SLAM14讲里面的知识 一 理论部分 我们先用一个简单的例子来说明如何求解最小二乘问题 x

  • VINS-Fusion ： EUROC、TUM、KITTI测试成功 + 程序进程详细梳理

    完成以下任务的前提是系统安装了必备的库 xff0c 比如cere Eigen3 3等 提前下载好了数据集EUROC xff0c KITTI等 一 相关论文 T Qin J Pan S Cao and S Shen A General Opt

  • ROS 简单理解

    https download csdn net download qq 30022867 11120759 utm medium 61 distribute pc relevant download none task download b

  • ROS系列：七、熟练使用rviz

    7 熟练使用rviz xff08 1 xff09 rviz整体界面 rviz是ROS自带的图形化工具 xff0c 可以很方便的让用户通过图形界面开发调试ROS 操作界面也十分简洁 xff0c 如图29 xff0c 界面主要分为上侧菜单区 左

  • ROS系列：八、图像消息和OpenCV图像之间进行转换-cv_bridge

    cv bridge是在ROS图像消息和OpenCV图像之间进行转换的一个功能包 一 xff09 在ROS图像和OpenCV图像之间转换 xff08 C 43 43 xff09 xff11 xff0e Concepts xff08 概念 xf

  • ROS系列：九、rosbag使用

    文章目录 解析rosbag中的 bag文件 xff0c 得到 jpg图片数据和 pcd点云数据 https blog csdn net weixin 40000540 article details 83859694 1 rosbag写入文

随机推荐

  • 三、松灵课堂 | SCOUT的仿真使用

    仿真环境的介绍 Gazebo Gazebo是一款3D动态模拟器 xff0c 能够在复杂的室内和室外环境中准确有效地模拟机器人群 与游戏引擎提供高保真度的视觉模拟类似 xff0c Gazebo提供高保真度的物理模拟 xff0c 其提供一整套传

  • 1PPS：秒脉冲 相关概念理解

    时钟模块上的GPS接收机负责接收GPS天线传输的射频信号 xff0c 然后进行变频解调等信号处理 xff0c 向基站提供1pps信号 xff0c 进行同步 GPS使用原子钟 xff08 原子钟 xff0c 是一种计时装置 xff0c 精度可

  • opencv GStreamer-CRITICAL

    使用openvino中的opencv跑之前的代码 碰到个问题 span class token punctuation span myProg span class token operator span span class token

  • 激光雷达 LOAM 论文 解析

    注意 xff1a 本人实验室买的是Velodyne VLP 16激光雷和 LOAM 论文中作者用的不一样 xff0c 在介绍论文之前先介绍一下激光雷达的工作原路 xff0c 这样更容易理解激光雷达的工作过程 xff0c 其实物图如下图1所示

  • VINS 细节系列 - 坐标转换关系

    前言 在学习VINS Mono过程中 xff0c 对初始化代码中的坐标转换关系做出了一些推导 xff0c 特意写了博客记录一下 xff0c 主要记录大体的变量转换关系 相机和IMU的外参 若需要VINS标定旋转外参 xff0c 则进入以下代

  • VINS 细节系列 - 光束法平差法（BA）Ceres 求解

    一 理论部分 学习过VINS的小伙伴应该知道 xff0c 在SFM xff08 structure from motion xff09 的计算中 光束法平差法 BA xff08 Bundle Adjustment xff09 的重要性 xf

  • Ceres 详解（一） Problem类

    引言 Ceres 是由Google开发的开源C 43 43 通用非线性优化库 xff08 项目主页 xff09 xff0c 与g2o并列为目前视觉SLAM中应用最广泛的优化算法库 xff08 VINS Mono中的大部分优化工作均基于Cer

  • VINS - Fusion GPS/VIO 融合 一、数据读取

    目录 一 相关概念 二 程序解读 2 1 参数读取 解析 xff1a 2 2 获取图像时间信息 解析 xff1a 2 3 获取图像时间信息 解析 xff1a 2 4 定义VIO结果输出路径和读取图像信息 解析 xff1a 2 5 读取GPS

  • VINS - Fusion GPS/VIO 融合 二、数据融合

    https zhuanlan zhihu com p 75492883 一 简介 源代码 xff1a VINS Fusion 数据集 xff1a KITTI 数据 程序入口 xff1a globalOptNode cpp 二 程序解读 2

  • VINS - Fusion GPS/INS/视觉 融合 0、 Kitti数据测试

    放两张图片 至于为什么 xff1f 后面会解释 xff01 程序下载 xff1a https github com HKUST Aerial Robotics VINS Fusion 数据集制作 xff1a https zhuanlan z

  • GPS/INS/视觉 融合 、 自己采集数据测试

    cd VIO GPS MapVIG cmake build cmake make run MapVIG 别忘了更新一下 一 运行程序 打开第一个终端 roscore 打开第二个终端 进入工作区间内 xff0c 分别输入 xff1a cd G

  • 九、 惯性导航解算

    一 概述 惯性导航解算就是从上一时刻的导航信息推导本时刻的导航信息 xff0c 包括姿态 速度 位置 具体来讲 xff0c 就是构建当前时刻导航信息与上一时刻导航信息 运动输入 角速度 加速度 之间的关系 xff0c 确切地说 xff0c

  • 十二、构建一个基本的组合导航系统

    一 概述 在之前的文章里 xff0c 我们从一个基本的IMU模块开始讲起 xff08 二 xff09 xff0c 利用Allan方差分析方法 xff08 三 四 xff09 xff0c 得出了其误差参数 xff0c 又通过标定 xff08

  • 电脑键盘工作原理

    xfeff xfeff 随着IBM PC机的发展 xff0c 键盘也分为XT AT PS 2键盘以至于后来的USB键盘 PC系列机使用的键盘有83键 84键 101键 102键和104键等多种 XT和AT机的标准键盘分别为83键和84键 x

  • 十三、扩展一个组合导航系统

    感谢大神分享 xff1a https zhuanlan zhihu com p 156958777 一 概述 上一篇文章我们讲述了怎样搭建一个基本的组合导航系统 xff0c 它仅仅包括IMU做预测 GPS做观测 xff0c 而实际中 xff

  • 三、FSAS + Novatel接收机和相机时间同步、数据保存、解码等 细节说明

    一 器件说明 1 GNSS 接收机是 P ropak6 如下图所示 xff1a 具体得介绍 xff1a 1 https blog csdn net hltt3838 article details 110622732 2 二 NovAtel

  • Word 中出现公式不能编辑问题（兼容模式）

    当word中的公式不能编辑 xff0c 多数原因是由于word是处于兼容模式下 xff0c 这个时候就需要取消world的兼容模式 xff0c 具体步骤如下 xff1a 一 打开word文档 二 点击 word中的文件 出现如下结果 xff

  • 理论三、 MSCKF 数学基础

    感谢 xff1a 紫薯萝卜 https zhuanlan zhihu com p 76793847 1 数学定义 后续将进行MSCKF数学基础介绍 xff0c 先总结一下数据符号定义 坐标系定义 物理量定义 估计量定义 四元数定义 xff1

  • 算法改进5：开源算法Open VINS试用

    1 Open VINS简介 Open VINS是Huang Guoquan老师团队在2019年8月份开源的一套基于MSCKF的VINS算法 xff0c 黄老师曾是Tango项目的核心成员 xff0c 在MSCKF这块非常的权威 Github

  • MSCKF 公式推导

    看完这篇博客建议再看 xff1a http www xinliang zhong vip msckf notes 目录 一 简介 二 符号说明 三 状态向量 3 1 真实状态向量 true state 3 2 误差状态向量 error st

热门标签