一步一步学习S-MSCKF状态向量预测及其增广
Posted liuzhenbo
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了一步一步学习S-MSCKF状态向量预测及其增广相关的知识，希望对你有一定的参考价值。
1 状态向量预测
为了处理离散时间的IMU数据，采用四阶Runge-Kutta数值积分方法对以下方程离散化，从而对IMU的估计量进行预测。
[_{G}^{I}{dot{hat q}}=frac{1}{2}Omega(hat w)_{G}^{I}hat q，
dot {hat b}_{g}(t)=0_{3 imes1}，^{G}dot {hat v}_{I}=C(_{G}^{I}hat q)^{T}{hat a}+{^{G}g}，
dot {hat b}_{a}=0_{3 imes1}，^{G}dot {hat p}_{I}={^{G}{hat v}_{I}}，
{_{C}^{I}hat q}=0_{4 imes1}，
{^{I}{hat p}_{C}}^{T}=0_{3 imes1}
]
具体的离散化推导可以在Indirect Kalman Filter for 3D Attitude Estimation中找到。
2 误差状态协方差矩阵运动更新
IMU误差状态方程离散化：
[{dot { ilde x}}_{I}=F{ ilde x_{I}}+G{n}_{I}
]
从(t_{k})到(t_{k+1})的状态转移矩阵(Phi_{k})和噪声项(Q_{k})为：
[Phi_{k}=Phileft(t_{k+1},t_{k}
ight)=expleft(int ^{t_{k+1}}_{t_{k}}F( au)d au
ight)
]
[Q_{k}=int ^{t_{k+1}}_{t_{k}}Phileft(t_{k+1}, au
ight)GQGPhi left(t_{k+1}, au
ight)^{T}d au
]
其中(Q=Eleft[n_{I}n_{I}^{T} ight])
因此离散化后得方程为：
[ ilde x_{I}(k+1)=Phileft(t_{k+1},t_{k}
ight) ilde x_{I}(k)+Q_{k}
]
IMU误差状态( ilde x_{I}(k))运动更新对系统误差状态协方差矩阵(P_{k|k})的影响：
系统误差状态协方差矩阵为：
[P_{k|k}=left(egin{matrix}
P_{II_{k|k}} & P_{IC_{k|k}} P^{T}_{IC_{k|k}} & P_{CC_{k|k}}
end{matrix}
ight)
]
(k+1)时刻预测的IMU误差状态协方差矩阵为：
[P_{II_{k+1|k}}=Phi_{k}P_{II_{k|k}}Phi_{k}^{T}+Q_{k}
]
(k+1)时刻预测的系统误差状态协方差矩阵为：
[P_{k+1|k}=left(egin{matrix}
P_{II_{k+1|k}} & Phi_{k}P_{IC_{k|k}} P^{T}_{IC_{k|k}}Phi_{k}^{T} & P_{CC_{k|k}}
end{matrix}
ight)
]
状态增广
当一个新的图像到达时，系统状态增加一个相机状态
以上是关于一步一步学习S-MSCKF状态向量预测及其增广的主要内容，如果未能解决你的问题，请参考以下文章