卡尔曼滤波器的推导及工作流程

1、假设AHRS测得的拖拉机三轴姿态角横滚角度、俯仰角度和航向角度分别为：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分别指航向角度、前进速度和加速度的测量偏差值，鼻、久、分别指航向角度、前进速度和加速度的随机测量误差。基于牛顿第二定律，建立导航控制点在2D平面坐标系下的运动方程：xck=xtk-

4、i+vk，C0Sk，dtYck=Yck-i+vg0kdt将(3)式代入上述表达式，得到Xck=%cfc-i+vkcosipkdt-vkksinpkdtyCk=yCk-i+UkSin叭dt+Uk邛kCos叭dt将上述等式以卡尔曼滤波器状态转移方程的形式表示为：Xk=4Xk_i+q+Uk其中，Xk=xyck,%，表示状态空间向量；yck,是状态转移矩阵，由陀螺仪累积航向角度10-vksiny/kdt4=01vkcosy/kdt001和前进速度的测量值实时更新;vkcosydtbk=VkSiiWkdt,吐=0,0,鼻,是状态转移方程的白噪声序列；系统过程0噪声协方差矩阵为Q,用于表示状态转移方程的误

5、差大小，本发明中Q设定为常数矩阵，在仿真和实验过程中整定矩阵参数。以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其