四元數(shù)姿態(tài)解算中地磁計(jì)融合解讀

四元數(shù)姿態(tài)解算中的地磁計(jì)融合解筆者最近在做四軸，涉及到地磁計(jì)的融合算法，網(wǎng)上大多數(shù)是x-IMU的融合代碼，但是這段代碼對(duì)于地磁計(jì)的融合說明沒有做過多的解釋，網(wǎng)上沒有相關(guān)討論，僅在阿莫論壇看到一篇相關(guān)的代碼解釋，里面有關(guān)于地磁計(jì)融合部分的解viewplaincopyto voidMahonyAHRSupdate(floatfloatfloat{floatfloatfloataz,floatfloatfloatfloatx,float,floathalfex,halfey,float(avoidsNaNinmagnetometer0.0f))}{=*+***=+=四元數(shù)姿態(tài)解算中的地磁計(jì)融合解筆者最近在做四軸，涉及到地磁計(jì)的融合算法，網(wǎng)上大多數(shù)是x-IMU的融合代碼，但是這段代碼對(duì)于地磁計(jì)的融合說明沒有做過多的解釋，網(wǎng)上沒有相關(guān)討論，僅在阿莫論壇看到一篇相關(guān)的代碼解釋，里面有關(guān)于地磁計(jì)融合部分的解viewplaincopyto voidMahonyAHRSupdate(floatfloatfloat{floatfloatfloataz,floatfloatfloatfloatx,float,floathalfex,halfey,float(avoidsNaNinmagnetometer0.0f))}{=*+***=+=*+**my+=*==**==*=*=*=*=*= ***+-=-+=-+my***+*+--+bx=* *+=2.0f(mx*--*++*-)dof=-=+;=--- -=*+-*=*+*+=**++-q1q1d-**halfwz-= y* halfey-*halfvz)+* -*=*-+ *-*{*/b*halfey/*integralFBz*/+=i}{==}=proportionalbx=* *+=2.0f(mx*--*++*-)dof=-=+;=--- -=*+-*=*+*+=**++-q1q1d-**halfwz-= y* halfey-*halfvz)+* -*=*-+ *-*{*/b*halfey/*integralFBz*/+=i}{==}=proportionalgy}**gz+=**/pmultiplycommon*//Freq));gz*=q0=*+=--*****-*+*--* *Normalise*=*+*+*+**=相信有很多人已經(jīng)理解了加速度計(jì)補(bǔ)償陀螺儀漂移的原理，這部分代碼x-IMU(http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithmsSebastianO.H.Madgwick20104（Anefficientorientationfilterfor相信有很多人已經(jīng)理解了加速度計(jì)補(bǔ)償陀螺儀漂移的原理，這部分代碼x-IMU(http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithmsSebastianO.H.Madgwick20104（Anefficientorientationfilterforinertialandinertial/magneticsensorarrays），x-IMU首先，東北天坐標(biāo)系我們稱之為n系（地理坐標(biāo)系，參考坐標(biāo)系），體坐標(biāo)系我們稱之為算，我們求的就是四元數(shù)的值；方向余弦矩陣（用于表示nb（nCb表示從b系到n的轉(zhuǎn)換矩陣，同理，bCn表示從n系到b的轉(zhuǎn)換矩陣顯然，上述矩陣是有誤差存在的。對(duì)于一個(gè)確定的向量n，用不同的坐數(shù)，我們是通過修正旋轉(zhuǎn)矩陣中的四元數(shù)來(lái)達(dá)到姿態(tài)解算的目的，而不要以為通過加速度計(jì)和地磁計(jì)來(lái)修正姿態(tài)，加速度計(jì)和地磁計(jì)只是測(cè)量工具和載體，通過這兩個(gè)器件表征旋轉(zhuǎn)矩陣的誤差存在，然后通過算法修正誤差，修正四元；和正方法只把bnXOYz無(wú)可奈何，稍后給詳細(xì)講解）但是，由于加速度計(jì)無(wú)法感知z軸上的旋轉(zhuǎn)運(yùn)我們知道加速度計(jì)在靜止時(shí)測(cè)量的是重力加速度，是有大小和方向的；同理，地磁計(jì)同樣測(cè)量的是地球磁場(chǎng)的大小和方向，只不過這個(gè)方向不再是豎直向下，而是與x軸（y軸）呈一個(gè)角度，與z軸呈一個(gè)角度。記作。倘若我們我們知道加速度計(jì)在靜止時(shí)測(cè)量的是重力加速度，是有大小和方向的；同理，地磁計(jì)同樣測(cè)量的是地球磁場(chǎng)的大小和方向，只不過這個(gè)方向不再是豎直向下，而是與x軸（y軸）呈一個(gè)角度，與z軸呈一個(gè)角度。記作。倘若我們bx和bz的精，那么我們就可以采用和加速度計(jì)一樣的修正方法來(lái)修正。只不過在，變成了地磁計(jì) 。不難現(xiàn)，地磁計(jì)無(wú)法感知關(guān)于y軸上的旋轉(zhuǎn)運(yùn)動(dòng)（bx2+bz2=常軸上旋轉(zhuǎn)運(yùn)動(dòng)了（而沒有地磁的加速度計(jì)是無(wú)法感知z軸旋轉(zhuǎn)運(yùn)動(dòng)的）。去測(cè)量當(dāng)?shù)氐牡卮艌?chǎng)相對(duì)于東北天坐標(biāo)的夾角，也就是bx和bz。那么現(xiàn)在怎么是將bn正確的轉(zhuǎn)化直到重合。nCb*旋轉(zhuǎn)矩陣是經(jīng)過加速度計(jì)校正后的矩陣，當(dāng)某個(gè)確定的向量（b）經(jīng)過這個(gè)矩陣旋轉(zhuǎn)之后（n），這兩個(gè)坐標(biāo)系XOY面上重合，只z軸旋轉(zhuǎn)上會(huì)存在一個(gè)偏航角的。下圖表示的是經(jīng)過nCb*bnb過四元數(shù)法從任意角度拉到與n系水平的位置上，這時(shí)，只剩下一個(gè)偏航角誤到這里，就好說了?，F(xiàn)在我們反過來(lái)從bnb到這里，就好說了?，F(xiàn)在我們反過來(lái)從bnb的投影為hx2+hy2。顯然，XOY平面上（n）的向的大小必定相同，所以有bx2=hx2+hy2。而對(duì)于bz的處理，我 將加速度計(jì)沒能做到的zXOY地磁力相同原理，得到了修正。于是乎，pitchroll后在這基礎(chǔ)之上（該地磁計(jì)補(bǔ)償