最近看了深蓝学院VIO课程, 学到了很多， 现将课程内容总结如下.

惯性测量单元(英语:Inertial measurement unit，简称IMU）测量物体三轴姿态角(或角速率)和加速度的装置，由陀螺仪和加速度计组成.

IMU的数学模型

IMU的误差模型

IMU误差包括确定性误差和随机误差，确定性误差包括：Bias Scale,非对齐误差等.

Bias:理论上，当没有外部作用时，IMU传感器的输出应为0。然而，实际数据存在偏置b。加速度计bias对位姿估计的影响：

verr=bat, perr=bat2/2

Scale:可视为实际值与传感器输出值的比值。

非对齐误差:多轴IMU在制作传感器时，由于制造过程的问题，会使xyz轴可能不垂直，如下图所示。

测量值= 尺度对齐矩阵x真实值 , 如下:

IMU随机误差包括高斯白噪声bias随机游走等.

高斯白噪声:IMU数据连续时间的平均值0，方差为 σ，高斯过程在各个时刻相互独立n(t).

Bias随机游走:通常用维纳过程(wiener process)来建模bias随时间不断变化的过程称为随机游走。

数学模型的加速度计

导航系统G是东北天(ENU)，g = (0, 0,?9.理论测量值为：

其中amB测量加速度值，aG该惯性系统下的估计值.假考虑高斯白噪声na，bias以及尺度因子Sa，则为：

由于单位矩阵，通常假设尺度因素。

陀螺仪的数学模型

考虑尺度因子、高斯白噪声和bias,陀螺仪误差模型如下：

考虑加速度对陀螺仪的影响，即低端传感器g-灵敏度：

陀螺仪受四种噪声的影响如下图所示：

运动模型

运动模型的连续积分

忽略scale只考虑白噪声和bias随机游走：

上标g表示gyro，a表示acc，w代表世界坐标系world，b表示imu机体坐标系body。IMU的真实值为 ω, a,测量值为 ω?, a?。P(ose),V(elocity),Q(uaternion)时间导数可以写成:

根据上述导数关系，可以从第一个开始i时刻的PVQ，通过对IMU积分测量值，得到第j时刻的PVQ:

j时间的位置等于i时间的位置，加上速度引起的位置变化，加上加速度引起的位置变化.

j时刻的速度等于i时刻的速度，加上加速度引起的速度变化.

Q变化量如上。

欧拉法运动模型的离散积分

使用欧拉法，即两个相邻的时刻k到k 1位置为第k时刻测量值a,ω 来计算。

其中，

运动模型的离散积分-中值法

使用mid-point方法，即两个相邻时刻k到k 1位置是两个时刻的测量值a,ω 计算平均值。

其中:

VIO

机器人定位有很多方案，其中IMU比较相机定位的优缺点：

可以看到,视觉和IMU定位方案具有一定的互补性：IMU适用于短时间、快速运动的计算；视觉适用于长时间、慢速运动的计算。视觉定位信息可以用来估计IMU零偏，减少IMU零偏差引起的发散和积累误差；相反，IMU能为视觉提供快速运动的定位。

IMU与视觉融合定位时，有两种方案：

松耦合:将IMU定位与视觉/视觉/定位与视觉/视觉/视觉/视觉/视觉/视觉/视觉/视觉/视觉/视觉/视觉/视觉/视觉/视觉/视觉/视觉/视觉/视觉/视觉/视觉/视觉/视觉/视觉/视觉/视觉/视觉/视觉/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视/视GNSS位置直接融合，融合过程不影响两者本身，作为后处理输出。典型的方案是卡尔曼滤波器。

紧耦合:融合过程本身会影响视觉和IMU参数(如IMU的零偏和视觉的尺度）。典型方案为MSCKF和非线性优化。

IMU预积分

IMU测量频率高，可达120Hz,相机的帧率一般只有30FPS,若考虑关键帧的化，频率更低.如下图所示：

同时,紧耦合VIO中对相机位置和IMU联合优化位置，导致与相机帧同时的时刻IMU测量值将不断优化.每次优化更新后，都需要重新进行积分，计算量大。

从第i时刻的PVQ对IMU的测量值进行积分得到第j时刻的PVQ:

以下简单公式将t时刻的积分模型转换为预积分模型：

则PVQ与第一个相比，积分公式中的积分项变成了i与世界坐标系相比，始终的姿态不是：

只跟随预积分量IMU测量值与一段时间内的测量值有关IMU直接积分数据得到预积分量，记下以下项目：

重新整理下PVQ有：

预积分误差:一段时间内IMU构建的预积分量作为测量值，约束两时之间的状态量：

上述误差中的位移、速度和偏置直接减少。第二个是四元数的旋转误差，其中[·]xyz表示只取四元数的虚拟 (x, y, z) 组成的三维向量。

预积分的离散形式: 这里使用 mid-point 方法，即两个相邻时刻 k 到 k+1 的位姿是用两个时刻的测量值 a, ω 的平均值来计算：

预积分量的方差

    假设已知了相邻时刻误差的线性传递方程：

状态量误差为 ηik = [δθik, δvik, δpik]，测量噪声为nk = [ngk, nak]。误差的传递由两部分组成：当前时刻的误差传递给下一时刻，当前时刻测量噪声传递给下一时刻。

协方差矩阵可以通过递推计算得到：

其中，Σn 是测量噪声的协方差矩阵，方差从 i 时刻开始进行递推，Σii = 0。

状态误差线性递推公式的推导

通常对于状态量之间的递推关系是非线性的方程如 xk = f(xk-1, uk-1)，其中状态量为 x，u 为系统的输入量。我们可以用两种方法来推导状态误差传递的线性递推关系：

    • 一种是基于一阶泰勒展开的误差递推方程。

    • 一种是基于误差随时间变化的递推方程。

基于一阶泰勒展开的误差递推方程

令状态量为 x = xˆ + δx，其中，真值为 xˆ，误差为 δx。另外，输入量 u 的噪声为 n。对非线性状态方程进行一阶泰勒展开有：

其中F 是状态量 xk 对状态量 xk-1 的雅克比矩阵，G 是状态量 xk 对输入量 uk-1 的雅克比矩阵。 则非线性系统 xk = f(xk-1, uk-1) 的状态误差的线性递推关系如下：

    用前面一阶泰勒展开的推导方式，我们希望能推导出如下的形式：

F, G 为两个时刻间的协方差传递矩阵, 这里我们直接给出 F, G 的最终形式:

其中: