主要整理了基于 IMU 测量数据进行位姿、速度解算的方法。
连续时间下的 IMU 运动模型
位置、速度、姿态更新
这里忽略尺度因子和轴偏差的影响，对 IMU 的运动模型定义如下：
$$
\begin{aligned}
\tilde{\boldsymbol{\omega}}^b &= \boldsymbol{\omega}^b + \boldsymbol{b}^g + \boldsymbol{n}^g \\
\tilde{\boldsymbol{a}}^b &= \boldsymbol{a}^b + \boldsymbol{b}^a + \boldsymbol{n}^a
\end{aligned}
$$
上式中，$\boldsymbol{b}$ 为零偏，$\boldsymbol{n}$ 为测量值中的高斯白噪声。上标 $b$ 表示 IMU 测量对象的 body 系，$a, g$ 分别对应加速度计和陀螺仪的误差值。
这里我们主要关注，在连续时间下，IMU 位置(Pose) 、速度(Velocity) 以及姿态(Attitude) 随时间的变化。要求一段时间内这些参数的变化量，思路是通过对他们的导数进行积分，位置和速度的导数很简单，分别是：
$$
\begin{aligned}
\dot{\boldsymbol{p}}_{wb} &= \boldsymbol{v}^w_t \\
\dot{\boldsymbol{v}}^w_t &= \boldsymbol{a}^w_t
\end{aligned}
$$
式中上标 $w$ 表示 $\boldsymbol{v, a}$ 分别表示（Body 坐标系在）参考惯性坐标系（I 系）下的速度和加速度，$\boldsymbol{p}_{wb}$ 表示 Body 坐标系在参考惯性坐标系下的位置。此外我们还需要求姿态的导数，由于姿态有旋转矩阵、四元数、欧拉角以及旋转向量等不同表示方法，需要分开讨论。在讨论求导的时候，我们一般使用四元数或者旋转向量来表示姿态，欧拉角因为有万向锁的问题实际计算中比较少使用，旋转向量实际上就是李代数，会在旋转矩阵（李群）求导时用上。根据我之前的整理，四元数和旋转向量对时间求导分别是：
四元数姿态对时间求导
$$
\dot{\boldsymbol{q}}_{wb_t} = \boldsymbol{q}\otimes\begin{bmatrix}
0 \\
\frac{1}{2}\boldsymbol{\omega}^{b_t}
\end{bmatrix}
$$
旋转矩阵姿态对时间求导
$$
\dot{\boldsymbol{R}}_{wb_t} = \boldsymbol{R}_{wb_t}\boldsymbol{\omega}^{b_t\wedge}
$$
上式中，$\boldsymbol{\omega}^{b_t\wedge}$ 表示 IMU 在参考惯性坐标系下的旋转角速度在 Body 系下的表示（其对应的旋转轴是在 Body 系下的），也是 IMU 中陀螺仪直接测量的参数。
下面对四元数和旋转矩阵分别进行连续时间(时间 $i$ 到 $j$ )下的运动模型推导：
基于四元数状态更新
姿态更新比较直观，$i~j$ 之间对姿态导数求积分可以求得此旗舰的姿态的变化量，前面还需要乘上原来的姿态；
$$
\begin{aligned}
\boldsymbol{q}_{wb_j} =\boldsymbol{q}_{wb_t}\otimes\int_{t\in[i,j]}{(\boldsymbol{q}_{wb_t}\otimes\begin{bmatrix}
0 \\
\frac{1}{2}\boldsymbol{\omega}^{b_t}
\end{bmatrix})}\delta t \\
\boldsymbol{q}_{wb_j} =\int_{t\in[i,j]}{(\boldsymbol{q}_{wb_t}\otimes\begin{bmatrix}
1 \\
\frac{1}{2}\boldsymbol{\omega}^{b_t}
\end{bmatrix})}\delta t
\end{aligned}
$$
速度更新则是在原有速度的基础上加上由于加速度产生的变化量，由于加速度计测量出的加速度测量值是在 IMU 的 Body 系下表示的（大小不变，但是加速度对应的轴是在 Body 系下），因此我们需要用旋转矩阵对其进行旋转至导航坐标系与重力加速度相减求其真实加速度，如下所示(这里的 $\boldsymbol{a, g}$ 需要齐次化转化为四元数的形式（实部为 0，虚部为对应元素）)：
$$
\boldsymbol{v}^w_j = \boldsymbol{v}^w_i + \int_{t\in[i,j]}{(\boldsymbol{q}_{wb_t}\boldsymbol{a}(\boldsymbol{q}_{wb_t}^{b_t})^{-1} - \boldsymbol{g}^w)}\delta t
$$
位置更新则是在原有的位置上加上两部分变化量，一部分是由初始速度在给定时间范围内产生的位置变化，另一部分是由加速度产生的位置变化，需要二重积分，如下所示：
$$
\boldsymbol{p}_{wb_j} = \boldsymbol{p}_{wb_i} + \boldsymbol{v}^w_i\Delta t + \int\int_{t\in[i,j]}{(\boldsymbol{q}_{wb_t}\boldsymbol{a}(\boldsymbol{q}_{wb_t}^{b_t})^{-1} - \boldsymbol{g}^w)}\delta t^2
$$
基于旋转矩阵状态更新
基于旋转矩阵也是类似的，将上述的四元数计算部分换成旋转矩阵的对应计算即可，如下所示：
$$
\begin{aligned}
\boldsymbol{R}_{wb_j} &= \boldsymbol{R}_{wb_i}\int_{t\in[i,j]}{(\boldsymbol{R}_{wb_t}(\boldsymbol{\omega}^{b_t})^\wedge)}\delta t \\
\boldsymbol{v}^w_j &= \boldsymbol{v}^w_i + \int_{t\in[i,j]}{(\boldsymbol{R}_{wb_t}\boldsymbol{a} - \boldsymbol{g}^w)}\delta t\\
\boldsymbol{p}_{wb_j} &= \boldsymbol{p}_{wb_t} + \boldsymbol{v}^w_i\Delta t + \int\int_{t\in[i,j]}{(\boldsymbol{R}_{wb_t}\boldsymbol{a} - \boldsymbol{g}^w)}\delta t^2
\end{aligned}
$$
连续积分具体运算
TODO
IMU 运动模型离散化
上述的积分是在连续时间下进行推导的，但实际使用中我们能获取的只有在离散时间内的采样值，因此有必要对积分离散化。这里有两种思路。
欧拉法离散积分
思路比较简单，从 $k$ 到 $k+1$ 的状态变化用第 $k$ 时刻的测量值来进行估算，即在采样时间内我们认为 IMU 在做以 $k$ 时刻测量值为准的定角速度定加速度运动，结果如下所示：
基于四元数的欧拉法离散积分：
$$
\begin{aligned}
\boldsymbol{a} &= \boldsymbol{q}_{wb_k}\boldsymbol{a}^{b_k}\boldsymbol{q}_{wb_k}^{-1} - \boldsymbol{g}^w \\
\boldsymbol{\omega} &= \boldsymbol{\omega}^{b_k}\\
\boldsymbol{q}_{wb_{k+1}} &= \boldsymbol{q}_{wb_k}\otimes\begin{bmatrix}
1\\
\frac{1}{2}\boldsymbol{\omega}\Delta t
\end{bmatrix}\\
\boldsymbol{v}_{k+1}^w &= \boldsymbol{v}_k^w + \boldsymbol{a}\Delta t\\
\boldsymbol{p}_{wb_{k+1}} &= \boldsymbol{p}_{wb_{k}} + \boldsymbol{v}_k^w\Delta t + \frac{1}{2}\boldsymbol{a}\Delta t^2
\end{aligned}
$$
其中前两个式子是分别对测量值减去重力角速度，注意再减去重力角速度之前要先将加速度转换至导航坐标系下。
基于旋转矩阵的欧拉法离散积分：
思路大同小异，如下所示：
$$
\begin{aligned}
\boldsymbol{a} &= \boldsymbol{R}_{wb_k}\boldsymbol{a}^{b_k} - \boldsymbol{g}^w \\
\boldsymbol{\omega} &= \boldsymbol{\omega}^{b_k}\\
\boldsymbol{R}_{wb_{k+1}} &= \boldsymbol{R}_{wb_k}\boldsymbol{R}_{b_kb_{k+1}} = \boldsymbol{R}_{wb_k}\exp{((\boldsymbol{\omega}\Delta t)^\wedge)}\\
\boldsymbol{v}_{k+1}^w &= \boldsymbol{v}_k^w + \boldsymbol{a}\Delta t\\
\boldsymbol{p}_{wb_{k+1}} &= \boldsymbol{p}_{wb_{k}} + \boldsymbol{v}_k^w\Delta t + \frac{1}{2}\boldsymbol{a}\Delta t^2
\end{aligned}
$$
注意上式中，我们没有考虑误差造成的影响，这一部分我们留到下一步误差分析进行详细推导。此外这里利用了旋转矢量对应的李代数通过指数映射求对应旋转矩阵的过程，具体计算过程可以参考我之前整理的方法。
中值法离散积分
中值法顾名思义则是利用 $k$ 和 $k+1$ 的测量值分别作为采样时间内的加速度和角速度进行积分，积分过程和上面一样，加速度和角速度计算如下：
基于四元数：
$$
\begin{aligned}
\boldsymbol{a} &= \frac{1}{2}[\boldsymbol{q}_{wb_k}\boldsymbol{a}^{b_k} \boldsymbol{q}_{wb_k}^{-1} - \boldsymbol{g}^w + \boldsymbol{q}_{wb_{k+1}}(\boldsymbol{a}^{b_{k+1}} - \boldsymbol{b}_{k}^a)\boldsymbol{q}_{wb_{k+1}}^{-1} - \boldsymbol{g}^w]\\
\boldsymbol{\omega} &= \frac{1}{2}[\boldsymbol{\omega}^{b_k} + \boldsymbol{\omega}^{b_{k+1}} ]
\end{aligned}
$$
基于旋转矩阵：
$$
\begin{aligned}
\boldsymbol{a} &= \frac{1}{2}(\boldsymbol{R}_{wb_k}\boldsymbol{a}^{b_k} - \boldsymbol{g}^w + \boldsymbol{R}_{wb_{k+1}}\boldsymbol{a}^{b_{k+1}} - \boldsymbol{g}^w) \\
\boldsymbol{\omega} &= \frac{1}{2}[\boldsymbol{\omega}^{b_k} + \boldsymbol{\omega}^{b_{k+1}} ]
\end{aligned}
$$
姿态更新中的坐标系讨论
在上述分析中，大部分时候我们都用世界坐标系来作为参考坐标系，但实际过程主要涉及到到三个坐标系：
- IMU 参考的惯性坐标系，一般以地心惯性系为参考，这个坐标系和 ECEF 系不同不会随着地球自转旋转
- 导航坐标系：一般以当地的东北天坐标系作为导航坐标系
- 机体坐标系：这里可以认为是 IMU 的 Body 系
不难发现载体和惯性系之间有一个相对旋转角速度 $\boldsymbol{\omega}_{in}$，由两部分组成：地球的自转引起的导航系和惯性系的旋转$\boldsymbol{\omega}_{ie}$、由于地球球体不是规则的球体，因此载体在运动的时候也会对当地的导航系形成一定的角速度 $\boldsymbol{\omega}_{en}$，如下所示。这一部分参考《捷联惯导算法与组合导航原理讲义》，这里不进行深入探讨。
$$
\begin{aligned}
\boldsymbol{\omega}_{in} &= \boldsymbol{\omega}_{ie} + \boldsymbol{\omega}_{en}\\
\boldsymbol{\omega}_{ie} &= \begin{bmatrix}
0 & \omega_{ie}\cos{L} & \omega_{ie}\cos{L}
\end{bmatrix}^T\\
\boldsymbol{\omega}_{en} &= \begin{bmatrix}
-\frac{v_N}{R_M+h} & \frac{v_E}{R_N + h} & \frac{v_E}{R_N + h}\tan{L}
\end{bmatrix}^T
\end{aligned}
$$
而 IMU 测量值是对于惯性系的，因此我们如果要得到导航系（东北天）下的加速度和角速度还需要做出旋转补偿，这里以旋转矩阵为例，如下所示：
$$
\begin{aligned}
\boldsymbol{R}_{nb} &= \boldsymbol{R}_{ni}\boldsymbol{R}_{ib}\\
&= \boldsymbol{R}_{n_tn_{t-1}}\boldsymbol{R}_{n_{t-1}i}\boldsymbol{R}_{ib_{t-1}}\boldsymbol{R}_{b_tb_{t-1}}
\end{aligned}
$$
这里面由于导航系（N 系）和载体系（B 系）相对于惯性系而言都是动坐标系，因此需要标注时间。
但由于在大部分情况下，SLAM 的场合都比较小，基本不需要考虑地速带来的影响，因此很多论文中将当地的导航系（东北天）当作不动的惯性系，作为世界坐标系来使用。下面的误差分析中以这种简化模型来进行推导。
误差分析
上面整理了怎么通过 IMU 的测量数据进行状态的更新，接下来看一下在更新过程误差是怎么变化的，这里将加速度计和陀螺仪的零偏也视作 IMU 的状态。
对于误差分析的过程一般如下所示，以下面例子为例，假设理想状态下：
$$
\begin{aligned}
\dot{z} &= x + y \\
\end{aligned}
$$
考虑误差为线性相加，则微分方程和各个真实值和理想值之间的关系：
$$
\begin{aligned}
\dot{\tilde{z}} &= \tilde{x} + \tilde{y}\\
\tilde{z} &= z + \delta z\\
\tilde{x} &= x + \delta x\\
\tilde{y} &= y + \delta y
\end{aligned}
$$
由于误差形式已知，很容易对考虑误差的状态值写出微分方程，并代入各个状态量：
$$
\begin{aligned}
\dot{z} + \delta\dot{z} = x + \delta x + y + \delta y
\end{aligned}
$$
而理想状态下的 $z$ 求导结果是已知的，可以直接代入并消掉状态量得：
$$
\begin{aligned}
x + y + \delta\dot{z} &= x + \delta x + y + \delta y\\
\Rightarrow \delta\dot{z} &= \delta x + \delta y
\end{aligned}
$$
通过最后的式子我们知道怎么不同状态量之间误差的传递方式。接下来按照这个步骤对姿态、速度和位置的误差更新进行分析。
姿态误差分析（旋转矩阵）
关于姿态误差分析，针对姿态的不同方法可以有不同的推导方法，这里用旋转矩阵作为旋转表示方法进行推导。另外，在大部分的 SLAM 论文中会将加速度计和陀螺仪的零偏也作为状态之一，因此这里将其考虑在内。
第一步，写出理想状态下姿态微分方程：
$$
\dot{\boldsymbol{R}}_{wb} = \boldsymbol{R}_{wb}((\boldsymbol{\omega}_{wb}^{b} - \boldsymbol{b}^{\omega})^\wedge)
$$
第二步，写出考虑误差时的微分方程：
$$
\dot{\tilde{\boldsymbol{R}}}_{wb} = \tilde{\boldsymbol{R}}_{wb}((\tilde{\boldsymbol{\omega}}_{wb}^{b} - \tilde{\boldsymbol{b}}^{\omega})^\wedge)
$$
第三步，写出真实值和测量值的误差构成形式，这里一共涉及三个变量，姿态 $\boldsymbol{R}_{wb}$，IMU 角速度 $\boldsymbol{\omega}_{wb}^b$，以及陀螺仪的零偏，下面一一进行分析。
首先是姿态 $\boldsymbol{R}_{wb}$ 的误差，这里的误差有可能是因为 Body 系或者导航系(世界坐标系)的误差构成，计算时将真实值和理想值的之间的误差用旋转向量表示，因此误差关系如下所示，并且由于误差是小量可以做近似：
$$
\tilde{\boldsymbol{R}}_{wb} = \boldsymbol{R}_{wb}\exp{(\delta\boldsymbol{\theta})^\wedge}\approx\boldsymbol{R}_{wb}(\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge)
$$
接下来看 IMU 相对于惯性系的角速度的误差模型，这里在之前的传感器误差模型中已经大致提到过，测量值的误差模型中大致包括尺度因子，轴偏差以及零偏。实际中一般不会考虑这么多误差参数，会根据需求取一部分变量作为误差估计，这里只考虑角速度的白噪声作为随机误差：
$$
\tilde{\boldsymbol{\omega}}_{wb}^b = \boldsymbol{\omega}_{wb}^b + \boldsymbol{n}^{\omega}
$$
最后是零偏误差：
$$
\tilde{\boldsymbol{b}}^{\omega} = \boldsymbol{b}^{\omega} + \delta\boldsymbol{b}^{\omega}
$$
第四步，将误差模型代入含误差的微分方程中：
误差模型如下：
$$
\begin{aligned}
\tilde{\boldsymbol{R}}_{wb} &\approx \boldsymbol{R}_{wb}(\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge)\\
\tilde{\boldsymbol{\omega}}_{wb}^b &= \boldsymbol{\omega}_{wb}^b + \boldsymbol{n}^{\omega}\\
\tilde{\boldsymbol{b}}^{\omega} &= \boldsymbol{b}^{\omega} + \delta\boldsymbol{b}^{\omega}
\end{aligned}
$$
在第二步中得到带误差的微分方程为：
$$
\dot{\tilde{\boldsymbol{R}}}_{wb} = \tilde{\boldsymbol{R}}_{wb}((\tilde{\boldsymbol{\omega}}_{wb}^{b} - \tilde{\boldsymbol{b}}^{\omega})^\wedge)
$$
代入误差模型得：
$$
\dot{(\boldsymbol{R}_{wb}(\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge))} = \boldsymbol{R}_{wb}(\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge)((\boldsymbol{\omega}_{wb}^b + \boldsymbol{n}^{\omega} - \boldsymbol{b}^{\omega} - \delta\boldsymbol{b}^{\omega})^\wedge)
$$
第五步，将理想情况下的微分方程代入上式中，可得：
$$
\begin{aligned}
\text{左边} &= \dot{(\boldsymbol{R}_{wb}(\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge))}\\
&= \dot{\boldsymbol{R}}_{wb}(\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge) + \boldsymbol{R}_{wb}\dot{(\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge)}\\
&= \boldsymbol{R}_{wb}((\boldsymbol{\omega}_{wb}^{b} - \boldsymbol{b}^{\omega})^\wedge)(\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge) + \boldsymbol{R}_{wb}\dot{\delta\boldsymbol{\theta}}^\wedge\\
\text{右边} &= \boldsymbol{R}_{wb}(\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge)((\boldsymbol{\omega}_{wb}^b
+ \boldsymbol{n}^{\omega} - \boldsymbol{b}^{\omega} - \delta\boldsymbol{b}^{\omega})^\wedge)
\end{aligned}
$$
最后整理化简，这里不难看出可以通过右乘 $\boldsymbol{R}_{bw}$ 来化简：
$$
\begin{aligned}
\mathrm{左边} &= ((\boldsymbol{\omega}_{wb}^{b} - \boldsymbol{b}^{\omega})^\wedge)(\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge) + \dot{\delta\boldsymbol{\theta}}^\wedge\\
\mathrm{右边} &= (\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge)((\boldsymbol{\omega}_{wb}^b
+ \boldsymbol{n}^{\omega} - \boldsymbol{b}^{\omega} - \delta\boldsymbol{b}^{\omega})^\wedge)\\
\end{aligned}
$$
进一步移项化简，中间会忽略二阶小量（小量和小量的乘积）：
$$
\begin{aligned}
\dot{\delta\boldsymbol{\theta}}^\wedge &= (\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge)((\boldsymbol{\omega}_{wb}^b + \boldsymbol{n}^{\omega} - \boldsymbol{b}^{\omega} - \delta\boldsymbol{b}^{\omega})^\wedge) - ((\boldsymbol{\omega}_{wb}^{b} - \boldsymbol{b}^{\omega})^\wedge)(\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge)\\
&= (\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge)((\boldsymbol{\omega}_{wb}^b - \boldsymbol{b}^{\omega})^\wedge +(\boldsymbol{n}^{\omega} - \delta\boldsymbol{b}^{\omega})^\wedge) - ((\boldsymbol{\omega}_{wb}^{b} - \boldsymbol{b}^{\omega})^\wedge)(\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge)\\
&= \delta\boldsymbol{\theta}^\wedge(\boldsymbol{\omega}_{wb}^b - \boldsymbol{b}^{\omega})^\wedge - (\boldsymbol{\omega}_{wb}^b - \boldsymbol{b}^{\omega})^\wedge\delta\boldsymbol{\theta}^\wedge + (\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge)(\boldsymbol{n}^{\omega} - \delta\boldsymbol{b}^{\omega})^\wedge\\
&= - ((\boldsymbol{\omega}_{wb}^b - \boldsymbol{b}^{\omega})^\wedge\delta\boldsymbol{\theta}^\wedge - \delta\boldsymbol{\theta}^\wedge(\boldsymbol{\omega}_{wb}^b - \boldsymbol{b}^{\omega})^\wedge) + (\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge)(\boldsymbol{n}^{\omega} - \delta\boldsymbol{b}^{\omega})^\wedge\\
&= - ((\boldsymbol{\omega}_{wb}^b - \boldsymbol{b}^{\omega})\times\delta\boldsymbol{\theta})^\wedge + (\boldsymbol{n}^{\omega} - \delta\boldsymbol{b}^{\omega})^\wedge\\
&= (-(\boldsymbol{\omega}_{wb}^b - \boldsymbol{b}^{\omega})\times\delta\boldsymbol{\theta} + \boldsymbol{n}^{\omega} - \delta\boldsymbol{b}^{\omega})^\wedge\\
\Rightarrow \dot{\delta\boldsymbol{\theta}} &= -(\boldsymbol{\omega}_{wb}^b - \boldsymbol{b}^{\omega})^\wedge\delta\boldsymbol{\theta} + \boldsymbol{n}^{\omega} - \delta\boldsymbol{b}^{\omega}
\end{aligned}
$$
上式化简中，利用了反对称矩阵这样一个性质: $\boldsymbol{v}_1^\wedge\boldsymbol{v}_2^\wedge - \boldsymbol{v}_2^\wedge\boldsymbol{v}_1^\wedge = (\boldsymbol{v}_1\times\boldsymbol{v}_2)^\wedge$
姿态误差分析（四元数）
由于四元数比较紧凑，很多论文也会用四元数作为旋转的表示方式，因此这里用四元数也进行一次姿态误差的推导。
第一步，写出理想状态下姿态微分方程：
$$
\dot{\boldsymbol{q}}_{wb} = \frac{1}{2}\boldsymbol{q}_{wb}\otimes\begin{bmatrix}
0\\\boldsymbol{\omega}_{wb}^{b} - \boldsymbol{b}^{\omega}
\end{bmatrix}
$$
第二步，写出考虑误差时的微分方程：
$$
\dot{\tilde{\boldsymbol{q}}}_{wb} = \frac{1}{2}\tilde{\boldsymbol{q}}_{wb}\otimes\begin{bmatrix}
0\\\tilde{\boldsymbol{\omega}}_{wb}^{b} - \tilde{\boldsymbol{b}}^{\omega}
\end{bmatrix}
$$
第三步，写出真实值和测量值的误差构成形式，和旋转矩阵基本一致：
$$
\begin{aligned}
\tilde{\boldsymbol{q}}_{wb} &= \boldsymbol{q}_{wb}\otimes\delta\boldsymbol{q}\\
\tilde{\boldsymbol{\omega}}_{wb}^b &= \boldsymbol{\omega}_{wb}^b + \boldsymbol{n}^{\omega}\\
\tilde{\boldsymbol{b}}^{\omega} &= \boldsymbol{b}^{\omega} + \delta\boldsymbol{b}^{\omega}
\end{aligned}
$$
四元数的变量量和失准角的关系为：
$$
\begin{aligned}
\delta\boldsymbol{q} &=
\begin{bmatrix}
\cos{(||\delta\boldsymbol{\theta}||/2)} \\ \sin{(||\delta\boldsymbol{\theta}||/2)\frac{\delta\boldsymbol{\theta}}{||\delta\boldsymbol{\theta}||}}
\end{bmatrix}\\
&\approx\begin{bmatrix}
1 \\ \delta\boldsymbol{\theta} /2
\end{bmatrix} \qquad ||\delta\boldsymbol{\theta}||\approx0
\end{aligned}
$$
第四步，将误差模型代入含误差的微分方程中：
$$
\dot{(\boldsymbol{q}_{wb}\otimes\delta\boldsymbol{q})} = \frac{1}{2}(\boldsymbol{q}_{wb}\otimes\delta\boldsymbol{q})\otimes\begin{bmatrix}
0\\(\boldsymbol{\omega}_{wb}^b + \boldsymbol{n}^{\omega}) - (\boldsymbol{b}^{\omega} + \delta\boldsymbol{b}^{\omega})
\end{bmatrix}
$$
第五步，将理想情况下的微分方程代入上式中，可得：
$$
\begin{aligned}
\text{左边} &= \dot{(\boldsymbol{q}_{wb}\otimes\delta\boldsymbol{q})} \\
&= \dot{\boldsymbol{q}}_{wb}\otimes\delta\boldsymbol{q} + \boldsymbol{q}_{wb}\otimes\dot{\delta\boldsymbol{q}}\\
&= \frac{1}{2}\boldsymbol{q}_{wb}\otimes\begin{bmatrix}
0\\\boldsymbol{\omega}_{wb}^{b} - \boldsymbol{b}^{\omega}
\end{bmatrix} \otimes\delta\boldsymbol{q} + \boldsymbol{q}_{wb}\otimes\dot{\delta\boldsymbol{q}}\\
\text{右边} &= \frac{1}{2}(\boldsymbol{q}_{wb}\otimes\delta\boldsymbol{q})\otimes\begin{bmatrix}
0\\(\boldsymbol{\omega}_{wb}^b + \boldsymbol{n}^{\omega}) - (\boldsymbol{b}^{\omega} + \delta\boldsymbol{b}^{\omega})
\end{bmatrix}
\end{aligned}
$$
第六步，移项化简：
$$
\begin{aligned}
\frac{1}{2}\boldsymbol{q}_{wb}\otimes\begin{bmatrix}
0\\\boldsymbol{\omega}_{wb}^{b} - \boldsymbol{b}^{\omega}
\end{bmatrix} \otimes\delta\boldsymbol{q} + \boldsymbol{q}_{wb}\otimes\dot{\delta\boldsymbol{q}} &= \frac{1}{2}(\boldsymbol{q}_{wb}\otimes\delta\boldsymbol{q})\otimes\begin{bmatrix}
0\\(\boldsymbol{\omega}_{wb}^b + \boldsymbol{n}^{\omega}) - (\boldsymbol{b}^{\omega} + \delta\boldsymbol{b}^{\omega})
\end{bmatrix}\\
\frac{1}{2}\begin{bmatrix}
0\\\boldsymbol{\omega}_{wb}^{b} - \boldsymbol{b}^{\omega}
\end{bmatrix} \otimes\delta\boldsymbol{q} + \dot{\delta\boldsymbol{q}} &= \frac{1}{2}\delta\boldsymbol{q}\otimes\begin{bmatrix}
0\\(\boldsymbol{\omega}_{wb}^b + \boldsymbol{n}^{\omega}) - (\boldsymbol{b}^{\omega} + \delta\boldsymbol{b}^{\omega})
\end{bmatrix}\\
\dot{\delta\boldsymbol{q}} &= \frac{1}{2}\delta\boldsymbol{q}\otimes\begin{bmatrix}
0\\(\boldsymbol{\omega}_{wb}^b + \boldsymbol{n}^{\omega}) - (\boldsymbol{b}^{\omega} + \delta\boldsymbol{b}^{\omega})
\end{bmatrix} - \frac{1}{2}\begin{bmatrix}
0\\\boldsymbol{\omega}_{wb}^{b} - \boldsymbol{b}^{\omega}
\end{bmatrix} \otimes\delta\boldsymbol{q}
\end{aligned}
$$
为了方便化简，将四元数乘法转换为矩阵乘法，可以参考：移动机器人位姿不同的表示方法-四元数的运算性质
设：
$$
\begin{aligned}
\boldsymbol{\omega}_1 &= (\boldsymbol{\omega}_{wb}^b + \boldsymbol{n}^{\omega}) - (\boldsymbol{b}^{\omega} + \delta\boldsymbol{b}^{\omega})\\
\boldsymbol{\omega}_2 &= \boldsymbol{\omega}_{wb}^{b} - \boldsymbol{b}^{\omega}
\end{aligned}
$$
则上式可以转换为：
$$
\begin{aligned}
\dot{\delta\boldsymbol{q}} &= \frac{1}{2}\delta\boldsymbol{q}\otimes\begin{bmatrix}
0\\\boldsymbol{\omega}_1
\end{bmatrix} - \frac{1}{2}\begin{bmatrix}
0\\\boldsymbol{\omega}_2
\end{bmatrix} \otimes\delta\boldsymbol{q}\\
&= \frac{1}{2}\left(\begin{bmatrix}0 \\\boldsymbol{\omega}_1\end{bmatrix}_R\delta\boldsymbol{q} - \begin{bmatrix}0 \\\boldsymbol{\omega}_2\end{bmatrix}_R\delta\boldsymbol{q}\right)\\
&= \frac{1}{2}\left(\begin{bmatrix}0 \\\boldsymbol{\omega}_1\end{bmatrix}_R - \begin{bmatrix}0 \\\boldsymbol{\omega}_2\end{bmatrix}_R\right)\delta\boldsymbol{q}\\
&= \frac{1}{2}
\begin{bmatrix}
0 & (\boldsymbol{\omega}_1 - \boldsymbol{\omega}_2)^T\\
(\boldsymbol{\omega}_1 - \boldsymbol{\omega}_2) & -(\boldsymbol{\omega}_1 + \boldsymbol{\omega}_2)^\wedge
\end{bmatrix}
\delta\boldsymbol{q}
\end{aligned}
$$
将角增量的微分代入：
$$
\begin{aligned}
\delta\boldsymbol{q} &\approx\begin{bmatrix}1 \\ \delta\boldsymbol{\theta} /2\end{bmatrix}\\
\dot{\delta\boldsymbol{q}} &\approx \begin{bmatrix}0 \\ \dot{\delta\boldsymbol{\theta}}/2\end{bmatrix}
\end{aligned}
$$
则有：
$$
\begin{aligned}
\begin{bmatrix}0 \\ \dot{\delta\boldsymbol{\theta}}/2\end{bmatrix} &=
\frac{1}{2}
\begin{bmatrix}
0 & (\boldsymbol{\omega}_1 - \boldsymbol{\omega}_2)^T\\
(\boldsymbol{\omega}_1 - \boldsymbol{\omega}_2) & -(\boldsymbol{\omega}_1 + \boldsymbol{\omega}_2)^\wedge
\end{bmatrix}
\begin{bmatrix}1 \\ \delta\boldsymbol{\theta} /2\end{bmatrix}\\
\Rightarrow \dot{\delta\boldsymbol{\theta}} &= (\boldsymbol{\omega}_1 - \boldsymbol{\omega}_2) - \frac{1}{2}(\boldsymbol{\omega}_1 + \boldsymbol{\omega}_2)^\wedge\delta\boldsymbol{\theta}\\
&= (\boldsymbol{n}^{\omega} - \delta\boldsymbol{b}^{\omega}) - \frac{1}{2}(2\boldsymbol{\omega}_{wb}^b + \boldsymbol{n}^{\omega} - 2\boldsymbol{b}^{\omega} - \delta\boldsymbol{b}^{\omega})^\wedge\delta\boldsymbol{\theta}\\
&\approx -(\boldsymbol{\omega}_{wb}^b - \boldsymbol{b}^{\omega})^\wedge\delta\boldsymbol{\theta} + \boldsymbol{n}^{\omega} - \delta\boldsymbol{b}^{\omega}
\end{aligned}
$$
最后获得了同样的结果，间接也证明了旋转矩阵和四元数是等效的。
速度误差分析
同样对速度误差进行分析，第一步写出理想情况微分方程，这里同样也将加速度计的零偏作为状态考虑：
$$
\dot{\boldsymbol{v}}^w = \boldsymbol{R}_{wb}(\boldsymbol{a}^b - \boldsymbol{b}^a) -\boldsymbol{g}^w
$$
第二步写出考虑误差的微分方程：
$$
\dot{\tilde{\boldsymbol{v}}}^w = \tilde{\boldsymbol{R}}_{wb}(\tilde{\boldsymbol{a}}^b - \tilde{\boldsymbol{b}}^a)- \tilde{\boldsymbol{g}}^w
$$
第三步写出各个量的误差模型，这里一共涉及五个量：
$$
\begin{aligned}
\tilde{\boldsymbol{v}}&=\boldsymbol{v}+\delta\boldsymbol{v}\\
\tilde{\boldsymbol{R}}_{wb} &\approx \boldsymbol{R}_{wb}(\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge)\\
\tilde{\boldsymbol{a}}^b &= \boldsymbol{a}^b + \boldsymbol{n}_a\\
\tilde{\boldsymbol{b}}^a &= \boldsymbol{b}^a + \delta\boldsymbol{b}^a\\
\tilde{\boldsymbol{g}}^w&= \boldsymbol{g}^w + \delta\boldsymbol{g}^w
\end{aligned}
$$
其中，$\boldsymbol{n}_a$ 是 IMU 加速度计的白噪声，其余误差分析方法和角速度测量误差类似，可以参考内参标定部分。
第四步，将误差模型代入含误差的微分方程中
$$
\begin{aligned}
\dot{\tilde{\boldsymbol{v}}}^w &= \tilde{\boldsymbol{R}}_{wb}(\tilde{\boldsymbol{a}}^b - \tilde{\boldsymbol{b}}^a)- \tilde{\boldsymbol{g}}^w\\
\frac{d(\boldsymbol{v}+\delta\boldsymbol{v})}{dt} &= \boldsymbol{R}_{wb}(\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge)(\boldsymbol{a}^b + \boldsymbol{n}_a - (\boldsymbol{b}^a + \delta\boldsymbol{b}^a)) -(\boldsymbol{g}^w + \delta\boldsymbol{g}^w)\\
\dot{\boldsymbol{v}} + \dot{\delta\boldsymbol{v}}&= \boldsymbol{R}_{wb}(\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge)(\boldsymbol{a}^b + \boldsymbol{n}_a - (\boldsymbol{b}^a + \delta\boldsymbol{b}^a)) -(\boldsymbol{g}^w + \delta\boldsymbol{g}^w)
\end{aligned}
$$
第五步，将理想微分方程代入上式得：
$$
\begin{aligned}
\dot{\boldsymbol{v}} + \dot{\delta\boldsymbol{v}}&= \boldsymbol{R}_{wb}(\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge)(\boldsymbol{a}^b + \boldsymbol{n}_a - (\boldsymbol{b}^a + \delta\boldsymbol{b}^a)) -(\boldsymbol{g}^w + \delta\boldsymbol{g}^w)\\
\boldsymbol{R}_{wb}(\boldsymbol{a}^b - \boldsymbol{b}^a) -\boldsymbol{g}^w + \dot{\delta\boldsymbol{v}}&= \boldsymbol{R}_{wb}(\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge)(\boldsymbol{a}^b + \boldsymbol{n}_a - (\boldsymbol{b}^a + \delta\boldsymbol{b}^a)) -(\boldsymbol{g}^w + \delta\boldsymbol{g}^w)\\
\dot{\delta\boldsymbol{v}}&= \boldsymbol{R}_{wb}(\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge)(\boldsymbol{a}^b + \boldsymbol{n}_a - (\boldsymbol{b}^a + \delta\boldsymbol{b}^a)) -(\boldsymbol{g}^w + \delta\boldsymbol{g}^w) - \boldsymbol{R}_{wb}(\boldsymbol{a}^b - \boldsymbol{b}^a) +\boldsymbol{g}^w
\end{aligned}
$$
最后一步，化简并消去二阶小量得：
$$
\begin{aligned}
\dot{\delta\boldsymbol{v}}&= \boldsymbol{R}_{wb}(\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge)(\boldsymbol{a}^b + \boldsymbol{n}_a - (\boldsymbol{b}^a + \delta\boldsymbol{b}^a)) -(\boldsymbol{g}^w + \delta\boldsymbol{g}^w) - \boldsymbol{R}_{wb}(\boldsymbol{a}^b - \boldsymbol{b}^a) +\boldsymbol{g}^w\\
&= \boldsymbol{R}_{wb}(\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge)(\boldsymbol{a}^b + \boldsymbol{n}_a - (\boldsymbol{b}^a + \delta\boldsymbol{b}^a))- \delta\boldsymbol{g}^w - \boldsymbol{R}_{wb}(\boldsymbol{a}^b - \boldsymbol{b}^a)\\
&= \boldsymbol{R}_{wb}(\boldsymbol{a}^b + \boldsymbol{n}_a - (\boldsymbol{b}^a + \delta\boldsymbol{b}^a)) +\boldsymbol{R}_{wb}\delta\boldsymbol{\theta}^\wedge(\boldsymbol{a}^b + \boldsymbol{n}_a - (\boldsymbol{b}^a + \delta\boldsymbol{b}^a))- \delta\boldsymbol{g}^w - \boldsymbol{R}_{wb}(\boldsymbol{a}^b - \boldsymbol{b}^a)\\
&= \boldsymbol{R}_{wb}(\boldsymbol{n}_a - \delta\boldsymbol{b}^a) +\boldsymbol{R}_{wb}\delta\boldsymbol{\theta}^\wedge(\boldsymbol{a}^b - \boldsymbol{b}^a)- \delta\boldsymbol{g}^w\\
&= \boldsymbol{R}_{wb}(\boldsymbol{n}_a - \delta\boldsymbol{b}^a) -\boldsymbol{R}_{wb}(\boldsymbol{a}^b - \boldsymbol{b}^a)^\wedge\delta\boldsymbol{\theta}- \delta\boldsymbol{g}^w\\
\end{aligned}
$$
在要求精度不高时可以忽略 $\delta\boldsymbol{g}^w$
位置误差分析
位置的误差分析更加简单，如下所示：
第一步，理想微分方程：
$$
\dot{\boldsymbol{p}} = \boldsymbol{v}
$$
第二步，含误差微分方程：
$$
\dot{\tilde{\boldsymbol{p}}} = \tilde{\boldsymbol{v}}
$$
第三步，误差模型：
$$
\tilde{\boldsymbol{p}} = \boldsymbol{p} + \delta\boldsymbol{p}\\
\tilde{\boldsymbol{v}} = \boldsymbol{v} + \delta\boldsymbol{v}
$$
第四步，误差模型代入含误差微分方程中：
$$
\dot{\boldsymbol{p}} + \delta\dot{\boldsymbol{p}} = \boldsymbol{v} + \delta\boldsymbol{v}
$$
第五步，理想微分方程代入上式：
$$
\boldsymbol{v} + \delta\dot{\boldsymbol{p}} = \boldsymbol{v} + \delta\boldsymbol{v}
$$
第六步，整理化简，消去二阶小量：
$$
\delta\dot{\boldsymbol{p}} = \delta\boldsymbol{v}
$$
零偏误差更新
零偏误差的微分可以不考虑或者使用 IMU 的随机游走来表示：
$$
\begin{aligned}
\delta\boldsymbol{b}^a &= 0\\
\delta\boldsymbol{b}^g &= 0\\
\end{aligned}
$$
或
$$
\begin{aligned}
\delta\boldsymbol{b}^a &= \boldsymbol{n}_{b^a}\\
\delta\boldsymbol{b}^w &= \boldsymbol{n}_{b^w}\\
\end{aligned}
$$
总结
IMU 状态误差的更新方式为：
$$
\begin{aligned}
\delta\dot{\boldsymbol{p}} &= \delta\boldsymbol{v}\\
\dot{\delta\boldsymbol{v}}&=\boldsymbol{R}_{wb}(\boldsymbol{n}_a - \delta\boldsymbol{b}^a) -\boldsymbol{R}_{wb}(\boldsymbol{a}^b - \boldsymbol{b}^a)^\wedge\delta\boldsymbol{\theta}- \delta\boldsymbol{g}^w\\
\delta\dot{\boldsymbol{\theta}}&= -(\boldsymbol{\omega}_{wb}^b - \boldsymbol{b}^{\omega})^\wedge\delta\boldsymbol{\theta} + \boldsymbol{n}^{\omega} - \delta\boldsymbol{b}^{\omega}\\
\delta\dot{\boldsymbol{b}}^a &= \boldsymbol{n}_{b^a} (\text{或者} 0)\\
\delta\dot{\boldsymbol{b}}^g &= \boldsymbol{n}_{b^w} (\text{或者} 0)
\end{aligned}
$$
误差分析的目的
误差分析主要有：
- 可以用来推导状态和观测方程
- 可以通过导航任务精度的要求，通过误差转移关系反推出所需传感器的精度来辅助器件选型。（另一种选型的方法是通过仿真来选型）
- 在 ESKF 中可以作为预测方程对误差进行扩散
补充
误差分析的内容很大一部分参考了《捷联惯导算法与组合导航原理讲义》，书里面的旋转矩阵（方向余弦矩阵）用 $\boldsymbol{C}^n_b$ 这种方式来表示，因为我习惯用 $\boldsymbol{R}_{nb}$ 来表示从 $b$ 系坐标转换为 $n$ 系坐标的旋转矩阵，所以都统一用这种表示方法了。