目录
提示：请使用 Firefox，Chrome，Edge 等较新的浏览器阅读，以获得完整的公式排版。IE 请使用 IE 11。
本文已授权「泡泡机器人 SLAM」微信公众号（paopaorobot_slam）发表。
0. 总览
IMU 是移动机器人、移动智能设备上常见的传感器。常见的 IMU 为六轴传感器，配备输出三轴加速度的加速度计和输出三轴角速度的陀螺仪。九轴 IMU 还会配备输出三轴姿态角的磁力计。我们这里只讨论六轴 IMU。
IMU 的状态量通常表示为：
X I M U = [ G I q ˉ T b g T G v I T b a T G p I T ] (0.0) {\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} X I M U = [ G I q ˉ T b g T G v I T b a T G p I T ] ( 0 . 0 ) 这里我们使用和 MSCKF [1] 一样的 notation。用 {I} 表示 IMU 坐标系，{G} 表示参考坐标系。IMU 的姿态由旋转量 G I q ˉ ^I_G \bar{q} G I q ˉ G p I ^G{\bf p}_I G p I G v I ^G{\bf v}_I G v I b g {\bf b}_g b g b a {\bf b}_a b a
对于 IMU 状态估计问题，需要提供运动模型、观测（噪声）模型、估计误差模型：
x ˙ = f ( x ) (0.1) \dot{\bf x} = f({\bf x}) \tag{0.1} x ˙ = f ( x ) ( 0 . 1 ) z = g ( x ) + n (0.2) {\bf z} = g({\bf x}) + {\bf n} \tag{0.2} z = g ( x ) + n ( 0 . 2 ) δ x = e ( x ^ , x ) (0.3) \delta {\bf x} = e(\hat{\bf x},{\bf x}) \tag{0.3} δ x = e ( x ^ , x ) ( 0 . 3 ) 这是一个通用模型，我们用 x \bf x x z {\bf z} z n {\bf n} n x ^ \hat{\bf x} x ^ x : = X I M U {\bf x} := {\bf X}_{IMU} x : = X I M U
1. IMU 运动模型
1-1. 前置(1)： 旋转量求导
这部分讲刚体动力学相关的前置知识，熟悉的读者可以跳过。
众所周知，一个刚体在同一个惯性坐标系下进行平移运动，其平移量对时间的一阶导和二阶导即速度和加速度：
p ˙ = v , v ˙ = a \dot{\bf p}={\bf v}, \dot{\bf v} = {\bf a} p ˙ = v , v ˙ = a 对于旋转量以及非惯性系参考坐标系，情况稍微复杂些。
首先，如下图（左）所示，考虑一个从原点出发的向量 r \bf r r u \bf u u θ ˙ \dot{\theta} θ ˙
角速度矢量可以表示为 ω = θ ˙ u {\boldsymbol \omega}=\dot{\theta}\bf u ω = θ ˙ u r \bf r r r \bf r r
d r d t = ω × r \frac{d{\bf r}}{dt} = {\boldsymbol \omega} \times {\bf r} d t d r = ω × r 现在考虑上图（右），坐标系 {B} 绕单位轴 u \bf u u
d i B d t = ω × i B , d j B d t = ω × j B , d k B d t = ω × k B \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 d t d i B = ω × i B , d t d j B = ω × j B , d t d k B = ω × k B 我们知道，[ i B j B k B ] [ {\bf i}_B \quad {\bf j}_B \quad {\bf k}_B ] [ i B j B k B ] R \bf R R R \bf R R
R ˙ = [ ω × i B ω × j B ω × k B ] = ω × R (1.0) \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} R ˙ = [ ω × i B ω × j B ω × k B ] = ω × R ( 1 . 0 ) 我们知道上面的叉乘运算可以转化为负对称矩阵的乘法：
R ˙ = ω ∧ R (1.1) \dot{\bf R} = {\boldsymbol \omega}^{\land} {\bf R} \tag{1.1} R ˙ = ω ∧ R ( 1 . 1 ) 其中负对称矩阵为
ω ∧ = [ 0 − ω 3 ω 2 ω 3 0 − ω 1 − ω 2 ω 1 0 ] \quad {\boldsymbol \omega}^{\land}= \begin{bmatrix}0 & -\omega_3 & \omega_2\\ \omega_3 & 0 & -\omega_1 \\ -\omega_2 & \omega_1 & 0\end{bmatrix} ω ∧ = ⎣ ⎢ ⎡ 0 ω 3 − ω 2 − ω 3 0 ω 1 ω 2 − ω 1 0 ⎦ ⎥ ⎤ 注意这里的角速度 ω {\boldsymbol \omega} ω B ω = R T ω {}^B{\boldsymbol \omega} = {\bf R}^T{\boldsymbol \omega} B ω = R T ω ω = R B ω {\boldsymbol \omega} = {\bf R}{}^B{\boldsymbol \omega} ω = R B ω ( 1.1 ) (1.1) ( 1 . 1 )
R ˙ = ( R B ω ) ∧ R (1.2) \dot{\bf R} = ({\bf R}{}^B{\boldsymbol \omega})^{\land} {\bf R} \tag{1.2} R ˙ = ( R B ω ) ∧ R ( 1 . 2 ) 这里我们要利用负对称矩阵的一个很好的性质：对任意旋转矩阵 R \bf R R v \bf v v ( R v ) ∧ = R v ∧ R T ({\bf R v})^{\land} = {\bf Rv^{\land}R}^T ( R v ) ∧ = R v ∧ R T (Rv)^ = Rv^R’ 的简单证明 》），于是 ( 1.2 ) (1.2) ( 1 . 2 )
R ˙ = R ( B ω ) ∧ (1.3) \dot{\bf R} = {\bf R}({}^B{\boldsymbol \omega})^{\land} \tag{1.3} R ˙ = R ( B ω ) ∧ ( 1 . 3 ) 比较一下 ( 1.1 ) (1.1) ( 1 . 1 ) ( 1.3 ) (1.3) ( 1 . 3 )
1-2. 前置(2)： 四元数
这部分讲四元数如何表示旋转的前置知识，熟悉的读者可以跳过。
用旋转矩阵来表示旋转很直观，但过于冗余，因为旋转只有三个自由度，而旋转矩阵有九个量。表征旋转还可以用欧拉角，但有万向锁问题，而且计算也不方便。旋转向量（即李代数 so(3)）和四元数是更常用的表征方法，在惯性导航中四元数似乎更普遍些。这里采用四元数。
一个四元数由一个实部和三个虚部构成，书写顺序各家不同，这里和 MSCKF [1] 一样，虚部在前实部在后：
q = q 1 i + q 2 j + q 3 k + q 4 = [ v T q 4 ] T {\bf q} = q_1 i + q_2 j + q_3 k + q_4 = [{\boldsymbol v}^T \quad q_4]^T q = q 1 i + q 2 j + q 3 k + q 4 = [ v T q 4 ] T 虚部 v = [ q 1 q 2 q 3 ] T {\boldsymbol v}=[q_1 \quad q_2 \quad q_3]^T v = [ q 1 q 2 q 3 ] T i , j , k i,j,k i , j , k i 2 = j 2 = k 2 = i j k = − 1 i^2=j^2=k^2=ijk=-1 i 2 = j 2 = k 2 = i j k = − 1 q ˉ \bar{\bf q} q ˉ
四元数和旋转向量有很直接的转换关系。绕单位轴 u \bf u u θ \theta θ
q = [ u sin θ 2 cos θ 2 ] (1.4) {\bf q} = [{\bf u}\sin\frac{\theta}{2} \quad \cos\frac{\theta}{2}] \tag{1.4} q = [ u sin 2 θ cos 2 θ ] ( 1 . 4 ) 四元数乘法 ⊗ \otimes ⊗
q ⊗ p = ( q 1 i + q 2 j + q 3 k + q 4 ) ( p 1 i + p 2 j + p 3 k + p 4 ) = ( q 1 p 4 + q 2 p 3 − q 3 p 2 + q 4 p 1 ) i + ( − q 1 p 3 + q 2 p 4 + q 3 p 1 + q 4 p 2 ) j + ( q 1 p 2 − q 2 p 1 + q 3 p 4 + q 4 p 3 ) k + ( − q 1 p 1 − q 2 p 2 − q 3 p 3 + q 4 p 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} q ⊗ p = = ( q 1 i + q 2 j + q 3 k + q 4 ) ( p 1 i + p 2 j + p 3 k + p 4 ) ( q 1 p 4 + q 2 p 3 − q 3 p 2 + q 4 p 1 ) i + ( − q 1 p 3 + q 2 p 4 + q 3 p 1 + q 4 p 2 ) j + ( q 1 p 2 − q 2 p 1 + q 3 p 4 + q 4 p 3 ) k + ( − q 1 p 1 − q 2 p 2 − q 3 p 3 + q 4 p 4 ) 这个计算结果可以表达为多种形式：
q ⊗ p = [ q 4 I 3 + v q ∧ v q − v q T q 4 ] [ v p p 4 ] = [ p 4 I 3 − v p ∧ v p − v p T p 4 ] [ v q q 4 ] \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} q ⊗ p = [ q 4 I 3 + v q ∧ − v q T v q q 4 ] [ v p p 4 ] = [ p 4 I 3 − v p ∧ − v p T v p p 4 ] [ v q q 4 ] 四元数乘法和其对应的两个旋转矩阵相乘物理意义是一样的，即 R ( q ⊗ p ) = R ( q ) R ( p ) {\bf R}({\bf q\otimes p})={\bf R}({\bf q}){\bf R}({\bf p}) R ( q ⊗ p ) = R ( q ) R ( p )
R ( q ) = ( 2 q 4 2 − 1 ) I 3 + 2 q 4 v ∧ + 2 v v T (1.5) {\bf R}({\bf q}) = (2q_4^2-1){\bf I}_3 +2q_4{\boldsymbol v}^{\land} + 2{\boldsymbol{vv}}^T \tag{1.5} R ( q ) = ( 2 q 4 2 − 1 ) I 3 + 2 q 4 v ∧ + 2 v v T ( 1 . 5 ) 四元数的逆为 q − 1 = [ − v T q 4 ] T {\bf q}^{-1} = [-{\boldsymbol v}^T \quad q_4]^T q − 1 = [ − v T q 4 ] T q ⊗ q − 1 = [ 0 0 0 1 ] T : = q I {\bf q}\otimes{\bf q}^{-1}=[0\quad 0\quad 0\quad 1]^T:={\bf q}_I q ⊗ q − 1 = [ 0 0 0 1 ] T : = q I q I {\bf q}_I q I
四元数对时间一阶导为
q ˙ = 1 2 [ ω ∧ ω − ω T 0 ] q : = 1 2 Ω ( ω ) q = 1 2 [ q 4 I 3 − v ∧ − v T ] ω (1.6) \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} q ˙ = 2 1 [ ω ∧ − ω T ω 0 ] q : = 2 1 Ω ( ω ) q = 2 1 [ q 4 I 3 − v ∧ − v T ] ω ( 1 . 6 ) 读者可能注意到了 ( 1.6 ) (1.6) ( 1 . 6 ) ( 1.1 ) (1.1) ( 1 . 1 ) ω \boldsymbol \omega ω ( 1.6 ) (1.6) ( 1 . 6 )
1-3. IMU 运动模型
有了前置知识的铺垫之后，我们可以给出 IMU 的运动模型：
G I q ˉ ˙ = 1 2 Ω ( ω ) G I q ˉ b ˙ g = n w g G v ˙ I = G a b ˙ a = n w a G p ˙ I = G v I (1.7) \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} G I q ˉ ˙ b ˙ g G v ˙ I b ˙ a G p ˙ I = 2 1 Ω ( ω ) G I q ˉ = n w g = G a = n w a = G v I ( 1 . 7 ) G I q ˉ ˙ {}^I_G \dot{\bar{q}} G I q ˉ ˙ ( 1.6 ) (1.6) ( 1 . 6 ) ω \boldsymbol \omega ω ( 1.1 ) (1.1) ( 1 . 1 ) G I q ˉ {}^I_G \bar{q} G I q ˉ ( 1.1 ) (1.1) ( 1 . 1 ) R \bf R R
2. IMU 观测和噪声模型
2-1 前置(1)： 科氏加速度
这部分在 1-1 的基础上，讨论参考坐标系不是惯性系的情况，熟悉科氏加速度的读者可以跳过。我们仍利用 1-1 中的图，但这次把绕惯性系 {A} 中固定单位轴 u \bf u u B r ^B{\bf r} B r r \bf r r R \bf R R r = R B r {\bf r}={\bf R}^B{\bf r} r = R B r
求一阶时间导，并利用公式 ( 1.1 ) (1.1) ( 1 . 1 )
v = r ˙ = R ˙ B r + R B r ˙ = ω ∧ R B r + R B r ˙ {\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} v = r ˙ = R ˙ B r + R B r ˙ = ω ∧ R B r + R B r ˙ 记 P 在 {B} 下速度为 B v ^B\bf v B v
v = ω ∧ r + R B v = ω × r + v r (2.0) \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} v = ω ∧ r + R B v = ω × r + v r ( 2 . 0 ) 请注意，这里用 v r {\bf v}_r v r v r {\bf v}_r v r v \bf v v B v ^B\bf v B v
再对 ( 2.0 ) (2.0) ( 2 . 0 )
a = v ˙ = ω ˙ × r + ω × r ˙ + R ˙ B v + R B v ˙ = α × r + ω × （ ω × r + v r ） + ω × R B v + R B a = α × r + ω × ( ω × r ) + 2 ω × v r + a r (2.1) \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} a = v ˙ = ω ˙ × r + ω × r ˙ + R ˙ B v + R B v ˙ = α × r + ω × （ ω × r + v r ） + ω × R B v + R B a = α × r + ω × ( ω × r ) + 2 ω × v r + a r ( 2 . 1 ) 我们来逐项分析上面这个式子。第一项中 α {\boldsymbol \alpha} α v r {\bf v}_r v r a r {\bf a}_r a r
2-2 前置(2)： 惯性导航相关坐标系定义
这部分讲惯性导航中经常出现的几个坐标系的定义 [5]。
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)： 高斯白噪声与随机游走
这部分讲高斯白噪声和随机游走(random walk)模型，及其离散化。这部分在kalibr 库中的 IMU noise model [4] 有简单的介绍，这里在其基础上添加了离散化的推导，因为离散化中部分内容还是有些令人疑惑的。离散化的推导部分参考自 [5]。
先讲高斯白噪声。一个连续时间的高斯白噪声 n ( t ) n(t) n ( t )
E [ n ( t ) ] = 0 E [ n ( t 1 ) n ( t 2 ) ] = σ g 2 δ ( t 1 − t 2 ) E[n(t)]=0 \\ E[n(t_1)n(t_2)] = \sigma_g^2 \delta(t_1-t_2) E [ n ( t ) ] = 0 E [ n ( t 1 ) n ( t 2 ) ] = σ g 2 δ ( t 1 − t 2 ) 其中 δ ( ) \delta() δ ( ) σ g 2 \sigma_g^2 σ g 2
将高斯白噪声离散化，可得到：
n d [ k ] = σ g d w [ k ] n_d[k]=\sigma_{gd} w[k] n d [ k ] = σ g d w [ k ] 其中
w [ k ] ∼ N ( 0 , 1 ) σ g d = σ g Δ t w[k] \sim \mathcal{N}(0,1) \\
\sigma_{gd}=\frac{\sigma_g}{\sqrt{\Delta t}} w [ k ] ∼ N ( 0 , 1 ) σ g d = Δ t σ g 其中 Δ t \Delta t Δ t Δ t \sqrt{\Delta t} Δ t n ( t ) n(t) n ( t )
n d [ k ] ≜ n ( t 0 + Δ t ) ≃ 1 Δ t ∫ t 0 t 0 + Δ t n ( τ ) d 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 n d [ k ] ≜ n ( t 0 + Δ t ) ≃ Δ t 1 ∫ t 0 t 0 + Δ t n ( τ ) d t E ( n d [ k ] 2 ) = E ( 1 Δ t 2 ∫ t 0 t 0 + Δ t ∫ t 0 t 0 + Δ t n ( τ ) n ( t ) d τ d t ) = E ( σ g 2 Δ t 2 ∫ t 0 t 0 + Δ t ∫ t 0 t 0 + Δ t δ ( t − τ ) d τ d t ) = E ( σ g 2 Δ t ) \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} E ( n d [ k ] 2 ) = E ( Δ t 2 1 ∫ t 0 t 0 + Δ t ∫ t 0 t 0 + Δ t n ( τ ) n ( t ) d τ d t ) = E ( Δ t 2 σ g 2 ∫ t 0 t 0 + Δ t ∫ t 0 t 0 + Δ t δ ( t − τ ) d τ d t ) = E ( Δ t σ g 2 ) 所以有 σ g d 2 = σ g 2 Δ t \sigma_{gd}^2=\frac{\sigma_g^2}{\Delta t} σ g d 2 = Δ t σ g 2 σ g d = σ g Δ t \sigma_{gd}=\frac{\sigma_g}{\sqrt{\Delta t}} σ g d = Δ t σ g
接下来讨论随机游走模型。准确地讲，随机游走其实是一个离散模型，其连续模型称为维纳过程（Wiener Process）。维纳模型是高斯白噪声的积分：
b ˙ g ( t ) = n ( t ) = σ b g w ( t ) \dot{b}_g(t)=n(t)=\sigma_{bg}w(t) b ˙ g ( t ) = n ( t ) = σ b g w ( t ) 其中 w w w
b d [ k ] = b d [ k − 1 ] + σ b g d w [ k ] b_d[k] = b_d[k-1]+\sigma_{bgd}w[k] b d [ k ] = b d [ k − 1 ] + σ b g d w [ k ] 其中
w [ k ] ∼ N ( 0 , 1 ) σ g d = σ b g Δ t w[k] \sim \mathcal{N}(0,1) \\
\sigma_{gd}=\sigma_{bg}\sqrt{\Delta t} w [ k ] ∼ N ( 0 , 1 ) σ g d = σ b g Δ t 这里多出来的 Δ t \sqrt{\Delta t} Δ t
b d [ k ] ≜ b ( t 0 ) + ∫ t 0 t 0 + Δ t n ( t ) d t b_d[k] \triangleq b(t_0) + \int_{t_0}^{t_0+\Delta t}n(t)dt b d [ k ] ≜ b ( t 0 ) + ∫ t 0 t 0 + Δ t n ( t ) d t E ( ( b d [ k ] − b d [ k − 1 ] ) 2 ) = E ( ∫ t 0 t 0 + Δ t ∫ t 0 t 0 + Δ t n ( t ) n ( τ ) d τ d t ) = E ( σ b g 2 ∫ t 0 t 0 + Δ t ∫ t 0 t 0 + Δ t δ ( t − τ ) d τ d t ) = E ( σ b g 2 Δ t ) \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} E ( ( b d [ k ] − b d [ k − 1 ] ) 2 ) = E ( ∫ t 0 t 0 + Δ t ∫ t 0 t 0 + Δ t n ( t ) n ( τ ) d τ d t ) = E ( σ b g 2 ∫ t 0 t 0 + Δ t ∫ t 0 t 0 + Δ t δ ( t − τ ) d τ d t ) = E ( σ b g 2 Δ t ) 所以有 σ b g d 2 = σ b g 2 Δ t \sigma_{bgd}^2=\sigma_{bg}^2\Delta t σ b g d 2 = σ b g 2 Δ t σ b g d = σ b g Δ t \sigma_{bgd}=\sigma_{bg}\sqrt{\Delta t} σ b g d = σ b g Δ t
于是我们得到随机游走模型的完整表达。实际上，观察离散模型的表达式，可以发现它生动阐释了「随机游走」的含义：每一时刻都是上一个采样时刻加上一个高斯白噪声得到的，犹如一个游走的粒子，踏出的下一步永远是随机的。在我们前面给出的 IMU 的运动模型中，bias 就设定为服从随机游走模型。
2-4. IMU 观测模型
根据上述前置知识，现在我们可以给出 IMU 的观测模型。需要注意的是，观测在不同参考坐标系下形式不同。
以 ECEF 为参考坐标系 ：这是 MSCKF 一代 [1] 的做法。因为 ECEF 不是惯性系，需要考虑地球自转，于是加速度模型中将会引入科氏加速度。记 ω G {\boldsymbol \omega}_G ω G G g {}^G{\bf g} G g ω m , a m {\boldsymbol \omega}_m,{\bf a}_m ω m , a m
ω m = ω + R ( G I q ˉ ) ω G + b g + n g (2.2) {\boldsymbol \omega}_m = {\boldsymbol \omega}+{\bf R}({}^I_G \bar{q}){\boldsymbol \omega}_G+{\bf b}_g+{\bf n}_g \tag{2.2} ω m = ω + R ( G I q ˉ ) ω G + b g + n g ( 2 . 2 ) a m = R ( G I q ˉ ) ( G a − G g + 2 ω G ∧ G v I + ( ω G ∧ ) 2 G p I ) + b a + n a (2.3) {\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} a m = R ( G I q ˉ ) ( G a − G g + 2 ω G ∧ G v I + ( ω G ∧ ) 2 G p I ) + b a + n a ( 2 . 3 ) 观测量都是在体坐标系 {I} 下表达的，所以在参考坐标系 {G} 下表达的量都需要左乘一个旋转矩阵转化到体坐标系。每个观测量的不确定量都用一个随机游走的 bias 和一个高斯白噪声之和来表达。陀螺仪的观测模型是比较易懂的。加速度计的观测模型，我们先将其改写为形如 ( 2.1 ) (2.1) ( 2 . 1 )
R T ( G I q ˉ ) ( a m − b a − n a ) = ( ω G ∧ ) 2 G p I + 2 ω G ∧ G v I + G a − G g {\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} R T ( G I q ˉ ) ( a m − b a − n a ) = ( ω G ∧ ) 2 G p I + 2 ω G ∧ G v I + G a − G g 但这还不够，因为各个量只是在 ECEF 坐标系 {G} 下的表达，而 ( 2.1 ) (2.1) ( 2 . 1 ) R G {\bf R}_G R G R G ω G = ω G {\bf R}_G{\boldsymbol \omega}_G = {\boldsymbol \omega}_G R G ω G = ω G R G {\bf R}_G R G
R G R T ( G I q ˉ ) ( a m − b a − n a ) = ω G × ( ω G × R G G p I ) + 2 ω G × ( R G G v I ) + R G ( G a − G 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}) R G R T ( G I q ˉ ) ( a m − b a − n a ) = ω G × ( ω G × R G G p I ) + 2 ω G × ( R G G v I ) + R G ( G a − G g ) 这里我们还利用了 R ( a × b ) = R a × R b {\bf R}(\bf a\times b)={\bf Ra}\times{\bf Rb} R ( a × b ) = R a × R b ( 2.1 ) (2.1) ( 2 . 1 ) a \bf a a α × r {\boldsymbol \alpha} \times\bf r α × r ω × ( ω × r ) {\boldsymbol \omega}\times({\boldsymbol \omega}\times{\bf r}) ω × ( ω × r ) 2 ω × v r 2{\boldsymbol \omega}\times{\bf v}_r 2 ω × v r a r {\bf a}_r a r
以 ECI 为参考坐标系 ：这是 MSCKF 二代 [3] 的做法。由于 ECI 为惯性系，不需要考虑地球自转，于是观测模型简单很多：
ω m = ω + b g + n g (2.4) {\boldsymbol \omega}_m = {\boldsymbol \omega}+{\bf b}_g+{\bf n}_g \tag{2.4} ω m = ω + b g + n g ( 2 . 4 ) a m = R ( G I q ˉ ) ( G a − G g ) + b a + n a (2.5) {\bf a}_m = {\bf R}({}^I_G \bar{q})({}^G{\bf a} -{}^G{\bf g} )+{\bf b}_a+{\bf n}_a \tag{2.5} a m = R ( G I q ˉ ) ( G a − G g ) + b a + n a ( 2 . 5 ) 因为比较简单，就不多做解释了。从文献上看，现在移动机器人领域 ECI 用得更多些。
至此，我们推导完了 IMU 的观测模型。
3. IMU 状态估计误差模型
3-1. 前置：四元数误差小量
旋转量是非线性的，不宜像线性量那样使用 x ~ = x − x ^ \tilde{ \bf x}= x-\hat{x} x ~ = x − x ^ ( 1.4 ) (1.4) ( 1 . 4 ) u \bf u u δ θ \delta \theta δ θ
δ q = [ u sin δ θ 2 cos δ θ 2 ] ≃ [ u δ θ 2 1 ] ≜ [ δ θ 2 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} δ q = [ u sin 2 δ θ cos 2 δ θ ] ≃ [ u 2 δ θ 1 ] ≜ [ 2 δ θ 1 ] 于是，可以用 δ q \delta {\bf q} δ q
q = δ q ⊗ q ^ {\bf q} = \delta {\bf q} \otimes\hat{\bf q} q = δ q ⊗ q ^ 直接使用 δ θ \boldsymbol{\delta \theta} δ θ
3-2. IMU 状态估计误差模型
我们直接给出和 MSCKF 一样的 IMU 状态估计误差模型：
X ~ I M U = [ δ θ I T b ~ g T G v ~ I T b ~ a T G p ~ I T ] (3.0) \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} X ~ I M U = [ δ θ I T b ~ g T G v ~ I T b ~ a T G p ~ I T ] ( 3 . 0 ) 其中旋转量按照四元数误差小量给出，其余直接由真实值和估计值相减得到。
4. 小结
本文从基础出发推导了 IMU 的运动模型( 1.7 ) (1.7) ( 1 . 7 ) ( 2.2 ) − ( 2.5 ) (2.2)-(2.5) ( 2 . 2 ) − ( 2 . 5 ) ( 3.0 ) (3.0) ( 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.
