目录
1. IMU的简单介绍
- IMU(Inertial measurement unit)中文名叫惯性测量单元。
- 可以测量两个东西,加速度 a a a 是沿三个轴 a x , a y , a z a_x,a_y,a_z ax,ay,az方向的线加速度;角速度 w w w就是这三个方向的角速度 w x , w y , w z w_x,w_y,w_z wx,wy,wz.
- IMU的频率比较高,一般都在100HZ以上。
- 在IMU内部,除了通常的白噪声,还有个特别的量零偏bias,在这是传感器内部机械、温度等各种物理因素产生的传感器内部误差的综合参数。
2. IMU的状态PVQ
IMU每个时刻可实现自身主体加速度 a a a 和角速度 w w w 的测量,那么有测量模型:
可以从第
i
i
i时刻的PVQ,通过对IMU的测试值进行积分,得到第
j
j
j时刻的PVQ:
3. IMU预积分
3.1 推导
问题: 每次
q
w
b
t
q _{wb_t}
qwbt 优化更新后,都需要重新进行积分,运算量较大。
解决办法:
通过下面的公式转换,可以将积分模型转为预积分模型:
那么,PVQ 积分公式中的积分项则变成相对于第 i 时刻的姿态,而不是相对于世界坐标系的姿态:
预积分量仅仅跟 IMU 测量值有关,它将一段时间内的 IMU 数据直接积分起来就得到了预积分量:
重新整理 PVQ 的积分公式得:
这里有一个前提假设,就是我们认为零偏
b
i
b_i
bi是已知的,即:
3.2 预积分的离散形式
使用中值法,即两个相邻时刻 k 到 k+1 的位姿是用两个时刻的测量值 a, ω 的平均值来计算:
4. IMU的预积分误差
定义: 一段时间内IMU 构建的预积分量作为测量值,对两时刻之间的状态量进行约束,
5. 预积分量的协方差
eg.
所以,要推导预积分量的协方差,我们需要知道 imu 噪声和预积分量之间的线性递推关系。
假设相邻时刻误差的线性传递方程如下,误差的传递由两部分组成:当前时刻的误差传递给下一时刻,当前时刻测量噪声传递给下一时刻:
协方差矩阵可以通过递推计算得到:
其中, Σ n Σ n Σn 是测量噪声的协方差矩阵,方差从 i 时刻开始进行递推, Σ i i Σ ii Σii = 0。
6. 状态误差线性递推公式推导_基于一阶泰勒展开
得到预积分误差传递的形式,
表示k+1时刻body系下,预积分的真实值与估计值之间的差异值。
F和G为两个时刻间的协方差传递矩阵,具体为:
其中的系数为,