没有合适的资源?快使用搜索试试~ 我知道了~
资源推荐
资源详情
资源评论
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 更新时都更新协方差矩阵。当每
一帧图像信息到来时由许多任务要做,包括增广当前状态向量,将当前相机
的位姿添加到滑动窗中,同时更新协方差。特征点跟踪也要跟踪之前一致跟
踪的特征点并检测这些被跟踪的特征点是否消失。如果特征点消失或是滑动
窗已达制定上限阈值,就开始进行量测更新;并在下一帧图像到来时重新开
始跟踪特征点。量测更新时通过三角测量和高斯牛顿迭代优化算法计算特征
点的位置。由于是跟踪特征在多个相机位姿处的观测,处理会有延时,但是
三角测量的结果很精确。预测的特征点的位置与量测值的重投影误差作为卡
尔曼滤波量测误差,以此进行状态估计,估计出状态误差之后反馈。这是视
觉/惯性卡尔曼滤波融合算法的流程。视觉与惯性的融合要求 IMU 与视觉测
量是同步的,每一帧图像产生时都有对应的 IMU 测量值,每帧图像对应整
数帧的 IMU 信息。
3.2 误差状态向量
误差状态向量如下:
T
T
I
GT
a
T
I
GT
g
T
I
IMU
pbv
X
b
~
(1)
式中波浪号代表误差量,除旋转外其他误差项都是直接相加即真实值=估计值+
误差值
旋转量的误差项叠加方式为:
= ⨂
(2)
≈ (0.5
ၗ 1)
(3)
式中^符号为估计值
当 k 时刻完整的状态向量为
=
ၗ
1
1
⋯
ၗ
(4)
式中 N 为相机个数,
状态向量维度为 15 维,一个相机维度为 6,整个状
态向量维度 15+6N
系统协方差矩阵为:
|
=
|
|
|
(5)
其中
为
15 × 15
的 IMU 状态对应的协方差,
|
是
6N × 6
的相机状
态对应的协方差,
|
是 IMU 与相机状态对应的协方差
3.3 运动模型推导
3.3.1 状态真实值的系统模型
各状态真实值之间的物理关系:
t = 0.5Ω
t = 0.5
0
⨂
t
=
()
=
()
=
()
=
()
其中
是 Global 坐标系下的加速度,
=
是 IMU 坐标系下
的角速度
= +
+
=
(
−
) +
+
3.3.2 状态估计值的系统模型
= 0.5Ω(
)
= 0
=
+
= 0
=
其中
=
−
,
=
−
, =
−
−
=
−
−
+
3.3.3 运动模型推导
根据状态估计量的系统模型推导 IMU 误差状态的运动模型
=
+
其中
=
选用误差状态的运动模型的理由:
1)位姿的误差状态是最小参数描述, 避免了冗余参数化导致协方差矩阵奇异;
2)误差状态系统通常在原点附近, 参数远离奇异点或万向节锁等问题, 从而确
保 EKF 线性化一直有效,而误差状态通常很小, 意味着二阶项可以忽略. 这使得
Jacobian 能够快速容易计算;误差状态的动态变化较慢, 这是因为大的信号动态
性被积分到了额定状态. 这意味着 EKF 更新频率可以比预测频率低.(VIO 中通常
IMU 数据频率为 100-500HZ, 而图像频率只有 20-30HZ 左右)
下面求 F 矩阵
先求ၗ
,由公式 2,3 可知
=
⨂
−1
=
1
2
ၗ
1
= ( ⨂
)
'
=
⨂
+ ⨂
=
1
2
0
⨂
其中 q 为真实值,w 为真实值, =
−
−
⨂
=
1
2
0
⨂ − ⨂
⨂
=
1
2
0
⨂ − ⨂
1
2
0
⨂
⨂
=
1
2
0
⨂⨂
− ⨂
1
2
0
⨂
将上式中
约去,可得
=
1
2
0
⨂ − ⨂
1
2
0
=
1
2
[]
−
1
2
[
]
1
2
ၗ
0
=
1
2
−
×
−
0
−
1
2
×
−
0
将 =
−
−
代入上式中,整理可得
ၗ
0
=
−
×
−
0
−
×
−
0
−
−
+
×
+
−
+
0
ၗ
0
=
−2
×
0
0 0
−
−
+
×
+
−
+
0
ၗ
0
=
−2
×
0
0 0
0.5ၗ
1
−
−
+
×
+
−
+
0
0.5ၗ
1
由上式中的第二行解方程可得
−
+
ၗ = 0
第一行解方程可得
ၗ
= −
×
ၗ + 0.5
+
×
ၗ − (
b
g
+ n
g
)
由于0.5
+
×
ၗ是高次项,忽略
剩余22页未读,继续阅读
资源评论
半不闲居士
- 粉丝: 208
- 资源: 2
上传资源 快速赚钱
- 我的内容管理 展开
- 我的资源 快来上传第一个资源
- 我的收益 登录查看自己的收益
- 我的积分 登录查看自己的积分
- 我的C币 登录后查看C币余额
- 我的收藏
- 我的下载
- 下载帮助
最新资源
- C语言基础-C语言编程基础之Leetcode编程题解之第30题串联所有单词的子串.zip
- C语言基础-C语言编程基础之Leetcode编程题解之第29题两数相除.zip
- C语言基础-C语言编程基础之Leetcode编程题解之第28题找出字符串中第一个匹配项的下标.zip
- 实验报告模板(1).docx
- C语言基础-C语言编程基础之Leetcode编程题解之第26题删除有序数组中的重复项.zip
- C语言基础-C语言编程基础之Leetcode编程题解之第25题K个一组翻转链表.zip
- hnu计算机系统作业-计算机系统基础课程大作业.zip
- 树莓派app.apk
- C++的基于同态加密技术的匿名电子投票系统源码.zip
- SW建模格式图.zip
资源上传下载、课程学习等过程中有任何疑问或建议,欢迎提出宝贵意见哦~我们会及时处理!
点击此处反馈
安全验证
文档复制为VIP权益,开通VIP直接复制
信息提交成功