互補(bǔ)濾波算法姿態(tài)解算

1、 基于互補(bǔ)濾波基于互補(bǔ)濾波AHRS姿態(tài)解算算法介紹姿態(tài)解算算法介紹Mini INS/GPS姿態(tài)儀姿態(tài)儀介紹內(nèi)容：介紹內(nèi)容： 1、四元數(shù)、四元數(shù) 2、姿態(tài)表示的方法、姿態(tài)表示的方法 3、姿態(tài)解算原理、姿態(tài)解算原理一、四元數(shù)一、四元數(shù)1.1 四元數(shù)定義四元數(shù)定義 其中，其中，i，j，k滿足滿足 i2=j2=k2=-1 ij=-ji=k, jk=-kj=i, ki=-ik=j則稱數(shù)則稱數(shù)q為四元數(shù)，而為四元數(shù)，而q0稱為四元數(shù)稱為四元數(shù)q的實部，稱的實部，稱 為為q的虛部。的虛部。 四元數(shù)的共軛為四元數(shù)的共軛為 1.2 四元數(shù)的表示方式四元數(shù)的表示方式一、四元數(shù)一、四元數(shù)1.1 四元數(shù)定義四元數(shù)定義

2、 設(shè)設(shè) , 其中，其中，i，j，k滿足滿足 i2=j2=k2=-1 ij=-ji=k, jk=-kj=i, ki=-ik=j則稱數(shù)則稱數(shù)q為四元數(shù)，而為四元數(shù)，而q0稱為四元數(shù)稱為四元數(shù)q的實部，稱的實部，稱 為為q的虛部。的虛部。 四元數(shù)的共軛為四元數(shù)的共軛為 0123qijkqqqq0123,Rq q q q 1.2 四元數(shù)的表示方式四元數(shù)的表示方式一、四元數(shù)一、四元數(shù)1.1 四元數(shù)定義四元數(shù)定義 設(shè)設(shè) , 其中，其中，i，j，k滿足滿足 i2=j2=k2=-1 ij=-ji=k, jk=-kj=i, ki=-ik=j則稱數(shù)則稱數(shù)q為四元數(shù)，而為四元數(shù)，而q0稱為四元數(shù)稱為四元數(shù)q的實部，

3、稱的實部，稱 為為q的虛部。的虛部。 四元數(shù)的共軛為四元數(shù)的共軛為 0123qijkqqqq0123,Rq q q q 1.2 四元數(shù)的表示方式四元數(shù)的表示方式一、四元數(shù)一、四元數(shù)1.1 四元數(shù)定義四元數(shù)定義 設(shè)設(shè) , 其中，其中，i，j，k滿足滿足 i2=j2=k2=-1 ij=-ji=k, jk=-kj=i, ki=-ik=j 則稱數(shù)則稱數(shù)q為四元數(shù)，為四元數(shù)， 而而q0稱為四元數(shù)稱為四元數(shù)q的實部，的實部， 稱稱 為為q的虛部。的虛部。 四元數(shù)的共軛為四元數(shù)的共軛為 0123qijkqqqq0123,Rq q q q 1.2 四元數(shù)的表示方式四元數(shù)的表示方式一、四元數(shù)一、四元數(shù)1.1 四

4、元數(shù)定義四元數(shù)定義 設(shè)設(shè) , 其中，其中，i，j，k滿足滿足 i2=j2=k2=-1 ij=-ji=k, jk=-kj=i, ki=-ik=j 則稱數(shù)則稱數(shù)q為四元數(shù)，為四元數(shù)， 而而q0稱為四元數(shù)稱為四元數(shù)q的實部，的實部， 稱稱 為為q的虛部。的虛部。 四元數(shù)的共軛為四元數(shù)的共軛為 0123qijkqqqq0123,Rq q q q123ijkq qq 1.2 四元數(shù)的表示方式四元數(shù)的表示方式一、四元數(shù)一、四元數(shù)1.1 四元數(shù)定義四元數(shù)定義 設(shè)設(shè) , 其中，其中，i，j，k滿足滿足 i2=j2=k2=-1 ij=-ji=k, jk=-kj=i, ki=-ik=j 則稱數(shù)則稱數(shù)q為四元數(shù)，為

5、四元數(shù)， 而而q0稱為四元數(shù)稱為四元數(shù)q的實部，的實部， 稱稱 為為q的虛部。的虛部。 四元數(shù)的共軛為四元數(shù)的共軛為 0123qijkqqqq0123,Rq q q q123ijkq qq0123ijkqqqqq 1.2 四元數(shù)的示方式四元數(shù)的示方式1.3 四元數(shù)運(yùn)算四元數(shù)運(yùn)算二、東北天坐標(biāo)系二、東北天坐標(biāo)系 東北天坐標(biāo)系（東北天坐標(biāo)系（表示為表示為n系系）是一種當(dāng)?shù)氐乩碜鴺?biāo)系，原點位于導(dǎo)航系統(tǒng)）是一種當(dāng)?shù)氐乩碜鴺?biāo)系，原點位于導(dǎo)航系統(tǒng)所處的位置所處的位置P點，坐標(biāo)軸指向北、東和當(dāng)?shù)卮咕€方向（向下），也有稱為點，坐標(biāo)軸指向北、東和當(dāng)?shù)卮咕€方向（向下），也有稱為北東地坐標(biāo)系北東地坐標(biāo)系三、姿態(tài)表示

6、方法三、姿態(tài)表示方法 三、姿態(tài)表示方法三、姿態(tài)表示方法 3.1 方向余弦矩陣方向余弦矩陣 方向余弦矩陣用符號方向余弦矩陣用符號 表示，是一個表示，是一個3*3階的矩陣，矩陣的列表示階的矩陣，矩陣的列表示載體坐標(biāo)系中的單位矢量在參考坐標(biāo)系中的投影。載體坐標(biāo)系中的單位矢量在參考坐標(biāo)系中的投影。 nbC三、姿態(tài)表示方法三、姿態(tài)表示方法 3.1 方向余弦矩陣方向余弦矩陣 方向余弦矩陣用符號方向余弦矩陣用符號 表示，是一個表示，是一個3*3階的矩陣，矩陣的列表示階的矩陣，矩陣的列表示載體坐標(biāo)系中的單位矢量在參考坐標(biāo)系中的投影。載體坐標(biāo)系中的單位矢量在參考坐標(biāo)系中的投影。 nbC1112132122233

7、13233nbcccCcccccc三、姿態(tài)表示方法三、姿態(tài)表示方法 3.1 方向余弦矩陣方向余弦矩陣 方向余弦矩陣用符號方向余弦矩陣用符號 表示，是一個表示，是一個3*3階的矩陣，矩陣的列表示階的矩陣，矩陣的列表示載體坐標(biāo)系中的單位矢量在參考坐標(biāo)系中的投影。載體坐標(biāo)系中的單位矢量在參考坐標(biāo)系中的投影。 第第i行、行、j列的元素表示參考坐標(biāo)系列的元素表示參考坐標(biāo)系i軸和載體坐標(biāo)系軸和載體坐標(biāo)系j軸夾角的余弦。軸夾角的余弦。 在載體坐標(biāo)系中定義的矢量在載體坐標(biāo)系中定義的矢量 ，可以通過該矢量左乘方向余弦矩陣，可以通過該矢量左乘方向余弦矩陣 ，即，即 nbC111213212223313233nbc

8、ccCccccccbrnbCnnbbCrr3.2 歐拉角歐拉角 一個坐標(biāo)系到另一個坐標(biāo)系的變換，可以通過繞不同坐標(biāo)軸的一個坐標(biāo)系到另一個坐標(biāo)系的變換，可以通過繞不同坐標(biāo)軸的3次次連續(xù)轉(zhuǎn)動來實現(xiàn)。從參考系到一個新的坐標(biāo)系的變換可以表示：連續(xù)轉(zhuǎn)動來實現(xiàn)。從參考系到一個新的坐標(biāo)系的變換可以表示： 繞參考坐標(biāo)系的繞參考坐標(biāo)系的z軸轉(zhuǎn)動軸轉(zhuǎn)動 角角 繞新坐標(biāo)系的繞新坐標(biāo)系的y軸轉(zhuǎn)動軸轉(zhuǎn)動 角角 繞新坐標(biāo)系的繞新坐標(biāo)系的x軸轉(zhuǎn)動軸轉(zhuǎn)動 角角 稱為歐拉轉(zhuǎn)動角稱為歐拉轉(zhuǎn)動角3.3 四元數(shù)四元數(shù) 四元數(shù)姿態(tài)表達(dá)式是一個四參數(shù)的表達(dá)式。它的基本思路是：一個坐標(biāo)四元數(shù)姿態(tài)表達(dá)式是一個四參數(shù)的表達(dá)式。它的基本思路是：

9、一個坐標(biāo)系到另一個坐標(biāo)系的變換可以通過繞一個定義在參考系中的矢量系到另一個坐標(biāo)系的變換可以通過繞一個定義在參考系中的矢量 的單的單次轉(zhuǎn)動來實現(xiàn)。次轉(zhuǎn)動來實現(xiàn)。 四元數(shù)用符號四元數(shù)用符號q表示，它是一個具有表示，它是一個具有4個元素的矢量，這些元素是該矢量個元素的矢量，這些元素是該矢量方向和轉(zhuǎn)動大小的函數(shù)。方向和轉(zhuǎn)動大小的函數(shù)。定義定義 的大小和方向是使參考系繞的大小和方向是使參考系繞 轉(zhuǎn)動一個角度轉(zhuǎn)動一個角度 ，就能與載體坐，就能與載體坐標(biāo)系重合。標(biāo)系重合。3.3 四元數(shù)四元數(shù) 四元數(shù)姿態(tài)表達(dá)式是一個四參數(shù)的表達(dá)式。它的基本思路是：一個坐標(biāo)四元數(shù)姿態(tài)表達(dá)式是一個四參數(shù)的表達(dá)式。它的基本思路是：

10、一個坐標(biāo)系到另一個坐標(biāo)系的變換可以通過繞一個定義在參考系中的矢量系到另一個坐標(biāo)系的變換可以通過繞一個定義在參考系中的矢量 的單的單次轉(zhuǎn)動來實現(xiàn)。次轉(zhuǎn)動來實現(xiàn)。 四元數(shù)用符號四元數(shù)用符號q表示，它是一個具有表示，它是一個具有4個元素的矢量，這些元素是該矢量個元素的矢量，這些元素是該矢量方向和轉(zhuǎn)動大小的函數(shù)。方向和轉(zhuǎn)動大小的函數(shù)。定義定義 的大小和方向是使參考系繞的大小和方向是使參考系繞 轉(zhuǎn)動一個角度轉(zhuǎn)動一個角度 ，就能與載體坐，就能與載體坐標(biāo)系重合。標(biāo)系重合。3.3 四元數(shù)四元數(shù) 四元數(shù)姿態(tài)表達(dá)式是一個四參數(shù)的表達(dá)式。它的基本思路是：一個坐標(biāo)四元數(shù)姿態(tài)表達(dá)式是一個四參數(shù)的表達(dá)式。它的基本思路是：

11、一個坐標(biāo)系到另一個坐標(biāo)系的變換可以通過繞一個定義在參考系中的矢量系到另一個坐標(biāo)系的變換可以通過繞一個定義在參考系中的矢量 的單的單次轉(zhuǎn)動來實現(xiàn)。次轉(zhuǎn)動來實現(xiàn)。 四元數(shù)用符號四元數(shù)用符號q表示，它是一個具有表示，它是一個具有4個元素的矢量，這些元素是該矢量個元素的矢量，這些元素是該矢量方向和轉(zhuǎn)動大小的函數(shù)。方向和轉(zhuǎn)動大小的函數(shù)。定義定義 的大小和方向是使參考系繞的大小和方向是使參考系繞 轉(zhuǎn)動一個角度轉(zhuǎn)動一個角度 ，就能與載體坐，就能與載體坐標(biāo)系重合。標(biāo)系重合。3.3 四元數(shù)四元數(shù) 四元數(shù)姿態(tài)表達(dá)式是一個四參數(shù)的表達(dá)式。它的基本思路是：一個坐標(biāo)四元數(shù)姿態(tài)表達(dá)式是一個四參數(shù)的表達(dá)式。它的基本思路是：

12、一個坐標(biāo)系到另一個坐標(biāo)系的變換可以通過繞一個定義在參考系中的矢量系到另一個坐標(biāo)系的變換可以通過繞一個定義在參考系中的矢量 的單的單次轉(zhuǎn)動來實現(xiàn)。次轉(zhuǎn)動來實現(xiàn)。 四元數(shù)用符號四元數(shù)用符號q表示，它是一個具有表示，它是一個具有4個元素的矢量，這些元素是該矢量個元素的矢量，這些元素是該矢量方向和轉(zhuǎn)動大小的函數(shù)。方向和轉(zhuǎn)動大小的函數(shù)。定義定義 的大小和方向是使參考系繞的大小和方向是使參考系繞 轉(zhuǎn)動一個角度轉(zhuǎn)動一個角度 ，就能與載體坐，就能與載體坐標(biāo)系重合。標(biāo)系重合。 利用四元數(shù)進(jìn)行矢量變換利用四元數(shù)進(jìn)行矢量變換 首先定義一個四元數(shù)首先定義一個四元數(shù)rb rb=ix+jy+kz rb=0+ix+jy+k

13、z有：有： rn=q*rb*q rn=(q0+iq1+jq2+kq3)(0+ix+jy+kz)(q0-iq1-jq2-kq3) =0+(q02+q12-q22-q32)x+2(q1q2-q0q3)y+2(q1q3+q0q2)zi +2(q1q2+q0q3)x+(q02-q12+q22-q32)y+2(q2q3-q0q1)zj +2(q1q3-q0q2)x+2(q2q3+q0q1)y+(q02-q12-q22+q32)zkrn也可以表示成矩陣形式也可以表示成矩陣形式nbrc r0 00 cc2222012312031302222212030123230122221302230101232()2(

14、)2()2()2()2()Cq q q qqq q qqq q qqq q qq q q qq q q qqq q qq q q qq q q q3.4 方向余弦、歐拉角和四元數(shù)的關(guān)系方向余弦、歐拉角和四元數(shù)的關(guān)系 這樣，四元數(shù)可以用方向余弦、歐拉角表示，同樣，歐拉角也可以用這樣，四元數(shù)可以用方向余弦、歐拉角表示，同樣，歐拉角也可以用方向余弦和四元數(shù)表示。方向余弦和四元數(shù)表示。2222012312031302222212030123230122221302230101232()2()2()2()2()2()q q q qqq q qqq q qqq q qq q q qq q q qqq q

15、qq q q qq q q q用方向余弦表示四元數(shù)用方向余弦表示四元數(shù) 1203 22 3101 33 1202 11 23011 12 23 321()41()41()4(1)qcccqccqqccqqccq用歐拉角表示四元數(shù)用歐拉角表示四元數(shù) q0 q1 q2 q3對于小角度位移，四元數(shù)參數(shù)可以用表示為對于小角度位移，四元數(shù)參數(shù)可以用表示為用方向余弦表示歐拉角用方向余弦表示歐拉角四元數(shù)表示歐拉角四元數(shù)表示歐拉角 23012222012323011203222201232()arctan()arcsin(2()2()arctan()q qq qqqqqq qq qq qq qqqqq90o四

16、、互補(bǔ)濾波的姿態(tài)解算算法四、互補(bǔ)濾波的姿態(tài)解算算法 姿態(tài)解算常用的算法有歐拉角法、方向余弦法和四元數(shù)法。姿態(tài)解算常用的算法有歐拉角法、方向余弦法和四元數(shù)法。歐拉角法歐拉角法在求在求解姿態(tài)時存在奇點（萬向節(jié)死鎖），不能用于全姿態(tài)的解算；解姿態(tài)時存在奇點（萬向節(jié)死鎖），不能用于全姿態(tài)的解算；方向余弦方向余弦可用于可用于全姿態(tài)的解算但計算量大，不能滿足實時性要求。全姿態(tài)的解算但計算量大，不能滿足實時性要求。四元數(shù)法四元數(shù)法，其計算量小，無，其計算量小，無奇點且可以滿足飛行器運(yùn)動過程中姿態(tài)的實時解算。奇點且可以滿足飛行器運(yùn)動過程中姿態(tài)的實時解算。四、互補(bǔ)濾波的姿態(tài)解算算法四、互補(bǔ)濾波的姿態(tài)解算算法 姿

17、態(tài)解算常用的算法有歐拉角法、方向余弦法和四元數(shù)法。歐拉角法在姿態(tài)解算常用的算法有歐拉角法、方向余弦法和四元數(shù)法。歐拉角法在求解姿態(tài)時存在奇點（萬向節(jié)死鎖），不能用于全姿態(tài)的解算；方向余弦可求解姿態(tài)時存在奇點（萬向節(jié)死鎖），不能用于全姿態(tài)的解算；方向余弦可用于全姿態(tài)的解算但計算量大，不能滿足實時性要求。四元數(shù)法，其計算量用于全姿態(tài)的解算但計算量大，不能滿足實時性要求。四元數(shù)法，其計算量小，無奇點且可以滿足飛行器運(yùn)動過程中姿態(tài)的實時解算。小，無奇點且可以滿足飛行器運(yùn)動過程中姿態(tài)的實時解算。 姿態(tài)解算的原理姿態(tài)解算的原理：對于一個確定的向量，用不同的坐標(biāo)系表示時，他們：對于一個確定的向量，用不同的坐

18、標(biāo)系表示時，他們所表示的大小和方向一定是相同的。但是由于這兩個坐標(biāo)系的旋轉(zhuǎn)矩陣存在所表示的大小和方向一定是相同的。但是由于這兩個坐標(biāo)系的旋轉(zhuǎn)矩陣存在誤差，那么當(dāng)一個向量經(jīng)過這么一個有誤差存在的旋轉(zhuǎn)矩陣后，在另一個坐誤差，那么當(dāng)一個向量經(jīng)過這么一個有誤差存在的旋轉(zhuǎn)矩陣后，在另一個坐標(biāo)系中肯定和理論值是有偏差的，我們通過這個偏差來修正這個旋轉(zhuǎn)矩陣。標(biāo)系中肯定和理論值是有偏差的，我們通過這個偏差來修正這個旋轉(zhuǎn)矩陣。這個旋轉(zhuǎn)矩陣的元素是四元數(shù)，我們修正的就是四元數(shù)，這樣姿態(tài)就被修正這個旋轉(zhuǎn)矩陣的元素是四元數(shù)，我們修正的就是四元數(shù)，這樣姿態(tài)就被修正了。了。 四、互補(bǔ)濾波的姿態(tài)解算算法四、互補(bǔ)濾波的姿態(tài)解

19、算算法 姿態(tài)解算常用的算法有歐拉角法、方向余弦法和四元數(shù)法。歐拉角法在姿態(tài)解算常用的算法有歐拉角法、方向余弦法和四元數(shù)法。歐拉角法在求解姿態(tài)時存在奇點（萬向節(jié)死鎖），不能用于全姿態(tài)的解算；方向余弦可求解姿態(tài)時存在奇點（萬向節(jié)死鎖），不能用于全姿態(tài)的解算；方向余弦可用于全姿態(tài)的解算但計算量大，不能滿足實時性要求。四元數(shù)法，其計算量用于全姿態(tài)的解算但計算量大，不能滿足實時性要求。四元數(shù)法，其計算量小，無奇點且可以滿足飛行器運(yùn)動過程中姿態(tài)的實時解算。小，無奇點且可以滿足飛行器運(yùn)動過程中姿態(tài)的實時解算。 姿態(tài)解算的原理：對于一個確定的向量，用不同的坐標(biāo)系表示時，他們姿態(tài)解算的原理：對于一個確定的向量，

20、用不同的坐標(biāo)系表示時，他們所表示的大小和方向一定是相同的。但是由于這兩個坐標(biāo)系的旋轉(zhuǎn)矩陣存在所表示的大小和方向一定是相同的。但是由于這兩個坐標(biāo)系的旋轉(zhuǎn)矩陣存在誤差，那么當(dāng)一個向量經(jīng)過這么一個有誤差存在的旋轉(zhuǎn)矩陣后，在另一個坐誤差，那么當(dāng)一個向量經(jīng)過這么一個有誤差存在的旋轉(zhuǎn)矩陣后，在另一個坐標(biāo)系中肯定和理論值是有偏差的，我們通過這個偏差來修正這個旋轉(zhuǎn)矩陣。標(biāo)系中肯定和理論值是有偏差的，我們通過這個偏差來修正這個旋轉(zhuǎn)矩陣。這個旋轉(zhuǎn)矩陣的元素是四元數(shù)，我們修正的就是四元數(shù)，這樣姿態(tài)就被修正這個旋轉(zhuǎn)矩陣的元素是四元數(shù)，我們修正的就是四元數(shù)，這樣姿態(tài)就被修正了。了。4.1 互補(bǔ)濾波器解算原理互補(bǔ)濾波器解

21、算原理 陀螺儀陀螺儀動態(tài)響應(yīng)特性良好，但計算姿態(tài)時會產(chǎn)生累積誤差。動態(tài)響應(yīng)特性良好，但計算姿態(tài)時會產(chǎn)生累積誤差。磁力計和加磁力計和加速度計速度計測量姿態(tài)沒有累積誤差，但動態(tài)響應(yīng)較差。因此他們在頻域上特性互測量姿態(tài)沒有累積誤差，但動態(tài)響應(yīng)較差。因此他們在頻域上特性互補(bǔ)，可以采用互補(bǔ)濾波器融合這三種傳感器的數(shù)據(jù)，提高測量精度和系統(tǒng)的補(bǔ)，可以采用互補(bǔ)濾波器融合這三種傳感器的數(shù)據(jù)，提高測量精度和系統(tǒng)的動態(tài)性能。動態(tài)性能。 互補(bǔ)濾波器的傳遞函數(shù)互補(bǔ)濾波器的傳遞函數(shù) 12( )( ),( )( )( )C sssssC ssC sGG因此，通過互補(bǔ)濾波器能夠消除高頻噪聲和低頻誤差的累積，能夠很好的融合各傳

22、感器的數(shù)據(jù)用用C（s）表示飛行器運(yùn)動過程中真實的姿態(tài)矩陣，表示飛行器運(yùn)動過程中真實的姿態(tài)矩陣，互補(bǔ)濾波器互補(bǔ)濾波器估計的姿態(tài)估計的姿態(tài)矩陣矩陣加速度計和磁力計加速度計和磁力計觀測到的姿態(tài)矩陣觀測到的姿態(tài)矩陣0HCC觀測到觀測到高頻噪高頻噪聲聲陀螺儀測量得到的數(shù)據(jù)陀螺儀測量得到的數(shù)據(jù)計算得到的姿態(tài)矩陣計算得到的姿態(tài)矩陣LCC低頻誤差累積低頻誤差累積 G1具有一階低通濾波特性，具有一階低通濾波特性，G2具有一階具有一階高通濾波特性高通濾波特性C（s）=kp+ki/s（1）初始化四元數(shù)）初始化四元數(shù) 姿態(tài)計算初始，將已知載體初始姿態(tài)角帶入下式，求出初始時刻的四姿態(tài)計算初始，將已知載體初始姿態(tài)角帶入下

23、式，求出初始時刻的四元數(shù)。元數(shù)。 q0 q1 q2 q34.2 四元數(shù)姿態(tài)解算步驟四元數(shù)姿態(tài)解算步驟（2）獲取角速度、加速度、磁力計值）獲取角速度、加速度、磁力計值 讀取到的讀取到的加速度加速度測量值測量值ax,ay,az;陀螺儀陀螺儀測量值測量值wx,wy,wz；磁力計磁力計測量值測量值 mx,my,mz (3) 將加速度計測量值、磁力計測量值化為單位向量將加速度計測量值、磁力計測量值化為單位向量（4）從四元數(shù)里獲得重力向量和磁場向量）從四元數(shù)里獲得重力向量和磁場向量 重力向量重力向量 13022301222012322220123120313020222201203012323011222

24、21302230101232()2()2()2()2()2()2()2()Txyzqqqqq qq qq qq qvq qq qqqqqq qq qvvq qq qq qq qqqqqq qq qq qq qqqq 2qn系中，加速度計系中，加速度計輸出（重力向量）輸出（重力向量）bCn（表示從（表示從n系到系到b系轉(zhuǎn)換矩陣）系轉(zhuǎn)換矩陣）乘以加速度計的實際測量值為加速度計的實際測量值為 (ax,ay,az)T，(vx,vy,vz)T和和(ax,ay,az)T 均表示在均表示在b系中系中的豎直向下的向量的豎直向下的向量(ex,ey,ez)T = (vx,vy,vz)T 叉乘叉乘(ax,ay,az

25、)T 地磁場向量地磁場向量可不可以采用和加速度計一樣的修正方法來修正？可不可以采用和加速度計一樣的修正方法來修正？加速度計在靜止時測量的是重力加速度加速度計在靜止時測量的是重力加速度(0,0,1) ，是有大小和方向的；，是有大小和方向的；而是與而是與x軸（或者軸（或者y軸）呈一個角度，與軸）呈一個角度，與z軸呈一個角度。記作軸呈一個角度。記作(bx,by,bz)T ，這里我，這里我們讓們讓x軸對準(zhǔn)北邊，所以軸對準(zhǔn)北邊，所以by=0，即，即 (bx,0,bz)T同理，地磁計同樣測量的是地球磁場的大小和方向，只不過這個方向不再是豎直向下同理，地磁計同樣測量的是地球磁場的大小和方向，只不過這個方向不再是豎直向下倘若我們知道倘若我們知道bx和和bz的精確值，那么我們就可以采用和加速度計一樣的修正方法來修正。的精確值，那么我們就可以采用和加速度計一樣的修正方法來修正。只不過在加速度計中，我們在只不過在加速度計中，我們在n系中的參考向量是（系中的參考向量是（0,0,1) ，變成了地磁計的，變成了地磁計的 (bx,0,bz)T但實際上，當(dāng)?shù)氐卮艌鱿鄬τ跂|北天坐標(biāo)的夾角無法測量但實際上，當(dāng)?shù)氐卮艌鱿鄬τ跂|北天坐標(biāo)的夾角無法測量我們的做法：反過來從我們的做法：反過來從b系推往系推往n系系22231 20 31 30