加速度計和陀螺儀都能計算出姿態，但為何要對它們融合呢，是因為加速度計對振動之類的擾動很敏感，但長期資料計算出的姿態可信，而陀螺儀雖然對振動這些不敏感，但長期使用陀螺儀會出現漂移，因此我們要進行互補，短期相信陀螺，長期相信加計。不過，其實加計無法對航向角進行修正，修正航向角需要磁力計， 在融合之前先要對感測器原始資料進行一些處理。理想情況下，加速度計水平放置時，XY軸應該是0輸出的，僅Z軸輸出1個G，因此，我們需要對加速度計進行XY軸的零點校準；同樣的，陀螺儀在水平靜止放置時各軸輸出應為0，因此需對陀螺儀進行三軸的校準。方法就是把機體標準水平靜止放置時採集它個一兩百次資料求個平均作為校準值儲存起來，然後工作狀態下各軸輸出的資料就是採集來的資料減去校準值。僅此還不夠，陀螺儀不進行濾波還可以接受，但加速度計噪聲比較大，所以需要來個滑動視窗濾波，視窗深度為8-10即可。

接下來上六軸資料融合程式碼：（基礎上再加些註釋）

////////////////////////////////////////////////////////////////////////////////

#defineKp 10.0f                        // 這裡的KpKi是用於調整加速度計修正陀螺儀的速度

#defineKi 0.008f                        

#definehalfT 0.001f             // 取樣週期的一半，用於求解四元數微分方程時計算角增量

 

floatq0 = 1, q1 = 0, q2 = 0, q3 = 0;    // 初始姿態四元數，由上篇博文提到的變換四元數公式得來

floatexInt = 0, eyInt = 0, ezInt = 0;    //當前加計測得的重力加速度在三軸上的分量

                                //與用當前姿態計算得來的重力在三軸上的分量的誤差的積分

voidIMUupdate(float gx, float gy, float gz, float ax, float ay, float az)   //g表陀螺儀，   a表加速度計

{

  float q0temp,q1temp,q2temp,q3temp;//四元數暫存變數，求解微分方程時要用

  float norm; //向量的模或四元數的範數

  float vx, vy, vz;//當前姿態計算得來的重力在三軸上的分量

  float ex, ey, ez;//當前加計測得的重力加速度在三軸上的分量

              //與用當前姿態計算得來的重力在三軸上的分量的誤差

 

  // 先把這些用得到的值算好

  float q0q0 = q0*q0;

  float q0q1 = q0*q1;

  float q0q2 = q0*q2;

  float q1q1 = q1*q1;

  float q1q3 = q1*q3;

  float q2q2 = q2*q2;

  float q2q3 = q2*q3;

  float q3q3 = q3*q3;

        

  if(ax*ay*az==0)//加計處於自由落體狀態時不進行姿態解算，因為會產生分母無窮大的情況

        return;

                  

  norm = sqrt(ax*ax + ay*ay + az*az);//單位化加速度計，

  ax = ax /norm;// 這樣變更了量程也不需要修改KP引數，因為這裡歸一化了

  ay = ay / norm;

  az = az / norm;

 

  //用當前姿態計算出重力在三個軸上的分量，

  //參考座標n系轉化到載體座標b系的用四元數表示的方向餘弦矩陣第三列即是（博文二中有提到）

  vx = 2*(q1q3 - q0q2);                                                                                                             

  vy = 2*(q0q1 + q2q3);

  vz = q0q0 - q1q1 - q2q2 + q3q3 ;

 

  //計算測得的重力與計算得重力間的誤差，向量外積可以表示這一誤差

  //原因我理解是因為兩個向量是單位向量且sin0等於0

  //不過要是夾角是180度呢~這個還沒理解

  ex = (ay*vz - az*vy) ;                                                                  

  ey = (az*vx - ax*vz) ;

  ez = (ax*vy - ay*vx) ;

 

  exInt = exInt + ex * Ki;                                           //對誤差進行積分

  eyInt = eyInt + ey * Ki;

  ezInt = ezInt + ez * Ki;

 

  // adjusted gyroscope measurements

  gx = gx + Kp*ex + exInt;  //將誤差PI後補償到陀螺儀，即補償零點漂移

  gy = gy + Kp*ey + eyInt;

  gz = gz + Kp*ez + ezInt;    //這裡的gz由於沒有觀測者進行矯正會產生漂移，表現出來的就是積分自增或自減

 

  //下面進行姿態的更新，也就是四元數微分方程的求解

  q0temp=q0;//暫存當前值用於計算

  q1temp=q1;//網上傳的這份演算法大多沒有注意這個問題，在此補上

  q2temp=q2;

  q3temp=q3;

  //採用一階畢卡解法

  q0 = q0temp + (-q1temp*gx - q2temp*gy -q3temp*gz)*halfT;

  q1 = q1temp + (q0temp*gx + q2temp*gz -q3temp*gy)*halfT;

  q2 = q2temp + (q0temp*gy - q1temp*gz +q3temp*gx)*halfT;

  q3 = q3temp + (q0temp*gz + q1temp*gy -q2temp*gx)*halfT;

 

  //單位化四元數在空間旋轉時不會拉伸，僅有旋轉角度，這類似線性代數裡的正交變換

  norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);

  q0 = q0 / norm;

  q1 = q1 / norm;

  q2 = q2 / norm;

  q3 = q3 / norm;

 

  //四元數到尤拉角的轉換，公式推導見博文二

  //其中YAW航向角由於加速度計對其沒有修正作用，因此此處直接用陀螺儀積分代替

  Q_ANGLE.Z = GYRO_I.Z; // yaw

  Q_ANGLE.Y = asin(-2 * q1 * q3 + 2 * q0* q2)*57.3; // pitch

  Q_ANGLE.X = atan2(2 * q2 * q3 + 2 * q0 * q1,-2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll

}