卡爾曼濾波器的推導(dǎo)及工作流程

假設(shè)AHRS測(cè)得的拖拉機(jī)三軸姿態(tài)角橫滾角度、俯仰角度和航向角度分別為：。，們羽，則拖拉機(jī)車體坐標(biāo)系下的空間矢量向大地導(dǎo)航坐標(biāo)系轉(zhuǎn)換的轉(zhuǎn)換矩陣K2n為：Kb2n=V^YR/cosy/—sin#o"cos。0sill0100sint//cost//00100cos?—sin。001-sin<90COS00sin。cosScos—cos。-siny/cos/sin0100sin/cos0cosy/sini//sin00CCS?—sin。-sin<90cos。0sin?cos。cos—cos。一siny/cos?+cosy/sinsin?sin“sin。+cos—sinOcos。sini//cos0cosy/cos?+siny/sinOsin破一cosy/sin?+sin“sin0cos。-sin。 cosCsin。 cosOcos。假設(shè)導(dǎo)航控制點(diǎn)到GPS天線的空間矢量在車體坐標(biāo)系下的表達(dá)形式為[abh「，GPS天線在大地導(dǎo)航坐標(biāo)系下的坐標(biāo)為RygZgf,導(dǎo)航控制點(diǎn)在大地導(dǎo)航坐標(biāo)系下的坐標(biāo)為依yc則有：(2)Thedefinitionusedfor'Euler-angles*hereisequivalentto'roll,pitch,yaw/heading*(alsoknownasCardan).TheEuler-anglesareofXYZEarthfixedtype(subsequentrotationaroundglobalX,YandZaxis,alsoknownasaerospacesequence).FromMTimanual.卡爾曼濾波器的推導(dǎo)及工作流程設(shè)定k時(shí)刻拖拉機(jī)本體的真實(shí)航向角度為為.拖拉機(jī)本體縱向方向上的真實(shí)前進(jìn)速度是Vk'，拖拉機(jī)本體縱向方向上的真實(shí)加速度是a「，則TOC\o"1-5"\h\z￥4'=氣+公+必 (3)《=%+%+& ()ak'=ak+公+盆 ()其中，必、*、ak分別指航向角度、前進(jìn)速度和加速度的測(cè)量值，公、&、%k分別指航向角度、前進(jìn)速度和加速度的測(cè)量偏差值，成、鼻、Mk分別指航向角度、前進(jìn)速度和加速度的隨機(jī)測(cè)量誤差?；谂ｎD第二定律，建立導(dǎo)航控制點(diǎn)在2D平面坐標(biāo)系下的運(yùn)動(dòng)方程：xck=xtk-i+vk，C0S￥<k，dtYck=Yck-i+v/sn^/dt將(3)式代入上述表達(dá)式，得到xCk= 1+wcosipkdt-VkS^kSinipkdtyCk=yCk-i+vksimpkdt+VkE^cosipkdt將上述等式以卡爾曼濾波器狀態(tài)轉(zhuǎn)移方程的形式表示為：Xk=AXi+b^+Uk其中，Xk=[x^’yck,公]，表示狀態(tài)空間向量；10-vksiny/kdt'yck,是狀態(tài)轉(zhuǎn)移矩陣，由陀螺儀累積航向角度劣=01vyck,是狀態(tài)轉(zhuǎn)移矩陣，由陀螺儀累積航向角度00 1

和前進(jìn)速度的測(cè)量值實(shí)時(shí)更新;vkcosy/kdt九=VkSin^dt,風(fēng)=[0,0,歸]，是狀態(tài)轉(zhuǎn)移方程的白噪聲序列；系統(tǒng)過(guò)程0噪聲協(xié)方差矩陣為Q,用于表示狀態(tài)轉(zhuǎn)移方程的誤差大小，本發(fā)明中Q設(shè)定為常數(shù)矩陣，在仿真和實(shí)驗(yàn)過(guò)程中整定矩陣參數(shù)。以GPS天線在大地導(dǎo)航坐標(biāo)系下的定位坐標(biāo)作為觀測(cè)向量，結(jié)合坐標(biāo)轉(zhuǎn)換公式（1）,得到卡爾曼濾波器的測(cè)量方程如下：7故=瓦為+。+*其中，Zgk=，Hk=「107」，dk=&-,"k=_ygk__。1，yk__%七將（3）式代入（1）式,sin/sin。+cos—sinQcos。一cos#sin?+sin—sinOcos?

COS0COS。cos—cos0-sin(//cos^+cos—sinOsin。sin/sin。+cos—sinQcos。一cos#sin?+sin—sinOcos?

COS0COS。一sin。 cos。sin。cosS+5)cos。一sin(/+幻cos?+cos(i//+c)sinsinsin(/+￡)sin+cos(,+?sinOcos。sin((//+?cos。cos(/+r)cos^+sin(—+￡)sinsin一cos(—+c)sin+sin(/+QsinQcos。一sin。 cosQsin? cosQcos。(cosy/-￡sin#)cos0-(sm*+ecos—)cos?+(cos#-￡$in材)sinOsrnS(sin#+￡cos")sin0+(cos#-^sin^)sin^cos^(sin#+￡cosy/)co(cosy/-￡smH)cosS+(sinj+ecosQsinOsin。-(cosy/-csin—)sin?+(sin—+￡cos#)sin0cos。-sin。 cosOsin。 cosOcosScos#cos0-￡sin#cos8-smy/cos^+cos材sinOsin。一￡cos#cosS-￡sin#sin8sm。sin#sin?+cos#sin0cosS+￡cosy/sin^sinv/sin^cos^sm*cos0+￡cos*cos8cos材cos?+sinjsmOsm。一￡sin#cosS+￡cos#sin0sinS-cos#sin?+stn#sin0cos?+￡sin^sin^+￡cos#sin0cos。-sin。 cosOsinS cosOcos。々=-acos^sing-bsin^sin%sin%-bcosRcos%+hsmRcos%-hsn^cos豹sin%&=acos^cos%+bsin^sinRcos%-bcosRsin%+hsm^sin%+hanqcqs0]cos%尊=acos旦cos必+bsin^sin^\cos%-bcos的sin%+hsin^sm%+hsin^cos%cos%勾=acosqsin%+bsin^sm%sin%+bcosRcos%-hsm%cos%+hsin弓cos0sm必，X表示OEMGPS板卡定位在水平面坐標(biāo)系下的隨機(jī)定位誤差。測(cè)雖:向量的噪聲方差矩陣為:F