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

假设AHRS测得的拖拉机三轴姿态角横滚角度、俯仰角度和航向角度分别为：，，，则拖拉机车体坐标系下的空间矢量向大地导航坐标系转换的转换矩阵nbR2为：coscossincossincossinsinsincossinsinsincoscoscossincossincossinsinsinsincoscossincoscoscossin0sincos0001cos0sinsinsincoscossinsincossincoscoscossin0sincos0001cos0sin010sin0cos1000cossin0sincos2XYZnbRRRR(1)假设导航控制点到GPS天线的空间矢量在车体坐标系下的表达形式为Thba，GPS天线在大地导航坐标系下的坐标为Tgggzyx，导航控制点在大地导航坐标系下的坐标为Tccczyx，则有：hbaRzyxzyxnbgggccc2（2）xyzThedefinitionusedfor'Euler‐angles'hereisequivalentto'roll,pitch,yaw/heading'(alsoknownasCardan).TheEuler‐anglesareofXYZEarthfixedtype(subsequentrotationaroundglobalX,YandZaxis,alsoknownasaerospacesequence).FromMTimanual.卡尔曼滤波器的推导及工作流程设定k时刻拖拉机本体的真实航向角度为'k，拖拉机本体纵向方向上的真实前进速度是'kv，拖拉机本体纵向方向上的真实加速度是'ka，则kkkk'（3）vkvkkkvv'（）akakkkaa'（）其中，k、kv、ka分别指航向角度、前进速度和加速度的测量值，k、vk、ak分别指航向角度、前进速度和加速度的测量偏差值，k、vk、ak分别指航向角度、前进速度和加速度的随机测量误差。基于牛顿第二定律，建立导航控制点在2D平面坐标系下的运动方程：dtvxxkkckck'cos'1dtvyykkckck'sin'1将（3）式代入上述表达式，得到𝑥𝑐𝑘=𝑥𝑐𝑘−1+𝑣𝑘𝑐𝑜𝑠𝜓𝑘𝑑𝑡−𝑣𝑘𝜀𝜓𝑘𝑠𝑖𝑛𝜓𝑘𝑑𝑡𝑦𝑐𝑘=𝑦𝑐𝑘−1+𝑣𝑘𝑠𝑖𝑛𝜓𝑘𝑑𝑡+𝑣𝑘𝜀𝜓𝑘𝑐𝑜𝑠𝜓𝑘𝑑𝑡将上述等式以卡尔曼滤波器状态转移方程的形式表示为：kkkkkubXAX1其中，],,[kckckkyxX，表示状态空间向量；kckkkkkkydtvdtvAckx100cos10sin01，是状态转移矩阵，由陀螺仪累积航向角度和前进速度的测量值实时更新；0sincosdtvdtvbkkkkk，],0,0[kku，是状态转移方程的白噪声序列；系统过程噪声协方差矩阵为kQ，用于表示状态转移方程的误差大小，本发明中kQ设定为常数矩阵，在仿真和实验过程中整定矩阵参数。以GPS天线在大地导航坐标系下的定位坐标作为观测向量，结合坐标转换公式（1），得到卡尔曼滤波器的测量方程如下：kkkkgkdXHZ其中，gkgkgkyxZ，ykkH1001xk，ykxkkd，gykgxkk将（3）式代入（1）式，kcoscossincossincossinsinsincossinsinsincoscoscossincossincossinsinsinsincoscossincoscos=kcoscossincossincossin)sin(sin)cos(sinsin)sin(cos)cos(cos)sin(cossin)cos(sin)sin(sinsin)cos(cos)sin(cos)cos(kcoscossincossincossin)cos(sinsin)sin(cossinsin)cos(sincos)sin(coscos)cos(sincossin)sin(cossin)cos(sinsinsin)sin(coscos)cos(sincos)sin(coskcoscossincossincossincossinsincossinsinsincossinsincoscossinsinsinsincoscoscoscoscossincossinsinsincoscossincossinsinsinsinsincoscossinsincoscossincossincoscoscossinsinsinsincoscossincossincossinxkkkkkkkkkkkkkabbhhcoscossinsincoscossinsinsinsincoscosxkkkkkkkkkkkkkabbhhcoscossinsincoscossinsinsinsincoscosykkkkkkkkkkkkkabbhhcossinsinsinsincoscossincossincossinykkkkkkkkkkkkkabbhhgxk，gyk表示OEMGPS板卡定位在水平面坐标系下的随机定位误差。测量向量的噪声方差矩阵为：2200ykxkkrrR其中，2xr，2yr分别为gxk，gyk的方差统计值。综合上述推导，采用线性离散卡尔曼滤波器的递归差分方程进行状态向量预测和测量向量校正：预测方程组为：kkkkbxAxˆˆ11kTkkkkQAPAP校正方程组为：1)(kTkkkTkkkRHPHHPK)ˆ(ˆˆkkkgkkkkdxHZKxxkkkkPHKIP)(