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) \]
状态增广
当一个新的图像到达时，系统状态增加一个相机状态