花了整整一天時間在地磁融合計算姿態(tài)上洗鸵，看了很多論壇越锈，發(fā)現(xiàn)都是同一篇文章，但是一直理解不了膘滨，后來發(fā)現(xiàn)就是這些筆者全部都是一字不差照搬照抄甘凭，而最初的博主又沒有說清楚導(dǎo)致。我還是結(jié)合原文加上我自己通俗的理解來解釋一下

首先給出x-IMU關(guān)于陀螺儀火邓、加速度計丹弱、地磁計的融合代碼：

void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {  
float recipNorm;  
float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;    
float hx, hy, bx, bz;  
float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;  
float halfex, halfey, halfez;  
float qa, qb, qc;  `

// Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)  
if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {  
    MahonyAHRSupdateIMU(gx, gy, gz, ax, ay, az);  
    return;  
}  

// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)  
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {  

    // Normalise accelerometer measurement  
    recipNorm = invSqrt(ax * ax + ay * ay + az * az);  
    ax *= recipNorm;  
    ay *= recipNorm;  
    az *= recipNorm;       

    // Normalise magnetometer measurement  
    recipNorm = invSqrt(mx * mx + my * my + mz * mz);  
    mx *= recipNorm;  
    my *= recipNorm;  
    mz *= recipNorm;     

    // Auxiliary variables to avoid repeated arithmetic  
    q0q0 = q0 * q0;  
    q0q1 = q0 * q1;  
    q0q2 = q0 * q2;  
    q0q3 = q0 * q3;  
    q1q1 = q1 * q1;  
    q1q2 = q1 * q2;  
    q1q3 = q1 * q3;  
    q2q2 = q2 * q2;  
    q2q3 = q2 * q3;  
    q3q3 = q3 * q3;     

    // Reference direction of Earth's magnetic field  
    hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));  
    hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));  
    bx = sqrt(hx * hx + hy * hy);  
    bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));  

    // Estimated direction of gravity and magnetic field  
    halfvx = q1q3 - q0q2;  
    halfvy = q0q1 + q2q3;  
    halfvz = q0q0 - 0.5f + q3q3;  
    halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);  
    halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);  
    halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);    
  
    // Error is sum of cross product between estimated direction and measured direction of field vectors  
    halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy);  
    halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz);  
    halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);  

    // Compute and apply integral feedback if enabled  
    if(twoKi > 0.0f) {  
        integralFBx += twoKi * halfex * (1.0f / sampleFreq);    // integral error scaled by Ki  
        integralFBy += twoKi * halfey * (1.0f / sampleFreq);  
        integralFBz += twoKi * halfez * (1.0f / sampleFreq);  
        gx += integralFBx;  // apply integral feedback  
        gy += integralFBy;  
        gz += integralFBz;  
    }  
    else {  
        integralFBx = 0.0f; // prevent integral windup  
        integralFBy = 0.0f;  
        integralFBz = 0.0f;  
    }  

    // Apply proportional feedback  
    gx += twoKp * halfex;  
    gy += twoKp * halfey;  
    gz += twoKp * halfez;  
}  
  
// Integrate rate of change of quaternion  
gx *= (0.5f * (1.0f / sampleFreq));     // pre-multiply common factors  
gy *= (0.5f * (1.0f / sampleFreq));  
gz *= (0.5f * (1.0f / sampleFreq));  
qa = q0;  
qb = q1;  
qc = q2;  
q0 += (-qb * gx - qc * gy - q3 * gz);  
q1 += (qa * gx + qc * gz - q3 * gy);  
q2 += (qa * gy - qb * gz + q3 * gx);  
q3 += (qa * gz + qb * gy - qc * gx);   
  
// Normalise quaternion  
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);  
q0 *= recipNorm;  
q1 *= recipNorm;  
q2 *= recipNorm;  
q3 *= recipNorm;  }  

相信有很多人已經(jīng)理解了加速度計補(bǔ)償陀螺儀漂移的原理，（其實我一點都不理解）铲咨，不過這部分代碼在x-IMU官網(wǎng)上已經(jīng)給出躲胳，大家可以自行下載(http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/) （這個里面有c程序，里面有兩個函數(shù)MahonyAHRSupdateIMU()和MahonyAHRSupdate()纤勒，前者是僅用加速度計補(bǔ)償陀螺儀的漂移坯苹，后者即是融合地磁儀補(bǔ)償陀螺儀的漂移），有能力的可以自行查看Sebastian O.H. Madgwick在2010年4月發(fā)表的一篇論文（An efficient orientation filter for inertial and inertial/magneticsensor arrays）摇天，可以發(fā)現(xiàn)x-IMU官網(wǎng)上的融合代碼就是基于此篇論文粹湃。

花了一天時間研究地磁融合代碼恐仑，總算弄明白了其地磁融合的原理。為了讓大家理解Madgwick對地磁量的處理方式为鳄，我先從加速度計補(bǔ)償開始說起裳仆。

首先，東北天坐標(biāo)系我們稱之為W系（世界坐標(biāo)系）孤钦，載體坐標(biāo)系我們稱之為r系（機(jī)體坐標(biāo)系）歧斟。對于四元數(shù)法的姿態(tài)解算，我們求的就是四元數(shù)司训；旋轉(zhuǎn)矩陣R（用于表示w系和r系的相對關(guān)系）中的元素本來應(yīng)該是三角函數(shù)构捡，這里由于我們四元數(shù)法，所以矩陣中的元素就成了四元數(shù)壳猜。所以我們的任務(wù)就是求解由四元數(shù)構(gòu)成的方向余弦矩陣wCr（wCr表示從r系到w的轉(zhuǎn)換矩陣勾徽，同理，rCw表示從w系到r的轉(zhuǎn)換矩陣统扳，他們的關(guān)系是轉(zhuǎn)置）喘帚。

顯然，上述矩陣是有誤差存在的咒钟。對于一個確定的向量n吹由，用不同的坐標(biāo)系表示時，他們所表示的大小和方向一定是相同的朱嘴。但是由于這兩個坐標(biāo)系的轉(zhuǎn)換矩陣存在誤差倾鲫，那么當(dāng)一個向量經(jīng)過這么一個有誤差存在的旋轉(zhuǎn)矩陣變換后，在另一個坐標(biāo)系中肯定和理論值是有偏差的萍嬉，我們通過這個偏差來修正這個旋轉(zhuǎn)矩陣乌昔。我們剛才說了，這個旋轉(zhuǎn)矩陣的元素是四元數(shù)壤追，這就是說我們修正的就是四元數(shù)磕道，于是乎我們的姿態(tài)就這樣被修正了，這才是姿態(tài)解算的原理行冰。(這一段說的很好溺蕉，是最基本的原理)

我們的姿態(tài)解算求的是四元數(shù)，我們是通過修正旋轉(zhuǎn)矩陣中的四元數(shù)來達(dá)到姿態(tài)解算的目的悼做，而不要以為通過加速度計和地磁計來修正姿態(tài)疯特，加速度計和地磁計只是測量工具和載體，通過這兩個器件表征旋轉(zhuǎn)矩陣的誤差存在贿堰，然后通過算法修正誤差辙芍，修正四元數(shù)，修正姿態(tài)。

我們可以邊看MahonyAHRSupdate這個函數(shù)邊理解以下的內(nèi)容故硅。

在w系中庶灿，加速度計輸出為

在r系中往踢，加速度計的測量值為

利用這個誤差來修正旋轉(zhuǎn)矩陣趣效，于是乎瘦癌，我們的四元數(shù)就在這樣一個過程中被修正了。（實際上這種修正方法只把b系和n系的XOY平面重合起來跷敬，對于z軸旋轉(zhuǎn)的偏航讯私，加速度計無可奈何，稍后給詳細(xì)講解）但是西傀，由于加速度計無法感知z軸上的旋轉(zhuǎn)運動斤寇，所以還需要用地磁計來進(jìn)一步補(bǔ)償。

以上部分就是單獨存在于MahonyAHRSupdateIMU這個函數(shù)內(nèi)的內(nèi)容拥褂，還算比較好理解

我們知道加速度計在靜止時測量的是重力加速度娘锁，是有大小和方向的；同理饺鹃，地磁計同樣測量的是地球磁場的大小和方向莫秆，只不過這個方向不再是豎直向下，而是與x軸（或者y軸）呈一個角度悔详，與z軸呈一個角度馏锡。（想象一下地磁場的磁感應(yīng)線）記作

這里我們讓x軸對準(zhǔn)北邊，所以by=0伟端，即

Paste_Image.png

倘若我們知道bx和bz的精確值医男，那么我們就可以采用和加速度計一樣的修正方法來修正砸狞。只不過在加速度計中，我們在n系中的參考向量是

前面已經(jīng)講了羽圃，我們的姿態(tài)解算就是求解旋轉(zhuǎn)矩陣乾胶，這個矩陣的作用就是將w系和r正確的轉(zhuǎn)化直到重合。
現(xiàn)在我們假設(shè)旋轉(zhuǎn)矩陣是經(jīng)過加速度計校正后的矩陣统屈，當(dāng)某個確定的向量（r系中）經(jīng)過這個矩陣旋轉(zhuǎn)之后（到w系）胚吁，這兩個坐標(biāo)系在XOY平面上重合，只是在z軸旋轉(zhuǎn)上會存在一個偏航角的誤差愁憔。下圖表示的是經(jīng)過nCb*旋轉(zhuǎn)之后的b系和n系的相對關(guān)系腕扶。可以明顯發(fā)現(xiàn)加速度計可以把b系通過四元數(shù)法從任意角度拉到與n系水平的位置上吨掌，這時半抱，只剩下一個偏航角誤差。這也是為什么加速度計無法誤差修正偏航的原因膜宋。

到這里窿侈，就好說了。現(xiàn)在我們反過來從r系推往w系（注意是r系轉(zhuǎn)換到w系）：設(shè)地磁計在r系中的輸出為

經(jīng)過r系到w系的旋轉(zhuǎn)之后得到

在這個XOY平面上（w系），

將加速度計沒能做到的z軸上的旋轉(zhuǎn)修正轮傍，通過地磁計在XOY平面上的地磁力相同原理暂雹，得到了修正。于是乎金麸，pitch和roll通過加速度計修正擎析，然后在這個基礎(chǔ)之上（該地磁計補(bǔ)償方法必須依靠加速度計修正提供一致的XOY平面，才會有bx2= hx2+hy2等式成立）挥下，yaw通過地磁計來補(bǔ)償揍魂，最終得到了沒有偏差的實時姿態(tài)（也就是由四元數(shù)組成的旋轉(zhuǎn)矩陣）。

至此棚瘟，我終于想通了所有的地方现斋，雖然依然存在局部的一些漏洞，但是總體思路已經(jīng)理清偎蘸，接下去就是看詳細(xì)看代碼就行了庄蹋。

以下為自己對代碼的理解歸納

0、測量參數(shù)（r系）

角加速度：gx, gy, gz
線加速度 : ax, ay, az
地磁計: mx, my, mz

1迷雪、歸一化[ax,ay,az]^T和[mx,my,mz]^T

2限书、將地磁計值[mx,my,mz]^T旋轉(zhuǎn)到w系，得到[hx,hy,hz]^T章咧，因為我們規(guī)定w系的x軸指向北倦西，所以理應(yīng)hy = 0，w系上的地磁計值理應(yīng)為[bx,0,bz]^T赁严，所以我們令bx=sqrt(hx²+hy²)扰柠，bz = hz。

3疼约、計算測量值和估計值之間的誤差（包括加速度和磁場）

1）將w系中的重力加速度[0,0,1]^T旋轉(zhuǎn)到r系中卤档，變成[vx,vy,vz]^T，計算測量值[ax,ay,az]^T與估計值[vx,vy,vz]^T的誤差程剥。
2）將w系中的地磁向量[bx,0,bz]^T旋轉(zhuǎn)到r系中劝枣，變成[wx,wy,wz]^T，計算測量值[mx,my,mz]^T與估計值[wx,wy,wz]^T的誤差织鲸。

4哨免、將誤差補(bǔ)償?shù)剿脑豙w,x,y,z]或者[q0,q1,q2,q3],更新旋轉(zhuǎn)矩陣