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

假设AHRS测得的拖拉机三轴姿态角横滚角度、俯仰角度和航向角度分别为：。，们羽，则拖拉机车体坐标系下的空间矢量向大地导航坐标系转换的转换矩阵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。假设导航控制点到GPS天线的空间矢量在车体坐标系下的表达形式为[abh「，GPS天线在大地导航坐标系下的坐标为RygZgf,导航控制点在大地导航坐标系下的坐标为依yc则有：(2)Thedefinitionusedfor'Euler-angles*hereisequivalentto'roll,pitch,yaw/heading*(alsoknownasCardan).TheEuler-anglesareofXYZEarthfixedtype(subsequentrotationaroundglobalX,YandZaxis,alsoknownasaerospacesequence).FromMTimanual.卡尔曼滤波器的推导及工作流程设定k时刻拖拉机本体的真实航向角度为为.拖拉机本体纵向方向上的真实前进速度是Vk'，拖拉机本体纵向方向上的真实加速度是a「，则TOC\o"1-5"\h\z¥4'=气+公+必 (3)《=%+%+& ()ak'=ak+公+盆 ()其中，必、*、ak分别指航向角度、前进速度和加速度的测量值，公、&、%k分别指航向角度、前进速度和加速度的测量偏差值，成、鼻、Mk分别指航向角度、前进速度和加速度的随机测量误差。基于牛顿第二定律，建立导航控制点在2D平面坐标系下的运动方程：xck=xtk-i+vk，C0S¥<k，dtYck=Yck-i+v/sn^/dt将(3)式代入上述表达式，得到xCk= 1+wcosipkdt-VkS^kSinipkdtyCk=yCk-i+vksimpkdt+VkE^cosipkdt将上述等式以卡尔曼滤波器状态转移方程的形式表示为：Xk=AXi+b^+Uk其中，Xk=[x^’yck,公]，表示状态空间向量；10-vksiny/kdt'yck,是状态转移矩阵，由陀螺仪累积航向角度劣=01vyck,是状态转移矩阵，由陀螺仪累积航向角度00 1

和前进速度的测量值实时更新;vkcosy/kdt九=VkSin^dt,风=[0,0,归]，是状态转移方程的白噪声序列；系统过程0噪声协方差矩阵为Q,用于表示状态转移方程的误差大小，本发明中Q设定为常数矩阵，在仿真和实验过程中整定矩阵参数。以GPS天线在大地导航坐标系下的定位坐标作为观测向量，结合坐标转换公式（1）,得到卡尔曼滤波器的测量方程如下：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板卡定位在水平面坐标系下的随机定位误差。测虽:向量的噪声方差矩阵为:F