卡爾曼濾波器的推導及工作流程

1、假設AHRS測得的拖拉機三軸姿態(tài)角橫滾角度、俯仰角度和航向角度分別為：0,0,0，則拖拉機車體坐標系下的空間矢量向大地導航坐標系轉換的轉換矩陣加為：cos屮-silli/0cos。0si100shipcosp00100cos0-sin0001-sin00cosC0sin0coscosy/cos。一sin%cos/sin010o:sii】pcos&cospsinsin0COS0_sin0-sin(90cosQ0sin0cos。cosy/cos。一sinpcos0+cospsinCsii】0sini/sin0+cospsin&cos0sii】pcos&cospcos0+sin(/sinsin0一c

2、ospsin0+sinpsin0cos0-sin9cos&sin0COS&COS0(2)假設導航控制點到GPS天線的空間矢量在車體坐標系下的表達形式為abhf,GPS天線在大地導航坐標系下的坐標為%ygZgf，導航控制點在大地導航坐標系下的坐標為Ryc則有：yc=Xyg_Rb2nab_Zc_z6_hThedefinitionusedforEuler-angles*hereisequivalenttoroll,pitch,yaw/heading*(alsoknownasCardan).TheEuler-anglesareofXYZEarthfixedtype(subsequentrotation

3、aroundglobalX,YandZaxis,alsoknownasaerospacesequence).FromMTimanual.卡爾曼濾波器的推導及工作流程設定k時刻拖拉機本體的真實航向角度為拖拉機本體縱向方向上的真實前進速度是,拖拉機本體縱向方向上的真實加速度是，則TOC o 1-5 h z0k=0k+%+d(3)()ak-ak+fak+akO其中，、弘分別指航向角度、前進速度和加速度的測量值，弘、色、0k分別指航向角度、前進速度和加速度的測量偏差值，鼻、久、分別指航向角度、前進速度和加速度的隨機測量誤差?；谂ｎD第二定律，建立導航控制點在2D平面坐標系下的運動方程：xck=xtk-

4、i+vk，C0Sk，dtYck=Yck-i+vg0kdt將(3)式代入上述表達式，得到Xck=%cfc-i+vkcosipkdt-vkksinpkdtyCk=yCk-i+UkSin叭dt+Uk邛kCos叭dt將上述等式以卡爾曼濾波器狀態(tài)轉移方程的形式表示為：Xk=4Xk_i+q+Uk其中，Xk=xyck,%，表示狀態(tài)空間向量；yck,是狀態(tài)轉移矩陣，由陀螺儀累積航向角度10-vksiny/kdt4=01vkcosy/kdt001和前進速度的測量值實時更新;vkcosydtbk=VkSiiWkdt,吐=0,0,鼻,是狀態(tài)轉移方程的白噪聲序列；系統(tǒng)過程0噪聲協(xié)方差矩陣為Q,用于表示狀態(tài)轉移方程的誤

5、差大小，本發(fā)明中Q設定為常數(shù)矩陣，在仿真和實驗過程中整定矩陣參數(shù)。以GPS天線在大地導航坐標系下的定位坐標作為觀測向量，結合坐標轉換公式(1),得到卡爾曼濾波器的測量方程如下：Z廠HkX.+4+匕其中，Zgk二九1Zyk*dk=sin妙sin0+cosy/sincos一cospsin0+sii】psii】8cos0COS0COS0將(3)式代入(1)式,cospcos。一sin0cos0+cospsiii&sin0sinpcos。cospcos+sin(/sinOsin0一sincosOsincosfy/+)cos&一sin(p+)cos0+cos(i/+c)sinsinsii】(p+)sin

6、+cos(p+)sii】0cos0sii】(0+)cos0cos(p+)cos0+sin(p+)sinsin一cos(p+c)sin(j)+sin(p+)sin0cos0一sin。cos0sin0cos0cos0表示OEMGPS板卡定位在水平面坐標系下的隨機定位誤差。(cos-sin0)cos&(sin0+cosy/)co-sin&-(sin0+cos#)cos0+(cos0$in肖)sin&sin。(cosy/-sni0)cos/+(sin0+cos0)sin&sm/cos&smQ(sinp+cos肖)sin0+(cosp-sin0)sin&cos0-(cos-$in#)sin0+(sin#

7、+wcosRsin&cDs。=cos&cos。cos0cos&-sm#cos&-sin0cos0+cospsin&sin。一wcospcos/-wsinpsin&sm。sinsin+cos肖sin&cos0+cosy/sinsin0sin&cDs0sin肖cos&+cos0cos&cosy/cos+sinsinsin-sin0cos/+wcospsin&sin/-cospsm0+sin#sin&cos0+$insin+wcospsin&cos/-sin&cosGsin/coscos=-acosQsin竹一banqanan%-bcosgcos%4-hsincosy/、-hsncossin7=acoscos竹+banqancos%-bcossin+hansin/+hanqcos%cos必=acos久cos%+bsinqsm內cos妁-bcos內sin+hsinsm妁+hsinqcoscos溝/=acosqsin+bsinqsm%sin必+b8scos溝-hsrncos%+hsinqcos內sinj測址向雖的噪聲方差矩陣為:prj0其