IMU校正以及姿态融合[通俗易懂]

大家好，又见面了，我是你们的朋友全栈君。

本文为博主“声时刻”原创文章，未经博主允许不得转载。
联系方式：shenshikexmu@163.com

缘起
有缘在简极科技兼职两年。接触了IMU，我去的时候那家公司还是一个要把IMU放进足球的公司，祝愿简极越来越好。IMU校正算法大概是接触传感器三个月做出来的，博客IMU加速度、磁力计校正－－椭球拟合的内容，那时只是把校准问题当作椭球拟合问题。融合算法大概是接触IMU一年做出来的，中途学习了kalman滤波。在安卓上的实时算法一年半完成，视频。
IMUCalibration-Gesture换了一个更好的校正算法，滤波算法不变。愿和大家一起学习并讨论。有些朋友邮件上问我问题，真的给我很大的鼓励。
2021年4月，我自己的论文TII《Low-cost IMU Calibration with Nonlinear Scale Factors》。

校正
这边有一篇讲IMU误差的很好的博客，直接引用了，IMU误差模型和校准。也是在《A Robust and Easy to implement method for imu calibration without External Equipments》框架下解释误差的。

图源于IMU误差模型和校准。
加速度
参照ICRA2014论文：《A Robust and Easy to implement method for imu calibration without External Equipments》
说明
摘数据的部分我没有按照ICRA2014的算法写，而是自己写了FindFixData这样一个函数。ICRA2014的摘数据的算法我也拿matlab写了一遍，算方差实在是太花时间了（可能我的那个算法需要优化），FindFixData算法轻便一些，就是要人为的设置参数，ICRA2014不需要人为设置参数。摘出的稳定数据校正角速度参数和磁力计参数，摘出的运动的数据用来校正角速度传感器。
角速度
参照ICRA2014论文：《A Robust and Easy to implement method for im calibration without External Equipments》
磁力计
算法1：mag2acc_matrix，假设重力与磁向量的夹角不变。

算法2：Cal_mag4acc_frame，利用不同姿态下传感器感受的磁通向量的变化与姿态变化的相关性，计算参数。

说明
算法中的校正传感器为MPU9250，这个传感器的加速度坐标系与磁力计坐标系的z轴反向。在校正时，把变换矩阵的$T_{33}$位置设为-1，如果其他传感器z轴相同，$T_{33}$设为1.

MPU9250
$Tm2a=\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& -1\end{array}\right]$

其他，是1还是-1，这个要看传感器资料了 。
$Tm2a=\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& 1\end{array}\right]$
参数
$ca{l}_{acc}=Ta\ast Ka\ast (ra{w}_{acc}+Ba)$
$ca{l}_{gyro}=Tg\ast Kg\ast (ra{w}_{gyro}+Bg)$
$ca{l}_{mag}=Tm2a\ast (ra{w}_{mag}+Bm)$

LM算法

增加了我自己写的Levenberg-Marquardt算法，matlab自带的lsqnonlin函数被注释，可以选择注释我手写的LM算法，选择取消注释lsqnonlin函数。

姿态
Mahony filter
参考 《Nonlinear Complementery Filters on the Special Orthogonal Group》
受启发于 http://blog.csdn.net/luoshi006/article/details/51513580
EKF
参考 《A Double-Stage Kalman Filter for Orientation Tracking with an Integrated Processor in 9-D IMU》

High Low pass
Gyro进行高通滤波器，Accelerate & Magnetic进行低通滤波。

滤波结果

校正与融合

开源代码
具体请参考github开源代码：IMUCalibration-Gesture.

如果您觉得此文对您所要做的工作有帮助，欢迎在github上标星星。

欢迎在“知识星球”更深入地探讨问题。