imu使用与卡尔曼融合定位

一、imu使用流程：

1、误差标定：
  imu误差包括确定误差和随机误差。
  确定误差包括：轴偏角误差、比例因子、零偏等。可使用imu_tk工具进行标定。
  随机误差：噪声等。可使用imu_utils工具进行标定。

2、选择实验场地，确定imu初始参数：
  初始参数包括：初始位置，初始速度，初始姿态（欧拉角）。如果0时刻载体坐标系与导航坐标系重合，且imu静止摆正，则上述都为0。
  通俗的讲，就是在实验场地建立一个xyz坐标系，确定一个原点，则这个坐标系即为导航坐标系。如果实验开始时imu设备在原点，且imu设备自身的xyz轴与实验场地xyz轴重合。则imu的上述参数都为0。

3、测量：
  做好误差标定和参数初始化后，即可运行imu进行测量。注意imu测量值遵循右手法则。即imu绕自身某一轴逆时针旋转，则输出为正。

4、角速度数据处理：
  imu测量结果包括角速度和加速度数据，以及帧数信息。通常先处理角速度数据。其处理流程包括：
  利用标定误差参数，校正角速度测量误差——>利用角速度计算欧拉角速度——>配合帧数信息，积分得到欧拉角（注意角度范围）——>利用四元数转换公式，计算各帧的四元数。

注意：
  （1）欧拉角速度以导航坐标系为参考，用于姿态更新。imu三轴角速度以载体坐标系为参考，是已知测量值。因此，欧拉角速度与imu三轴角速度不相等，且存在转换关系。
  （2）计算欧拉角仅用于观察imu运行姿态，后续Cnb计算使用四元数。一是因为欧拉角有死锁，四元数没有；二是因为四元数计算量更小。

5、加速度数据处理：
  加速度测量值是基于imu自身坐标系，我们要的是导航坐标系下的加速度值。所以对于每一帧的加速度数据，都要先计算当前帧的坐标变换矩阵Cnb。完整的加速度数据处理流程如下：
  利用标定误差参数，校正加速度误差——>利用各帧的四元数，计算各帧下的坐标变换矩阵Cnb——>计算Cnb * [ax，ay，az]T，得到导航系下的加速度值——>消去重力加速度影响：[ax，ay，az]T - [0，0，g]T——>配合帧数信息，积分得到imu在导航坐标系下的位置信息（建议中值积分，减小积分误差）。

二、卡尔曼融合定位

  imu通常不会单独使用，误差累积的性质使得imu必须和其他传感器融合定位。而常用的融合定位算法为卡尔曼融合。

  imu在卡尔曼里面的角色定位是系统估计，其他传感器作测量值，用来校正imu的估计。而imu虽有误差累积的缺点，但在短时间内，imu的精度仍然较高。

  imu的累积误差有两方面，一是角速度的测量误差，导致积分计算姿态时，存在误差累积。二是加速度测量误差与姿态累积误差，在导航坐标系下的加速度中结合，导致加速度积分求位置时，进一步发生误差累积。

  因此imu与某传感器融合后，最好能将imu的位置与姿态信息同时纠正。如果仅纠正imu的位置，而不管imu的姿态信息，则imu仍存在误差累积，这会影响到导航系下的加速度值，后期imu位置可能发生大幅偏移。

  然而，即便imu后期发生大幅偏移，只要推算出imu的估计误差，且另一传感器测量精度一定，则它们融合后的定位精度仍然可观。由于卡尔曼遵循贝叶斯定律，两个围绕真值的独立高斯分布融合后，新的分布比两者的方差都要小，即相对真值的误差分布更小。但通常情况下不建议这样做，如果某传感器无法直接修正imu的姿态信息，则可以考虑其他方法，如零速修正。综合传感器和imu角速度、加速度的输出，确定imu的运动状态。在静止状态下，利用加速度值修正imu的pitch和roll，也可获得良好效果。

  最后关于卡尔曼收敛的理解：先验估计和测量值在后验估计的权重占比，最终由先验误差和测量误差决定。与初始后验方差无关。

  举个例子，如果先验估计误差小，测量误差大。则后验估计中，先验估计的权重理应占比大。如果此时将初始后验方差设置较大，则起始一段时间内，初始后验方差会影响先验估计在后验估计的占比。但随着后验方差的迭代，其值逐渐变小。最终后验估计的占比，还是由先验估计误差和测量误差决定。