从运动学到惯性测量单元IMU

Inertial Measurement Unit

1、旋转矩阵求导

考虑从世界坐标系 { W } \{W\} {W}原点出发的向量$\mathbf{p}$绕单位轴$\mathbf{u}$旋转，角速度矢量可以表示为$\mathbf{\omega }=\dot{\theta }\mathbf{u}$。易得向量$\mathbf{p}$末端点的速度矢量，即$\mathbf{p}$对时间的一阶导数为
$$\frac{\mathrm{d}\mathbf{p}}{\mathrm{d}t}=\mathbf{\omega }\times \mathbf{p}$$
事实上，当角速度大小和方向随时间改变时，上式仍然成立。

设机体坐标系 { B } \{B\} {B}到世界坐标系 { W } \{W\} {W}的旋转矩阵${\mathbf{R}}_{wb}=\left[{\mathbf{i}}_B,{\mathbf{j}}_B,{\mathbf{k}}_B\right]$，若三个轴${\mathbf{i}}_B,{\mathbf{j}}_B,{\mathbf{k}}_B$以角速度$\mathbf{\omega }$旋转，则根据上式，三个轴对时间的一阶导数为
$$\frac{\mathrm{d}{\mathbf{i}}_B}{\mathrm{d}t}=\mathbf{\omega }\times {\mathbf{i}}_B,\frac{\mathrm{d}{\mathbf{j}}_B}{\mathrm{d}t}=\mathbf{\omega }\times {\mathbf{j}}_B,\frac{\mathrm{d}{\mathbf{k}}_B}{\mathrm{d}t}=\mathbf{\omega }\times {\mathbf{k}}_B$$
故${\mathbf{R}}_{wb}$对时间的一阶导数为
$${\dot{\mathbf{R}}}_{wb}=[\mathbf{\omega }\times {\mathbf{i}}_B,\mathbf{\omega }\times {\mathbf{j}}_B,\mathbf{\omega }\times {\mathbf{k}}_B]=\mathbf{\omega }\times {\mathbf{R}}_{wb}$$
这里的角速度$\mathbf{\omega }$为机体坐标系 { B } \{B\} {B}相对于世界坐标系 { W } \{W\} {W}的角速度，且是在世界坐标系下表达的，故也记为${\mathbf{\omega }}_{wb}^w$

由叉乘运算和反对称矩阵乘法的对应关系得到
$${\dot{\mathbf{R}}}_{wb}=({\mathbf{\omega }}_{wb}^w{)}^{\wedge }{\mathbf{R}}_{wb}\text{\hspace{1em}(1)}$$
机体坐标系 { B } \{B\} {B}相对于世界坐标系 { W } \{W\} {W}的角速度，在机体坐标系下的表达为${\mathbf{\omega }}_{wb}^b={\mathbf{R}}_{bw}{\mathbf{\omega }}_{wb}^w$，故有${\mathbf{\omega }}_{wb}^w={\mathbf{R}}_{wb}{\mathbf{\omega }}_{wb}^b$
$${\dot{\mathbf{R}}}_{wb}=({\mathbf{R}}_{wb}{\mathbf{\omega }}_{wb}^b{)}^{\wedge }{\mathbf{R}}_{wb}\text{\hspace{1em}(2)}$$
根据§1，$(2)$可写为
$${\dot{\mathbf{R}}}_{wb}={\mathbf{R}}_{wb}({\mathbf{\omega }}_{wb}^b{)}^{\wedge }\text{\hspace{1em}(3)}$$
若${\mathbf{\omega }}_b$为定值，给定$t_0$时刻初始旋转矩阵为$\mathbf{R}(t_0)$，则微分方程$(3)$的解析解为
$$\mathbf{R}(t)=\mathbf{R}(t_0)\exp \left(({\mathbf{\omega }}_{wb}^b{)}^{\wedge }(t-t_0)\right)$$
简便起见，可以将上式写成
$$\mathbf{R}(t)=\mathbf{R}(t_0)\mathrm{Exp}\left({\mathbf{\omega }}_{wb}^b(t-t_0)\right)$$

2、车体坐标系与世界坐标系的速度，加速度转换关系

车的运动速度指的是车体坐标系 { B } \{B\} {B}相对于世界坐标系 { W } \{W\} {W}的速度在世界坐标系下的表示，记作${\mathbf{v}}_{wb}^w$，车体坐标系原点在车体坐标系下一直为原点，速度为零，所以不谈论该点在车体坐标系下的速度，我们谈论的是车体坐标系 { B } \{B\} {B}相对于世界坐标系 { W } \{W\} {W}的速度在车体坐标系下的表示

$${\mathbf{v}}_{wb}^b={\mathbf{R}}_{bw}{\mathbf{v}}_{wb}^w$$
这个速度可以通过轮子、电机等设备测到。实际上，${\mathbf{v}}_{wb}^b={\dot{\mathbf{p}}}_{wb}^b$的值与世界坐标系的选取无关，${\mathbf{v}}_{wb}^w={\dot{\mathbf{p}}}_{wb}^w$的取值依赖于世界坐标系的选取

为方便推导，可将上式改写为
$${\mathbf{v}}_{wb}^w={\mathbf{R}}_{wb}{\mathbf{v}}_{wb}^b$$
对上式进行求导，可以得到
$$\begin{array}{rl}{\dot{\mathbf{v}}}_{wb}^w& ={\dot{\mathbf{R}}}_{wb}{\mathbf{v}}_{wb}^b+{\mathbf{R}}_{wb}{\dot{\mathbf{v}}}_{wb}^b\\ & ={\mathbf{R}}_{wb}({\mathbf{\omega }}_{wb}^b{)}^{\wedge }{\mathbf{v}}_{wb}^b+{\mathbf{R}}_{wb}{\dot{\mathbf{v}}}_{wb}^b\end{array}\text{\hspace{1em}(1)}$$
记${\mathbf{a}}_{wb}^b$为车体坐标系下的位移加速度，而车体坐标系下的总加速度${\dot{\mathbf{v}}}_{wb}^b$除位移加速度外，还包含了因车体坐标系以角速度${\mathbf{\omega }}_{wb}^b$旋转而产生的加速度，该旋转亦可看作${\mathbf{v}}_{wb}^b$以$(-{\mathbf{\omega }}_{wb}^b)$的角速度绕车体坐标系旋转，故车体坐标系下总加速度为
$${\dot{\mathbf{v}}}_{wb}^b={\mathbf{a}}_{wb}^b-{\mathbf{\omega }}_{wb}^b\times {\mathbf{v}}_{wb}^b\text{\hspace{1em}(2)}$$
故$(1)$式可以写为
$$\begin{array}{rl}{\dot{\mathbf{v}}}_{wb}^w& ={\mathbf{R}}_{wb}({\mathbf{\omega }}_{wb}^b{)}^{\wedge }{\mathbf{v}}_{wb}^b+{\mathbf{R}}_{wb}({\mathbf{a}}_{wb}^b-{\mathbf{\omega }}_{wb}^b\times {\mathbf{v}}_{wb}^b)\\ & ={\mathbf{R}}_{wb}{\mathbf{a}}_{wb}^b\end{array}\text{\hspace{1em}(3)}$$
记${\mathbf{a}}_{wb}^w={\dot{\mathbf{v}}}_{wb}^w$，则$(3)$可写为
$${\mathbf{a}}_{wb}^w={\mathbf{R}}_{wb}{\mathbf{a}}_{wb}^b$$

3、IMU测量模型

IMU根据陀螺仪(gyroscope)测量传感器自身的角速度${\tilde{\mathbf{\omega }}}_{ws}^s$（波浪线表示测量值），根据加速度计(accelerator)测量传感器身的加速度${\tilde{\mathbf{a}}}_{ws}^s$。设传感器加速度为${\mathbf{a}}_{ws}^s$，传感器坐标系相对车体坐标系的位姿为已通过校准得到的定值${\mathbf{R}}_{bs},{\mathbf{p}}_{bs}^b$，车体坐标系相对世界坐标系的位姿为${\mathbf{R}}_{wb},{\mathbf{p}}_{wb}^w$。重力加速度$\mathbf{g}=(0,0,-9.8)$。

在世界坐标系下，传感器坐标系原点的位置为
$${\mathbf{p}}_{ws}^w={\mathbf{P}}_{wb}^w+{\mathbf{R}}_{wb}{\mathbf{p}}_{bs}^b$$
对上式对时间求导得到
$$\begin{array}{rl}{\dot{\mathbf{p}}}_{ws}^w& ={\dot{\mathbf{p}}}_{wb}^w+{\dot{\mathbf{R}}}_{wb}{\mathbf{p}}_{bs}^b+{\mathbf{R}}_{wb}{\dot{\mathbf{p}}}_{bs}^b\\ & ={\dot{\mathbf{p}}}_{wb}^w+{\mathbf{R}}_{wb}({\mathbf{\omega }}_{wb}^b{)}^{\wedge }{\mathbf{p}}_{bs}^b+{\mathbf{R}}_{wb}{\dot{\mathbf{p}}}_{bs}^b\end{array}$$
注意到${\dot{\mathbf{p}}}_{bs}^b=0$，故有
$${\dot{\mathbf{p}}}_{ws}^w={\dot{\mathbf{p}}}_{wb}^w+{\mathbf{R}}_{wb}({\mathbf{\omega }}_{wb}^b\times {\mathbf{p}}_{bs}^b)$$
再对时间求导得到
$${\ddot{\mathbf{p}}}_{ws}^w={\ddot{\mathbf{p}}}_{wb}^w+{\dot{\mathbf{R}}}_{wb}({\mathbf{\omega }}_{wb}^b\times {\mathbf{p}}_{bs}^b)+{\mathbf{R}}_{wb}\dot{({\mathbf{\omega }}_{wb}^b\times {\mathbf{p}}_{bs}^b)}$$
由于$\dot{(\mathbf{a}\times \mathbf{b})}=\dot{\mathbf{a}}\times \mathbf{b}+\mathbf{a}\times \dot{\mathbf{b}}$，故
$$\begin{array}{rl}{\ddot{\mathbf{p}}}_{ws}^w& ={\ddot{\mathbf{p}}}_{wb}^w+{\mathbf{R}}_{wb}({\mathbf{\omega }}_{wb}^b{)}^{\wedge }({\mathbf{\omega }}_{wb}^b\times {\mathbf{p}}_{bs}^b)+{\mathbf{R}}_{wb}({\dot{\mathbf{\omega }}}_{wb}^b\times {\mathbf{p}}_{bs}^b)\\ & ={\ddot{\mathbf{p}}}_{wb}^w+{\mathbf{R}}_{wb}({\mathbf{\omega }}_{wb}^b\times {\mathbf{\omega }}_{wb}^b\times {\mathbf{p}}_{bs}^b+{\dot{\mathbf{\omega }}}_{wb}^b\times {\mathbf{p}}_{bs}^b)\end{array}\text{\hspace{1em}(4)}$$
画出IMU测量加速度的原理图

由矢量分析可得
$${\tilde{\mathbf{a}}}_{ws}^s={\mathbf{a}}_{ws}^s-{\mathbf{R}}_{sw}\mathbf{g}$$
由不同坐标系间加速度的转换关系得到
$${\mathbf{a}}_{ws}^s={\mathbf{R}}_{sw}{\ddot{\mathbf{p}}}_{ws}^w$$
故有
$${\tilde{\mathbf{a}}}_{ws}^s={\mathbf{R}}_{sw}({\ddot{\mathbf{p}}}_{ws}^w-\mathbf{g})\text{\hspace{1em}(5)}$$
将$(4)$代入$(5)$得到
$${\tilde{\mathbf{a}}}_{ws}^s={\mathbf{R}}_{sb}\left({\mathbf{R}}_{bw}({\ddot{\mathbf{p}}}_{wb}^w-\mathbf{g})+{\mathbf{\omega }}_{wb}^b\times {\mathbf{\omega }}_{wb}^b\times {\mathbf{p}}_{bs}^b+{\dot{\mathbf{\omega }}}_{wb}^b\times {\mathbf{p}}_{bs}^b\right)$$
画出陀螺仪的测量原理图

测得的角速度与车体坐标系下的角速度的关系是
$${\tilde{\mathbf{\omega }}}_{ws}^s={\mathbf{R}}_{sb}{\mathbf{\omega }}_{wb}^b$$
综上所述，将加速度计和陀螺仪模型写成矩阵形式，就得到IMU的测量模型
$$\begin{array}{rl}\left[\begin{array}{c}{\tilde{\mathbf{a}}}_{ws}^s\\ {\tilde{\mathbf{\omega }}}_{ws}^s\end{array}\right]& =\left[\begin{array}{c}{\mathbf{R}}_{sb}\left({\mathbf{R}}_{bw}({\ddot{\mathbf{p}}}_{wb}^w-\mathbf{g})+{\mathbf{\omega }}_{wb}^b\times {\mathbf{\omega }}_{wb}^b\times {\mathbf{p}}_{bs}^b+{\dot{\mathbf{\omega }}}_{wb}^b\times {\mathbf{p}}_{bs}^b\right)\\ {\mathbf{R}}_{sb}{\mathbf{\omega }}_{wb}^b\end{array}\right]\end{array}$$
故若IMU坐标系与车体坐标系保持一致，即${\mathbf{R}}_{sb}=\mathbf{I},{\mathbf{p}}_{bs}=0$，又因为车体在世界坐标系下的加速度${\mathbf{a}}_{wb}^w={\ddot{\mathbf{p}}}_{wb}^w$，故此时上式可近似写为
$$\begin{array}{rl}\left[\begin{array}{c}{\tilde{\mathbf{a}}}_{ws}^s\\ {\tilde{\mathbf{\omega }}}_{ws}^s\end{array}\right]& =\left[\begin{array}{c}{\mathbf{R}}_{bw}({\mathbf{a}}_{wb}^w-\mathbf{g})\\ {\mathbf{\omega }}_{wb}^b\end{array}\right]\end{array}\text{\hspace{1em}(6)}$$

4、IMU噪声模型

IMU的噪声由两部分组成：测量噪声和零偏，记加速度计和陀螺仪的测量噪声分别为${\mathbf{\eta }}_a,{\mathbf{\eta }}_g$，同时记二者零偏为${\mathbf{b}}_a,{\mathbf{b}}_g$

省去测量方程$(6)$中与坐标系相关的下标，该方程可记为
$$\begin{array}{rl}\tilde{\mathbf{a}}& =\mathbf{R}(\mathbf{a}-\mathbf{g})+{\mathbf{b}}_a+{\mathbf{\eta }}_a\\ \tilde{\mathbf{\omega }}& =\mathbf{\omega }+{\mathbf{b}}_g+{\mathbf{\eta }}_g\end{array}\text{\hspace{1em}(7a)(7b)}$$
在连续时间下，可以认为IMU的测量噪声是一个零均值白噪声高斯过程，即各时刻的噪声随机变量均值为$0$，且彼此不相关（等价于相互独立）。
$$\begin{array}{rl}{\mathbf{\eta }}_a& \sim \mathcal{G}\mathcal{P}(0,\mathrm{Cov}({\mathbf{\eta }}_a)\delta (t-t^{\prime }))\\ {\mathbf{\eta }}_g& \sim \mathcal{G}\mathcal{P}(0,\mathrm{Cov}({\mathbf{\eta }}_g)\delta (t-t^{\prime }))\end{array}$$
而零偏是一个导数为零均值白噪声高斯过程的随机过程，也称为维纳过程，布朗运动或随机游走
$$\begin{array}{rl}{\dot{\mathbf{b}}}_a={\mathbf{\eta }}_{{\mathbf{b}}_a}& \sim \mathcal{G}\mathcal{P}(0,\mathrm{Cov}({\mathbf{b}}_a)\delta (t-t^{\prime }))\\ {\dot{\mathbf{b}}}_g={\mathbf{\eta }}_{{\mathbf{b}}_g}& \sim \mathcal{G}\mathcal{P}(0,\mathrm{Cov}({\mathbf{b}}_g)\delta (t-t^{\prime }))\end{array}$$
其中$\delta$为狄拉克函数，其满足
$$\delta (x)=0(x\ne 0),{\int }_{-\infty }^{+\infty }\delta (x)dx=1$$
引入狄拉克函数，一方面可以轻松地从连续时间高斯过程推导离散时间采样之后的测量噪声和零偏的差分，最为重要的是，高斯白噪声的积分是维纳过程，或者可以方便地理解为高斯白噪声是维纳过程的导数。

因为IMU测量到的数据是离散的，故需要对测量噪声和零偏进行离散化处理，其应用见$(11)$

先讨论测量噪声的离散化，记
$${\mathbf{\eta }}_{ad,k}=\frac{1}{\Delta t}{\int }_{t_k}^{t_k+\Delta t}{\mathbf{\eta }}_a(t)dt,{\mathbf{\eta }}_{gd,k}=\frac{1}{\Delta t}{\int }_{t_k}^{t_k+\Delta t}{\mathbf{\eta }}_g(t)dt$$
其中下标$d$表示离散，将相互独立的正态分布和仍为正态分布的思想拓展到积分，可知${\mathbf{\eta }}_{ad,k},{\mathbf{\eta }}_{gd,k}$仍服从正态分布，易知二者均值$\mathbf{\mu }({\mathbf{\eta }}_{ad,k})=0,\mathbf{\mu }({\mathbf{\eta }}_{gd,k})=0$，下面以${\mathbf{\eta }}_{ad}(k)$为例求二者协方差矩阵
$$\begin{array}{rl}\mathbf{\Sigma }({\mathbf{\eta }}_{ad,k})& =E[({\mathbf{\eta }}_{ad,k}-\mathbf{\mu }({\mathbf{\eta }}_{ad,k}))({\mathbf{\eta }}_{ad,k}-\mathbf{\mu }({\mathbf{\eta }}_{ad,k}){)}^T]\\ & =E[{\mathbf{\eta }}_{ad,k}{\mathbf{\eta }}_{ad,k}^T]\\ & =E\left[\frac{1}{(\Delta t{)}^2}\left({\int }_{t_k}^{t_k+\Delta t}{\mathbf{\eta }}_a(t)dt\right){\left({\int }_{t_k}^{t_k+\Delta t}{\mathbf{\eta }}_a(\tau )d\tau \right)}^T\right]\\ & =E\left[\frac{1}{(\Delta t{)}^2}{\int }_{t_k}^{t_k+\Delta t}d\tau {\int }_{t_k}^{t_k+\Delta t}{\mathbf{\eta }}_a(t){\mathbf{\eta }}_a(\tau {)}^{T}dt\right]\\ & =\frac{1}{(\Delta t{)}^2}{\int }_{t_k}^{t_k+\Delta t}d\tau {\int }_{t_k}^{t_k+\Delta t}E[{\mathbf{\eta }}_a(t){\mathbf{\eta }}_a(\tau {)}^T]dt\end{array}\text{\hspace{1em}(8)}$$
其中${\mathbf{\eta }}_a(t)$表示随机过程${\mathbf{\eta }}_a$在$[t_k,t_k+\Delta t]$上的一个样本函数，上式中被积分项表示该过程在$t$和$\tau$时刻之间的协方差矩阵，即
$$E[{\mathbf{\eta }}_a(t){\mathbf{\eta }}_a^T(\tau )]=\mathrm{Cov}({\mathbf{\eta }}_a)\delta (t-\tau )\text{\hspace{1em}(9)}$$
将$(9)$代入$(8)$可得
$$\begin{array}{rl}\mathbf{\Sigma }({\mathbf{\eta }}_{ad,k})& =\frac{1}{(\Delta t{)}^2}{\int }_{t_k}^{t_k+\Delta t}d\tau {\int }_{t_k}^{t_k+\Delta t}\delta (t-\tau )dt\\ & =\frac{\mathrm{Cov}({\mathbf{\eta }}_{\mathbf{a}})}{\Delta t}\end{array}$$
故测量噪声服从分布
$$\begin{gathered} {\mathbf{\eta }}_{ad,k}\sim \mathcal{N}\left(0,\frac{\mathrm{Cov}({\mathbf{\eta }}_{\mathbf{a}})}{\Delta t}\right) \\ {\mathbf{\eta }}_{gd,k}\sim \mathcal{N}\left(0,\frac{\mathrm{Cov}({\mathbf{\eta }}_{\mathbf{g}})}{\Delta t}\right) \end{gathered}$$
再讨论零偏的离散化，记
$${\mathbf{b}}_{ad,k+1}={\mathbf{b}}_{ad,k}+{\int }_{t_k}^{t_k+\Delta t}{\mathbf{\eta }}_{{\mathbf{b}}_a}(t)dt,{\mathbf{b}}_{gd,k+1}={\mathbf{b}}_{gd,k}+{\int }_{t_k}^{t_k+\Delta t}{\mathbf{\eta }}_{{\mathbf{b}}_g}(t)dt$$
由上面的分析易知，${\mathbf{b}}_{ad,k+1}-{\mathbf{b}}_{a,k}$服从均值为$0$的正态分布，其协方差为
$$\mathbf{\Sigma }({\mathbf{b}}_{ad,k+1}-{\mathbf{b}}_{ad,k})=\Delta t\mathrm{Cov}({\mathbf{b}}_a)$$
故零偏的差分服从分布
$$\begin{gathered} {\mathbf{b}}_{ad,k+1}-{\mathbf{b}}_{ad,k}\sim \mathcal{N}(0,\Delta t\mathrm{Cov}({\mathbf{b}}_a)) \\ {\mathbf{b}}_{gd,k+1}-{\mathbf{b}}_{gd,k}\sim \mathcal{N}(0,\Delta t\mathrm{Cov}({\mathbf{b}}_g)) \end{gathered}$$

估算具体时刻零偏的分布较为复杂，暂时不讨论。

注意此处$\mathrm{Cov}$称为能量谱密度矩阵，$\mathbf{\Sigma }$才真正表示协方差矩阵。通常情况下，$\mathrm{Cov}$为对角矩阵，且对角线上元素一致，也即每个$\mathrm{Cov}$和$\mathbf{\Sigma }$可以用单个值的平方表示。设每个能量谱密度矩阵对应的值为$\sigma ({\mathbf{\eta }}_a),\sigma ({\mathbf{\eta }}_g),\sigma ({\mathbf{b}}_a),\sigma ({\mathbf{b}}_g)$，每个离散时间协方差矩阵的对应的值（即标准差）为${\sigma }_k({\mathbf{\eta }}_a),{\sigma }_k({\mathbf{\eta }}_g),{\sigma }_k({\mathbf{b}}_a),{\sigma }_k({\mathbf{b}}_g)$，这些值之间的关系为
$$\begin{array}{llc}& {\sigma }_k({\mathbf{\eta }}_a)=\frac{1}{\sqrt{\Delta t}}\sigma ({\mathbf{\eta }}_a)& {\sigma }_k({\mathbf{\eta }}_g)=\frac{1}{\sqrt{\Delta t}}\sigma ({\mathbf{\eta }}_g)\\ & {\sigma }_k({\mathbf{b}}_a)=\sqrt{\Delta t}\sigma ({\mathbf{b}}_a)& {\sigma }_k({\mathbf{b}}_g)=\sqrt{\Delta t}\sigma ({\mathbf{b}}_g)\end{array}$$
由于离散时间的测量噪声和零偏是直接加在被测物理量上的，故其单位应与被测物理量保持一致，故
$${\sigma }_k({\mathbf{\eta }}_a)\leftarrow \frac{\mathrm{m}}{{\mathrm{s}}^2},{\sigma }_k({\mathbf{\eta }}_g)\leftarrow \frac{\mathrm{rad}}{\mathrm{s}},{\sigma }_k({\mathbf{b}}_a)\leftarrow \frac{\mathrm{m}}{{\mathrm{s}}^2},{\sigma }_k({\mathbf{b}}_g)\leftarrow \frac{\mathrm{rad}}{\mathrm{s}}$$
等价地，能量谱密度矩阵对应的值的单位是
$$\sigma ({\mathbf{\eta }}_a)\leftarrow \frac{\mathrm{m}}{\mathrm{s}\sqrt{\mathrm{s}}},\sigma ({\mathbf{\eta }}_g)\leftarrow \frac{\mathrm{rad}}{\sqrt{\mathrm{s}}},\sigma ({\mathbf{b}}_a)\leftarrow \frac{\mathrm{m}}{{\mathrm{s}}^2\sqrt{\mathrm{s}}},\sigma ({\mathbf{b}}_g)\leftarrow \frac{\mathrm{rad}}{\mathrm{s}\sqrt{\mathrm{s}}}$$

5、使用IMU数据进行轨迹推算

通过IMU数据递推可得到一个简单的INS系统（Inertial Navigation System，惯性导航系统）

省略坐标系相关的下标，IMU的运动模型为
$$\begin{array}{rl}\dot{\mathbf{R}}& =\mathbf{R}{\mathbf{\omega }}^{\wedge }\\ \dot{\mathbf{v}}& =\mathbf{a}\\ \dot{\mathbf{p}}& =\mathbf{v}\end{array}$$

上式中的值均为真实值。现研究在短时间段$[t,t+\Delta t]$内的位姿和速度变化，对于旋转矩阵，其精确表达式较为复杂，但由于短时间段内角速度变化量很小，故可以给出$(9\mathrm{a})$中的近似；对于速度和加速度，可以不加近似地得到其的精确表达式$(10\mathrm{b}),(10\mathrm{c})$
$$\begin{array}{rl}\mathbf{R}(t+\Delta t)& \approx \mathbf{R}(t)\mathrm{Exp}\left({\int }_t^{t+\Delta t}\mathbf{\omega }(\tau )d\tau \right)\\ \mathbf{v}(t+\Delta t)& =\mathbf{v}(t)+{\int }_t^{t+\Delta t}\mathbf{a}(\tau )d\tau \\ \mathbf{p}(t+\Delta t)& =\mathbf{p}(t)+{\int }_t^{t+\Delta t}\mathbf{v}(\tau )d\tau =\mathbf{p}(t)+\mathbf{v}(t)\Delta t+{\int }_t^{t+\Delta t}d{t}^{\prime }{\int }_t^{t^{\prime }}\mathbf{a}(\tau )d\tau \end{array}\text{\hspace{1em}(10a)(10b)(10c)}$$
将IMU测量方程变形，得到
$$\begin{array}{rl}\mathbf{a}& =\mathbf{R}(\tilde{\mathbf{a}}-{\mathbf{b}}_a-{\mathbf{\eta }}_a)+\mathbf{g}\\ \mathbf{\omega }& =\tilde{\mathbf{\omega }}-{\mathbf{b}}_g-{\mathbf{\eta }}_g\end{array}\text{\hspace{1em}(11a)(11b)}$$
上式表示离散时刻的测量值和真实值的关系。设时间$t$对应采样次数$k$，时间$t+\Delta t$对应采样次数$k+1$，将$(10)$代入$(9)$，应用欧拉方法计算角速度和加速度的积分，即认为${\int }_a^{b}f(x)dx=f(a)(b-a)$，并把采样次数写到下标位置，得到
$$\begin{array}{rl}{\mathbf{R}}_{k+1}& ={\mathbf{R}}_k\mathrm{Exp}(({\tilde{\mathbf{\omega }}}_k-{\mathbf{b}}_{g,k}-{\mathbf{\eta }}_{gd,k})\Delta t)\\ {\mathbf{v}}_{k+1}& ={\mathbf{v}}_k+\mathbf{g}\Delta t+{\mathbf{R}}_k({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_{a,k}-{\mathbf{\eta }}_{ad,k})\Delta t\\ {\mathbf{p}}_{k+1}& ={\mathbf{p}}_k+{\mathbf{v}}_k\Delta t+\frac{1}{2}\mathbf{g}\Delta t^2+\frac{1}{2}{\mathbf{R}}_k({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_{a,k}-{\mathbf{\eta }}_{ad,k})\Delta t^2\end{array}\text{\hspace{1em}(12a)(12b)(12c)}$$

该过程称为一次积分。

附录

§1

任意${\mathbf{\varphi }}^{\wedge }\in {\mathbb{R}}^3$，$\mathbf{R}\in SO(3)$满足
$$\mathbf{R}{\mathbf{\varphi }}^{\wedge }{\mathbf{R}}^T=(\mathbf{R}\mathbf{\varphi }{)}^{\wedge }\text{\hspace{1em}(A1)}$$