## 从零开始的 IMU 状态模型推导

2016-11-20

### 0. 总览

IMU 是移动机器人、移动智能设备上常见的传感器。常见的 IMU 为六轴传感器，配备输出三轴加速度的加速度计和输出三轴角速度的陀螺仪。九轴 IMU 还会配备输出三轴姿态角的磁力计。我们这里只讨论六轴 IMU。

IMU 的状态量通常表示为：

${\bf X}_{IMU} = [ ^I_G \bar{q}^T \quad {\bf b}_g^T \quad ^G{\bf v}_I^T \quad {\bf b}_a^T \quad ^G{\bf p}_I^T] \tag{0.0}$

$\dot{\bf x} = f({\bf x}) \tag{0.1}$ ${\bf z} = g({\bf x}) + {\bf n} \tag{0.2}$ $\delta {\bf x} = e(\hat{\bf x},{\bf x}) \tag{0.3}$

### 1. IMU 运动模型

#### 1-1. 前置(1)： 旋转量求导

$\dot{\bf p}={\bf v}, \dot{\bf v} = {\bf a}$

$\frac{d{\bf r}}{dt} = {\boldsymbol \omega} \times {\bf r}$

$\frac{d{\bf i}_B}{dt} = {\boldsymbol \omega} \times {\bf i}_B, \frac{d{\bf j}_B}{dt} = {\boldsymbol \omega} \times {\bf j}_B, \frac{d{\bf k}_B}{dt} = {\boldsymbol \omega} \times {\bf k}_B$

$\dot{\bf R} = [ {\boldsymbol \omega} \times {\bf i}_B \quad {\boldsymbol \omega} \times {\bf j}_B \quad {\boldsymbol \omega} \times {\bf k}_B ] = {\boldsymbol \omega} \times {\bf R} \tag{1.0}$

$\dot{\bf R} = {\boldsymbol \omega}^{\land} {\bf R} \tag{1.1}$

$\quad {\boldsymbol \omega}^{\land}= \begin{bmatrix}0 & -\omega_3 & \omega_2\\ \omega_3 & 0 & -\omega_1 \\ -\omega_2 & \omega_1 & 0\end{bmatrix}$

$\dot{\bf R} = ({\bf R}{}^B{\boldsymbol \omega})^{\land} {\bf R} \tag{1.2}$

$\dot{\bf R} = {\bf R}({}^B{\boldsymbol \omega})^{\land} \tag{1.3}$

#### 1-2. 前置(2)： 四元数

${\bf q} = q_1 i + q_2 j + q_3 k + q_4 = [{\boldsymbol v}^T \quad q_4]^T$

${\bf q} = [{\bf u}\sin\frac{\theta}{2} \quad \cos\frac{\theta}{2}] \tag{1.4}$

\begin{aligned} {\bf q}\otimes {\bf p} = & (q_1 i + q_2 j + q_3 k + q_4)(p_1 i+p_2j+p_3k+p_4) \\ = & (q_1p_4+q_2p_3-q_3p_2+q_4p_1)i+(-q_1p_3+q_2p_4+q_3p_1+q_4p_2)j+\\ &(q_1p_2-q_2p_1+q_3p_4+q_4p_3)k + (-q_1p_1-q_2p_2-q_3p_3+q_4p_4) \end{aligned}

\begin{aligned} {\bf q}\otimes {\bf p} & = \begin{bmatrix} q_4{\bf I}_3+{\boldsymbol v}_q^{\land} & {\boldsymbol v}_q \\ -{\boldsymbol v}_q^T & q_4 \end{bmatrix} \begin{bmatrix} {\boldsymbol v}_p \\ p_4 \end{bmatrix} \\ & = \begin{bmatrix} p_4{\bf I}_3-{\boldsymbol v}_p^{\land} & {\boldsymbol v}_p \\ -{\boldsymbol v}_p^T & p_4 \end{bmatrix} \begin{bmatrix} {\boldsymbol v}_q \\ q_4 \end{bmatrix} \end{aligned}

${\bf R}({\bf q}) = (2q_4^2-1){\bf I}_3 +2q_4{\boldsymbol v}^{\land} + 2{\boldsymbol{vv}}^T \tag{1.5}$

\begin{aligned} \dot{\bf q} &= \frac{1}{2} \begin{bmatrix} {\boldsymbol \omega}^{\land} & {\boldsymbol\omega} \\ -{\boldsymbol \omega}^T & 0 \end{bmatrix} {\bf q} := \frac{1}{2}{\boldsymbol \Omega}({\boldsymbol \omega}){\bf q} \tag{1.6} \\ &= \frac{1}{2} \begin{bmatrix} q_4{\bf I}_3-{\boldsymbol v}^{\land} \\ -{\boldsymbol v}^T \end{bmatrix} {\boldsymbol \omega} \end{aligned}

#### 1-3. IMU 运动模型

\begin{aligned} {}^I_G \dot{\bar{q}} &= \frac{1}{2}{\boldsymbol \Omega}({\boldsymbol \omega}){}^I_G \bar{q}\\ \dot{\bf b}_g &= {\bf n}_{wg}\\ {}^G\dot{\bf v}_I &= {}^G{\bf a} \tag{1.7}\\ \dot{\bf b}_a &= {\bf n}_{wa} \\ {}^G\dot{\bf p}_I &= {}^G{\bf v}_I \end{aligned}

${}^I_G \dot{\bar{q}}$ 由 $(1.6)$ 直接得到。注意这里角速度 $\boldsymbol \omega$ 是在体坐标系 {I} 下表达的，与 $(1.1)$ 处相反。原因是 ${}^I_G \bar{q}$ 表示的旋转方向与 $(1.1)$ 处的 $\bf R$ 是相反的。其他的四项，速度和加速度都很简单，bias 两项在下面观测模型部分讲。

### 2. IMU 观测和噪声模型

#### 2-1 前置(1)： 科氏加速度

${\bf v} = \dot{\bf r} = \dot{\bf R} {}^B{\bf r} + {\bf R}^B\dot{\bf r} = {\boldsymbol \omega}^{\land}{\bf R}{}^B{\bf r}+ {\bf R}^B\dot{\bf r}$

\begin{aligned} {\bf v} & = {\boldsymbol \omega}^{\land}{\bf r} + {\bf R}^B{\bf v} \\ & = {\boldsymbol \omega}\times{\bf r}+ {\bf v}_r \tag{2.0} \end{aligned}

\begin{aligned} {\bf a} = \dot{\bf v} & = \dot{\boldsymbol \omega}\times {\bf r} + {\boldsymbol \omega} \times \dot{\bf r}+ \dot{\bf R} {}^B{\bf v} + {\bf R}{}^B\dot{\bf v} \\ & = {\boldsymbol \alpha}\times {\bf r}+{\boldsymbol \omega}\times（{\boldsymbol \omega}\times{\bf r}+ {\bf v}_r）+ {\boldsymbol \omega}\times{\bf R}{}^B{\bf v}+{\bf R}{}^B{\bf a} \\ & = {\boldsymbol \alpha}\times {\bf r}+{\boldsymbol \omega}\times({\boldsymbol \omega}\times{\bf r})+2{\boldsymbol \omega}\times{\bf v}_r+{\bf a}_r \tag{2.1} \end{aligned}

#### 2-2 前置(2)： 惯性导航相关坐标系定义

Earth-Centered-Earth-Fixed (ECEF) Frame：地心地固坐标系 ECEF。以地心为坐标原点，向北为 z 轴，x-y 平面为赤道平面，x 轴指向经纬度 (0,0) 点。ECI 固连在地球上，跟随地球自转，非惯性坐标系。MSCKF 一代 [1] 使用 ECEF 为参考坐标系 {G}。

Earth-Centered-Inertial (ECI) Frame：地心惯性坐标系 ECI。以地心为坐标原点，向北为 z 轴，x-y 平面为赤道平面，x 轴指向春分点（vernal equinox point，即每年春分时日心-地心连线与赤道的交点）。ECI 不跟随地球自转，在惯性导航中视为惯性坐标系。MSCKF 二代 [3] 使用 ECI 为参考坐标系 {G}。

Body Frame：体坐标系。原点在导航体的质心，固连在导航体上，用来表示导航体的姿态。在本文前置推导部分为 {B}，在 MSCKF 中为 {I}。

#### 2-3 前置(3)： 高斯白噪声与随机游走

$E[n(t)]=0 \\ E[n(t_1)n(t_2)] = \sigma_g^2 \delta(t_1-t_2)$

$n_d[k]=\sigma_{gd} w[k]$

$w[k] \sim \mathcal{N}(0,1) \\ \sigma_{gd}=\frac{\sigma_g}{\sqrt{\Delta t}}$

$n_d[k] \triangleq n(t_0+\Delta t)\simeq\frac{1}{\Delta t}\int_{t_0}^{t_0+\Delta t}n(\tau)dt$ \begin{aligned} E(n_d[k]^2) &= E(\frac{1}{\Delta t^2}\int_{t_0}^{t_0+\Delta t}\int_{t_0}^{t_0+\Delta t}n(\tau)n(t)d\tau dt) \\ &= E( \frac{\sigma_g^2}{\Delta t^2}\int_{t_0}^{t_0+\Delta t}\int_{t_0}^{t_0+\Delta t}\delta(t-\tau)d \tau dt)\\ &= E(\frac{\sigma_g^2}{\Delta t}) \end{aligned}

$\dot{b}_g(t)=n(t)=\sigma_{bg}w(t)$

$b_d[k] = b_d[k-1]+\sigma_{bgd}w[k]$

$w[k] \sim \mathcal{N}(0,1) \\ \sigma_{gd}=\sigma_{bg}\sqrt{\Delta t}$

$b_d[k] \triangleq b(t_0) + \int_{t_0}^{t_0+\Delta t}n(t)dt$ \begin{aligned} E((b_d[k]-b_d[k-1])^2) &=E(\int_{t_0}^{t_0+\Delta t}\int_{t_0}^{t_0+\Delta t}n(t)n(\tau)d \tau dt)\\ &= E({\sigma_{bg}^2}\int_{t_0}^{t_0+\Delta t}\int_{t_0}^{t_0+\Delta t}\delta(t-\tau)d \tau dt)\\ &= E(\sigma_{bg}^2\Delta t) \end{aligned}

#### 2-4. IMU 观测模型

${\boldsymbol \omega}_m = {\boldsymbol \omega}+{\bf R}({}^I_G \bar{q}){\boldsymbol \omega}_G+{\bf b}_g+{\bf n}_g \tag{2.2}$ ${\bf a}_m = {\bf R}({}^I_G \bar{q})({}^G{\bf a} -{}^G{\bf g} +2{\boldsymbol \omega}_G^{\land}{}^G{\bf v}_I+({\boldsymbol \omega}_G^{\land})^2{}^G{\bf p}_I)+{\bf b}_a+{\bf n}_a \tag{2.3}$

${\bf R}^T({}^I_G \bar{q})({\bf a}_m-{\bf b}_a-{\bf n}_a)=({\boldsymbol \omega}_G^{\land})^2{}^G{\bf p}_I+2{\boldsymbol \omega}_G^{\land}{}^G{\bf v}_I+{}^G{\bf a}-{}^G{\bf g}$

${\bf R}_G{\bf R}^T({}^I_G \bar{q})({\bf a}_m-{\bf b}_a-{\bf n}_a)={\boldsymbol \omega}_G\times({\boldsymbol \omega}_G\times{\bf R}_G{}^G{\bf p}_I) + 2{\boldsymbol \omega}_G\times({\bf R}_G{}^G{\bf v}_I)+{\bf R}_G({}^G{\bf a}-{}^G{\bf g})$

${\boldsymbol \omega}_m = {\boldsymbol \omega}+{\bf b}_g+{\bf n}_g \tag{2.4}$ ${\bf a}_m = {\bf R}({}^I_G \bar{q})({}^G{\bf a} -{}^G{\bf g} )+{\bf b}_a+{\bf n}_a \tag{2.5}$

### 3. IMU 状态估计误差模型

#### 3-1. 前置：四元数误差小量

\begin{aligned} \delta {\bf q} &= \begin{bmatrix} {\bf u}\sin{\frac{\delta \theta}{2}} \\ \cos{\frac{\delta \theta}{2}} \end{bmatrix}\\ &\simeq \begin{bmatrix} {\bf u}\frac{\delta \theta}{2} \\ 1 \end{bmatrix} \triangleq \begin{bmatrix} \frac{\boldsymbol{\delta \theta}}{2} \\ 1 \end{bmatrix} \end{aligned}

${\bf q} = \delta {\bf q} \otimes\hat{\bf q}$

#### 3-2. IMU 状态估计误差模型

$\tilde{\bf X}_{IMU} = [ \boldsymbol{\delta \theta}_I^T \quad \tilde{\bf b}_g^T \quad ^G\tilde{\bf v}_I^T \quad \tilde{\bf b}_a^T \quad ^G\tilde{\bf p}_I^T] \tag{3.0}$

### 参考文献

[1] Mourikis, Anastasios I., and Stergios I. Roumeliotis. “A multi-state constraint Kalman filter for vision-aided inertial navigation.” Proceedings 2007 IEEE International Conference on Robotics and Automation. IEEE, 2007.

[2] Trawny, Nikolas, and Stergios I. Roumeliotis. “Indirect Kalman filter for 3D attitude estimation.” University of Minnesota, Dept. of Comp. Sci. & Eng., Tech. Rep 2 (2005): 2005.

[3] Li, Mingyang. “Visual-inertial odometry on resource-constrained systems.” (2014).

[4] IMU noise model https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model

[5] Crassidis, John L., and John L. Junkins. Optimal estimation of dynamic systems. CRC press, 2011.