版權宣告：本文為博主原創文章，未經博主允許不得轉載。

博主：shenshikexmu

聯絡方式：[email protected]

緣起

有緣在簡極科技兼職兩年。接觸了IMU，我去的時候那家公司還是一個要把IMU放進足球的公司，祝願簡極越來越好。IMU校正演算法大概是接觸感測器三個月做出來的，部落格IMU加速度、磁力計校正－－橢球擬合的內容，那時只是把校準問題當作橢球擬合問題。融合演算法大概是接觸IMU一年做出來的，中途學習了kalman濾波。在安卓上的實時演算法一年半完成，視訊。
IMUCalibration-Gesture換了一個更好的校正演算法，濾波演算法不變。願和大家一起學習並討論。有些朋友郵件上問我問題，真的給我很大的鼓勵。

校正

這邊有一篇講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

MPU9250
$Tm2a=\begin{bmatrix} T_{11} & T_{12} &T_{13} \\T_{21} & T_{22}&T_{23} \\T_{31} &T_{32}&-1\end{bmatrix}$

其他，是1還是-1，這個要看感測器資料了 。
$Tm2a=\begin{bmatrix}T_{11} & T_{12} &T_{13} \\T_{21} & T_{22}&T_{23} \\T_{31} &T_{32}&1\end{bmatrix}$

引數

$cal_{acc}=Ta*Ka*(raw_{acc}+Ba)$
$cal_{gyro}=Tg*Kg*(raw_{gyro}+Bg)$
$cal_{mag}=Tm2a*(raw_{mag}+Bm)$

姿態

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上標星星。