多传感器融合定位六-惯性导航原理及误差分析

Reference:

1. 深蓝学院-多传感器融合
2. 多传感器融合定位理论基础
3. What is MEMS? Accelerometer, Gyroscope & Magnetometer with Arduino
4. 《自动驾驶与机器人中的 SLAM 技术》高翔
5. 深蓝学院-从零开始手写 VIO
6. 例说姿态解算与导航18-IMU误差源
7. 武汉大学惯性导航课程合集【2021年秋】
8. 严恭敏官网：PSINS（高精度捷联惯导算法）
9. 卡尔曼滤波与组合导航原理

文章跳转：

1. 多传感器融合定位一-3D激光里程计其一：ICP
2. 多传感器融合定位二-3D激光里程计其二：NDT
3. 多传感器融合定位三-3D激光里程计其三：点云畸变补偿
4. 多传感器融合定位四-3D激光里程计其四：点云线面特征提取
5. 多传感器融合定位五-点云地图构建及定位
6. 多传感器融合定位六-惯性导航原理及误差分析
7. 多传感器融合定位七-惯性导航解算及误差分析其一
8. 多传感器融合定位八-惯性导航解算及误差分析其二
9. 多传感器融合定位九-基于滤波的融合方法Ⅰ其一
10. 多传感器融合定位十-基于滤波的融合方法Ⅰ其二
11. 多传感器融合定位十一-基于滤波的融合方法Ⅱ
12. 多传感器融合定位十二-基于图优化的建图方法其一
13. 多传感器融合定位十三-基于图优化的建图方法其二
14. 多传感器融合定位十四-基于图优化的定位方法
15. 多传感器融合定位十五-多传感器时空标定(综述)

前面几章讲了怎样用单个传感器把图建出来然后再做定位。接下来就进入了融合的环节，即怎样把图和定位建的更好。

1. 惯性技术简介

1.1 惯性技术发展历史

1687年，伟大的英国科学家牛顿提出力学三大定律，为惯性导航技术奠定了理论基础。
自20世纪60年代起，出现了挠性陀螺仪和动力谐调陀螺仪，同时平台式惯导系统发展迅速，并大量装备各种飞机、舰船、导弹和航天飞行器。

1.2 惯性器件

1.2.1 陀螺仪

定义：一种用于测量相对于惯性参考系的角速率的传感器。

1.2.1.1 机械陀螺(几乎没人用了)

• 定轴性： 当陀螺转子以高速旋转时，在没有任何外力矩作用在陀螺仪上时，陀螺仪的自转轴在惯性空间中的指向保持稳定不变，即指向一个固定的方向；同时反抗任何改变转子轴向的力量。

• 进动性： 当转子高速旋转时，若外力矩作用于外环轴，陀螺仪将绕内环轴转动；若外力矩作用于内环轴，陀螺仪将绕外环轴转动。其转动角速度方向与外力矩作用方向互相垂直。(受到的力为科里奥利力，后面会讲)

既然这个轴这么稳定，以它作为参照，那么船、车辆等姿态一直变化的载体，相比于陀螺轴线的角度变化测量出来，那么斜了多少、偏了多少就知道了。

1.2.1.2 激光陀螺

Sagnac效应 由法国物理学家 Sagnac 于 1913 年发现，其原理是干涉仪相对惯性空间静止时，光路 A 和 B 的光程相等，当有角速度时，光程不相等，便会产生干涉。(正反向传播光束之间的相移正比于旋转角速度)

如上图所示有一个绿块，这个绿块会发射两束光，中间是一个光路，沿着光路正方向发射一个，反方向再发射一个。理论上讲，正方向和反方向发射的光束走过的路程应该是一样的。那么我在正方向发射的光束和反方向发射的光束假如都打在同一个地方，那么对这两个光束做干涉，如果陀螺没有运动，这两个光的相位应该是完全一样的，这时会产生一个干涉条纹。现在假设陀螺有了运动，那么两个方向光束走过的路程就不一样了。比如当陀螺如上左所示逆时针运动，那么逆时针的走的到绿块处的光长就变长了；顺时针走过的光长就变短了。我们知道光速是不变的，那么光长变长和光长变短的两束光，相位会发生变化，即两个相位不完全一致了------这导致使用干涉条纹去看的时候，干涉条纹会发生偏移。通过检测干涉条纹的偏移，可以检测陀螺在这个时间内转过多少角度，就可以算出来角速度了。

为什么使用干涉条纹：直接检测光的相位不太容易，而干涉条纹打在一个地方的时候，检测什么地方亮什么地方暗就比较好检测了。

1.2.1.3 光纤陀螺

同样基于Sagnac效应，传播介质改成了光纤。（相比激光陀螺，成本低很多）

与前面相比，是通过线(光纤)而不是玻璃块(不是普通玻璃块，材质上有要求，在加工时要求极其精密，并需要打磨。玻璃块间的粘贴不是通过胶水，而是靠分子力----在打磨的特别平放一起时，它俩基本等同于一个东西，靠分子力吸在一起就拉不开了)传播，所以激光陀螺的玻璃块加工周期特别长，成本极高，而光纤陀螺相对来说低很多。

1.2.1.4 MEMS陀螺(常用）

MEMS陀螺都是基于振动陀螺原理制造的：

• 框架式角振动陀螺仪；
• 音叉式线振动陀螺仪；
• 振动轮式陀螺仪。

振动陀螺仪利用科里奥利效应测量角速度：

科里奥利力(Coriolis force，简称为科/哥氏力)，是对旋转体系（比如自传的地球，旋转的圆盘等）中进行直线运动的质点由于惯性相对于旋转体系产生的直线运动的偏移的一种描述。如上图所示，当一个物体以一个特定的速度向一个特定的方向运动，且一个外部角速度被施加时，一个力会产生，这会引起物体垂直方向的位移。

下图左有一个圆盘，圆盘静止的时候，有一个球从里往外去运动的时候，画出来肯定是一条直线。现在假设这个圆盘它旋转了一下，旋转以后这个球画出来的就不是一条直线了，它是一个弧线。也就是说一个物体往前运动，旋转它的载体(圆盘)时，这个运动的物体会受到一个力，往另一个方向走。

回到 MEMS。右图有一个不断移动或震荡的质量块(与加速计不同，它是一直来回摆动的)，它在一个微机械的装置内来回摆动，在静态不动的时候，它就沿直线运动。但是当它有旋转的时候，因为科里奥利力的存在，它的摆动就会画弧线而不是走直线了。这时，支撑着这个摆的四个构件，就会检测到一个扭动的力，这个扭动力量的大小，就反应了当前角速度的大小。通过这种方式来测量角速度。(类似于下面的加速度计，这个位移会引起电容的变化，这将会被测量、处理，它对应一个特定的角度率)
核心就是惯性-------即保持原来的运动状态不变。进动性----不想改变转轴，我要反抗这个旋转。

有的低端 MEMS 陀螺上会采用 音叉振动陀螺(叉子的中间为旋转轴，叉子左右两个质量块，做方向相反的正弦运动，质量块收到的科氏力方向相反)，测量方式有区别，但均使用了科里奥利力，这里就不做过多介绍。

1.2.2 加速度计

定义：一种用于测量相对于惯性参考系的加速度(称之为比力)的传感器。

• $f=a-g$(惯导比力方程)

  • $f=$ 加速度计输出(比力，Specific Force)
  • $a=$ 相对于惯性空间的运动加速度
  • $g=$ 万有引力加速度

关于为什么公式内为 $-g$，可以看看下面的受力分析：

即如果 IMU $z$ 轴测量方向向上，静置时 IMU 测量重力加速度约为 $9.81m/s^2$；

MEMS 加速度计利用电容或电阻桥等原理来测量加速度计测量值 $a_m$。它的微观结构是这样的：它有一个 质量块(mass) 附着在一个弹簧(spring)上，弹簧被限制沿着一个方向运动，下左图还可以看见有一个固定的外板。

所以当载体在特定方向上施加加速度时，仪表壳体也随之做相对运动，质量块保持惯性，朝着与加速度方向相反的方向产生位移（拉伸或压缩弹簧）。当位移量达到一定值时，弹簧给出的力使质量块以同一加速度相对惯性空间做加速运动，加速度的大小与方向影响质量块相对位移的方向及拉伸量。这时质量块与固定板之间的电容就会发生改变，每个电容的变化对应一个特定的加速度值。（在静止不动时，加速度计是不会摆动的。）

视频(右)截自[博世加速度传感器的工作原理]

简易的结构如下所示：

1.3 概念

1.3.1 惯性测量单元(IMU)

惯性测量单元(Inertial Measurement Unit， IMU) 已经非常普及了。我们在绝大多数电子设备中都能找到 IMU：车辆、手机、手表，头盔，甚至足球当中都内置了 IMU。它们体积很小，安装在设备内部后，就可以提供有效的局部运动估计，实现一些有趣的功能。在自动驾驶中，惯性导航器件也是十分基础的定位装置。惯性导航提供的定位效果基本与外部环境和其他传感器数据无关，具有很高的泛用性和可靠性。

典型的六轴 IMU 由 陀螺仪(gyroscope) 和 加速度计(accelerometer) 组成。虽然它们测量的目标都是物体的惯性，但其实现手段却是非常多样的，从低成本的 MEMS(Micro-electromechanical systems, 微机电) 惯导到昂贵的光纤陀螺，它们的目的都是精确地测量物体的惯性。

IMU 通常安装在一个运动的系统中。我们通过测量运动载体的惯性，来推断物体本身的状态。这些与惯性相关的物理量，通常并不是直接的位置和旋转，而是微分之后的物理量。IMU 的陀螺仪可以测量物体的角速度，而加速度计则测量物体的加速度。它们内部可以根据受力或者时间等其他物理量来推算角速度和加速度，但从外部来看，我们只须关心它们对角速度和加速度测量是否精确，以及这些量和车辆位置、姿态之间的关系。

• 惯性测量单元 = $3$ 轴加速度计 + $3$ 轴陀螺
  • 加速度计测量相互正交的三个方向上的比力，陀螺测量相互正交的三个方向上的角速度；
  • 通常三轴陀螺和三轴加速度计的轴向是平行对齐的；
  • 传感器与载体固联，载体系原点为三轴加速度计的交点。(即测量中心是以加速度计的测量中心为准。与陀螺没关系是因为陀螺测角运动，这是因为刚体上任何一个位置的角运动基本都是一样的，但是任何一个位置的速度、加速度可是不一样的。)

1.3.2 ISA, IMU, INS

Inertial Sensor Assembly (ISA)	• 3轴陀螺+3轴加速度计 • 输出原始传感器数据
Inertial Measurement Unit (IMU)	• ISA经误差标定补偿（零偏和比例因子误差等）和数据转换 • 输出补偿后的数据
Inertial Navigation System (INS)	• IMU + 惯性导航算法（惯导机械编排） • 输出位置、速度、姿态角等导航状态量+补偿后的传感器数据

2. 惯性器件误差分析

拿到一个 IMU，首要任务是对其器件误差进行分析，包括陀螺仪的误差和加速度计的误差。由于陀螺仪与加速度计的误差特性类似，仅是测量的物理量含义不同，因此我们就以陀螺仪为准来介绍随机误差成分、原理及分析方法。加速度计的误差分析直接照葫芦画瓢就行。

2.1 常见随机过程的离散化

一些常见的简单随机过程的离散化：

1. 一阶马尔可夫(相比随机游走，多了个倍率)：
   $\dot{X}(t)=-\beta X(t)+w(t)X_k=a_1{X}_{k-1}+W_{k-1}$
2. 二阶马尔可夫：
   $\ddot{X}(t)=-2\beta \dot{X}(t)-{\beta }^{2}X(t)+w(t)X_k=a_1{X}_{k-1}+a_2{X}_{k-2}+W_{k-1}$
3. 随机游走：
   $\dot{X}(t)=w(t)X_k=X_{k-1}+W_{k-1}$
4. 随机常值：
   $\dot{X}(t)=0X_k=X_0$

可将白噪声和随机游走当作是一阶马尔可夫过程的两种极端情况，即分别取参数 $a_1=0$ 或 $a_1=1$，三者之间关系如下图所示：

2.2 信号误差组成

陀螺仪的误差主要包括这样几项：

1. 量化噪声 $Q$
   一切量化操作所固有的噪声，是数字传感器必然出现的噪声；
   产生原因：通过AD采集把连续时间信号采集成离散信号的过程中，精度会损失，精度损失的大小和AD转换的步长有关，步长越小，量化噪声越小。(AD：模拟信号，这个比较好理解，连续信号采集成离散信号，比较采集是有频率的)
   

2. 角度随机游走 $N$
   宽带角速率白噪声：陀螺输出角速率是含噪声的，该噪声中的白噪声成分；
   产生原因：计算姿态的本质是对角速率做积分，这必然会对噪声也做了积分。白噪声的积分并不是白噪声，而是一个马尔可夫过程，即当前时刻的误差是在上一时刻误差的基础上累加一个随机白噪声得到的。
   角度误差中所含的马尔可夫性质的误差，称为角度随机游走。(测量出的角速率是有一个白噪声的，因为我们要利用角速率解算，积分成角度，这时的白噪声也包含在积分里面了，那不就算的不准了)

   ---

   宽带(Broadband) 是一个用来描述传感器响应范围或频率范围的术语。在宽带角速率(Broadband Angular Rate)中，宽带指的是陀螺仪能够有效测量的角速率范围，涵盖了一定范围内的角速度变化。通常情况下，传感器的响应范围可以分为两类：宽带和窄带。

   • 宽带(Broadband)：宽带传感器能够在较广的频率范围内准确测量信号。在陀螺仪中，宽带角速率指的是陀螺仪能够测量的角速率范围，涵盖了高频到低频的变化。这对于一些需要测量较大角速度变化的应用非常重要，例如飞行器的快速转动或高频震动。
   • 窄带(Narrowband)：窄带传感器只能在较窄的频率范围内准确测量信号。它们在特定频率范围内可能具有更高的测量精度，但在超出这个频率范围时可能失效。这对于一些需要高精度测量特定频率信号的应用非常有用。

   ---

3. 角速率随机游走 $K$
   与角度随机游走类似，角速率误差中所含的马尔可夫性质的误差，称为角速率随机游走。而这个马尔可夫性质的误差是由宽带角加速率白噪声累积的结果。(角加速率里面也是有一个白噪声的)

   与角度随机游走的区别在于，一个是角速率白噪声累积造成的，一个是角加速率白噪声累积造成的。

4. 零偏不稳定性噪声 $B$
   陀螺仪内最常见的一个误差项，如果一个陀螺仪只让使用一个指标来体现精度，那必然就是它了！

   零偏：即常说的 bias，一般不是一个固定参数，而是在一定范围内缓慢随机漂移。
   零偏不稳定性：零偏随时间缓慢变化，其变化值无法预估，需要假定一个概率区间描述它有多大的可能性落在这个区间内。时间越长，区间越大。

   实际上，如果你真的测的时间足够长，会发现它也不会无限制增长下去，所以，这个对概率区间的描述只是近似有效，或者一定时间内有效，由于这个有效时间比较长，所以我们一般仍然使用这种方式来描述，只是在理解上要知道这一点的存在。(这玩意好像不是很好描述，但是在工程上，我们需要给它一个近似的模型)

5. 速率斜坡 $R$
   该误差是趋势性误差，而不是随机误差。
   随机误差，是指你无法用确定性模型去拟合并消除它，最多只能用概率模型去描述它，这样得到的预测结果也是概率性质的。
   趋势性误差，是可以直接拟合消除的，在陀螺里产生这种误差最常见的原因是温度引起零位变化，可以通过温补来消除。
   (一个角速度，在不考虑之前说的那些随机误差的影响下，那误差就是一个稳定的白噪声。现在它随着时间变化，会有一个趋势性，也就是误差也在飘，但是这个误差是趋势性的。理论上讲这个误差不能叫完全的随机误差，因为趋势性很多时候是有些确定性的模型来影响的，比如说温补------随着温度的升高，误差会变大，这个模型是可以拟合的。但是在随机信号分析的时候，温补的误差是可以分析出来的。)

6. 零偏重复性
   多次启动时，零偏不相等，因此会有一个重复性误差。在实际使用中，需要每次上电都重新估计一次。(意思是，不同次启动时，零偏会发生多大的变化。与前面的所有误差是有区别的，前五项一次充电采集很多信号就可以分析出来。而零偏重复性就需要反复上电断电上电断电，统计一下每次上电断电，这个bias发生了多少变化)
   Allan方差分析时，不包含对零偏重复性的分析。(即Allan方差只能分析前五项，不能分析第六项，毕竟每一次上电都不一样，也没法分析)

(零偏、零偏不稳定性、重复性是三个概念。比如在刚打开IMU，可以在静止状态下估计IMU的零偏，而每次打开时零偏的大小变化由零偏重复性描述。后面IMU在运行的过程中，其值会在初始零偏附近的范围内移动，这是由于零偏不稳定性噪声造成的。)

出自《卡尔曼滤波与组合导航原理》严恭敏
图中 $\dot{x}(t)=-1/\tau \cdot x(t)+w(t)$ 代表一个典型的物理量的一阶马尔可夫过程。 $\mathrm{U}$ 表示任意物理量，比如电压的单位 $V$ 或者电流的单位 $A$，或者角度的单位 $°$。

求这个物理量的噪声(Unit: $U/s$)的方差：其中 $\delta (t-\tau )$ 代表冲击函数(狄拉克函数)，单位为 $1/\mathrm{s}$ 所以 $\mathrm{q}$ 的单位就是 $U^2/s$ 或者写成：$(U/s{)}^2/Hz$。其中 $(U/s{)}^2$ 不就是功率的意思么，那么 $(U/s{)}^2/Hz$ 就是一个功率频谱的概念。那么再开根：$\sqrt{q}$ 就是噪声密度的概念。

2.3 Allan方差分析

随机信号Allan方差的物理意义及应用在本质上来源于它与功率谱之间的关系。
功率谱(功率谱密度函数)：单位频带内的信号功率，它表示信号功率随着频率的变化情况，即信号功率在频域的分布状况。

假设把随机过程 $X_{\alpha }$ 的功率谱表示为: $\mathrm{PSD}\left[X_{\alpha }\right]=h_{\alpha }{f}^{\alpha }$ 其中 $f^{\alpha }$ 是频率， $h_n$ 为相应的系数。(频率和功率之间的关系)

若多个随机过程相互独立，则其满足线性相加性质，即 $X=\sum X_a$ 此时，其功率谱也同样可以线性相加 $\mathrm{PSD}[X]=\sum \mathrm{PSD}\left[X_{\alpha }\right]$(多个功率谱是可以叠加的)

• Allan方差分析方法的基本思路：
  在惯性器件随机误差分析中，以上提到的 $5$ 种误差相互独立，且 $\alpha$ 值不同，因此若绘制“时间间隔-方差双对数曲线”(时间间隔是频率的倒数，方差是功率谱的积分)，则得到的曲线斜率必不相同。

  根据曲线斜率识别出各项误差，并计算出对应的误差强度。

• 具体流程如下：

  1. 保持传感器绝对静止获取数据；

  2. 对数据进行分段，设定时间段的时长，如下图所示：
     

  3. 将传感器数据按照时间段进行平均；

  4. 计算方差，绘制Allan曲线。

(公式知道就行了，这时信号分析的东西，不属于融合范畴)
量化噪声 $Q$ 满足下式：
$${\log }_{10}{\sigma }_{QN}(\tau )={\log }_{10}(\sqrt{3}Q)-{\log }_{10}\tau$$角度随机游走 $N$ 满足下式：
$${\log }_{10}{\sigma }_{ARW}(\tau )={\log }_{10}N-1/2\ast {\log }_{10}\tau$$角速率游走 $K$ 满足下式：
$${\log }_{10}{\sigma }_{RRW}(\tau )={\log }_{10}(K/\sqrt{3})+1/2\ast {\log }_{10}\tau$$零偏不稳定性 $B$ 满足下式：
$${\log }_{10}{\sigma }_{BI}(\tau )={\log }_{10}(2B/3)$$速率斜坡 $R$ 满足下式：
$${\log }_{10}{\sigma }_{RR}(\tau )={\log }_{10}(R/\sqrt{2})+{\log }_{10}\tau$$以上公式中，$\tau$ 为时间间隔。

根据以上公式分析，可知曲线的形状如下：

即各随机噪声对应的斜率分别为 -1, -1/2, 0, 1/2, 1(即上面的$-{\log }_{10}\tau$、$-1/2\ast {\log }_{10}\tau$…)，找到曲线后，方程就可以求出来了(也就是𝑄、𝑁、𝐾、𝐵、𝑅)

同时，令 $\tau =1$ ，则 ${\log }_{10}(\tau )=0$
其含义是求曲线与 $\tau =1$ 的交点，此时有：

噪声类型	与 $\tau$ 的交点
量化噪声	$\sqrt{3}Q$
角度随机游走	$N$
角速率游走	$K/\sqrt{3}$
零偏不稳定性	$2B/3$
速率斜坡	$R/\sqrt{2}$

此时可容易地求出 𝑄、𝑁、𝐾、𝐵、𝑅，也就是我们想求的这五项误差的系数。

怎么使用这些误差：
开源代码：https://github.com/gaowenliang/imu_utils

• P1：五个误差全用么？
  不全用，这五个误差更多的是，在生产器件时，知道以哪几个误差为主，来改进生产。但假如是用户，我现在的主要目标是做融合，那么在实际过程中五项误差不全是需要关心的了。
  主要关心的有以下两个：

  用法(以陀螺仪为例)：

  1. 角度随机游走，在融合时作为陀螺仪的噪声使用。(有时也以零偏不稳定性当做噪声，因为很多手册里面都不提供角度随机游走，而零偏不稳定性是必须会给的)(对应的是卡尔曼滤波里面的 $Q$)
  2. 角速度随机游走，作为陀螺仪微分项中的噪声。(详细内容在介绍融合时介绍)

注意：
1）其他误差项，仅起到了解器件精度水平的作用；
2）实际融合时，Allan分析的结果，只是作为初值使用，需要在此基础上调参。

单个IMU三个陀螺仪的Allan方差曲线

零偏重复性：之前的几个误差都被计算出来了，但是可以看到所有的模型都在估计一个bias。那么为什么需要估计一个bias出来而不能提前估计出来一直用呢？就是因为零偏重复性的问题，它在每一次上电的时候都不一样。假如我的器件，零偏不稳定性是 $5°/h$，那么它的零偏重复性可能达到 $500°/h$，也就是说bias上面有一个每小时 $500°$ 的值，而这个值是事先测不出来的，这么大的误差，必然是需要在线估计的。

根据前面所说的代码：https://github.com/gaowenliang/imu_utils 和 Allan Variance: Noise Analysis for
Gyroscopes，主要关注噪声为测量(白噪声)噪声和零偏不稳定性。

2.3.1 仿真数据

制作一个仿真 IMU 数据集。设定，加速度的高斯白噪声为 $0.019$，陀螺仪的高斯白噪声为 $0.014$；加速度 bias 的随机游走噪声为 $5e^{-4}$，陀螺仪的 bias 随机游走噪声为 $5e^{-5}$。

加速度的 Allan 曲线如下：
依照上面公式，$0.019\approx 0.019248$，$2\ast 5e^{-4}/3\approx 0.000376$，合理。

2.3.2 IMU 离散时间噪声模型

前面讨论的都是在连续时间下的 IMU 噪声方程，而实际当中的 IMU 会按照固定时间间隔进行采样，所以我们拿到的数据总可以看成是离散的。因为 IMU 传感器按照固定频率进行采样，不妨设每次采样间隔为 $\Delta t$，那么对于噪声来说，陀螺仪和加速度计的离散测量噪声可以简化地描述为：
$$\begin{array}{l}{\mathbf{\eta }}_g(k)\sim \mathcal{N}\left(0,\frac{1}{\Delta t}\mathrm{Cov}\left({\mathbf{\eta }}_g\right)\right),\\ {\mathbf{\eta }}_a(k)\sim \mathcal{N}\left(0,\frac{1}{\Delta t}\mathrm{Cov}\left({\mathbf{\eta }}_a\right)\right).\end{array}$$而对于零偏部分，则可以写为：
$$\begin{array}{l}{\mathbf{b}}_g(k+1)-{\mathbf{b}}_g(k)\sim \mathcal{N}\left(0,\Delta t\mathrm{Cov}\left({\mathbf{b}}_g\right)\right),\\ {\mathbf{b}}_a(k+1)-{\mathbf{b}}_a(k)\sim \mathcal{N}\left(0,\Delta t\mathrm{Cov}\left({\mathbf{b}}_a\right)\right).\end{array}$$因此，在离散时间系统中 (也是我们平时操作的系统)，两个噪声都是非常便于处理的。而且，在很多系统实现中，甚至不考虑用协方差矩阵来表达 IMU 测量噪声和零偏随机游走，而是简单地将它们表达为对角矩阵，这实际上忽略了各个轴之间的相关性。在程序里，我们往往使用诸 如 ${\sigma }_g,{\sigma }_a$ 的参数来表达 IMU 的噪声标准差，用 ${\sigma }_{bg},{\sigma }_{ba}$ 参数来表达零偏游走的标准差。这时，离散时间下的噪声标准差应该写为：
$$\begin{array}{rl}{\sigma }_g(k)=\frac{1}{\sqrt{\Delta t}}{\sigma }_g,& {\sigma }_a(k)=\frac{1}{\sqrt{\Delta t}}{\sigma }_a,\\ {\sigma }_{bg}(k)=\sqrt{\Delta t}{\sigma }_{bg},& {\sigma }_{ba}(k)=\sqrt{\Delta t}{\sigma }_{ba}.\end{array}$$从物理单位上来看，离散时间的噪声是直接加在被测的物理量上的，很容易确定它们的物理单位。离散时间的零偏本身是加在被测物理量上的，因此它们与被测物理量具有相同单位。
$${\sigma }_g(k)\to \frac{\mathrm{rad}}{s},{\sigma }_a(k)\to \frac{m}{s^2},{\sigma }_{bg}(k)\to \frac{\mathrm{rad}}{s},{\sigma }_{ba}(k)\to \frac{m}{s^2}$$而连续时间的方差需要在离散方差上乘或除以一个开方时间单位，因此它们的物理单位变为：
$${\sigma }_g\to \frac{rad}{\sqrt{s}},{\sigma }_a\to \frac{m}{s\sqrt{s}},{\sigma }_{bg}\to \frac{rad}{s\sqrt{s}},{\sigma }_{ba}\to \frac{m}{s^2\sqrt{s}}.$$请注意同样的单位之间可以进行大小的转换，例如弧度可以转换为度，秒也可以转换为分钟、小时，等等。也有的材料里，把 $\frac{1}{\Delta t}$ 的单位记成 $Hz$，所以上述变量的物理单位也可以记为(与上文 imu_utils 图像内 Units 有细微差别，但是将 $\frac{1}{\Delta t}$ 与 $Hz$ 替换，就是等同的)：
$${\sigma }_g\to \frac{rad}{s\sqrt{Hz}},{\sigma }_a\to \frac{m}{s^2\sqrt{Hz}},{\sigma }_{bg}\to \frac{rad}{s^2\sqrt{Hz}},{\sigma }_{ba}\to \frac{m}{s^3\sqrt{Hz}}$$

2.3.3 IMU数据手册

典型IMU的数据手册参数

1. 测量噪声：在整个运动模型看来是角度随机游走和速度随机游走，对应连续时间噪声模型中的 ${\sigma }_g$、${\sigma }_a$。可见这个 IMU 的指标是 $0.66°/\sqrt{\text{小时}}$ 和 $0.11m/s/\sqrt{小时}$。直观上可以理解为，在正确找到零偏的情况下，如果我们对这个 IMU 进行积分，那么每小时它积分误差的标准差应该为 $0.66°$ 和 $0.11m/s$。

2. 零偏随机游走方差：也就是噪声模型中的 ${\sigma }_{bg}$、${\sigma }_{ba}$。但这个量通常没有直接在手册中对应，在实际当中也很难测量到。手册里经常给出的是零偏重复性和运行时偏置稳定性来代替。在我们刚打开 IMU 时，可以在静止状态下估计 IMU 的零偏。每次零偏的大小变化就由零偏重复性来描述。另一方面，如果其他客观条件不变，这个开机零偏在运行过程中也会发生一定改变，其幅值就由运行时零偏稳定性描述，在该手册中为 $14.5°/小时$。

严老师总结：
Allan 方差分析的一个用途是分析陀螺的性能或对比不同陀螺的性能，相比于其它分析法Allan法还是很好用的，比较全面。另一个用途是获得噪声参数，用于组合导航的 Kalman 滤波噪声参数设置。不是所有的 Allan 方差噪声系数都有用，主要有用的是角度随机游走系数（用于设置卡尔曼Q）和零偏不稳定性系数（用于设置一阶马氏过程的方差），其实这两个系数量级大小差不多就行了，太精细也没用，毕竟 Allan 方差分析得出的陀螺静态性能，鬼知道实际应用动态的时候陀螺误差会变化多大，存在数量级差别都很有可能。

Allan 方差只是大概判断陀螺仪/加速度计精度的一种手段，只是表现了静态性能，动态性能还得看振动对陀螺偏置影响系数 Vibration Rectified Error(VRE) 更为重要。

---

3. 惯性器件内参标定

内参标定是惯性器件里面非常核心的一个环节，虽然大家在实践过程中可能不需要做这个东西，因为买一个IMU模块，大多数情况下都给标好了。但是这方面的技能还是需要掌握的。

3.1 惯性器件内参误差模型

3.1.1 零偏

误差解释： 陀螺仪或加速度计输出中的常值偏移，即常说的 bias。(注意陀螺仪和加速度计都有一个)
误差特性： 由于零偏存在不稳定性，因此零偏并不是固定不变的。
解决办法： 实际使用中，只能一段时间内近似为常值。
(在多数情况下可以认为是一个常值，在前面说误差分析时，说零偏有随机游走、每次上电还有零偏重复性这类的问题。但是不管怎样，在一次通电的很短的时间内，可以将它认为是一个常值来用，这样可以简化模型，不把模型搞得太复杂。)

加速度计的零偏表示为：
$$b_a=\left[\begin{array}{lll}{b}_{ax}& b_{ay}& b_{az}\end{array}\right]$$陀螺仪的零偏表示为：
$$b_g=\left[\begin{array}{lll}{b}_{gx}& b_{gy}& b_{gz}\end{array}\right]$$

3.1.2 刻度系数误差

误差解释：器件的输出往往为脉冲值或模数转换得到的值，需要乘以一个刻度系数才能转换成角速度或加速度值，若该系数不准，便存在刻度系数误差。
误差特性：不一定是常值，它会随着输入大小的不同而发生变化，这个就是标度因数的非线性。
解决办法：如果非线性程度比较大，则需要在标定之前先拟合该非线性曲线，并补偿成线性再去做标定。
(假设有$x,y,z$三个轴，绕$z$轴旋转 10°/s，在实际测量的时候，会发现$z$输出的角速度不是 10°/s，可能是 9.9°/s，也就是说上面有一个 0.1°/s 的误差。这时在旋转的时候，要在上面乘上一个比例因子，这个比例因子就是 $S=\frac{10}{9.9}$，最后补偿过的角速度才是真实的角速度结果。每个轴上都有一个这样比例系数的补偿因子，所以建成的是一个对角矩阵的形式，与$\left[\begin{array}{l}x\\ y\\ z\end{array}\right]$相乘。)

关于拟合：输入 10°/s 时得到一个角速度输出，20°/s、30°/s 时都能得到一个角速度输出值。但是比例因子不一定是相同的，在 10°/s 时有一个数，在 20°/s 时可能就是另外一个数了。这里有两种方法处理：

1. 10°/s、20°/s、30°/s 均得到一个点，拟合出一个二次曲线。这样是可以的，但是很多时候标定不是单参数的标定-----它与bias和安装误差都是耦合在一起的，也就是说拟合的结果里面有其他误差的耦合系数。
2. 另一种办法是，仍然是拟合，但是拟合的时候只拟合它的趋势项，比如说它二次曲线的弯曲程度是什么样子的。相当于是补偿这个二次曲线后，变成一个一次曲线。这个一次曲线和真实的一次曲线可能有一个偏移量的差异，这个没有关系。只有它变成了一次曲线，相当于是仍然变成了一个常值。也就是说，我们把二次曲线，先补偿一下变成一次曲线，然后我们再去标一次曲线的平移量就可以了。(工程中是比较复杂的，但实际中，经常就当一次曲线搞就行了。绝大多数情况还是一个线性的，如果出现很强的非线性，再按照这个方法来)

加速度计的标度因数(scale)，表示如下：
$$S_a=\left[\begin{array}{lll}{S}_{ax}& & \\ & S_{ay}& \\ & & S_{az}\end{array}\right]$$陀螺仪的标度因数，表示为：
$$S_g=\left[\begin{array}{lll}{S}_{gx}& & \\ & S_{gy}& \\ & & S_{gz}\end{array}\right]$$

3.1.3 安装误差

误差解释： 如下图所示，b坐标系是正交的imu坐标系，g坐标系的三个轴是分别对应三个陀螺仪。由于加工工艺原因，陀螺仪的三个轴并不正交，而且和b坐标系的轴不重合，二者之间的偏差即为安装误差。
误差特性： 实际系统中，由于硬件结构受温度影响，安装误差也会随温度发生变化。
解决办法： 在不同温度下做标定，补偿温度变化量。
(比方说绕 $z$ 轴旋转，理论上其他轴是没有角速度输出的。但是有一个问题，如果 $x$ 与 $z$ 轴不是正交的，比如正交是 $90°$，而它俩是 $89.9°$，会造成一个现象，即绕 $z$ 旋转的时候，$x$ 上就有了角速度输出了，因为它上面因为不完全垂直而有分量了。)

因为 $z$ 在 $x$、$y$ 方向上有误差；因为 $y$ 在 $x$、$z$ 方向上有误差；因为 $x$ 在 $y$、$z$ 方向上有误差，总共有六个误差量。(6 个安装误差)
陀螺仪的安装误差，表示如下(使用 M(misalignment) 代替图中的 $S$)：
$$M_g=\left[\begin{array}{ccc}0& M_{gxy}& M_{gxz}\\ M_{gyx}& 0& M_{gyz}\\ M_{gzx}& M_{gzy}& 0\end{array}\right]$$加速度计的安装误差，表示为
$$M_a=\left[\begin{array}{ccc}0& M_{axy}& M_{axz}\\ M_{ayx}& 0& M_{ayz}\\ M_{azx}& M_{azy}& 0\end{array}\right]$$

如 $M_{gxy}$ 表示，绕 $z$ 轴旋转，角速度在 $x$ 轴上的分量；$M_{gxz}$ 表示，绕 $y$ 轴旋转，角速度在 $x$ 轴上的分量。

3.1.4 惯性器件内参误差模型

利用下面公式(以陀螺仪为例)，可以把各项误差综合在一起：
$$W=S_g\left(I+M_g\right)\omega +b_g\approx \left(S_g+M_g\right)\omega +b_g$$

1. $\omega$ 为角速度输入，即想求得的理想角速度，真实输入；

2. $W$是角速度的输出，也就是直接在器件上面测量出来的结果。

   可以看到 $S_g$ 和 $M_g$ 乘在一起了，耦合在一起就很难解耦，这个模型标定的时候就很难标定了，所以要做一个化简，就得到了最右边的样子。核心原因在于 $S_g=I+\Delta S_g$，而 $\Delta S_g$ 是一个很小的量，$(I+\Delta S_g)(I+M_g)=I+\Delta S_g+M_g+\Delta S_g{M}_g$，因为 $M_g$ 也是个小量，$\Delta S_g{M}_g$ 可以被忽略。

陀螺仪的输出可以展开为：
$$\left[\begin{array}{l}{W}_x\\ W_y\\ W_z\end{array}\right]=\left[\begin{array}{lll}{S}_{gx}& M_{gxy}& M_{gxz}\\ M_{gyx}& S_{gy}& M_{gyz}\\ M_{gzx}& M_{gzy}& S_{gz}\end{array}\right]\left[\begin{array}{c}{\omega }_x\\ {\omega }_y\\ {\omega }_z\end{array}\right]+\left[\begin{array}{l}{b}_{gx}\\ b_{gy}\\ b_{gz}\end{array}\right]$$加速度计的输出可以展开为：
$$\left[\begin{array}{c}{A}_x\\ A_y\\ A_z\end{array}\right]=\left[\begin{array}{lll}{S}_{ax}& M_{axy}& M_{axz}\\ M_{ayx}& S_{ay}& M_{ayz}\\ M_{azx}& M_{azy}& S_{az}\end{array}\right]\left[\begin{array}{l}{a}_x\\ a_y\\ a_z\end{array}\right]+\left[\begin{array}{l}{b}_{ax}\\ b_{ay}\\ b_{gz}\end{array}\right]$$

3.2 惯性器件内参误差标定

3.2.1 标定方法概述

标定的本质是参数辨识，参数包括陀螺仪和加速度计各自的零偏、刻度系数误差、安装误差。
辨识方法包括：
1）解析法或最小二乘
2）迭代优化方法
3）滤波（Kalman等）
常见标定方法与上面辨识方法的对应关系为：

1. 基于转台的标定：解析法、最小二乘；
2. 不需要转台的标定：梯度下降迭代优化；
3. 系统级标定：Kalman 滤波(该方法只适用于高精度惯导，而 MEMS 本身就属于精度不高的一类，这里不做讲解)。

3.2.2 基于转台的标定

在IMU的误差模型中，陀螺仪和加速度计的误差方程是互相独立的，可分别标定。

以加速度计为例，其误差模型方程为：
$$\left[\begin{array}{c}{A}_x\\ A_y\\ A_z\end{array}\right]=\left[\begin{array}{lll}{S}_{ax}& M_{axy}& M_{axz}\\ M_{ayx}& S_{ay}& M_{ayz}\\ M_{azx}& M_{azy}& S_{az}\end{array}\right]\left[\begin{array}{l}{a}_x\\ a_y\\ a_z\end{array}\right]+\left[\begin{array}{l}{b}_{ax}\\ b_{ay}\\ b_{az}\end{array}\right]$$左边 $A_x,A_y,A_z$ 是测量出来的，肯定是已知的；$a_x,a_y,a_z$是输入，标定时也是已知的。误差模型方程是一个包含12个未知参数的方程组，显然方程组没有唯一解。此时，通过改变输入，获得多个不同方程 (大于$12$个)，组成的方程组便可求解参数。

以上就是分立级标定方法的思路，具体求解方法包括解析法和最小二乘法。

该标定方法的核心：通过旋转 IMU，改变其输入构造方程组，并且每个位置对应的加速度输入和角速度输入都必须是已知的。

构建方程组时，不仅要方程组数量足够，而且要能够使误差参数可解，即系数矩阵可逆。

为了满足这一点，常见的转位方案有六位置、八位置、十二位置等。

在实际使用时，通过判断系数矩阵是否满秩便可判断，理论上，只要转位方案能满足这一条件，就可以使用。

3.2.2.1 加速度计标定

3.2.2.1.1 解析法：

当 IMU 水平向上放置时，得：
$$\{\begin{array}{l}{a}_x=0\\ a_y=0\\ a_z=g\end{array}$$其中， $\mathrm{g}$ 为重力加速度。带入加速度计误差模型，
$$\left[\begin{array}{c}{A}_x\\ A_y\\ A_z\end{array}\right]=\left[\begin{array}{lll}{S}_{ax}& M_{axy}& M_{axz}\\ M_{ayx}& S_{ay}& M_{ayz}\\ m_{azx}& M_{azy}& S_{az}\end{array}\right]\left[\begin{array}{l}{a}_x\\ a_y\\ a_z\end{array}\right]+\left[\begin{array}{l}{b}_{ax}\\ b_{ay}\\ b_{az}\end{array}\right]$$可得：
$$\{\begin{array}{l}{A}_x=M_{axz}\ast g+b_{ax}\\ A_y=M_{ayz}\ast g+b_{ay}\\ A_z=S_{az}\ast g+b_{az}\end{array}$$同理，当 IMU 水平向下放置时，得：
$$\{\begin{array}{l}{A}_x^{\prime }=-M_{axz}\ast g+b_{ax}\\ A_y^{\prime }=-M_{ayz}\ast g+b_{ay}\\ A_z^{\prime }=-S_{az}\ast g+b_{az}\end{array}$$联立这两个方程组，便可解出 6 个参数。随后，再次改变 IMU 放置方式，可解其他参数。
并且，由此可以看出，转台需要调平。

3.2.2.1.2 最小二乘法：

加速度计误差模型：
$$\left[\begin{array}{c}{A}_x\\ A_y\\ A_z\end{array}\right]=\left[\begin{array}{lll}{S}_{ax}& M_{axy}& M_{axz}\\ M_{ayx}& S_{ay}& M_{ayz}\\ M_{azx}& M_{azy}& S_{az}\end{array}\right]\left[\begin{array}{l}{a}_x\\ a_y\\ a_z\end{array}\right]+\left[\begin{array}{l}{b}_{ax}\\ b_{ay}\\ b_{az}\end{array}\right]$$可得到：
$$\left[\begin{array}{c}{A}_x\\ A_y\\ A_z\end{array}\right]=x\left[\begin{array}{c}{S}_{ax}\\ S_{ay}\\ S_{az}\\ M_{axy}\\ M_{axz}\\ M_{ayx}\\ M_{ayz}\\ M_{azx}\\ M_{azy}\\ {\nabla }_x\\ {\nabla }_y\\ {\nabla }_z\end{array}\right]\to y=x\theta$$其中：
$$\begin{array}{rl}x& =\left[\begin{array}{ll}F& I_{3\times 3}\end{array}\right]\\ F& =\left[\begin{array}{ccccccccc}{a}_x& 0& 0& a_y& a_z& 0& 0& 0& 0\\ 0& a_y& 0& 0& 0& a_x& a_z& 0& 0\\ 0& 0& a_z& 0& 0& 0& 0& a_x& a_y\end{array}\right]\end{array}$$转台在每个位置都可以得到一个方程：$y_i=x_i\theta$

所有位置对应的方程联立可得：$Y=X\theta$

其中 $Y={\left[\begin{array}{llll}{y}_0^T& y_1^T& \dots & y_n^T\end{array}\right]}^{T}X={\left[\begin{array}{llll}{x}_0^T& x_1^T& \dots & x_n^T\end{array}\right]}^T$

经过变形，标定问题变为线性拟合问题，当第 $i$ 次把 IMU 朝不同方向放置后，便得到一个方程组。

参数拟合问题等效为最小二乘问题，其解为：$\theta ={\left(X^{T}X\right)}^{-1}{X}^{T}Y$(最小二乘通解)，由此便得到标定参数。

3.2.2.2 陀螺仪标定：

3.2.2.2.1 方法思想

转台一般角速度不如角度精度高，因此不是直接以角速度作为真值，而是以积分得到的角度作为真值。

3.2.2.2.2 解析法

求解刻度系数和安装误差：
首先，计算输出与输入的关系(以绕IMU的 $z$ 轴逆时针旋转为例)
$$\left[\begin{array}{l}{W}_x\\ W_y\\ W_z\end{array}\right]=\left[\begin{array}{lll}{S}_{gx}& M_{gxy}& M_{gxz}\\ M_{gyx}& S_{gy}& M_{gyz}\\ M_{gzx}& M_{gzy}& S_{gz}\end{array}\right]\left[\begin{array}{c}0\\ 0\\ \omega \end{array}\right]+\left[\begin{array}{c}{b}_{gx}\\ b_{gy}\\ b_{gz}\end{array}\right]$$展开可得：
$$\{\begin{array}{l}{W}_x=M_{gxz}\ast \omega +b_{gx}\\ W_y=M_{gyz}\ast \omega +b_{gy}\\ W_z=S_{gz}\ast \omega +b_{gz}\end{array}$$对等式两侧进行积分：
$$\{\begin{array}{l}{\theta }_{W_x}=M_{gxz}\ast {\theta }_{\omega }+{\theta }_{b_{gx}}\\ {\theta }_{W_y}=M_{gyz}\ast {\theta }_{\omega }+{\theta }_{b_{gy}}\\ {\theta }_{W_z}=S_{gz}\ast {\theta }_{\omega }+{\theta }_{b_{gz}}\end{array}$$绕 IMU 的 $z$ 轴顺时针旋转时，同样方法可得：
$$\{\begin{array}{l}{\theta }_{W_x}^{\prime }=-M_{gxz}\ast {\theta }_{\omega }+{\theta }_{b_{gx}}\\ {\theta }_{W_y}^{\prime }=-M_{gyz}\ast {\theta }_{\omega }+{\theta }_{b_{gy}}\\ {\theta }_{W_z}^{\prime }=-S_{gz}\ast {\theta }_{\omega }+{\theta }_{b_{gz}}\end{array}$$正逆时针的两式可以求解出 $M_{gxz}{M}_{gyz}{S}_{qz}$
此处不通过两式求解零偏，因为旋转所用时间偏短，零偏造成的角度输出太小。

求解零偏：
当转台静止时，可以简单认为陀螺仪输出只有零偏，即(单纯放置着静止不动即可)：
$$\left[\begin{array}{l}{W}_x\\ W_y\\ W_z\end{array}\right]=\left[\begin{array}{l}{b}_{gx}\\ b_{gy}\\ b_{gz}\end{array}\right]$$此时采集一段时间内的数据，取平均值，即可得到零偏。
需要强调的是:
a. 有时标定需要考虑地球自转角速度的影响，此时模型比较复杂，可自行参考《惯性仪器测试与数据分析》第 10 章。
b. MEMS 陀螺仪的零偏重复性极差，因此每次上电都要在线估计零偏，因此离线标定时，零偏标与不标区别不大。

3.2.3 不需要转台的标定

使用转台的方法很好理解、精度也很高，但是问题在于转台很贵，有很多时候是没有转台的。这个时候输入是没有了的----没有很明确的三个轴的输入，但是还是有一个值可以用的，就是重力加速度。也就是说三个加速度计测量出的矢量的和的模，它的值必须和重力加速度值相等，这时候的加速度计才被认为是没有误差的，否则认为是有误差的。

3.2.3.1 整体思路

加速度输入(重力加速度)是已知的，已知值与测量值的差异作为残差，通过优化，估计内参。
参考资料：
论文：A Robust and Easy to Implement Method for IMU Calibration without External Equipments

3.2.3.2 内参模型

回顾基于转台的标定方法，我们定义了 $12$ 项安装误差，他们表示的是加速度计、陀螺仪的各个敏感轴与 IMU 的坐标轴（即直角坐标系 b 系）之间的关系，那这里有一个疑问，b系是怎么来的？或者说，为什么把它规定在现在这个方向，而不是别的方向？此处的意思是指，如果我把一个和现在的 b 系非常接近，只差 $0.1°$ 的一个直角坐标系规定为新的 b 系，有问题吗？好像没什么问题。也就是说 b 系是可以人为规定的。

在基于转台的标定方法里，IMU 的 b 系其实默认被规定成了和转台的坐标系重合，因为这样转台的输入，才真的是 IMU 的输入，上面的各种基于转台标定的模型和方法才成立。

而当标定方法脱离转台时，这种约束关系就不存在了，而 b 系又是可以认为规定的，那么就有一种规定方法，可以简化内参模型。

在定义坐标系时，若令 IMU 坐标系的 $X_b$ 轴与加速度计的 $X_a$ 轴重合，且 $X_{b}O{Y}_b$ 与 $X_{a}O{Y}_a$ 共面(如下图)，则加速度计的安装误差只剩下三个参数($z$ 不共面，所以有两个参数)。(按照定义方式不同，有些模型中会表示成上三角矩阵)
新的安装误差矩阵为 $M_a=\left[\begin{array}{ccc}0& 0& 0\\ M_{ayx}& 0& 0\\ M_{azx}& M_{azy}& 0\end{array}\right]$
此时，加速度计内参的待估参数为
$${\mathbf{\theta }}^{acc}=\left[M_{ayx},M_{azx},M_{azy},S_{ax},S_{ay},S_{az},b_{ax},b_{ay},b_{az}\right]$$陀螺仪的误差模型保持不变，但此处并没有估计陀螺仪零偏，因此陀螺仪内参的待估参数为(因为坐标系定的是和IMU坐标系一致，不能说再定义一个陀螺仪的)
$${\mathbf{\theta }}^{gyro}=\left[M_{gxy},M_{gxz},M_{gyx},M_{gyz},M_{gzx},M_{gzy},S_{gx},S_{gy},S_{gz}\right]$$

3.2.3.3 优化模型–估计加速度计内参

按照内参定义，加速度计输出与输入的关系为：
$$A=S_a\left(I+M_a\right)a+b_a$$即由测量值可以得到真实值为(已知的真实值为 $g$ 以及测量值 $A$)
$$a={\left(I+M_a\right)}^{-1}{S}_a^{-1}\left(A-b_a\right)$$在求解时，逆运算的存在使模型变得复杂，因此使用以下方式进行简化(对角阵的逆的性质，为对角的倒数)
$$S_a^{\prime }=\left[\begin{array}{lll}{S}_{ax}^{\prime }& & \\ & S_{ay}^{\prime }& \\ & & S_{az}^{\prime }\end{array}\right]=S_a^{-1}=\left[\begin{array}{ccc}\frac{1}{S_{ax}}& & \\ & \frac{1}{S_{ay}}& \\ & & \frac{1}{S_{az}}\end{array}\right]$$当 $M_a$ 是小量时，满足关系${\left(I+M_a\right)}^{-1}\approx I-M_a$，因为$(I-M_a)(I+M_a)\approx I$
$$\begin{array}{rl}a& \approx \left(I-M_a\right)S_a^{\prime }\left(A-b_a\right)\end{array}$$当imu静止时，输入只有重力加速度。

把加速度计矢量定义为 $\mathbf{g}={\left[\begin{array}{lll}0& 0& g_0\end{array}\right]}^T$ ，其中 $g_0$ 为当地重力大小。

当内参完全准确时，有 $\Vert \mathbf{g}{\Vert }^2=\Vert \mathbf{a}{\Vert }^2$

当内参存在误差时，可写出残差函数为：
$$f\left({\theta }^{acc}\right)=\Vert \mathbf{g}{\Vert }^2-\Vert \mathbf{a}{\Vert }^2$$根据高斯牛顿的流程，有此残差函数便可以推导雅可比，通过优化求解出内参。

需要注意的是，一个静止位置的测量不能完全求解出参数，需要按不同的姿态，在多个位置静止(如下图黑色曲线所包含的时间段)，所有位置的测量放在同一个优化任务中，才能求解全部参数。(必须静止的时候才能建方程，不然加速度$\Vert \mathbf{g}{\Vert }^2-\Vert \mathbf{a}{\Vert }^2$是不相等的，图中三个彩色的轴是加速度计的三个输出。因为只有静止不动的时候才是能建方程的，这里通过黑框将静止不动的东西圈出来了。)

3.2.3.4 优化模型–估计陀螺仪内参

陀螺仪内参估计在加速度计标定完成后进行，这时可以认为此时加速度计无误差。(使用加速度计标陀螺仪)另外，在这种方法中，我们并不使用优化的方式标定陀螺仪的零偏，主要原因还是因为零偏造成的影响偏小，标不准。而且零偏的标定，可以使用前述静止的方法去求解，简单易行。

令 ${\mathbf{u}}_{a,k}$ 代表在第 $\mathrm{k}$ 个静止位置时，三个加速度计的输出构成的矢量在IMU坐标系下的表示（当前位置下加速度计的输出），即：
$${\mathbf{u}}_{a,k}=R_{b_{k}w}\mathbf{g}$$其中 $R_{b_{k}w}$ 表示从世界坐标系(w系，和水平面平行且不随IMU旋转而旋转的坐标系)到第 $\mathrm{k}$ 个位置对应的IMU坐标系的转换矩阵。

注意：并不需要已知 $R_{b_{k}w}$，因为 ${\mathbf{u}}_{a,k}$ 是直接测量的。

在第 $k+1$ 个位置时，同样有：
$${\mathbf{u}}_{a,k+1}=R_{b_{k+1}w}\mathbf{g}$$从第 $\mathrm{k}$ 个位置，到第 $\mathrm{k}+1$ 个位置，可以根据陀螺仪测量计算出两个位置之间的相对旋转 $R_{b_{k+1}{b}_k}$，根据该旋转可以算出一个第 $\mathrm{k}+1$ 位置加速度计输出矢量的推测值：
$${\mathbf{u}}_{g,k+1}=R_{b_{k+1}{b}_k}{\mathbf{u}}_{a,k}$$可见，推测值的误差就体现了陀螺仪的误差，因此可以根据推测值与观测值构建残差函数：
$$f\left({\theta }^{\text{gyro }}\right)={\mathbf{u}}_{a,k+1}-{\mathbf{u}}_{g,k+1}$$雅可比推导需要用到IMU解算知识。

3.2.3.4 加速度计的雅克比

加速度计对应残差对加速度内参的雅克比推导如下：

• 已知量：
  残差：$r_a=\Vert g{\Vert }^2-\Vert a{\Vert }^2$，其中 $\mathbf{a}=\left(\mathbf{I}-{\mathbf{M}}_a\right){\mathbf{S}}_a^{\prime }\left(\mathbf{A}-{\mathbf{b}}_a\right)$
  ${\mathbf{M}}_a=\left[\begin{array}{ccc}0& 0& 0\\ M_{ayx}& 0& 0\\ M_{azx}& M_{azy}& 0\end{array}\right]{\mathbf{S}}_a^{\prime }=\left[\begin{array}{ccc}{S}_{ax}^{\prime }& 0& 0\\ 0& S_{ay}^{\prime }& 0\\ 0& 0& S_{az}^{\prime }\end{array}\right]{\mathbf{b}}_a=\left[\begin{array}{l}{b}_{ax}\\ b_{ay}\\ b_{az}\end{array}\right]$

• 待估计参数：
  $\mathbf{\theta }={\left[M_{ayx},M_{azx},M_{azy},S_{ax}^{\prime },S_{ay}^{\prime },S_{az}^{\prime },b_{ax},b_{ay},b_{az}\right]}^T$

• 求解：
  加速度向量可展开为：
  $\begin{array}{rl}\mathbf{a}& =\left[\begin{array}{ccc}1& 0& 0\\ -M_{ayx}& 1& 0\\ -M_{azx}& -M_{azy}& 1\end{array}\right]\left[\begin{array}{ccc}{S}_{ax}^{\prime }& 0& 0\\ 0& S_{ay}^{\prime }& 0\\ 0& 0& S_{az}^{\prime }\end{array}\right]\left[\begin{array}{l}{A}_x-b_{ax}\\ A_y-b_{ay}\\ A_z-b_{az}\end{array}\right]\\ & =\left[\begin{array}{ccc}{S}_{ax}^{\prime }& 0& 0\\ -M_{ayx}{S}_{ax}^{\prime }& S_{ay}^{\prime }& 0\\ -M_{azx}{S}_{ax}^{\prime }& -M_{azy}{S}_{ay}^{\prime }& S_{az}^{\prime }\end{array}\right]\left[\begin{array}{l}{A}_x-b_{ax}\\ A_y-b_{ay}\\ A_z-b_{az}\end{array}\right]\\ & =\left[\begin{array}{c}{S}_{ax}^{\prime }\left(A_x-b_{ax}\right)\\ -M_{azx}{S}_{ax}^{\prime }\left(A_x-b_{ax}\right)-M_{azy}{S}_{ay}^{\prime }\left(A_y-b_{ay}\right)+S_{az}\left(A_z-b_{az}\right)\end{array}\right]\end{array}$

  根据链式求导分解为：$\frac{d{r}_a}{d\mathbf{\theta }}=\frac{d{r}_a}{d\mathbf{a}}\frac{d\mathbf{a}}{d\mathbf{\theta }}$
  其中：
  $\frac{d{r}_a}{d\mathbf{a}}=-2{\mathbf{a}}^T$
  $\begin{array}{l}\frac{d\mathbf{a}}{d{\mathbf{\theta }}_{123}}=\left[\begin{array}{ccc}0& 0& 0\\ -S_{ax}^{\prime }\left(A_x-b_{ax}\right)& 0& 0\\ 0& -S_{ax}^{\prime }\left(A_x-b_{ax}\right)& -S_{ay}^{\prime }\left(A_y-b_{ay}\right)\end{array}\right]\\ \frac{d\mathbf{a}}{d{\mathbf{\theta }}_{456}}=\left[\begin{array}{ccc}\left(A_x-b_{ax}\right)& 0& 0\\ -M_{ayx}\left(A_x-b_{ax}\right)& \left(A_y-b_{ay}\right)& 0\\ -M_{azx}\left(A_x-b_{ax}\right)& -M_{azy}\left(A_y-b_{ay}\right)& \left(A_z-b_{az}\right)\end{array}\right]\\ \frac{d\mathbf{a}}{d{\mathbf{\theta }}_{789}}=\left[\begin{array}{ccc}-S_{ax}^{\prime }& 0& 0\\ M_{ayx}{S}_{ax}^{\prime }& -S_{ay}^{\prime }& 0\\ M_{azx}{S}_{ax}^{\prime }& M_{azy}{S}_{ay}^{\prime }& -S_{az}\end{array}\right]\end{array}$

3.2.3.5 标定方法比较

1. 基于转台的标定精度较高，但标定成本高；
2. 不依赖转台的标定精度差，但成本低、效率高，对一般 MEMS 的标定需求已经足够。

4. 惯性器件温补

现在来说一说惯性器件的温补， 温补的影响很大，但是很多时候不需要自己做，因为厂商给做好了，买了厂商的东西，输出结果自然是给补偿好的。

温补的本质是系统辨识，既要找出合适的物理模型，又要识别物理模型的参数。(说白了就是不同温度能得到不同的bias，问题就在于，怎样得到这个bias)

理论上讲，对于MEMS，静止情况下，陀螺的输出本身就是bias，这里不做区分。只是说在高精度的情况下，静止情况下的输出还包含别的因素，所以它不等于bias，但是对于低精度来讲无所谓，可以当成bias来看待。

4.1 物理模型辨识

和温度相关的变量为 $T$ 和 $\Delta T$ ，分别代表温度、变温率(表示温度变化的快慢)(注意，不仅和温度点相关，还跟温度变化的速度是相关的)，温补要做的是识别出器件 bias(B)和这两者的关系：
$$B=f(T,\Delta T)$$而 $f$ 的具体表达多数是靠尝试。

常见的模型为：
$$\begin{array}{rl}B=& a\ast T^3+b\ast T^2+c\ast T+\\ & e\ast \Delta T^2+f\ast \Delta T+\\ & g\ast T\ast \Delta T+h\end{array}$$实际使用时，可根据情况在此基础上做减法，去掉一些高阶项。(曲线是二次三次四次都有可能)

4.2 参数辨识

在选定的物理模型基础上做最小二乘曲线拟合，与分立级标定时所用最小二乘原理相同。

4.3 其他改进方法

1. 分段拟合
   bias随温度变化曲线多是不规则曲线，无法用一个完整的曲线模型做拟合。
   常见的方法是按照温度把曲线分成多个区间，每个区间单独拟合一个模型。

2. 基于神经网络
   温补最大的问题是物理模型未知，而神经网络不需要已知物理模型，理论上比较合理。
   但是实际使用中，由于很多处理器只是简单的嵌入式板子，运算能力有限，而且多项式方法已经能解决大部分问题，因此这种方法在实际使用中用的不多。(文献有，实际用的不多)

4.4 关于温补的讨论

1. 温补在器件误差补偿中是最重要的，但也是最“没有技术含量”的。
2. 温变的本质是和器件整体温度场相关，而不只是和局部温度点相关，但温度传感器只能测量后者。
3. 永远无法找到完全准确的温补模型，但是却能知道什么是够用的。自动控制领域有一句名言 “All models are wrong, but some are useful“。