一、開篇
? ? ? ? 還是沒能進入到源碼部分研究，對姿態解算過程太過于模糊，所以主要開始研究一下關于姿態解算的過程和實現，本篇博文主要是以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，把轉速轉換到電壓值），測量范圍越小氣靈敏度越好。也就是說測量的是角度的導數，即角速度，要將角速度對時間積分才能得到角度。陀螺儀就是內部有一個陀螺，它的軸由于陀螺效應始終與初始方向平行，這樣就可以通過與初始方向的偏差計算出旋轉方向和角度。
偏移
? ? ? ? 偏移就是在陀螺沒有轉動的時候卻又輸出，這個輸出量的大小和供電電壓以及溫度有關，該偏移可以在陀螺儀上電時通過一小段時間的測量來修正。
漂移陀螺儀在短時間內有很大的參考價值。
溢出
? ? ? ? 就是轉速超過了其測量的最大轉速范圍。關于這個問題的解決辦法，在DCM IMU:Theory中給出來三種解決辦法，不寫了。
2）加速度計加速度計測量加速度是通過比力來測量( 比力方程，秦永元的書中有介紹)，而不是通過加速度。加速度計僅僅測量的是重力加速度,而重力加速度與剛才所說的R坐標系（EarthFrame）是固連的,通過這種關系,可以得到加速度計所在平面與地面的角度關系。
? ? ? ? 加速度計僅僅測量的是重力加速度，3軸加速度計輸出重力加速度在加速度計所在機體坐標系3個軸上的分量大小。重力加速度的方向和大小是固定的。通過這種關系，可以得到加速度計所在平面與地面的角度關系。加速度計若是繞著重力加速度的軸轉動，則測量值不會改變，也就是說加速度計無法感知這種水平旋轉。
Accelerometersare used for roll-pitch drift correction because they have zero drift ? ? ? ? 有關陀螺儀和加速度計和關系，姿態解算融合的原理，再把下面這個比喻放到這里一遍。 3）磁傳感器
? ? ? ? 可以測量磁場，在沒有其他磁場的情況下，僅僅測量的是地球的磁場，而地磁也是和R坐標系固連的，通過這種關系，可以得到平面A和地平面的關系。(平面A：和磁場方向垂直的平面)，同樣的，若是沿著磁場方向的軸旋轉，測量值不會改變，無法感知這種旋轉。
? ? ? ? 綜合考慮， 加速度計和磁傳感器都是極易受外部干擾的傳感器，都只能得到2維的角度關系，但是測量值隨時間的變化相對較小，結合加速度計和磁傳感器可以得到3維的角度關系。陀螺儀可以積分得到三維的角度關系，動態性能好，受外部干擾小，但測量值隨時間變化比較大。可以看出，它們優缺點互補，結合起來才能有好的效果。
4）關于數據融合
? ? ? ? 現在有了三個傳感器，都能在一定程度上測量角度關系，但是究竟相信誰？根據剛才的分析，應該是在短時間內更加相信陀螺儀，隔三差五的問問加速度計和磁傳感器，角度飄了多少了？有一點必須非常明確，陀螺儀才是主角，加速度計和磁傳感器僅僅是跑龍套的。其實加速度計無法對航向角進行修正，修正航向角需要磁力計。
參考crazypony的分析：http://www.crazepony.com/wiki/mpu6050.html和《DCM IMU:Theory》 五、正文
1、首先就是基于mahony算法的AHRS姿態解算的一套源碼，理論基礎是DCM IMU:Theory，很有參考價值。先自己分析一下，看看每一行具體是做什么的，是如何實現姿態解算和修正的。然后會給出相應的分析
[plain] view plain
copy ???/*?? ????*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;?? }?? 2、上述算法的源碼分析 [plain] view plain
copy //陀螺儀、加速度計、磁力計數據融合?? ????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;?? }?? 3、在深入一點 ? ? ? ? 1）無人機控制中，主要就是姿態解算和姿態控制部分，該部分主要介紹姿態解算，下面是來一張比較好理解的框圖。
? ? ? ? 由上面兩幅圖很容易理解整個控制和姿態解算了吧，在第四部分也介紹了關于陀螺儀、加速計、磁力計它們的作用，所以不再單獨介紹了。不理解的往上翻翻再看看吧。
? ? ? ? 2）在姿態解算過程中，到底用什么表示無人機的姿態呢？姿態表示的方法有很多種，比如歐拉角、四元數、DCM，各有的各的優勢，比較常用的就是四元數，方便計算。上面的姿態解算算法也是基于四元數的。下面介紹一個它們三個的優缺點。
姿態解算方法的比較：
算法
優點
缺點
PS
歐拉角法（3參數）
1、? 通過歐拉角微分方程直接解算出pitch、roll、yaw
2、? 概念直觀，易于理解
3、? 解算結果永遠是正交的，無需再次正交化處理
1、? 方程中有三角函數的運算，接超越函數有一定的困難
2、? 當俯仰角接近90°時出現退化現象
1、? 適應于水平姿態角變化不大的情況
2、? 不適用于全姿態運載體
方向余弦法（9參數）
1、? 對姿態矩陣微分方程的求解，避免了歐拉角法中出現的退化現象
2、? 可以全姿態運行
1、參數量過多，計算量大，實時性不好
很少在工程中使用
四元數法（4參數）
1、? 直接求解四元數微分方程
2、? 只需要求解四個參數，計算量小
3、算法簡單，易于操作
漂移比較嚴重
可以在實現過程中修正漂移，應用比較廣泛
? ? ? ? ?3）廢話不多說，進入正題。上述算法主要就是利用加速度計和磁力計修正陀螺儀的誤差，算法是使用了PI反饋控制器實現反饋修正的。如下圖：
? ? ? ? 首先，東北天坐標系是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 軸上的旋轉運動，所以還需要用地磁計來進一步補償。
? ? ? ? PS：公式不好加，只能直接切圖了~~~ ? ? ? ? 加速度計在靜止時測量的是重力加速度，是有大小和方向的；同理，地磁計同樣測量的是地球磁場的大小和方向，只不過這個方向不再是豎直向下，而是與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矩陣的，源碼如下，這一部分研究了很久，需要的基礎知識比較多。先上源碼
[plain] view plain
copy //?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）+增量，因為公式里有求導，必須離散化后才能計算機處理，感謝青龍的指導。?
[plain] view plain
copy //?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;//增加累加到原始數據上?? }?? ? ? ? ? 公式太多，沒辦法還是上圖吧~~~ 最后來一張神人做的圖。
六、總結 ? ? ? ? 算法的理解過程比較艱難，千萬不能閉門造車，要多多交流，思想的碰撞才能產生出火花，多謝各位群友的無私幫助。通過上述的分析以后，對整個姿態解算過程有了整體的框架理解，接下來會結合上面算法的分析去分析ardupilot中關于姿態解算的源碼，應該會理解的快一點了吧，希望如此~~~
版權聲明：. https://blog.csdn.net/qq_21842557/article/details/50993809
總結
以上是生活随笔 為你收集整理的Pixhawk之姿态解算篇（2）_mahony算法分析 的全部內容，希望文章能夠幫你解決所遇到的問題。
如果覺得生活随笔 網站內容還不錯，歡迎將生活随笔 推薦給好友。
