1、假設(shè)AHRS 測得的拖拉機(jī)三軸姿態(tài)角橫滾角度、俯仰角度和航向角度分別為：，則拖拉機(jī)車體坐標(biāo)系下的空間矢量向大地導(dǎo)航坐標(biāo)系轉(zhuǎn)換的轉(zhuǎn)換矩陣R b 2n 為：R b 2n =R R R Z Y X00cos -sin 0cos 0sin 100cos -sin =sin cos 010010-sin 0cos 0sin cos 00cos cos -sin cos sin 1 =sin cos cos sin sin 0cos -sin 0cos -sin 0sin cos cos cos -sin cos +cos sin sin sin sin +cos sin cos =sin cos cos

2、 cos +sin sin sin -cos sin +sin sin cos cos sin cos cos -sin (1假設(shè)導(dǎo)航控制點(diǎn)到GPS 天線的空間矢量在車體坐標(biāo)系下的表達(dá)形式為aTGPS 天線在大地導(dǎo)航坐標(biāo)系下的坐標(biāo)為x g b h ，y gT導(dǎo)航控制點(diǎn)z g ，在大地導(dǎo)航坐標(biāo)系下的坐標(biāo)為x cy cz c ，則有：Tx c x g a y =y -R b （2） b 2n c g z c h z g The definition used for 'Eulerangles' here is equivalent to 'roll, pitch, yaw/

3、heading' (also known as Cardan. The Eulerangles are of XYZ Earth fixed type (subsequent rotation around global X, Y and Z axis, also known as aerospace sequence. From MTi manual.卡爾曼濾波器的推導(dǎo)及工作流程設(shè)定k 時(shí)刻拖拉機(jī)本體的真實(shí)航向角度為k ' ，拖拉機(jī)本體縱向方向上的真實(shí)前進(jìn)速度是v k ' ，拖拉機(jī)本體縱向方向上的真實(shí)加速度是a k ' ，則k ' =k +k +k （

4、3）v k ' =v k +vk +vk （）a k ' =a k +ak +ak （）其中，前進(jìn)速度和加速度的測量值，k 、vk 、k 、v k 、a k 分別指航向角度、ak 分別指航向角度、前進(jìn)速度和加速度的測量偏差值，k 、vk 、ak 分別指航向角度、前進(jìn)速度和加速度的隨機(jī)測量誤差。基于牛頓第二定律，建立導(dǎo)航控制點(diǎn) 在2D 平面坐標(biāo)系下的運(yùn)動(dòng)方程：x ck =x ck -1+v k ' cos k ' dt y ck =y ck -1+v k ' sin k ' dt將（3）式代入上述表達(dá)式，得到xck=xck1+vkcoskdtvkk

5、sinkdt yck=yck1+vksinkdt+vkkcoskdt將上述等式以卡爾曼濾波器狀態(tài)轉(zhuǎn)移方程的形式表示為：X k =A k X k -1+b k +u k其中，X k =x ck , y ck , k ，表示狀態(tài)空間向量；10-v k sin k dt x ck y ，是狀態(tài)轉(zhuǎn)移矩陣，由陀螺儀累積航向角度A k =01v cos dt k k ck 100k 和前進(jìn)速度的測量值實(shí)時(shí)更新；v k cos k dt ，b k =v sin dt k k u k =0, 0, k ，是狀態(tài)轉(zhuǎn)移方程的白噪聲序列；系統(tǒng)過程0噪聲協(xié)方差矩陣為Q k ，用于表示狀態(tài)轉(zhuǎn)移方程的誤差大小，本發(fā)明中Q

6、 k 設(shè)定為常數(shù)矩陣，在仿真和實(shí)驗(yàn)過程中整定矩陣參數(shù)。以GPS 天線在大地導(dǎo)航坐標(biāo)系下的定位坐標(biāo)作為觀測向量，結(jié)合坐標(biāo)轉(zhuǎn)換公式（1），得到卡爾曼濾波器的測量方程如下：Z gk =H k X k +d k +k10xk xk gxk x gk d =其中，Z gk =，H k =，k k01y yk yk gyk gk 將（3）式代入（1）式，cos cos sin cos -sin -sin cos +cos sin sin cos cos +sin sin sin cos sin sin sin +cos sin cos -cos sin +sin sin cos =cos cos ksin

7、(+ sin +cos(+ sin cos -cos(+ sin +sin(+ sin cos =cos cos k+ cos cos(sin(+ cos -sin -sin(+ cos +cos(+ sin sin cos(+ cos +sin(+ sin sin cos sin (cos-sin cos (sin+cos cos -(sin+cos cos +(cos-sin sin sin (cos-sin cos +(sin+cos sin sin (sin+cos sin +(cos-sin sin cos -(cos-sin sin +(sin+cos sin cos =-sin

8、cos sin cos cos -sin cos -sin cos +cos sin sin -cos cos -sin sin sin sin cos +cos cos cos cos +sin sin sin -sin cos +cos sin sin -sin cos sin xk =-a cos k sin k -b sin k sin k sin k -b cos k cos k +h sin k cos k-h sin sin k cos k kxk =a cos k cos k +b sin k sin k cos k -b cos k sin k +h sin k sin k+

9、h sin k cos k cos kyk =a cos k cos k +b sin k sin k cos k -b cos k sin k +h sin k sin k+h sin k cos k cos kyk =a cos k sin k +b sin k sin k sin k +b cos k cos k -h sin k cos k+h sin k cos k sin kgxk ，gyk 表示OEM GPS板卡定位在水平面坐標(biāo)系下的隨機(jī)定位誤差。cos cos ksin sin +cos sin cos +cos sin -sin sin cos -cos sin +sin sin cos +sin sin +cos sin cos cos cos k測量向量的噪聲方差矩陣為：r xk 2R k =002 r yk 其中，r x 2，r y 2分別為gxk ，gyk 的方差統(tǒng)計(jì)值。綜合上述推導(dǎo)，采用線性離散卡爾曼濾波器的遞歸差分方程進(jìn)行狀態(tài)向量預(yù)測和測量向量校正： 預(yù)