IMU内参标定

参考文献：A Robust and Easy to Implement Method for IMU Calibration without
External Equipments

标定方法

首先静止IMU一段时间，作为IMU的初始化，之后改变IMU的位姿并静止一段时间，重复这个步骤一定此时，完成标定数据的采样，最后由标定算法计算出IMU标定参数。

IMU误差模型

由于安装精度，IMU中无法保证三个线加速度传感器或者角速度传感器之间成两两垂直的几何关系，因此产生错位误差，IMU需要将原始测量数据转换为真实的物理量，在转换过程中会产生尺度误差，同时会产生零偏误差。可以使用矩阵描述误差模型，也就是IMU原始测量数据加上零偏误差和随机误差，在左乘尺度误差矩阵和错误误差矩阵，最终获得IMU测量数据。

标定方法

在标定过程中，获得了一组组不同位姿并且静止下的IMU输出数据，根据这些数据使用标定算法完成IMU的标定，标定算法分为静态区间检测、线加速度标定、角速度标定。
因此首先需要准确检测标定数据中的静止区间，获得不同位姿静止条件下对应的IMU输出数据，然后标定线加速度，构建非线性最小二乘方法，代价函数是IMU线加速度的大小与重力加速度常数的差值，通过迭代计算得到线加速度标定参数。然后根据完成标定的线加速度数据，根据两两静态位姿之间的角速度数据，计算旋转矩阵，将当前静止区间的下加速度根据这个旋转矩阵变换到下一静止区间，与下一静止区间线加速度的差值，作为代价函数，最终求得角速度标定参数。

静态区间检测方法

静态区间的阈值对静态区间检测存在很大的影响，首先在计算静止初始化阶段内数据的方差，取静态初始化方差的不同倍数，作为静态区间检测的阈值。然后根据一定大小的滑动窗口，对某一采样数据，计算它滑动窗口内数据的方差的大小。如果某数据的滑动窗口方程小于阈值，就作为静态区间的左端点，大于阈值旧作为静态区间的右端点。最终获取静态区间。

线加速度标定

对每个参数生成的静态区间，计算线加速度标定参数，最后取最优的标定参数。
取每个静态区间中线加速度数据的均值，消除随机误差的影响，构建非线性最小二乘问题，参数块是错位参数、尺度参数、零偏参数，代价函数是各个静态区间加速度均值大小与重力加速度常数之间的差值，最终通过非线性优化，计算出线加速度的标定参数。

角速度标定

首先计算初始化静止状态下角速度数据的均值，作为角速度的零偏，获得去除零偏后的角速度数据。构建非线性最小二乘问题，参数块是错位参数、尺度参数、零偏参数，角速度标定需要使用完成标定后的线加速度数据，通过两两静态位姿之间的角速度数据，计算得到两两位姿之间的旋转矩阵，这样就可以通过旋转矩阵和当前静态区间的线加速度，预测下一静态区间的线加速度，代价函数就是所有两两静态区间之间预测线加速度与IMU线加速度之间的差，通过非线性优化，最终计算出角速度标定参数。

allan方差描述角速度去除零偏

龙格库塔法对角速度进行差值获得旋转矩阵