IMU建模与状态估计

惯性测量单元(Inertial Measurement Unit, IMU)通常包含三个互相垂直的速率陀螺仪(rate-gyroscpoe)以及三个互相垂直的加速度计(accelerometer)，分别用于测量角速度和线加速度。在VIO，LIO中，IMU常以紧耦合的方式融入里程计中，进行组合状态估计。此时需要对IMU建立测量模型，状态模型，误差模型。

IMU模型基础

测量原理

为了推导IMU测量模型，噪声模型，先对IMU中加速度计和陀螺仪测量原理简单介绍。

加速度计

加速度计测量物体受到的比力，即去掉重力后整体加速度，或者单位质量上作用的非引力。压电式和电容式加速度计是通过受力导致的电压或电容变化来计算出压力的大小。三个相互正交的加速度计可以测量出三个方向的比力。

陀螺仪

科里奥利力

计算公式如下：

\[F=2mv\omega\]

压电式或电容式陀螺仪通过受力导致的电压或电容变化来计算科里奥利力的大小。

误差来源

系统误差

陀螺仪和加速度计的系统误差一般包含以下几种：

• 零偏，一个常值误差。当然在实际使用时零偏不是稳定的，对零偏的不稳定性分析引发了各个噪声参数
• 比力因子误差：传感器输出值与实际值之间的比例关系。由于传感器测量到输出数据，要经过ADC，数据转换中导致实际输出数据和测量数据之间存在比例关系。
• 安装误差：由于加工工艺原因，陀螺仪的三个轴并不完全正交，这个误差可以表示为

（来源 https://www.guyuehome.com/35086)

这样安装误差是12个参数。

由此得到误差模型：

系统误差标定

标定过程相当于求解误差模型中的参数，如果我们有了一组真实角速度，加速度和测量值，就可以建立一个方程，这种方法就是基于转台的标定。然而由于转台比较昂贵，也有不依赖转台的方法，主要依靠输入和真值之间得差异（静止的时候加速度应该是重力加速度）。

（1）基于转台的标定通过转台控制真实的角速度，加速度，再读取陀螺仪和加速度计的读数。这里有一个问题，陀螺仪的输入是角速度，但是转台一般角速度不如角度精度高，因此不是直接以角速度作为真值，而是以积分得到的角度作为真值。

（2）不依赖于转台的标定：当加速度计静止时，测到的三个加速度的矢量和就是重力加速度，即\(\|g\|^2=\|a\|^2\). 而实际上两者模的差值就和误差参数相关。通过多个不同静止位置加速度计的测量值可以计算出加速度计的内参。校正完加速度计后，再估计陀螺仪内参中的安装误差和尺度因子误差（零偏由于在短时间的累积中对姿态影响较小，不通过这种方法标，而是用静止状态标定）。

假设第k个位置得到加速度测量1，旋转一定角度到第k+1个位置，得到加速度测量2，在这段时间对角速度测量值积分可以得到姿态变换。通过姿态变换可以得到推测的加速度测量2',则测量2和2'的差就与误差参数有关。

随机误差

参考 https://www.guyuehome.com/35085

主要包括量化误差，随机游走，零偏不稳定性，速率斜坡，零偏重复性。目前主要通过Allen方差（分析测量值方差和时间的双对数曲线）估计这些参数。

IMU 状态估计

IMU状态量

那么在实际应用中，IMU的确定性误差如比例因子，非正交安装误差，一个恒定的零偏，假设出厂前已经过校准。随机误差中温度引起的误差假设也已经校准，零偏重复性是在两次上电之间的，在单次运行中不考虑。而零偏不稳定性和随机游走需要在测量模型中加入。

IMU的状态量通常表示为：

\[\textbf{X}_{IMU}=[\textbf{q}^I_G,\textbf{p}^I_G,\textbf{v}^I_G,\textbf{b}_g,\textbf{b}_a]\]

分别是IMU坐标系相对于世界坐标系的姿态（四元数），位置，速度，加速度。陀螺仪和加速度计的偏差。

对于 IMU 状态估计问题，需要提供运动模型、观测（噪声）模型、估计误差模型：

\[x=f(\dot{x})\] \[z=g(x)+n\] \[\delta x = e(\hat{x},x)\]

这是一个通用的模型，x是系统的状态量，\(\dot{x}\)是状态量对时间的导数。z是对状态量x的观测值，\(\delta x\)是观测值和真实值之间的误差。接下来我们推导对于IMU的这三个方程。

运动模型

\[\dot{\textbf{p}}_G^I=\textbf{v}^I_G\]

\[\dot{\textbf{v}}_G^I=\textbf{a}^I_G\]

\[\dot{\textbf{q}}^I_G = \frac{1}{2} \left [\begin{matrix} - \lfloor \omega \times \rfloor & \omega \\ -\omega^T & 0 \end{matrix} \right ] \textbf{q}_G^I (3) = \frac{1}{2} \Omega(\omega)\textbf{q}_G^I\]

\[\dot{\textbf{b}}_g = \textbf{n}_{\omega g}\]

\[\dot{\textbf{b}}_a = \textbf{n}_{\omega a}\]

前两个公式比较好理解。

公式3推导可参考：文献[2]

公式4,5: 零偏被建模为一个随机游走过程，其导数是高斯分布的。也就是\(\textbf{n}_{\omega g} \sim N(0,\sigma_g^2), \textbf{n}_{\omega a} \sim N(0,\sigma_a^2)\)

IMU测量模型

需要注意的是，观测在不同参考坐标系下形式不同。（根据参考坐标系是否为惯性坐标系，加速度的观测不同）

ECEF-非惯性坐标系

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

记 \({\boldsymbol \omega}_G\) 为地球自转角速度， \({}^G{\bf g}\) 为重力加速度， \({\boldsymbol \omega}_m,{\bf a}_m\) 为陀螺仪和加速度计的观测量，观测模型由以下公式给出

\[{\boldsymbol \omega}_m = {\boldsymbol \omega}+{\bf R}({}^I_G \bar{q}){\boldsymbol \omega}_G+{\bf b}_g+{\bf n}_g \]

\[ {\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 \]

推导参考链接[2]

ECI-惯性坐标系

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

观测模型为

\[ {\boldsymbol \omega}_m = {\boldsymbol \omega}+{\bf b}_g+{\bf n}_g \]

\[ {\bf a}_m = {\bf R}({}^I_G \bar{q})({}^G{\bf a} -{}^G{\bf g} )+{\bf b}_a+{\bf n}_a \]

参考资料

[1] https://fzheng.me/2016/11/20/imu_model_eq/#0-%E6%80%BB%E8%A7%88

[2] Trawny N, Roumeliotis S I. Indirect Kalman filter for 3D attitude estimation[J]. University of Minnesota, Dept. of Comp. Sci. & Eng., Tech. Rep, 2005, 2: 2005.

[3] https://www.guyuehome.com/35085