sMSCKF公式推导(与代码一致).pdf - CSDN文库

需积分: 50 30 浏览量更新于2023-05-10 1 收藏 552KB PDF 举报

身份认证购VIP最低享 7 折!

领优惠券(最高得80元）

资源详情

资源推荐

sMSCKF 公式推导

1、 MSCKF 视觉惯性融合算法简介

惯性导航利用惯性测量单元(IMU)测量得到的角速度、加速度信息进行

惯性导航解算得到运载体的位置、速度、姿态(含航向)等信息，具有实时性

好、动态性能好等优点；但是由于其积分式特点，使得传感器和算法解算的

误差会持续累积，导致长时间精度很低，特别是对于低端IMU。即使是军用

战略性武器上使用的高精度惯导系统也在努力与别的信息进行融合，提高实

际导航精度。视觉信息包含了丰富的三维场景信息，通过视觉跟踪可以对运

载体的运动进行测量，视觉测量可以给出运动增量。惯性与视觉融合算法利

用视觉观测与IMU预测联合得到重投影误差作为观测量来进行系统状态估

计。分为滑动窗SWF与多状态约束卡尔曼滤波MSCKF两种技术方案。作为惯

性导航专业背景的从业人员，比较青睐基于卡尔曼滤波的MSCKF算法。这里

给出msckf_vio算法的数学推导过程。这里的公式表达和推导过程按照惯性

导航的习惯，对于非惯性导航专业的可能会感觉有点陌生。

2、 MSCKF视觉前端

后续补上

3、 MSCKF后端

3．1MSCKF 算法流程

MSCKF 算法估计 IMU 系 b 在全局导航系(世界系)n 里的位姿。相机系 C 和

INS 固联，两者之间由包含旋转

b

c

C

和平移

b

c

p

的外参表达。IMU 的采样率和更

新速率都比较高，一般在 100Hz 以上；相机的测量和视觉算法的更新速率较

低，一般在 10-30Hz 左右。在每一步 IMU 更新时都更新协方差矩阵。当每

一帧图像信息到来时由许多任务要做，包括增广当前状态向量，将当前相机

的位姿添加到滑动窗中，同时更新协方差。特征点跟踪也要跟踪之前一致跟

踪的特征点并检测这些被跟踪的特征点是否消失。如果特征点消失或是滑动

窗已达制定上限阈值，就开始进行量测更新；并在下一帧图像到来时重新开

始跟踪特征点。量测更新时通过三角测量和高斯牛顿迭代优化算法计算特征

剩余22页未读，继续阅读

半不闲居士

粉丝: 213
资源: 2

会员权益专享

图片转文字

全年可省5，000元立即开通

最新资源

资源上传下载、课程学习等过程中有任何疑问或建议，欢迎提出宝贵意见哦~我们会及时处理！点击此处反馈