Quaternion kinematics for ESKF－预测

前言

在看vins-mono, okvis, msckf等基于imu的vslam论文时，发现这些主流的slam前端vio都需要计算imu的预积分及其协方差，而计算方法类似，都是参考Quaternion kinematics for the error-state KF或Indirect Kalman Filter for 3D Attitude Estimation，可见这两篇文章的影响力，抱着这样的好奇心开始了对其中一篇文章的阅读．

四元素和旋转操作(基础)

1. 四元素定义和表示形式

定义:
$$Q=a+bi+cj+dk$$
其中a为实部，(b,c,d)为虚部．也可表示成下面形式:
$$\mathbf{q}=\left[\begin{array}{c}{q}_w\\ {\mathbf{q}}_v\end{array}\right]=\left[\begin{array}{c}{q}_w\\ q_x\\ q_y\\ q_z\end{array}\right]$$
用一个四维向量表示，这个是最常见的表示形式

2. 一些四元素性质

• 加法和乘法:
  $$\begin{array}{c}\mathbf{p}+\mathbf{q}=\left[\begin{array}{c}{p}_w+q_w\\ {\mathbf{q}}_v+{\mathbf{p}}_v\end{array}\right]\\ \mathbf{p}\otimes \mathbf{q}=\left[\begin{array}{c}{p}_w{q}_w-p_x{q}_x-p_y{q}_y-p_z{q}_z\\ p_w{q}_x+p_x{q}_w+p_y{q}_z-p_z{q}_y\\ p_w{q}_y-p_x{q}_z+p_y{q}_w+p_z{q}_x\\ p_w{q}_z+p_x{q}_y-p_y{q}_x+p_z{q}_w\end{array}\right]\end{array}$$
• 左乘右乘矩阵:
  $$\begin{array}{c}{\mathbf{q}}_1\otimes {\mathbf{q}}_2={\mathbf{Q}}_1^{+}{\mathbf{q}}_2\to {\mathbf{Q}}_1^{+}=q_w\mathbf{I}+\left[\begin{array}{cc}0& -{\mathbf{q}}_v^T\\ {\mathbf{q}}_v& {\left[\begin{array}{c}{\mathbf{q}}_v\end{array}\right]}_{\times }\end{array}\right]\\ {\mathbf{q}}_1\otimes {\mathbf{q}}_2={\mathbf{Q}}_2^{-}{\mathbf{q}}_1\to {\mathbf{Q}}_2^{-}=q_w\mathbf{I}+\left[\begin{array}{cc}0& -{\mathbf{q}}_v^T\\ {\mathbf{q}}_v& -{\left[\begin{array}{c}{\mathbf{q}}_v\end{array}\right]}_{\times }\end{array}\right]\\ {\left[\begin{array}{c}{\mathbf{q}}_v\end{array}\right]}_{\times }=\left[\begin{array}{ccc}0& -q_z& q_y\\ q_z& 0& -q_x\\ -q_y& q_x& 0\end{array}\right]\end{array}$$
• 共轭四元素:
  $${\mathbf{q}}^{\ast }=\left[\begin{array}{c}{q}_w\\ {-\mathbf{q}}_v\end{array}\right]$$
• 单位或归一化四元素性质:
  $$\begin{array}{c}{\mathbf{q}}^{-1}={\mathbf{q}}^{\ast }\end{array}$$

3. 旋转和交叉关系

• 3D向量(也即旋转向量，轴角)表示旋转:
  $$\begin{array}{c}\left(\mathbf{u},\theta \right)\\ \mathbf{u}=u_{x}i+u_{y}j+u_{z}k\end{array}$$
• 四元素和旋转向量:
  $$\begin{array}{c}\mathbf{q}=\left[\begin{array}{c}\cos \left(\theta /2\right)\\ \mathbf{u}\sin \left(\theta /2\right)\end{array}\right]\end{array}$$
• 旋转向量和旋转矩阵(Rodrigues公式):
  $$\mathbf{R}=\mathbf{I}+\sin \theta \left[\begin{array}{c}{\mathbf{u}}_{\times }\end{array}\right]+\left(1-\cos \theta \right){\left[\begin{array}{c}\mathbf{u}\end{array}\right]}_{\times }^2$$
• 四元素转旋转矩阵:
  $$R=\left[\begin{array}{ccc}{q}_w^2+q_x^2-q_y^2-q_z^2& 2\left(q_x{q}_y-q_w{q}_z\right)& 2\left(q_x{q}_z+q_w{q}_y\right)\\ 2\left(q_x{q}_y+q_w{q}_z\right)& q_w^2-q_x^2+q_y^2-q_z^2& 2\left(q_y{q}_z-q_w{q}_x\right)\\ 2\left(q_x{q}_z-q_w{q}_y\right)& 2\left(q_y{q}_z+q_w{q}_x\right)& q_w^2-q_x^2-q_y^2+q_z^2\end{array}\right]$$

4. 扰动和时间微分

• 扰动:
  $$\Delta {\mathbf{q}}_L=\left[\begin{array}{c}1\\ \Delta {\theta }_L/2\end{array}\right]+O({\Vert \Delta {\theta }_L\Vert }^2),\Delta {\mathbf{R}}_L=\mathbf{I}+[\Delta \theta {]}_{\times }+O({\Vert \Delta {\theta }_L\Vert }^2)$$
• 时间微分:
  $$\begin{array}{c}\dot{\mathbf{q}}=\begin{array}{c}\frac{1}{2}\mathbf{\Omega }({\mathbf{\omega }}_L)\mathbf{q}=\frac{1}{2}\mathbf{q}\otimes {\mathbf{\omega }}_L\end{array},\dot{\mathbf{R}}=\mathbf{R}[{\mathbf{\omega }}_L{]}_{\times }\\ \mathbf{\Omega }({\mathbf{\omega }}_L)=Q^{-}(\mathbf{\omega })=\left[\begin{array}{cc}0& -{\mathbf{\omega }}^T\\ \mathbf{\omega }& -[\mathbf{\omega }{]}_{\times }\end{array}\right]=\left[\begin{array}{cccc}0& -{\omega }_x& -{\omega }_y& -{\omega }_z\\ {\omega }_x& 0& {\omega }_z& -{\omega }_y\\ {\omega }_y& -{\omega }_z& 0& {\omega }_x\\ {\omega }_z& {\omega }_y& -{\omega }_x& 0\end{array}\right]\end{array}$$
• 时间微分推导:
  $$\begin{array}{c}\dot{\mathbf{q}}=\frac{\lim }{\Delta t\to 0}\frac{\mathbf{q}(t+\Delta t)-\mathbf{q}(t)}{\Delta t}\\ =\frac{\lim }{\Delta t\to 0}\frac{\mathbf{q}\otimes \Delta {\mathbf{q}}_L-\mathbf{q}}{\Delta t}\\ =\frac{\lim }{\Delta t\to 0}\frac{\mathbf{q}\otimes (\left[\begin{array}{c}1\\ \Delta {\theta }_L/2\end{array}\right]-\left[\begin{array}{c}1\\ 0\end{array}\right])}{\Delta t}\\ =\frac{\lim }{\Delta t\to 0}\frac{\mathbf{q}\otimes \left[\begin{array}{c}0\\ \Delta {\theta }_L/2\end{array}\right]}{\Delta t}\\ =\frac{1}{2}\mathbf{q}\otimes \left[\begin{array}{c}0\\ {\mathbf{\omega }}_L\end{array}\right]\end{array}$$
• 乘积微分:
  $$\dot{({\mathbf{q}}_1\otimes {\mathbf{q}}_2)}=\dot{{\mathbf{q}}_1}\otimes {\mathbf{q}}_2+{\mathbf{q}}_1\otimes \dot{{\mathbf{q}}_2}$$

5. 旋转速率的时间积分

• 前向积分:
  q n + 1 = q n ⊗ q { ω n Δ t } \mathbf{q}_{n+1}=\mathbf{q}_{n}\otimes \mathbf{q}\left \{ \boldsymbol{\omega _{n}}\Delta t\right \} qn+1​=qn​⊗q{ωn​Δt}
• 后向积分:
  q n + 1 = q n ⊗ q { ω n + 1 Δ t } \mathbf{q}_{n+1}=\mathbf{q}_{n}\otimes \mathbf{q}\left \{ \boldsymbol{\omega _{n+1}}\Delta t\right \} qn+1​=qn​⊗q{ωn+1​Δt}
• 中值积分:
  $${\mathbf{q}}_{n+1}\approx {\mathbf{q}}_n\otimes \mathbf{q}\left\{\frac{{\mathbf{\omega }}_{n+1}+{\mathbf{\omega }}_n}{2}\Delta t\right\}$$
  
  

IMU驱动系统的误差状态运动学

1. 为什么要选择误差状态而不是实际的运动状态?

• 方向误差状态极小（即，它具有与自由度相同的参数数量），避免了与过度参数化（或冗余）有关的问题以及由此产生的相关协方差矩阵奇异的风险．
• 误差状态系统始终在接近原点的位置运行，因此远离可能的参数奇异性，万向节锁定问题等，从而保证了线性化的有效性．
• 误差状态始终很小，这意味着所有二阶乘积都可以忽略不计。 这使得Jacobian的计算非常容易和快速。 一些雅可比行列甚至可能是恒定的，或者等于可用状态的大小．
• 误差状态变化缓慢，因为所有的大的信号变化已经被积分在名义状态里了．因此，在卡尔曼滤波过程中，相较于估计过程，我们可以以更低的频率来执行更新过程．

2. 连续时间内的系统运动方程

• 状态表示
  
• 真实状态
  $$\left[\begin{array}{c}\dot{{\mathbf{p}}_t}={\mathbf{v}}_t\\ \dot{{\mathbf{v}}_t}={\mathbf{a}}_t\\ \dot{{\mathbf{q}}_t}=\frac{1}{2}{\mathbf{q}}_t\otimes {\mathbf{\omega }}_t\\ \dot{{\mathbf{a}}_{bt}}={\mathbf{a}}_{\omega }\\ \dot{{\mathbf{\omega }}_{bt}}={\mathbf{\omega }}_{\omega }\\ \dot{{\mathbf{g}}_t}=0\end{array}\right]\underset{{\mathbf{\omega }}_m={\mathbf{\omega }}_t+{\mathbf{\omega }}_{bt}+{\mathbf{\omega }}_n}{\overset{{\mathbf{a}}_m={\mathbf{R}}_t^T({\mathbf{a}}_t-{\mathbf{g}}_t)+{\mathbf{a}}_{bt}+{\mathbf{a}}_n}{\to }}\left[\begin{array}{c}\dot{{\mathbf{p}}_t}={\mathbf{v}}_t\\ \dot{{\mathbf{v}}_t}={\mathbf{R}}_t({\mathbf{a}}_m-{\mathbf{a}}_{bt}-{\mathbf{a}}_n+{\mathbf{g}}_t)\\ \dot{{\mathbf{q}}_t}=\frac{1}{2}{\mathbf{q}}_t\otimes ({\mathbf{\omega }}_m-{\mathbf{\omega }}_{bt}-{\mathbf{\omega }}_n)\\ \dot{{\mathbf{a}}_{bt}}={\mathbf{a}}_{\omega }\\ \dot{{\mathbf{\omega }}_{bt}}={\mathbf{\omega }}_{\omega }\\ \dot{{\mathbf{g}}_t}=0\end{array}\right]$$
  式中$({\mathbf{a}}_m,{\mathbf{\omega }}_m)$为IMU传感器读数(带有噪声)，$({\mathbf{a}}_t,{\mathbf{\omega }}_t)$为真实的加速度和角速率．
• 名义状态
  $$\left[\begin{array}{c}\dot{\mathbf{p}}=\mathbf{v}\\ \dot{\mathbf{v}}=\mathbf{R}({\mathbf{a}}_m-{\mathbf{a}}_b)+\mathbf{g}\\ \dot{\mathbf{q}}=\frac{1}{2}\mathbf{q}\otimes ({\mathbf{\omega }}_m-{\mathbf{\omega }}_b)\\ \dot{{\mathbf{a}}_b}=0\\ \dot{{\mathbf{w}}_b}=0\\ \dot{\mathbf{g}}=0\end{array}\right]$$
• 误差状态
  $$\left[\begin{array}{c}\dot{\delta \mathbf{p}}=\delta \mathbf{v}\\ \dot{\delta \mathbf{v}}=-\mathbf{R}[{\mathbf{a}}_m-{\mathbf{a}}_b{]}_{\times }\delta \mathbf{\theta }-\mathbf{R}\delta {\mathbf{a}}_b+\delta \mathbf{g}-\mathbf{R}{\mathbf{a}}_n\\ \dot{\delta \mathbf{\theta }}=-[{\mathbf{\omega }}_m-{\mathbf{\omega }}_b{]}_{\times }\delta \mathbf{\theta }-\delta {\mathbf{\omega }}_b-{\mathbf{\omega }}_n\\ \dot{\delta {\mathbf{a}}_b}={\mathbf{a}}_{\omega }\\ \dot{\delta {\mathbf{\omega }}_b}={\mathbf{\omega }}_{\omega }\\ \dot{\delta \mathbf{g}}=0\end{array}\right]$$
• 误差状态的推导
  在进行误差状态方程推导前，必须明确: 误差状态＝真实状态－名义状态
  位置误差状态: $\dot{\delta {\mathbf{p}}_t}=\dot{{\mathbf{p}}_t}-\dot{\mathbf{p}}={\mathbf{v}}_t-\mathbf{v}=\delta \mathbf{v}$
  加速度偏置误差状态: $\dot{\delta {\mathbf{a}}_b}=\dot{{\mathbf{a}}_{bt}}-\dot{{\mathbf{a}}_b}={\mathbf{a}}_{\omega }$
  角速度偏置误差状态: $\dot{\delta {\mathbf{\omega }}_b}=\dot{{\mathbf{\omega }}_{bt}}-\dot{{\mathbf{\omega }}_b}={\mathbf{\omega }}_{\omega }$
  
  速度误差状态:
  根据扰动公式有: ${\mathbf{R}}_t=\mathbf{R}\Delta \mathbf{R}=\mathbf{R}(\mathbf{I}+[\delta \mathbf{\theta }{]}_{\times })+O({\Vert [\delta \mathbf{\theta }]\Vert }^2)$
  令 ${\mathbf{a}}_{\beta }={\mathbf{a}}_m-{\mathbf{a}}_b$为加速度的大信号部分，$\delta {\mathbf{a}}_{\beta }=-\delta {\mathbf{a}}_b-{\mathbf{a}}_n$为加速度的小信号部分，则有:
  $$\begin{array}{c}\dot{v}={\mathbf{R}\mathbf{a}}_{\beta }+\mathbf{g}\\ {\mathbf{a}}_t={\mathbf{R}}_t({\mathbf{a}}_{\beta }+\delta {\mathbf{a}}_{\beta }+\mathbf{g}+\delta \mathbf{g})\end{array}$$
  于是可得:
  $$\begin{array}{c}\dot{{\mathbf{v}}_t}=\dot{\mathbf{v}}+\dot{\delta \mathbf{v}}={\mathbf{R}\mathbf{a}}_{\beta }+\mathbf{g}+\dot{\delta \mathbf{v}}\\ \dot{{\mathbf{v}}_t}=\mathbf{R}(\mathbf{I}+[\delta \mathbf{\theta }{]}_{\times })({\mathbf{a}}_{\beta }+\delta {\mathbf{a}}_{\beta })+\mathbf{g}+\delta \mathbf{g}={\mathbf{R}\mathbf{a}}_{\beta }+\mathbf{R}\delta {\mathbf{a}}_{\beta }+\mathbf{R}[\delta \mathbf{\theta }{]}_{\times }{\mathbf{a}}_{\beta }+\mathbf{R}[\delta \mathbf{\theta }{]}_{\times }\delta {\mathbf{a}}_{\beta }+\mathbf{g}+\delta \mathbf{g}\end{array}$$
  合并上述两个等式消去$\dot{{\mathbf{v}}_t}$，并且消去二阶误差项可得:
  $$\dot{\delta \mathbf{v}}=\mathbf{R}(\delta {\mathbf{a}}_{\beta }-[{\mathbf{a}}_{\beta }{]}_{\times })+\delta \mathbf{g}$$
  再将${\mathbf{a}}_{\beta },\delta {\mathbf{a}}_{\beta }$替换回去，可得:
  $$\dot{\delta \mathbf{v}}=-\mathbf{R}[{\mathbf{a}}_m-{\mathbf{a}}_b{]}_{\times }\delta \mathbf{\theta }-\mathbf{R}\delta \mathbf{g}-{\mathbf{R}\mathbf{a}}_n$$
  因为${\mathbf{a}}_n$为IMU测量噪声向量，是一个高斯分布，因此${\mathbf{R}\mathbf{a}}_n$也为高斯分布，只用重新定义${\mathbf{a}}_n\to {\mathbf{R}\mathbf{a}}_n$为噪声向量即可得:
  $$\dot{\delta \mathbf{v}}=-\mathbf{R}[{\mathbf{a}}_m-{\mathbf{a}}_b{]}_{\times }\delta \mathbf{\theta }-\mathbf{R}\delta \mathbf{g}-{\mathbf{a}}_n$$
  
  角度误差状态:
  令$\mathbf{\omega }={\mathbf{\omega }}_m-{\mathbf{\omega }}_b$角速率大信好部分，$\delta \mathbf{\omega }=-\delta {\mathbf{\omega }}_b-{\mathbf{\omega }}_n$为角速率小信号部分，所以有${\mathbf{\omega }}_t=\mathbf{\omega }+\delta \mathbf{\omega }$.
  下面以两种不同的方式计算$\dot{{\mathbf{q}}_t}$:
  $$\begin{array}{c}\dot{{\mathbf{q}}_t}=\frac{1}{2}{\mathbf{q}}_t\otimes {\mathbf{\omega }}_t\\ \dot{{\mathbf{q}}_t}=\dot{(\mathbf{q}\otimes \delta \mathbf{q})}=\dot{\mathbf{q}}\otimes \delta \mathbf{q}+\mathbf{q}\otimes \dot{\delta \mathbf{q}}=\frac{1}{2}\mathbf{q}\otimes \mathbf{\omega }\otimes \delta \mathbf{q}+\mathbf{q}\otimes \dot{\delta \mathbf{q}}\end{array}$$
  合并上述两个等式消去$\dot{{\mathbf{q}}_t}$，可得:
  $$\begin{array}{c}2\dot{\delta \mathbf{q}}=\delta \mathbf{q}\otimes {\mathbf{\omega }}_t-\mathbf{\omega }\otimes \delta \mathbf{q}\\ ={\mathbf{Q}}^{-}({\mathbf{\omega }}_t)\delta \mathbf{q}-{\mathbf{Q}}^{+}(\mathbf{\omega })\delta \mathbf{q}\\ =\left[\begin{array}{cc}0& -({\mathbf{\omega }}_t-\mathbf{\omega }{)}^T\\ ({\mathbf{\omega }}_t-\mathbf{\omega })& -[{\mathbf{\omega }}_t+\mathbf{\omega }{]}_{\times }\end{array}\right]\left[\begin{array}{c}1\\ \delta \mathbf{\theta }/2\end{array}\right]+O({\Vert \delta \mathbf{\theta }\Vert }^2)\\ =\left[\begin{array}{cc}0& -\delta {\mathbf{\omega }}^T\\ \delta \mathbf{\omega }& -[2\mathbf{\omega }+\delta \mathbf{\omega }{]}_{\times }\end{array}\right]\left[\begin{array}{c}1\\ \delta \mathbf{\theta }/2+O({\Vert \delta \mathbf{\theta }\Vert }^2)\end{array}\right]\end{array}$$
  又根据扰动公式$\delta \mathbf{q}\approx \left[\begin{array}{c}1\\ \delta \mathbf{\theta }/2\end{array}\right]$， 可知$2\dot{\delta \mathbf{q}}=\left[\begin{array}{c}0\\ \dot{\delta \mathbf{\theta }}\end{array}\right]$，所以有:
  $$\begin{array}{c}0=\delta {\mathbf{\omega }}^T\delta \mathbf{\theta }+O({\left|\delta \mathbf{\theta }\right|}^2)\\ \dot{\delta \mathbf{\theta }}=\delta \mathbf{\omega }-[\mathbf{\omega }{]}_{\times }\delta \mathbf{\theta }-\frac{1}{2}[\delta \mathbf{\omega }{]}_{\times }\delta \mathbf{\theta }+O({\Vert \delta \mathbf{\theta }\Vert }^2)\end{array}$$
  忽略上面第二个等式中的二阶误差项并将$\mathbf{\omega },\delta \mathbf{\omega }$带回该等式，得:
  $$\dot{\delta \mathbf{\theta }}=-[{\mathbf{\omega }}_m-{\mathbf{\omega }}_b{]}_{\times }\delta \mathbf{\theta }-\delta {\mathbf{\omega }}_b-{\mathbf{\omega }}_n$$

3. 离散时间内系统的运动方程

• 名义状态
  p ← p + v Δ t + 1 2 ( R ( a m − a b ) + g ) Δ t 2 v ← v + ( R ( a m − a b ) + g ) Δ t q ← q ⊗ q { ( ω m − ω b ) Δ t } a b ← a b ω b ← ω b g ← g \begin{matrix} \boldsymbol{p}\leftarrow \boldsymbol{p}+\boldsymbol{v}\Delta{t}+\frac{1}{2}(\mathbf{R}(\mathbf{a}_{m}-\mathbf{a}_{b})+g)\Delta{t^{2}}\\ \mathbf{v}\leftarrow\mathbf{v}+(\mathbf{R}(\mathbf{a}_{m}-\mathbf{a}_{b})+g)\Delta{t}\\ \mathbf{q}\leftarrow\mathbf{q}\otimes\mathbf{q}\left \{ (\boldsymbol{\omega}_{m}-\boldsymbol{\omega}_{b})\Delta{t} \right \}\\ \mathbf{a}_{b}\leftarrow\mathbf{a}_{b}\\ \boldsymbol{\omega}_{b}\leftarrow\boldsymbol{\omega}_{b}\\ \mathbf{g}\leftarrow\mathbf{g} \end{matrix} p←p+vΔt+21​(R(am​−ab​)+g)Δt2v←v+(R(am​−ab​)+g)Δtq←q⊗q{(ωm​−ωb​)Δt}ab​←ab​ωb​←ωb​g←g​
• 误差状态
  根据导数的定义有: $x_{k+1}-x_k=\dot{x}{\Delta }_t$，依据此定义对上一节连续误差状态方程进行离散化，结果如下:
  δ p ← δ p + δ v Δ t δ v ← δ v + ( − R [ a m − a b ] × δ θ − R δ a b + δ g ) Δ t + v i δ θ ← R T { ( ω m − ω b ) Δ t } δ θ − δ ω b Δ t + θ i δ a b ← δ a b + a i δ ω b ← δ ω b + ω i δ g ← δ g \begin{matrix} \delta\boldsymbol{p}\leftarrow\delta{\mathbf{p}}+\delta{\mathbf{v}}\Delta{t}\\ \delta\mathbf{v}\leftarrow\delta\mathbf{v}+(-\mathbf{R}[\mathbf{a}_{m}-\mathbf{a}_{b}]_{\times}\delta\boldsymbol{\theta}-\mathbf{R}\delta\mathbf{a}_{b}+\delta\mathbf{g})\Delta{t}+\mathbf{v}_{i}\\ \delta\boldsymbol{\theta}\leftarrow\mathbf{R}^{T}\left \{ (\boldsymbol{\omega}_{m}-\boldsymbol{\omega}_{b})\Delta{t} \right.\left.\right\}\delta\boldsymbol{\theta}-\delta\boldsymbol{\omega}_{b}\Delta{t}+\boldsymbol{\theta}_{i}\\ \delta\mathbf{a}_{b}\leftarrow\delta\mathbf{a}_{b}+\mathbf{a}_{i}\\ \delta\boldsymbol{\omega}_{b}\leftarrow\delta\boldsymbol{\omega}_{b}+\boldsymbol{\omega}_{i}\\ \delta\boldsymbol{g}\leftarrow\delta\boldsymbol{g} \end{matrix} δp←δp+δvΔtδv←δv+(−R[am​−ab​]×​δθ−Rδab​+δg)Δt+vi​δθ←RT{(ωm​−ωb​)Δt}δθ−δωb​Δt+θi​δab​←δab​+ai​δωb​←δωb​+ωi​δg←δg​
  其中
  $$\begin{array}{c}{\mathbf{v}}_{\mathbf{i}}\sim N(0,{\mathbf{V}}_{\mathbf{i}}={\sigma }_{a_n}^2\Delta t^2\mathbf{I}[m^2/s^2])\\ {\mathbf{\theta }}_{\mathbf{i}}\sim N(0,{\mathbf{\Theta }}_{\mathbf{i}}={\sigma }_{{\omega }_n}^2\Delta t^2\mathbf{I}[ra{d}^2])\\ {\mathbf{a}}_{\mathbf{i}}\sim N(0,{\mathbf{A}}_{\mathbf{i}}={\sigma }_{a_{\omega }}^2\Delta t\mathbf{I}[m^2/s^4])\\ {\mathbf{\Omega }}_{\mathbf{i}}\sim N(0,{\sigma }_{{\omega }_{\omega }}^2\Delta t\mathbf{I}[ra{d}^2/s^2])\end{array}$$
  其中${\sigma }_{a_n},{\sigma }_{{\omega }_n},{\sigma }_{a_{\omega }},{\sigma }_{{\omega }_{\omega }}$是通过Allan_variance统计方法获取的方差值．已经有专门的开源库来统计该方差值，地址https://github.com/gaowenliang/imu_utils.

4. ESKF预测

• 状态变量定义
  　名义状态: $\mathbf{X}={\left[\begin{array}{cccccc}\mathbf{p}& \mathbf{v}& \mathbf{q}& {\mathbf{a}}_b& {\mathbf{\omega }}_b& \mathbf{g}\end{array}\right]}^T$
  　误差状态: $\delta \mathbf{X}={\left[\begin{array}{cccccc}\delta \mathbf{p}& \delta \mathbf{v}& \delta \mathbf{q}& \delta {\mathbf{a}}_b& \delta {\mathbf{\omega }}_b& \delta \mathbf{g}\end{array}\right]}^T$
  　测量值: ${\mathbf{u}}_m={\left[\begin{array}{cc}{\mathbf{a}}_m& {\mathbf{\omega }}_m\end{array}\right]}^T$
  　误差值: $\mathbf{i}={\left[\begin{array}{cccc}{\mathbf{v}}_{\mathbf{i}}& {\mathbf{\theta }}_{\mathbf{i}}& {\mathbf{a}}_{\mathbf{i}}& {\mathbf{\omega }}_{\mathbf{i}}\end{array}\right]}^T$
• 误差状态预测方程
  $$\delta \mathbf{x}\leftarrow f(\mathbf{X},\delta \mathbf{x},{\mathbf{u}}_m,\mathbf{i})={\mathbf{F}}_{\mathbf{x}}(\mathbf{X},{\mathbf{u}}_m).\delta \mathbf{x}+{\mathbf{F}}_{\mathbf{i}}.\mathbf{i}（泰勒展开）$$
  其中$f(\ast )$为第3节的离散误差状态方程，泰勒展开后结果如下
  F x = ∂ f ∂ δ x ∣ x , u m = [ I I Δ t 0 0 0 0 0 I − R [ a m − a b ] × Δ t − R Δ t 0 I Δ t 0 0 R T { ( ω m − ω b ) Δ t } 0 − I Δ t 0 0 0 0 I 0 0 0 0 0 0 I 0 0 0 0 0 0 I ] \mathbf{F_{x}}=\frac{\partial f}{\partial \delta\mathbf{x}}|_{\mathbf{x},\mathbf{u}_{m}}= \begin{bmatrix} \mathbf{I} & \mathbf{I}\Delta{t} & 0 & 0 & 0 & 0\\ 0 & \mathbf{I} & -\mathbf{R}[\mathbf{a}_{m}-\mathbf{a}_{b}]_{\times}\Delta{t} & -\mathbf{R}\Delta{t} & 0 & \mathbf{I}\Delta{t}\\ 0 & 0 & \mathbf{R}^{T}\left \{ (\boldsymbol{\omega}_{m}-\boldsymbol{\omega}_{b})\Delta{t} \right \} & 0 & -\mathbf{I}\Delta{t} & 0\\ 0 & 0 & 0 & \mathbf{I} & 0 & 0\\ 0 & 0 & 0 & 0 & \mathbf{I} & 0\\ 0 & 0 & 0 & 0 & 0 & \mathbf{I} \end{bmatrix} Fx​=∂δx∂f​∣x,um​​=⎣⎢⎢⎢⎢⎢⎢⎡​I00000​IΔtI0000​0−R[am​−ab​]×​ΔtRT{(ωm​−ωb​)Δt}000​0−RΔt0I00​00−IΔt0I0​0IΔt000I​⎦⎥⎥⎥⎥⎥⎥⎤​
  $${\mathbf{F}}_{\mathbf{i}}=\frac{\partial f}{\partial \mathbf{i}}{|}_{\mathbf{X},{\mathbf{u}}_m}=\left[\begin{array}{cccc}0& 0& 0& 0\\ \mathbf{I}& 0& 0& 0\\ 0& \mathbf{I}& 0& 0\\ 0& 0& \mathbf{I}& 0\\ 0& 0& 0& \mathbf{I}\\ 0& 0& 0& 0\end{array}\right],{\mathbf{Q}}_{\mathbf{i}}=\left[\begin{array}{cccc}{\mathbf{V}}_{\mathbf{i}}& 0& 0& 0\\ 0& {\mathbf{\Theta }}_{\mathbf{i}}& 0& 0\\ 0& 0& {\mathbf{A}}_{\mathbf{i}}& 0\\ 0& 0& 0& {\mathbf{\Omega }}_{\mathbf{i}}\end{array}\right]$$
  上式中的状态变量都是通过离散化的名义状态方程获取的名义状态
• 预测
  $$\begin{array}{c}\widehat{\delta \mathbf{x}}\leftarrow {\mathbf{F}}_{\mathbf{x}}(\mathbf{x},{\mathbf{u}}_m)\widehat{\delta \mathbf{x}}\\ \mathbf{P}\leftarrow {\mathbf{F}}_{\mathbf{x}}\mathbf{P}{{\mathbf{F}}_{\mathbf{x}}}^T+{{\mathbf{F}}_{\mathbf{i}}{\mathbf{Q}}_{\mathbf{i}}{\mathbf{F}}_{\mathbf{i}}}^T\end{array}$$