imu_tk源码分析

前言

imu_tk是一个开源的用于低精度IMU标定的代码，github地址为：imu_tk，主要用了非线性优化方法做加速度计和陀螺仪的标定，标定参数包括bias（3个），标度因子（3个），非正交安装误差（3个），一共12个参数。

算法思路简介

详细的算法思路可以参考它给出的两篇论文，加速度计的标定利用了加速度计在静止状态下的三轴数据模值等于重力加速度这一条件，采集加速度计在不同状态的静止数据，通过优化算法即可求出标定参数。加速度计单独即可完成标定，陀螺仪的标定需要加速度计数据的参与，所以需要先标定好加速度计。在标定陀螺仪时，IMU从静止状态A，通过一段时间，到了静止状态B，通过陀螺仪可以算出来TA→BT_{A \rightarrow B}TA→B的旋转矩阵，而通过加速度数据也可以算出来TA→BT_{A \rightarrow B}TA→B的旋转矩阵，最小化这一差异即可实现优化求解。

代码解读

test_imu_calib

importAsciiData：通过函数读入.mat格式的文件，加速度数据和陀螺仪数据是分开读取的。mu_tk::TIMESTAMP_UNIT_SEC是在imu_tk/io_utils.h文件中定义的一个枚举类型，可以实现不同时间单位之间的转换，TIMESTAMP_UNIT_SEC = 1，即为转换为秒。这里要注意数据的单位。
setBias：加速度计设定了初始的bias。
setScale：陀螺仪设定了初始的标度因子。
setInitStaticIntervalDuration：采集的数据持续的时间，可以方便选择部分时间用于IMU参数的标定。
setGravityMagnitude：设置重力加速度数值，不同地方的重力加速度是不同的，这块可能需要改下，默认设置的数值是9.81744。
enableVerboseOutput：相当于一个debug开关，打开后会输出更多的调试信息，方便调试。
enableAccUseMeans：函数默认设置为否，意思是把所有的加速度静止段数据作为优化函数的输入，而不是每一静止段的平均值。
calibrateAccGyro：主要的代码思路在这个函数中。

calibrateAccGyro函数

calibrateAcc函数是加速度计的标定。

initialInterval：根据设定的时间，找到对应的最后一个数据的索引，这里在寻找过程中用到了二分法。
dataVariance：求整个时间段的数据的方差，此方差是作为判断IMU静止状态时的一个阈值使用，再一个这个方差是所有数据的方差。
staticIntervalsDetector：这是个静止状态监测器，利用数据方差（三个数据方差的模值）作为判断静止与否的阈值条件，对于第i个数据，通过计算其左领域，右临域共win_size大小的数据方差， win_size是函数输入参数，默认大小为101，按照100Hz的输出数据速率，大约为1s的数据。之后得到静止段数据所对应的开始索引和结束索引。
extractIntervalsSamples：该函数是找到静止区域索引所对应的惯导数据，静止区域的数据个数要大于interval_n_samps，也就是1s。如果使用平均值的模式，就用时间段的中间的时间戳作为这个平均值的时间戳。
注意以下代码

    if(verbose_output_)cout<<"Accelerometers calibration: extracted "<<extracted_intervals.size()<<" intervals using threshold multiplier "<<th_mult<<" -> ";// TODO Perform here a quality testif( extracted_intervals.size() < min_num_intervals_) //至少是12个数据(有12个参数需要标定){if( verbose_output_) cout<<"Not enough intervals, calibration is not possible"<<endl;continue;}

min_num_intervals_设定的是静止区域的个数，静止区域的个数要不小于待标定的参数的个数，对于加速度计，待标定参数个数是9个，如果更改标定模型，参数min_num_intervals_可能需要改变。
接下来就是用ceres优化库，去解待标定参数，这里比较关键的是构建优化函数。

calibration函数

h文件中描述了加速度计和陀螺仪采用的标定模型。

参考论文

D. Tedaldi, A. Pretto and E. Menegatti, “A Robust and Easy to Implement Method for IMU Calibration without External Equipments”. In: Proceedings of the IEEE International Conference on Robotics and Automation (ICRA 2014), May 31 - June 7, 2014 Hong Kong, China, Page(s): 3042 - 3049
A. Pretto and G. Grisetti, “Calibration and performance evaluation of low-cost IMUs”. In Proceedings of the 20th IMEKO TC4 International Symposium, Sep. 15 - 17, 2014 Benevento, Italy, pages: 429 - 434