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\times1}，\^{G}\dot {\hat v}_{I}=C(_{G}^{I}\hat q)^{T}{\hat a}+{^{G}g}，
\dot {\hat b}_{a}=0_{3\times1}，\^{G}\dot {\hat p}_{I}={^{G}{\hat v}_{I}}，
{_{C}^{I}\hat q}=0_{4\times1}，
{^{I}{\hat p}_{C}}^{T}=0_{3\times1}
\]
具体的离散化推导可以在Indirect Kalman Filter for 3D Attitude Estimation中找到。
2 误差状态协方差矩阵运动更新
IMU误差状态方程离散化：
\[{\dot {\tilde x}}_{I}=F{\tilde x_{I}}+G{n}_{I}
\]
从\(t_{k}\)到\(t_{k+1}\)的状态转移矩阵\(\Phi_{k}\)和噪声项\(Q_{k}\)为：
\[\Phi_{k}=\Phi\left(t_{k+1},t_{k}\right)=exp\left(\int ^{t_{k+1}}_{t_{k}}F(\tau)d\tau\right)
\]
\[Q_{k}=\int ^{t_{k+1}}_{t_{k}}\Phi\left(t_{k+1},\tau\right)GQG\Phi \left(t_{k+1},\tau\right)^{T}d\tau
\]
其中\(Q=E\left[n_{I}n_{I}^{T}\right]\)
因此离散化后得方程为：
\[\tilde x_{I}(k+1)=\Phi\left(t_{k+1},t_{k}\right)\tilde x_{I}(k)+Q_{k}
\]
IMU误差状态\(\tilde x_{I}(k)\)运动更新对系统误差状态协方差矩阵\(P_{k|k}\)的影响：
系统误差状态协方差矩阵为：
\[P_{k|k}=\left(\begin{matrix}
P_{II_{k|k}} & P_{IC_{k|k}} \P^{T}_{IC_{k|k}} & P_{CC_{k|k}}
\end{matrix}\right)
\]
\(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(\begin{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}\right)
\]
状态增广
当一个新的图像到达时，系统状态增加一个相机状态
原文地址：https://www.cnblogs.com/liuzhenbo/p/12545361.html