多传感器融合定位八-惯性导航解算及误差分析其二

Reference:

1. 深蓝学院-多传感器融合
2. 多传感器融合定位理论基础

文章跳转：

1. 多传感器融合定位一-3D激光里程计其一：ICP
2. 多传感器融合定位二-3D激光里程计其二：NDT
3. 多传感器融合定位三-3D激光里程计其三：点云畸变补偿
4. 多传感器融合定位四-3D激光里程计其四：点云线面特征提取
5. 多传感器融合定位五-点云地图构建及定位
6. 多传感器融合定位六-惯性导航原理及误差分析
7. 多传感器融合定位七-惯性导航解算及误差分析其一
8. 多传感器融合定位八-惯性导航解算及误差分析其二
9. 多传感器融合定位九-基于滤波的融合方法Ⅰ其一
10. 多传感器融合定位十-基于滤波的融合方法Ⅰ其二
11. 多传感器融合定位十一-基于滤波的融合方法Ⅱ
12. 多传感器融合定位十二-基于图优化的建图方法其一
13. 多传感器融合定位十三-基于图优化的建图方法其二
14. 多传感器融合定位十四-基于图优化的定位方法
15. 多传感器融合定位十五-多传感器时空标定(综述)

3. 惯性导航解算

3.1 惯性导航解算概述

1. 目的: 利用IMU测量的角速度、加速度，根据上一时刻导航信息，推算出当前时刻导航信息，包括姿态解算、速度解算、位置解算。(根据角速度+加速度，推算位姿+速度)

2. 方法: 由姿态、速度、位置的微分方程，推导出它们的解，并转变成离散时间下的近似形式，从而可以在离散时间采样下，完成导航信息求解。

3. 优点: IMU不受外界信号干扰，可以得到稳定、平滑的导航结果。

4. 缺点: 误差随时间累计，一般时间越长，累计误差越大。因此需要融合，而导航解算(本小节)及其误差分析(下一小节)，是融合的基础。

因此通常用于与其他传感器做一个优势互补，IMU 提供一个连续、稳定的位姿解算；而其他传感器提供不随时间发散，但是有噪声的观测。

3.2 姿态更新

3.2.1 基于旋转矩阵的姿态更新

根据前面推导，旋转矩阵的微分方程为：
$${\dot{\mathbf{R}}}_{wb}={\mathbf{R}}_{wb}[\mathbf{\omega }{]}_{\times }$$根据微分方程的求解方法，可以写出由 $k-1$ 时刻求解 $k$ 时刻旋转矩阵的公式为：
$${\mathbf{R}}_{w{b}_k}={\mathbf{R}}_{w{b}_{k-1}}{e}^{{\int }_{t_{k-1}}^{t_k}[\mathbf{\omega }{]}_{\times }d\tau }$$指数上的积分结果其实就是 $\mathrm{k}-1$ 时刻到 $k$ 时刻之间的相对旋转对应的等效旋转矢量构成的反对称矩阵。

因此上式可以重新写为：
$${\mathbf{R}}_{w{b}_k}={\mathbf{R}}_{w{b}_{k-1}}{e}^{{\mathbf{\varphi }}_{\times }}$$由于反对称矩阵的指数函数前面已经推导完毕，因此上式又可以写为：
$${\mathbf{R}}_{w{b}_k}={\mathbf{R}}_{w{b}_{k-1}}{\mathbf{R}}_{b_{k-1}{b}_k}$$其中(罗德里格斯again)
$${\mathbf{R}}_{b_{k-1}{b}_k}=I+\frac{\sin \varphi }{\varphi }({\varphi }_{\times })+\frac{1-\cos \varphi }{{\varphi }^2}({\varphi }_{\times }{)}^2$$可以看出，解算过程中需要求解等效旋转矢量 $\varphi$，需要借助其微分方程实现。

在旋转矢量的微分方程 $\dot{\varphi }\approx \omega +\frac{1}{2}\varphi \times \omega$ 中(可见2.3节)，对于右侧第二项，根据叉乘的定义可知，当叉乘的两个矢量之间方向重合时，则结果为 $0$，在 $k-1$ 时刻到 $k$ 时刻的极短时间内，可认为接近重合，因此，旋转矢量微分方程可进一步化简为：
$$\dot{\varphi }\approx \mathbf{\omega }\Rightarrow \varphi \approx {\int }_{t_{k-1}}^{t_k}\mathbf{\omega }(\tau )d\tau$$a. 欧拉法: $\varphi ={\mathbf{\omega }}_{k-1}\left(t_k-t_{k-1}\right)$
b. 中值法: $\varphi =\frac{{\omega }_{k-1}+{\omega }_k}{2}\left(t_k-t_{k-1}\right)$
中值法精度大于欧拉法，后续本课程解算全部采用中值法进行。
另有四阶龙格库塔法，在IMU采样频率较高(>100HZ)时，收益不大，此处不做介绍。

3.2.2 基于四元数的姿态更新

根据前面推导，四元数的微分方程为：
$${\dot{\mathbf{q}}}_{wb}={\mathbf{q}}_{wb}\otimes \frac{1}{2}\left[\begin{array}{c}0\\ \mathbf{\omega }\end{array}\right]$$微分方程带着 $\otimes$ 不太好解，转成以下矩阵形式：
$${\dot{\mathbf{q}}}_{wb}=\frac{1}{2}{\left[\begin{array}{c}0\\ \mathbf{\omega }\end{array}\right]}_R{\mathbf{q}}_{wb}$$同样按照解微分方程的方法，并对指数项做积分，可以得到：
$${\dot{\mathbf{q}}}_{wb}={\mathrm{e}}^{\frac{1}{2}\Theta }{\mathbf{q}}_{wb}$$其中：
$$\mathbf{\Theta }=\left[\begin{array}{cccc}0& -{\varphi }_x& -{\varphi }_y& -{\varphi }_z\\ {\varphi }_x& 0& {\varphi }_z& -{\varphi }_y\\ {\varphi }_y& -{\varphi }_z& 0& {\varphi }_x\\ {\varphi }_z& {\varphi }_y& -{\varphi }_x& 0\end{array}\right]$$为了求解该方程，要对指数函数进行泰勒展开，同样包含高次幂，展开方法与前述方法相同（可以自行推导，此处直接给出结论）：
$${\mathrm{e}}^{\frac{1}{2}\Theta (T)}=I\cos \frac{\varphi }{2}+\frac{\Theta }{\varphi }\sin \frac{\varphi }{2}$$因此有：
$${\mathbf{q}}_{w{b}_k}=\left[I\cos \frac{\varphi }{2}+\frac{\Theta }{\varphi }\sin \frac{\varphi }{2}\right]{\mathbf{q}}_{w{b}_{k-1}}$$由于：
$${\left[\begin{array}{c}\cos \frac{\varphi }{2}\\ \frac{\mathbf{\varphi }}{\varphi }\sin \frac{\varphi }{2}\end{array}\right]}_R=\left[I\cos \frac{\varphi }{2}+\frac{\Theta }{\varphi }\sin \frac{\varphi }{2}\right]$$令：
$${\mathbf{q}}_{b_{k-1}{b}_k}=\left[\begin{array}{c}\cos \frac{\varphi }{2}\\ \frac{\mathbf{\varphi }}{\varphi }\sin \frac{\varphi }{2}\end{array}\right]$$则 ${\mathbf{q}}_{w{b}_k}={\mathbf{q}}_{w{b}_{k-1}}\otimes {\mathbf{q}}_{b_{k-1}{b}_k}$
旋转矢量 $\varphi$ 的计算仍采用中值法进行，即 $\mathbf{\varphi }=\frac{{\mathbf{\omega }}_{k-1}+{\omega }_k}{2}\left(t_k-t_{k-1}\right)$。注意 ${\mathbf{q}}_{b_{k-1}{b}_k}$ 需要归一化，即求 norm()。

3.3 速度更新

速度微分方程为：
$$\dot{\mathbf{v}}={\mathbf{R}}_{wb}\mathbf{a}-\mathbf{g}$$其中 $\mathbf{a}=\left[\begin{array}{lll}{a}_x& a_y& a_z\end{array}\right]$ 为测量加速度，$\mathbf{g}=\left[\begin{array}{lll}0& 0& g_0\end{array}\right]$ 为重力加速度。
该微分方程的通解形式为：
$$\Delta \mathbf{v}=\left({\mathbf{R}}_{wb}\mathbf{a}-\mathbf{g}\right)\Delta t$$对应的基于中值法的速度更新形式为：
$${\mathbf{v}}_k={\mathbf{v}}_{k-1}+\left(\frac{{\mathbf{R}}_{w{b}_k}{\mathbf{a}}_k+{\mathbf{R}}_{w{b}_{k-1}}{\mathbf{a}}_{k-1}}{2}-\mathbf{g}\right)\left(t_k-t_{k-1}\right)$$

3.4 位置更新

位置微分方程为：
$$\dot{\mathbf{p}}=\mathbf{v}$$其通解形式为 $\Delta \mathbf{p}=\mathbf{v}\Delta t$
此处 $\mathbf{v}$ 指的是该时间段内的平均速度，该形式对应的基于中值法的离散形式为：
$${\mathbf{p}}_k={\mathbf{p}}_{k-1}+\frac{{\mathbf{v}}_k+{\mathbf{v}}_{k-1}}{2}\left(t_k-t_{k-1}\right)$$另外，通解还可以写为：
$$\Delta \mathbf{p}=\mathbf{v}\Delta t+\frac{1}{2}\mathbf{a}\Delta t^2$$需要注意的是，此处的 $\mathbf{v}$ 指的是该时间段起始时刻速度(上下两个$\mathbf{v}$表示的东西不太一样)，
该形式对应的基于中值法的离散形式为：
$$\begin{array}{rl}{\mathbf{p}}_k=& {\mathbf{p}}_{k-1}+{\mathbf{v}}_{k-1}\left(t_k-t_{k-1}\right)\\ & +\frac{1}{2}\left(\frac{{\mathbf{R}}_{w{b}_k}{\mathbf{a}}_k+{\mathbf{R}}_{w{b}_{k-1}}{\mathbf{a}}_{k-1}}{2}-\mathbf{g}\right){\left(t_k-t_{k-1}\right)}^2\end{array}$$ 这个公式的好处在于，不需要提前求 $v_k$，求 $v_{k-1}$ 就可以了。

3.5 惯性导航解算总结

3.5.1 姿态解算

a. 基于旋转矩阵
$${\mathbf{R}}_{w{b}_k}={\mathbf{R}}_{w{b}_{k-1}}\left(I+\frac{\sin \varphi }{\varphi }(\varphi \times )+\frac{1-\cos \varphi }{{\varphi }^2}(\varphi \times {)}^2\right)\text{ 其中 }\mathbf{\varphi }=\frac{{\mathbf{\omega }}_{k-1}+{\omega }_k}{2}\left(t_k-t_{k-1}\right)$$b. 基于四元数
$${\mathbf{q}}_{w{b}_k}={\mathbf{q}}_{w{b}_{k-1}}\otimes \left[\begin{array}{c}\cos \frac{\varphi }{2}\\ \frac{\mathbf{\varphi }}{\varphi }\sin \frac{\varphi }{2}\end{array}\right]\text{ 其中 }\mathbf{\varphi }=\frac{{\mathbf{\omega }}_{k-1}+{\mathbf{\omega }}_k}{2}\left(t_k-t_{k-1}\right)$$

3.5.2 速度解算

$${\mathbf{v}}_k={\mathbf{v}}_{k-1}+\left(\frac{{\mathbf{R}}_{\mathbf{w}{b}_k}{\mathbf{a}}_k+{\mathbf{R}}_{\mathbf{w}{b}_{k-1}}{\mathbf{a}}_{k-1}}{2}-\mathbf{g}\right)\left(t_k-t_{k-1}\right)$$

3.5.3 位置解算

$${\mathbf{p}}_k={\mathbf{p}}_{k-1}+\frac{{\mathbf{v}}_k+{\mathbf{v}}_{k-1}}{2}\left(t_k-t_{k-1}\right)\text{ 或 }{\mathbf{p}}_k={\mathbf{p}}_{k-1}+{\mathbf{v}}_{k-1}\left(t_k-t_{k-1}\right)+\frac{1}{2}\left(\frac{{\mathbf{R}}_{w{b}_k}{\mathbf{a}}_k+{\mathbf{R}}_{w{b}_{k-1}}{\mathbf{a}}_{k-1}}{2}-\mathbf{g}\right){\left(t_k-t_{k-1}\right)}^2$$

4. 惯性导航误差分析

4.1 误差方程推导方法

误差分析的输出形式为误差方程。

误差方程：状态量(速度误差、位置误差、姿态误差、bias误差等)误差形式的表示。误差方程及其推导方法，是后续滤波、图优化等融合方案的基础，是重中之重。误差方程的推导有固定的套路，举例说明具体步骤：

1. 写出不考虑误差时的微分方程
   $$\dot{z}=x+y$$
2. 写出考虑误差时的微分方程
   $$\dot{\tilde{z}}=\tilde{x}+\tilde{y}$$
3. 写出真实值与理想值之间的关系(这里的 $\delta$ 表示误差量)
   $$\begin{array}{l}\tilde{z}=z+\delta z\\ \tilde{x}=x+\delta x\\ \tilde{y}=y+\delta y\end{array}$$
4. 把 3) 中的关系，带入 2)
   $$\dot{z}+\delta \dot{z}=x+\delta x+y+\delta y$$
5. 把 1) 中的关系，带入 4)
   $$x+y+\delta \dot{z}=x+\delta x+y+\delta y$$
6. 化简方程
   $$\delta \dot{z}=\delta x+\delta y$$

我们把这个简单的例子抽象成六个步骤，后面无论有多么复杂的问题，都可以用这六个步骤解决。

4.2 姿态误差方程

1. 写出不考虑误差的微分方程
   $${\dot{\mathbf{q}}}_t=\frac{1}{2}{\mathbf{q}}_t\otimes \left[\begin{array}{c}0\\ {\mathbf{\omega }}_t-{\mathbf{b}}_{{\omega }_t}\end{array}\right]$$这里考虑了零偏 ${\mathbf{b}}_{{\omega }_t}$，所以在测量的 ${\mathbf{\omega }}_t$ 上，要减去一个 ${\mathbf{b}}_{{\omega }_t}$。

2. 写出考虑误差的微分方程
   $$\dot{\widetilde{{\mathbf{q}}_t}}=\frac{1}{2}{\tilde{\mathbf{q}}}_t\otimes \left[\begin{array}{c}0\\ {\tilde{\mathbf{\omega }}}_t-{\tilde{\mathbf{b}}}_{{\omega }_t}\end{array}\right]$$每个量上面都有误差。

3. 写出带误差的值与理想值之间的关系
   $$\begin{array}{l}{\tilde{\mathbf{q}}}_t={\mathbf{q}}_t\otimes \delta \mathbf{q}\\ {\tilde{\mathbf{\omega }}}_t={\mathbf{\omega }}_t+{\mathbf{n}}_{\omega }\\ {\tilde{\mathbf{b}}}_{{\omega }_t}={\mathbf{b}}_{{\omega }_t}+\delta {\mathbf{b}}_{\omega }\end{array}$$其中 ${\mathbf{n}}_{\omega }$ 为陀螺仪白噪声。

   其中
   $$\delta \mathbf{q}=\left[\begin{array}{c}\cos \left(\frac{|\delta \theta |}{2}\right)\\ \frac{\delta \mathbf{\theta }}{|\delta \theta |}\sin \left(\frac{|\delta \theta |}{2}\right)\end{array}\right]\approx \left[\begin{array}{c}1\\ \frac{\delta \mathbf{\theta }}{2}\end{array}\right]$$$\delta \mathbf{\theta }$ 是姿态误差对应的旋转矢量(有时被称为失准角)，因为 $\delta \mathbf{\theta }$ 是个非常小的量，所以 $\cos \left(\frac{|\delta \theta |}{2}\right)$ 约等于 $1$；$\sin \left(\frac{|\delta \theta |}{2}\right)$ 约等于它本身。$\delta \mathbf{\theta }$ 代表的是一个旋转的大小，需要注意的是，它是用等效旋转矢量表示的，它绕固定轴旋转，旋转一定角度，用一个矢量表示，矢量方向就是轴的方向，矢量的大小就是轴的大小。

4. 将 3) 中的关系带入 2)
   $$\dot{\left({\mathbf{q}}_t\otimes \delta \mathbf{q}\right)}=\frac{1}{2}{\mathbf{q}}_t\otimes \delta \mathbf{q}\otimes \left[\begin{array}{c}0\\ {\mathbf{\omega }}_t+{\mathbf{n}}_{\omega }-{\mathbf{b}}_{{\omega }_t}-\delta {\mathbf{b}}_{\omega }\end{array}\right]$$

5. 把 1) 中的关系带入 4)
   $$\begin{array}{rl}& \dot{\left({\mathbf{q}}_t\otimes \delta \mathbf{q}\right)}\\ =& {\dot{\mathbf{q}}}_t\otimes \delta \mathbf{q}+{\mathbf{q}}_t\otimes \delta \dot{\mathbf{q}}\\ =& \frac{1}{2}{\mathbf{q}}_t\otimes \left[\begin{array}{c}0\\ {\mathbf{\omega }}_t-{\mathbf{b}}_{{\omega }_t}\end{array}\right]\otimes \delta \mathbf{q}+{\mathbf{q}}_t\otimes \delta \dot{\mathbf{q}}\\ =& \frac{1}{2}{\mathbf{q}}_t\otimes \delta \mathbf{q}\otimes \left[\begin{array}{c}0\\ {\mathbf{\omega }}_t+{\mathbf{n}}_{\omega }-{\mathbf{b}}_{{\omega }_t}-\delta {\mathbf{b}}_{\omega }\end{array}\right]\end{array}$$最后一行为公式 4) 所得

6. 化简方程
   首先把 5) 中最后两行左乘 ${\left({\mathbf{q}}_t\right)}^{-1}$ 并移项可得：
   $$\begin{array}{rl}\delta \dot{\mathbf{q}}=& \frac{1}{2}\delta \mathbf{q}\otimes \left[\begin{array}{c}0\\ {\mathbf{\omega }}_t+{\mathbf{n}}_{\omega }-{\mathbf{b}}_{{\omega }_t}-\delta {\mathbf{b}}_{\omega }\end{array}\right]\\ & -\frac{1}{2}\left[\begin{array}{c}0\\ {\mathbf{\omega }}_t-{\mathbf{b}}_{{\omega }_t}\end{array}\right]\otimes \delta \mathbf{q}\end{array}$$根据前面所讲，四元数相乘可以转换成矩阵与向量相乘：
   $$\begin{array}{cc}& \mathbf{p}\otimes \mathbf{q}=[\mathbf{p}{]}_L\mathbf{q}=[\mathbf{q}{]}_R\mathbf{p}\\ & [\mathbf{p}{]}_L=\left[\begin{array}{cc}{p}_0& -{\mathbf{p}}_v^{\mathrm{T}}\\ {\mathbf{p}}_v& p_0\mathbf{I}+{\left[{\mathbf{p}}_v\right]}_{\times }\end{array}\right]\\ & [\mathbf{q}{]}_R=\left[\begin{array}{cc}{q}_0& -{\mathbf{q}}_v^{\mathrm{T}}\\ {\mathbf{q}}_v& q_0\mathbf{I}-{\left[{\mathbf{q}}_v\right]}_{\times }\end{array}\right]\end{array}$$令：
   $$\begin{array}{l}{\mathbf{\omega }}_1={\mathbf{\omega }}_t+{\mathbf{n}}_{\omega }-{\mathbf{b}}_{{\omega }_t}-\delta {\mathbf{b}}_{\omega }\\ {\mathbf{\omega }}_2={\mathbf{\omega }}_t-{\mathbf{b}}_{\omega }\end{array}$$则：
   $$\begin{array}{rl}\delta \dot{\mathbf{q}}& =\frac{1}{2}{\left[\begin{array}{c}0\\ {\mathbf{\omega }}_1\end{array}\right]}_R\delta \mathbf{q}-\frac{1}{2}{\left[\begin{array}{c}0\\ {\mathbf{\omega }}_2\end{array}\right]}_L\delta \mathbf{q}\\ & =\frac{1}{2}\left[\begin{array}{cc}0& {\left({\mathbf{\omega }}_2-{\mathbf{\omega }}_1\right)}^T\\ \left({\mathbf{\omega }}_1-{\mathbf{\omega }}_2\right)& -{\left[{\mathbf{\omega }}_1+{\mathbf{\omega }}_2\right]}_{\times }\end{array}\right]\delta \mathbf{q}\end{array}$$由于融合时，状态量中往往不是使用四元数，而是使用失准角，因此要把上式转成失准角的微分形式。
   由于：
   $$\delta \mathbf{q}\approx \left[\begin{array}{c}1\\ \frac{\delta \mathbf{\theta }}{2}\end{array}\right]\Rightarrow \delta \dot{q}\approx \left[\begin{array}{c}0\\ \frac{\delta \dot{\mathbf{\theta }}}{2}\end{array}\right]$$把它代入上式，又可以得到：
   $$\begin{array}{rl}\delta \dot{\mathbf{\theta }}=& -{\left[{\mathbf{\omega }}_1+{\mathbf{\omega }}_2\right]}_{\times }\frac{\delta \mathbf{\theta }}{2}+\left({\mathbf{\omega }}_1-{\mathbf{\omega }}_2\right)\\ =& -{\left[2{\mathbf{\omega }}_t+{\mathbf{n}}_{\omega }-2{\mathbf{b}}_{{\omega }_t}-\delta {\mathbf{b}}_{\omega }\right]}_{\times }\frac{\delta \mathbf{\theta }}{2}\\ & +{\mathbf{n}}_{\omega }-\delta {\mathbf{b}}_{\omega }\end{array}$$忽略其中的二阶小项，可得：
   $$\delta \dot{\mathbf{\theta }}=-{\left[{\mathbf{\omega }}_t-{\mathbf{b}}_{{\omega }_t}\right]}_{\times }\delta \mathbf{\theta }+{\mathbf{n}}_{\omega }-\delta {\mathbf{b}}_{\omega }$$

4.3 速度误差方程

1. 写出不考虑误差时的微分方程
   $${\dot{\mathbf{v}}}_t={\mathbf{R}}_t\left({\mathbf{a}}_t-{\mathbf{b}}_{a_t}\right)$$
2. 写出考虑误差时的微分方程
   $${\dot{\tilde{\mathbf{v}}}}_t={\tilde{\mathbf{R}}}_{\mathbf{t}}\left({\tilde{\mathbf{a}}}_t-{\tilde{\mathbf{b}}}_{a_t}\right)$$
3. 写出真实值和理想值之间的关系
   $$\begin{array}{rl}\tilde{\mathbf{v}}& =\mathbf{v}+\delta \mathbf{v}\\ {\tilde{\mathbf{R}}}_t& ={\mathbf{R}}_t\exp \left([\delta \mathbf{\theta }{]}_{\times }\right)\\ & \approx {\mathbf{R}}_t\left(\mathbf{I}+[\delta \mathbf{\theta }{]}_{\times }\right)\\ {\tilde{\mathbf{a}}}_t& ={\mathbf{a}}_t+{\mathbf{n}}_a\\ {\tilde{\mathbf{b}}}_{a_t}& ={\mathbf{b}}_{a_t}+\delta {\mathbf{b}}_a\end{array}$$其中 ${\mathbf{n}}_a$ 为加速度计白噪声。
4. 将3)中的关系带入2)
   $$\begin{array}{rl}& \dot{\mathbf{v}}+\delta \dot{\mathbf{v}}\\ =& {\mathbf{R}}_t\left(\mathbf{I}+[\delta \mathbf{\theta }{]}_{\times }\right)\left({\mathbf{a}}_t+{\mathbf{n}}_a-{\mathbf{b}}_{a_t}-\delta {\mathbf{b}}_a\right)\end{array}$$
5. 将1)中的关系带入4)
   $$\begin{array}{rl}& {\mathbf{R}}_t\left({\mathbf{a}}_t-{\mathbf{b}}_{a_t}\right)+\delta \dot{\mathbf{v}}\\ =& {\mathbf{R}}_t\left(\mathbf{I}+[\delta {]}_{\times }\right)\left({\mathbf{a}}_t+{\mathbf{n}}_a-{\mathbf{b}}_{a_t}-\delta {\mathbf{b}}_a\right)\end{array}$$
6. 化简方程(忽略二阶小项)
   $$\begin{array}{rl}& \delta \dot{\mathbf{v}}\\ =& {\mathbf{R}}_t[\delta \mathbf{\theta }{]}_{\times }\left({\mathbf{a}}_t-{\mathbf{b}}_{a_t}\right)+{\mathbf{R}}_t\left({\mathbf{n}}_a-\delta {\mathbf{b}}_a\right)\\ =& -{\mathbf{R}}_t{\left[{\mathbf{a}}_t-{\mathbf{b}}_{a_t}\right]}_{\times }\delta \mathbf{\theta }+{\mathbf{R}}_t\left({\mathbf{n}}_a-\delta {\mathbf{b}}_a\right)\end{array}$$最后一步使用的性质为：$[V_1{]}_{\times }{V}_2=-[V_2{]}_{\times }{V}_1$

4.4 位置误差方程

1. 写出不考虑误差时的微分方程：
   $$\dot{p}=\mathbf{v}$$
2. 写出考虑误差时的微分方程：
   $$\dot{\tilde{\mathbf{p}}}=\tilde{\mathbf{v}}$$
3. 写出真实值与理想值之间的关系：
   $$\begin{array}{l}\tilde{\mathbf{v}}=\mathbf{v}+\delta \mathbf{v}\\ \tilde{\mathbf{p}}=\mathbf{p}+\delta \mathbf{p}\end{array}$$
4. 将 3) 中的关系带入 2)：
   $$\dot{\mathbf{p}}+\delta \dot{\mathbf{p}}=\mathbf{v}+\delta \mathbf{v}$$
5. 把 1) 中的关系带入 4)：
   $$\mathbf{v}+\delta \dot{\mathbf{p}}=\mathbf{v}+\delta \mathbf{v}$$
6. 化简方程：
   $$\delta \dot{\mathbf{p}}=\delta \mathbf{v}$$

4.5 bias误差方程

在IMU精度较高时，bias认为是常值，即有：
$$\begin{array}{rl}\delta {\dot{\mathbf{b}}}_a& =0\\ \delta {\dot{\mathbf{b}}}_{\omega }& =0\end{array}$$但自动驾驶和机器人领域所用的mems多数达不到这种精度，因为角速度随机游走和加速度随机游走较大，因此误差方程常写为：
$$\begin{array}{rl}\delta {\dot{\mathbf{b}}}_a& ={\mathbf{n}}_{b_a}\\ \delta {\dot{\mathbf{b}}}_{\omega }& ={\mathbf{n}}_{b\omega }\end{array}$$其中 ${\mathbf{n}}_{b_a}$ 和 ${\mathbf{n}}_{b\omega }$ 分别为加速度计和陀螺仪的随机游走对应的白噪声。
以上两种形式都很常见，没有绝对的对和错。

4.6 惯性导航误差分析总结

$$\begin{array}{l}\delta \dot{\mathbf{p}}=\delta \mathbf{v}\\ \delta \dot{\mathbf{v}}=-{\mathbf{R}}_t{\left[{\mathbf{a}}_t-{\mathbf{b}}_{a_t}\right]}_{\times }\delta \mathbf{\theta }+{\mathbf{R}}_t\left({\mathbf{n}}_a-\delta {\mathbf{b}}_a\right)\\ \delta \dot{\mathbf{\theta }}=-{\left[{\mathbf{\omega }}_t-{\mathbf{b}}_{{\omega }_t}\right]}_{\times }\delta \mathbf{\theta }+{\mathbf{n}}_{\omega }-\delta {\mathbf{b}}_{\omega }\\ \delta {\dot{\mathbf{b}}}_a={\mathbf{n}}_{b_a}\text{ 或 }\delta {\dot{\mathbf{b}}}_a=0\\ \delta {\dot{\mathbf{b}}}_{\omega }={\mathbf{n}}_{b\omega }\text{或 }\delta {\dot{\mathbf{b}}}_{\omega }=0\end{array}$$