一、開篇

  還是沒能進入到原始碼部分研究，對姿態解算過程太過於模糊，所以主要開始研究一下關於姿態解算的過程和實現，本篇博文主要是以mahony的演算法為基礎理解姿態解算的過程，主要參考的論文就是William Premerlani and Paul Bizard的關於DCM的一篇經典論文《Direction Cosine Matrix IMU_Theory》，一定要搞透這篇論文，沒看過它都不敢稱自己研究過飛控演算法；然後接下來還有madgwick和mahony的論文需要研究，看英文的比較費時間，但是還是得慢慢的啃啊~~~

        然後就是結合國內的一本很不錯的書籍《捷聯慣性導航技術》，不需要細看，只需要瞭解其中關於姿態解算部分的即可。國內的課本嘛，大家都懂的，恨不得手把手的教你了。國外的論文就不一樣了，繼續啃吧。

三、實驗平臺

Software Version：ArduCopter（Ver_3.3）

Hardware Version：pixhawk

IDE：eclipse Juno （Windows）

1、 陀螺儀和加速計（特性分析）

1）陀螺儀

         Gyro sensitivity、 operating range、offset、drift、calibrationandsaturation must be taken into account in the implementation of DCM。

靈敏度

        測量角速度，具有高動態特性，它是一個間接測量角度的器件其中一個關鍵引數就是gyro sensitivity（其單位是millivolts per degree persecond，把轉速轉換到電壓值），測量範圍越小氣靈敏度越好。也就是說測量的是角度的導數，即角速度，要將角速度對時間積分才能得到角度。陀螺儀就是內部有一個陀螺，它的軸由於陀螺效應始終與初始方向平行，這樣就可以通過與初始方向的偏差計算出旋轉方向和角度。

偏移

        偏移就是在陀螺沒有轉動的時候卻又輸出，這個輸出量的大小和供電電壓以及溫度有關，該偏移可以在陀螺儀上電時通過一小段時間的測量來修正。

漂移
        它是由於在時間的積累下偏移和噪聲相互影響的結果，例如有一個偏置（offset）0.1dps加在上面，於是測量出來是0.1dps，積分一秒之後，得到的角度是0.1度，1分鐘之後是6度，還能忍受，一小時之後是360度，轉了一圈，也就是說，陀螺儀在短時間內有很大的參考價值。
白噪聲
        電訊號的測量中，一定會帶有白噪聲，陀螺儀資料的測量也不例外。所以獲得的陀螺儀資料中也會帶有白噪聲，而且這種白噪聲會隨著積分而累加。
積分誤差

溢位

        就是轉速超過了其測量的最大轉速範圍。關於這個問題的解決辦法，在DCM IMU:Theory中給出來三種解決辦法，不寫了。

2）加速度計
         加速度計的低頻特性好，可以測量低速的靜態加速度。在無人機上就是對重力加速度g的測量和分析。當把加速度計拿在手上隨意轉動時，看的是重力加速度在三個軸上的分量值。加速度計在自由落體時，其輸出為0。為什麼會這樣呢？這裡涉及到加速度計的設計原理：加速度計測量加速度是通過比力來測量(比力方程，秦永元的書中有介紹)，而不是通過加速度。加速度計僅僅測量的是重力加速度,而重力加速度與剛才所說的R座標系（EarthFrame）是固連的,通過這種關係,可以得到加速度計所在平面與地面的角度關係。

        加速度計僅僅測量的是重力加速度，3軸加速度計輸出重力加速度在加速度計所在機體座標系3個軸上的分量大小。重力加速度的方向和大小是固定的。通過這種關係，可以得到加速度計所在平面與地面的角度關係。加速度計若是繞著重力加速度的軸轉動，則測量值不會改變，也就是說加速度計無法感知這種水平旋轉。

3）磁感測器

        可以測量磁場，在沒有其他磁場的情況下，僅僅測量的是地球的磁場，而地磁也是和R座標系固連的，通過這種關係，可以得到平面A和地平面的關係。(平面A：和磁場方向垂直的平面)，同樣的，若是沿著磁場方向的軸旋轉，測量值不會改變，無法感知這種旋轉。

        綜合考慮，加速度計和磁感測器都是極易受外部干擾的感測器，都只能得到2維的角度關係，但是測量值隨時間的變化相對較小，結合加速度計和磁感測器可以得到3維的角度關係。陀螺儀可以積分得到三維的角度關係，動態效能好，受外部干擾小，但測量值隨時間變化比較大。可以看出，它們優缺點互補，結合起來才能有好的效果。

4）關於資料融合

        現在有了三個感測器，都能在一定程度上測量角度關係，但是究竟相信誰？根據剛才的分析，應該是在短時間內更加相信陀螺儀，隔三差五的問問加速度計和磁感測器，角度飄了多少了？有一點必須非常明確，陀螺儀才是主角，加速度計和磁感測器僅僅是跑龍套的。其實加速度計無法對航向角進行修正，修正航向角需要磁力計。

五、正文

1、首先就是基於mahony演算法的AHRS姿態解算的一套原始碼，理論基礎是DCM IMU:Theory，很有參考價值。先自己分析一下，看看每一行具體是做什麼的，是如何實現姿態解算和修正的。然後會給出相應的分析

   /*
    *AHRS
   */
   // AHRS.c
   // Quaternion implementation of the 'DCM filter' [Mayhony et al]. Incorporates the magnetic distortion
   // compensation algorithms from my filter [Madgwick] which eliminatesthe need for a reference
   // direction of flux (bx bz) to be predefined and limits the effect ofmagnetic distortions to yaw
   // axis only.
   // User must define 'halfT' as the (sample period / 2), and the filtergains 'Kp' and 'Ki'.
   // Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elementsrepresenting the estimated
   // orientation.  See my report foran overview of the use of quaternions in this application.
   // User must call 'AHRSupdate()' every sample period and parsecalibrated gyroscope ('gx', 'gy', 'gz'),
   // accelerometer ('ax', 'ay', 'ay') and magnetometer ('mx', 'my', 'mz')data.  Gyroscope units are
   // radians/second, accelerometer and magnetometer units are irrelevantas the vector is normalised.                                
   #include "stm32f10x.h"
   #include "AHRS.h"
   #include "Positioning.h"
   #include <math.h>
   #include <stdio.h>
/* Private define------------------------------------------------------------*/
   #define Kp 2.0f                       // proportional gain governs rate of convergence toaccelerometer/magnetometer
    #define Ki 0.005f          // integral gain governs rate of convergenceof gyroscope biases
    #define halfT 0.0025f      // half the sample period  
   #define ACCEL_1G 1000    //theacceleration of gravity is: 1000 mg
/* Private variables---------------------------------------------------------*/
   static float q0 = 1, q1 = 0, q2 = 0, q3 = 0;        // quaternion elements representing theestimated orientation
   static float exInt = 0, eyInt = 0, ezInt = 0;        // scaled integral error
/* Public variables----------------------------------------------------------*/
   EulerAngle_Type EulerAngle;       //unit: radian
    u8InitEulerAngle_Finished = 0;
   float Magnetoresistor_mGauss_X = 0, Magnetoresistor_mGauss_Y = 0,Magnetoresistor_mGauss_Z = 0;//unit: milli-Gauss                                                                                                                                                                                                      
   float Accelerate_mg_X, Accelerate_mg_Y, Accelerate_mg_Z;//unit: mg                                                              
   float AngularRate_dps_X, AngularRate_dps_Y, AngularRate_dps_Z;//unit:dps: degree per second       
   int16_t Magnetoresistor_X, Magnetoresistor_Y, Magnetoresistor_Z;                                                                                                                                                                                                      
   uint16_t Accelerate_X = 0, Accelerate_Y = 0, Accelerate_Z = 0;                                                                                                                                                                                              
   uint16_t AngularRate_X = 0, AngularRate_Y = 0, AngularRate_Z = 0;
   u8 Quaternion_Calibration_ok = 0;
   /* Private macro-------------------------------------------------------------*/
   /* Private typedef-----------------------------------------------------------*/
   /* Private function prototypes -----------------------------------------------*/
/******************************************************************************
    *Function Name  : AHRSupdate
    *Description    : None
    *Input          : None
    *Output         : None
    *Return         : None
*******************************************************************************
   void AHRSupdate(float gx, float gy, float gz, float ax, float ay, floataz, float mx, float my, float mz) {
           float norm;
           float hx, hy, hz, bx, bz;
           float vx, vy, vz, wx, wy, wz; 
           float ex, ey, ez;
 
           // auxiliary variables to reduce number of repeated operations
           float q0q0 = q0*q0;
           float q0q1 = q0*q1;
           float q0q2 = q0*q2;
            float q0q3 = q0*q3;
           float q1q1 = q1*q1;
           float q1q2 = q1*q2;
           float q1q3 = q1*q3;
           float q2q2 = q2*q2;
           float q2q3 = q2*q3;
           float q3q3 = q3*q3;
          
           // normalise the measurements
           norm = sqrt(ax*ax + ay*ay + az*az);
           ax = ax / norm;
           ay = ay / norm;
           az = az / norm;
           norm = sqrt(mx*mx + my*my + mz*mz);
           mx = mx / norm;
            my = my / norm;
           mz = mz / norm;
          
           // compute reference direction of magnetic field
           hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
           hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
           hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 -q2q2);        
           bx = sqrt((hx*hx) + (hy*hy));
           bz = hz;
          
// estimated direction of gravity and magnetic field (v and w)
           vx = 2*(q1q3 - q0q2);
           vy = 2*(q0q1 + q2q3);
           vz = q0q0 - q1q1 - q2q2 + q3q3;
           wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
           wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
           wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2); 
          
// error is sum ofcross product between reference direction of fields and directionmeasured by sensors 
           ex = (ay*vz - az*vy) + (my*wz - mz*wy);
           ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
           ez = (ax*vy - ay*vx) + (mx*wy - my*wx);
          
           // integral error scaled integral gain
           exInt = exInt + ex*Ki* (1.0f / sampleFreq);
           eyInt = eyInt + ey*Ki* (1.0f / sampleFreq);
           ezInt = ezInt + ez*Ki* (1.0f / sampleFreq);
           // adjusted gyroscope measurements
           gx = gx + Kp*ex + exInt;
           gy = gy + Kp*ey + eyInt;
           gz = gz + Kp*ez + ezInt;
          
           // integrate quaternion rate and normalize
           q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
           q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
           q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
           q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT; 
          
           // normalise quaternion
           norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
           q0 = q0 / norm;
           q1 = q1 / norm;
           q2 = q2 / norm;
           q3 = q3 / norm;
}

//陀螺儀、加速度計、磁力計資料融合
    void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
            float norm;
            float hx, hy, hz, bx, bz;
            float vx, vy, vz, wx, wy, wz; //v*當前姿態計算得來的重力在三軸上的分量
            float ex, ey, ez;

            // auxiliary variables to reduce number of repeated operations
            float q0q0 = q0*q0;
            float q0q1 = q0*q1;
            float q0q2 = q0*q2;
            float q0q3 = q0*q3;
            float q1q1 = q1*q1;
            float q1q2 = q1*q2;
            float q1q3 = q1*q3;
            float q2q2 = q2*q2;
            float q2q3 = q2*q3;
            float q3q3 = q3*q3;
           
            // normalise the measurements
            norm = sqrt(ax*ax + ay*ay + az*az); 
            ax = ax / norm;
            ay = ay / norm;
            az = az / norm;
            norm = sqrt(mx*mx + my*my + mz*mz); 
            mx = mx / norm;
            my = my / norm;
            mz = mz / norm;
           
            // compute reference direction of magnetic field
            hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
            hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
            hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);         
            bx = sqrt((hx*hx) + (hy*hy));
            bz = hz; 
           
// estimated direction of gravity and magnetic field (v and w) 
//參考座標n系轉化到載體座標b系的用四元數表示的方向餘弦矩陣第三列即是。
        //處理後的重力分量
            vx = 2*(q1q3 - q0q2);
            vy = 2*(q0q1 + q2q3);
            vz = q0q0 - q1q1 - q2q2 + q3q3;
//處理後的mag，反向使用DCM得到
            wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
            wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
            wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);  
           
// error is sum of cross product between reference direction of fields and direction measured by sensors 體現在加速計補償和磁力計補償，因為僅僅依靠加速計補償沒法修正Z軸的變差，所以還需要通過磁力計來修正Z軸。（公式28）。《四元數解算姿態完全解析及資料彙總》的作者把這部分理解錯了，不是什麼叉積的差，而叉積的計算就是這樣的。計算方法是公式10。
            ex = (ay*vz - az*vy) + (my*wz - mz*wy);
            ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
            ez = (ax*vy - ay*vx) + (mx*wy - my*wx);
           
            // integral error scaled integral gain 
            exInt = exInt + ex*Ki* (1.0f / sampleFreq);
            eyInt = eyInt + ey*Ki* (1.0f / sampleFreq);
            ezInt = ezInt + ez*Ki* (1.0f / sampleFreq);
            // adjusted gyroscope measurements
//將誤差PI後補償到陀螺儀，即補償零點漂移。通過調節Kp、Ki兩個引數，可以控制加速度計修正陀螺儀積分姿態的速度。（公式16和公式29）
            gx = gx + Kp*ex + exInt;
            gy = gy + Kp*ey + eyInt;
            gz = gz + Kp*ez + ezInt;
           
            // integrate quaternion rate and normalize
 //一階龍格庫塔法更新四元數
            q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
            q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
            q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
            q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;  
           
            // normalise quaternion
            norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
            q0 = q0 / norm;
            q1 = q1 / norm;
            q2 = q2 / norm;
            q3 = q3 / norm;
}

        1）無人機控制中，主要就是姿態解算和姿態控制部分，該部分主要介紹姿態解算，下面是來一張比較好理解的框圖。

        由上面兩幅圖很容易理解整個控制和姿態解算了吧，在第四部分也介紹了關於陀螺儀、加速計、磁力計它們的作用，所以不再單獨介紹了。不理解的往上翻翻再看看吧。

        2）在姿態解算過程中，到底用什麼表示無人機的姿態呢？姿態表示的方法有很多種，比如尤拉角、四元數、DCM，各有的各的優勢，比較常用的就是四元數，方便計算。上面的姿態解算演算法也是基於四元數的。下面介紹一個它們三個的優缺點。

姿態解算方法的比較：

        首先，東北天座標系是n系（地理座標系，參考座標系），載體座標系是b系，就是我們飛行器的座標系。對於四元數法的姿態解算，需要求取的就是四元數的值；方向餘弦矩陣（用於表示n系和b系的相對關係）中的元素本來應該是三角函式，這裡由於使用四元數法，所以矩陣中的元素就成了四元數。所以姿態解算的任務就是求解由四元數構成的方向餘弦矩陣nCb（nCb表示從b繫到n的轉換矩陣，同理，bCn表示從n繫到b的轉換矩陣，它們的關係是轉置）。

        顯然，上述矩陣是有誤差存在的。對於一個確定的向量n，用不同的座標系表示時，他們所表示的大小和方向一定是相同的。但是由於這兩個座標系的轉換矩陣存在誤差，那麼當一個向量經過這麼一個有誤差存在的旋轉矩陣變換後，在另一個座標系中肯定和理論值是有偏差的，我們通過這個偏差來修正這個旋轉矩陣。這個旋轉矩陣的元素是四元數，這就是說修正的就是四元數，於是乎姿態就這樣被修正了，這才是姿態解算的原理。姿態解算求的是四元數，是通過修正旋轉矩陣中的四元數來達到姿態解算的目的，而不要以為通過加速度計和地磁計來修正姿態，加速度計和地磁計只是測量工具和載體，通過這兩個器件表徵旋轉矩陣的誤差存在，然後通過演算法修正誤差，修正四元數，修正姿態。

加速度計修正pitch_roll

        加速度計可以修正pitch_roll，但是我們必須要考慮離心加速度（centrifugalacceleration），離心加速度就等於旋轉率向量（即gyro vector）和速度向量的叉積（沒有原因，平均得來的並且還相當準確，速度可以用GPS獲取）。我們假設飛機方向和X軸平行。

        在機體上測得的重力的為：

Pitch_roll的旋轉修正向量是由DCM的Z行與歸一化以後的重力參考向量的叉積。

        在n系中，加速度計輸出為，經過bCn（用四元數表示的轉換矩陣）轉換之後到b系中的值為；在b系中，加速度計的測量值為，現在和均表示在b系中的豎直向下的向量，由此，我們來做向量積（叉積），得到誤差，利用這個誤差來修正bCn矩陣，於是四元數就在這樣一個過程中被修正了。但是，由於加速度計無法感知z軸上的旋轉運動，所以還需要用地磁計來進一步補償。

        加速度計在靜止時測量的是重力加速度，是有大小和方向的；同理，地磁計同樣測量的是地球磁場的大小和方向，只不過這個方向不再是豎直向下，而是與x軸（或者y軸）呈一個角度，與z軸呈一個角度。記作，假設x軸對準北邊，所以by=0，即。倘若知道bx和bz的精確值，那麼就可以採用和加速度計一樣的修正方法來修正。只不過在加速度計中，在n系中的參考向量是，變成了地磁計的。如果我們知道bx和bz的精確值，那麼就可以擺脫掉加速度計的補償，直接用地磁計和陀螺儀進行姿態解算，但是你看過誰只用陀螺儀和地磁計進行姿態解算嗎？沒有，因為沒人會去測量當地的地磁場相對於東北天座標的夾角，也就是bx和bz（插曲：關於這個bx和bz的理解：可以對比重力加速度的理解，就像vx vy vz似的，因為在每一處的歸一化以後的重力加速度都是0 0 1然後旋轉到機體座標系，而地球每一處的地磁大小都不一樣的，不能像重力加速度那樣直接旋轉得到了，只能用磁力計測量到的資料去強制擬合。）。那麼現在怎麼辦？前面已經講了，姿態解算就是求解旋轉矩陣，這個矩陣的作用就是將b系和n正確的轉化直到重合。現在我們假設nCb旋轉矩陣是經過加速度計校正後的矩陣，當某個確定的向量（b系中）經過這個矩陣旋轉之後（到n系），這兩個座標系在XOY平面上重合（參考DCM IMU:Theory的Drift cancellation部分），只是在z軸旋轉上會存在一個偏航角的誤差。下圖表示的是經過nCb旋轉之後的b系和n系的相對關係。可以明顯發現加速度計可以把b系通過四元數法從任意角度拉到與n系水平的位置上，此時只剩下一個偏航角誤差。這也是為什麼加速度計無法修正偏航的原因。

        為什麼在word中使用公式編譯器編輯的公式沒辦法複製到CSDN中呢？？？？？？？？？？？？

        此方式在數學上沒有任何問題，但是由於地磁感測器極易受到各種干擾，而此演算法又將地磁感測器所指示的方向過度的融入到了姿態當中，導致實際使用中資料會非常不穩定。怪不得crazypony的演算法中沒有使用磁力計修正YAW，這應該就是原因所在吧。

六、來一點ardupilot原始碼分析

就理解了一個通過gyro_vector更新DCM的演算法，這個應該是在renormalization直接就需要了解，可以前期沒理解到底是幹嘛的，現在補上；關於renormalization的演算法可以參考上一篇博文。

        姿態解算過程中不僅需要修正陀螺儀的各種errors，還需要實時的更新的DCM。上週一直沒能理解的一個問題，在AP_AHRS_DCM.cpp中的matrix_update(delta_t)，就是實時更新DCM矩陣的，原始碼如下，這一部分研究了很久，需要的基礎知識比較多。先上原始碼

// update the DCM matrix using only the gyros
Void AP_AHRS_DCM::matrix_update(float _G_Dt)
{
    _omega.zero();
    // average across first two healthy gyros. This reduces noise on
    // systems with more than one gyro. We don't use the 3rd gyro
    // unless another is unhealthy as 3rd gyro on PH2 has a lot more
    // noise
    uint8_t healthy_count = 0;
    Vector3f delta_angle;
    for (uint8_t i=0; i<_ins.get_gyro_count(); i++) {
        if (_ins.get_gyro_health(i) && healthy_count < 2) {
            Vector3f dangle;
            if (_ins.get_delta_angle(i, dangle)) {
                healthy_count++;
                delta_angle += dangle;
            }
        }
    }
    if (healthy_count > 1) {
        delta_angle /= healthy_count;
    }
    if (_G_Dt > 0) {
        _omega = delta_angle / _G_Dt;
        _omega += _omega_I;
        _dcm_matrix.rotate((_omega + _omega_P + _omega_yaw_P) * _G_Dt);
    }
}

          首先就是通過陀螺儀獲取兩次gyro_vector，然後取平均以降低誤差；然後就是比較專業的演算法實現_dcm_matrix.rotate((_omega +_omega_P + _omega_yaw_P) * _G_Dt)了。進入到matrix3.cpp中有rotate()函式，該函式就是實現DCM更新的演算法實現，演算法主要是用陀螺儀的輸出值與DCM矩陣的乘積再加回到DCM中去，處理過程中使用了離散化的概念，即dcm（k+1）=dcm（k）+增量，因為公式裡有求導，必須離散化後才能計算機處理，感謝青龍的指導。 

// apply an additional rotation from a body frame gyro vector
// to a rotation matrix.
template <typename T>
void Matrix3<T>::rotate(const Vector3<T> &g)
{
    Matrix3<T> temp_matrix;
    temp_matrix.a.x = a.y * g.z - a.z * g.y;
    temp_matrix.a.y = a.z * g.x - a.x * g.z;
    temp_matrix.a.z = a.x * g.y - a.y * g.x;
    temp_matrix.b.x = b.y * g.z - b.z * g.y;
    temp_matrix.b.y = b.z * g.x - b.x * g.z;
    temp_matrix.b.z = b.x * g.y - b.y * g.x;
    temp_matrix.c.x = c.y * g.z - c.z * g.y;
    temp_matrix.c.y = c.z * g.x - c.x * g.z;
    temp_matrix.c.z = c.x * g.y - c.y * g.x;

    (*this) += temp_matrix;//增加累加到原始資料上
}