1 IMU真实状态运动模型
状态向量：
\(x_{I}=\left[{{_{G}^{I}{q(t)}}^{T},{b_{g}(t)}^{T},{^{G}v_{I}(t)}^{T},{b_{a}(t)}^{T},{^{G}p_{I}(t)}^{T}},{{_{C}^{I}q(t)}^{T}},{{^{I}p(t)_{C}}^{T}}\right]^{T}\)
四元数\({_{G}^{I}{q(t)}}\)代表惯性系到IMU坐标系的旋转，\({^{G}v_{I}(t)}\)和\({^{G}p_{I}(t)}\)代表IMU坐标系在惯性系中的速度和位置，\({b_{g}(t)}\)和\({b_{a}(t)}\)表示在IMU坐标系中测量值角速度与线加速度的biases，\({_{C}^{I}q(t)}\)和\({^{I}p(t)_{C}}\)表示相机坐标系和IMU坐标系的相对位置，其中相机坐标系取左相机坐标系。
数量关系：
\(_{G}^{I}{\dot q(t)}=\frac{1}{2}\Omega(w(t))_{G}^{I}q(t)\)
\(\dot b_{g}(t)=n_{wg}(t)\)
\(^{G}\dot v_{I}(t)={^{G}a_{I}(t)}\)
\(\dot b_{a}(t)=n_{wa}(t)\)
\(^{G}\dot p_{I}(t)={^{G}v_{I}(t)}\)
\({_{C}^{I}{\dot q(t)}}=0_{4\times1}\)
\({^{I}{\dot p}(t)_{C}}=0_{3\times1}\)
以上\(w(t)=[w_{x}(t),w_{y}(t),w_{z}(t)]^{T}\)是IMU角速度在IMU系中的坐标。
IMU的观测值为：
\(w_{m}=w+C(_{G}^{I}q)w_{G}+b_{g}+n_{g}\)
\(a_{m}=C(_{G}^{I}q)\left(^{G}a_{I}-{^{G}g}+2[w_{G}\times]{^{G}v_{I}}+{[w_{G}\times]}^{2}{(^{G}p_{I})}\right)+b_{a}+n_{a}\)
其中\(w_{G}\)为地球的自转速度在G系的坐标，在某些VIO实现中，会将地球自转的影响忽略不计，比如S-MSCKF，以后的推导中也会不计地球自转影响。
将地球自转忽略：
\(w_{m}=w+b_{g}+n_{g}\)
\(a_{m}=C(_{G}^{I}q)\left(^{G}a_{I}-{^{G}g}\right)+b_{a}+n_{a}\)
2 IMU估计状态运动模型
状态向量：
\(\hat x_{I} =\left[{{_{G}^{I}{\hat q}}^{T},{\hat b_{g}}^{T},{^{G}\hat v_{I}}^{T},{\hat b_{a}}^{T},{^{G}\hat p_{I}}^{T}},{_{C}^{I}\hat q}^{T},{^{I}{\hat p}_{C}}^{T}\right]^{T}\)
数量关系：
\(_{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}\)
其中：
\(\hat w = w_{m}-{\hat b}_{g}\)
\(\hat a= a_{m}-{\hat b}_{a}\)
\(\Omega(\hat w)=
\left(\begin{matrix}
-[\hat w_{\times}] & {\hat w} \-{\hat w}^{T} & 0
\end{matrix}\right)\)
3 IMU误差状态运动模型
定义IMU误差状态：
将上述真实状态与估计状态做“差”：
\(\tilde x = x-{\hat x}\)
其中四元数做差和普通的减法不一样，这里引入了误差四元数\(\delta q\)来表示旋转误差：
\(q={\delta q} \bigotimes {\hat q}\)
\({\delta q}\simeq [\frac{1}{2}{\delta\theta}^{T},1]^{T}\)
所以可以用三维向量\(\delta\theta\)来表示旋转误差，从而定义IMU误差状态向量为：
\(\tilde x_{I} =\left[{{_{G}^{I}{\tilde\theta}^{T}},{\tilde b_{g}}^{T},{^{G}\tilde v_{I}}^{T},{\tilde b_{a}}^{T},{^{G}\tilde p_{I}}^{T}},{_{C}^{I}{\tilde\theta}^{T}},{^{I}{\tilde p}_{C}}^{T}\right]^{T}\)
连续误差状态运动方程：
\({\dot {\tilde x}}_{I}=F{\tilde x_{I}}+G{n}_{I}\)
其中\(n_{I}^{T}=\left({n_{g}^{T}},{n_{wg}^{T}},{n_{a}^{T}},{n_{wa}^{T}}\right)^{T}\)。向量\(n_{g}\)和\(n_{a}\)代表陀螺仪与加速度计的测量噪声(高斯)，\(n_{wg}\)和\(n_{wa}\)是陀螺仪与加速度计biases的随机游走速率。
4 推导F与G
4.1 求\(\delta\theta\)
对\(_{G}^{I}q=\delta q \bigotimes{_{I}^{G}\hat q}\)左右两边同时求导得到：
\(_{G}^{I}\dot q=\dot {\delta q} \bigotimes {_{G}^{I}\hat q} + \delta q \bigotimes {_{G}^{I}\dot {\hat q}}\)
\(\frac{1}{2}\left[\begin{matrix}
w \0
\end{matrix}\right] \bigotimes {_{G}^{I}q}=
\dot {\delta q} \bigotimes {_{G}^{I}{\hat q}}+
\delta q \bigotimes \frac{1}{2}\left[\begin{matrix}
{\hat w} \0
\end{matrix}\right] \bigotimes {_{G}^{I}{\hat q}}\)
两边同时乘以\({_{G}^{I}{\hat q}}^{-1}\)得到：
\(\frac{1}{2}\left[\begin{matrix}
w \0
\end{matrix}\right] \bigotimes {\delta q}=
\dot {\delta q}+
\frac{1}{2}\delta q \bigotimes \left[\begin{matrix}
{\hat w} \0
\end{matrix}\right]\)
整理得：
\(\left[\begin{matrix}
{\dot {\delta\theta}} \{\dot {2}}
\end{matrix}\right]=
\left[\begin{matrix}
w \0
\end{matrix}\right] \bigotimes \delta q-
\delta q \bigotimes\left[\begin{matrix}
{\hat w} \0
\end{matrix}\right]\)
由\(w_{m}=w+b_{g}+n_{g}\)和\(\hat w =w_{m}-{\hat b}_{g}\)可以求出\(w\)：
\(w=\hat w + {\hat b}_{g}-b_{g}-n_{g}=\hat w - {\tilde b}_{g}-n_{g}\)
带入\(w\)得到：
\(\left[\begin{matrix}
{\dot {\delta\theta}} \{0}
\end{matrix}\right]=
\left[\begin{matrix}
{\hat w - {\tilde b}_{g}-n_{g}} \0
\end{matrix}\right] \bigotimes \delta q-
\delta q \bigotimes\left[\begin{matrix}
{\hat w} \0
\end{matrix}\right]\)
\(\left[\begin{matrix}
{\dot {\delta\theta}} \{0}
\end{matrix}\right]=
\left[\begin{matrix}
{\hat w} \0
\end{matrix}\right] \bigotimes \delta q-
\delta q \bigotimes\left[\begin{matrix}
{\hat w} \0
\end{matrix}\right]-
\left[\begin{matrix}
{{\tilde b}_{g}+n_{g}} \0
\end{matrix}\right] \bigotimes \delta q\)
利用四元数乘法的性质得到：
\(\left[\begin{matrix}
{\dot {\delta\theta}} \{0}
\end{matrix}\right]=
\left[\begin{matrix}
-[{\hat w}_{\times}] & {\hat w} \-{\hat w}^{T} & 0
\end{matrix}\right] \delta q-
\left[\begin{matrix}
[{\hat w}_{\times}] & {\hat w} \-{\hat w}^{T} & 0
\end{matrix}\right]\delta q-
\left[\begin{matrix}
-[{(\tilde b_{g}+n_{g})}_{\times}] & {(\tilde b_{g}+n_{g})} \-{(\tilde b_{g}+n_{g})}^{T} & 0
\end{matrix}\right] \left[\begin{matrix}
{\frac{1}{2}\delta\theta} \{1}
\end{matrix}\right]\)
忽略掉极小量相乘的项得：
\(\left[\begin{matrix}
{\dot {\delta\theta}} \{0}
\end{matrix}\right]=
\left[\begin{matrix}
-2[{\hat w}_{\times}] & 0 \0 & 0
\end{matrix}\right] \left[\begin{matrix}
{\frac{1}{2}\delta\theta} \{1}
\end{matrix}\right]-
\left[\begin{matrix}
{\tilde b_{g}+n_{g}} \{0}
\end{matrix}\right]\)
\({\dot {\delta\theta}}=-[{\hat w}_{\times}]\delta\theta -\tilde b_{g} -n_{g}\)
4.2 求\(\dot{\tilde b}_{g}\)
\(\dot{\tilde b}_{g}=\dot b_{g}-\dot{\hat b}_{g}=n_{wg}\)
4.3 求\(^{G}\dot {\tilde v}_{I}\)
\(^{G}\dot {\tilde v}_{I}={^{G}\dot v_{I}-^{G}\dot {\hat v}_{I}}\)
\(^{G}\dot v_{I}={^{G}a_{I}}={C(_{G}^{I}q)}^{T}\left(a_{m}-b_{a}-n_{a}\right)+{^{G}g}\)
\(^{G}\dot {\hat v}_{I}=C(_{G}^{I}\hat q)^{T}{\hat a}+{^{G}g}\)
\(^{G}\dot {\tilde v}_{I}={C(_{G}^{I}q)}^{T}\left(a_{m}-b_{a}-n_{a}\right)+{^{G}g}-C(_{G}^{I}\hat q)^{T}{\hat a}-{^{G}g}\)
\(={C(_{G}^{I}{\hat q})}^{T}\left(I+[\delta\theta_{\times}]\right)\left(a_{m}-b_{a}-n_{a}\right)-C(_{G}^{I}\hat q)^{T}{\hat a}\)
由\(\hat a=a_{m}-\hat b_{a}\)得：
\(={C(_{G}^{I}{\hat q})}^{T}\left(I+[\delta\theta_{\times}]\right)\left(\hat a-\tilde b_{a}-n_{a}\right)-C(_{G}^{I}\hat q)^{T}{\hat a}\)
省略掉高次项\([\delta\theta_{\times}]\left(-\tilde b_{a}-n_{a}\right)\)得到：
\(={C(_{G}^{I}{\hat q})}^{T}\left([\delta\theta_{\times}]\hat a -\tilde b_{a}-n_{a}\right)\)
\(=-{C(_{G}^{I}{\hat q})}^{T}[\hat a_{\times}]\delta\theta-{C(_{G}^{I}{\hat q})}^{T}\tilde b_{a}-{C(_{G}^{I}{\hat q})}^{T}n_{a}\)
4.4 求\(\dot {\tilde b}_{a}\)
\(\dot {\tilde b}_{a}=\dot b_{a}-\dot {\hat b}_{a}=n_{wa}\)
4.5 求\(^{G}\dot {\tilde p}_{I}\)
\(^{G}\dot {\tilde p}_{I}={^{G}p_{I}}-{^{G}\dot {\hat p}_{I}}\)
\(={^{G}v_{I}}-{^{G}{\hat v}_{I}}={^{G}{\tilde v}_{I}}\)
4.6求\(_{C}^{I}\tilde \theta\)和\(^{I} \tilde p_{C}\)
\(_{C}^{I}\tilde \theta\)和\(^{I} \tilde p_{C}\)是相机与IMU的相对位置，所以有：
\(_{C}^{I}\tilde \theta=0_{3\times 1}\)
\(^{I} \tilde p_{C}=0_{3\times 1}\)
4.7 写成矩阵形式
\[\dot {\tilde x}_{I}=\left[\begin{matrix}
-[\hat w_{\times}] & -I_{3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3}\0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \-C\left({_{G}^{I}\hat q}\right)^{T}[\hat a_{\times}] & 0_{3 \times 3} & 0_{3 \times 3} & -C\left(^{I}_{G}\hat q\right)^{T} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \0_{3 \times 3} & 0_{3 \times 3} & I_{3\times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3}
\end{matrix}\right]{\tilde x_{I}}+
\left[\begin{matrix}
-I_{3\times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \0_{3 \times 3} & I_{3\times 3} & 0_{3 \times 3} & 0_{3 \times 3} \0_{3 \times 3} & 0_{3 \times 3} & -C\left(^{I}_{G}\hat q\right)^{T} & 0_{3 \times 3} \0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & I_{3\times 3} \0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3}
\end{matrix}\right]{n_{I}}\]
参考资料
(1) A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation
(2) Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight
(3) Indirect Kalman Filter for 3D Attitude Estimation
原文地址：https://www.cnblogs.com/liuzhenbo/p/12536671.html