邱笑晨 ares43490@126.com
5
2
Exp
1
2
wwb
wb
bt t bt
www
www w
tt
tt t tt
tt t tt tt
RRω
vva
ppv a
★【其中,对 Rotation 更新方程做详细解释如下(参考秦永元《惯性导航》第 1 版 P292。
9.2.2 节):
b
wb
tω 表示 t 时刻“角速度矢量”在 b 系下的坐标
b
wb
ttω 表示“旋转矢量”在 b 系下的坐标,又有
Exp
相当于罗德里格斯公式
(Rodrigues’ Rotation Formula)
Exp
bt
b
wb
bt t
tt
ωR
Exp
bt
wb w w
wb
bt bt bt t bt t
tt
Rω RRR】★
注意到,IMU 预积分的理论是建立在欧拉积分的基础上的,并不是捷联惯导中传统的 LK4
等积分方式。
·为了符号简明,下面省略一些上下标记号:
w
bt
t RR
;
b
wb
ttωω
;
b
ttff;
w
ttvv
;
w
ttpp
;
w
·将测量模型代入离散运动方程
2
2
Exp
Exp
1
2
1
2
ggd
w
aad
w
aad
tt t tt
ttttt
tt t tt
ttt t ttt
tt t tt tt
ttt tt t t t
RRω
Rωbη
vva
vRfb η g
ppv a
pv Rfb η g
22
11
22
aad
ttt t tt t tt pv g Rfb η
·上述公式中,注意到噪声项采用
d
η
和
ad
(d 表示 discrete),它们与连续噪声项
η
和
a
是不同的,离散噪声和连续噪声的协方差有如下关系:
1
Cov Cov
1
Cov Cov
gd g
ad a
tt
t
tt
t
ηη
ηη
预积分 paper 中给出该关系的参考文献为《Sigme-point Kalman filtering for integration GPS
and inertial navigation》。
·进一步假设
t
恒定(即采样频率不变),每个离散时刻由
0,1, 2,...k
表示,前述三个离
散运动方程可进一步简化(符号简化)为: