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

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