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

《卡尔曼滤波器的推导及工作流程.docx》由会员上传分享，免费在线阅读，更多相关内容在教育资源-天天文库。

1、假设AHRS测得的拖拉机三轴姿态角横滚角度、俯仰角度和航向角度分别为：，，，则拖拉机车体坐标系下的空间矢量向大地导航坐标系转换的转换矩阵为：(1)假设导航控制点到GPS天线的空间矢量在车体坐标系下的表达形式为，GPS天线在大地导航坐标系下的坐标为，导航控制点在大地导航坐标系下的坐标为，则有：（2）The definition used for 'Euler‐angles' here is equivalent to 'roll, pitch, yaw/heading' (also known as Cardan). The Euler‐angles are

2、 of XYZ Earth fixed type (subsequent rotation around global X, Y and Z axis, also known as aerospace sequence). FromMTimanual.卡尔曼滤波器的推导及工作流程设定k时刻拖拉机本体的真实航向角度为，拖拉机本体纵向方向上的真实前进速度是，拖拉机本体纵向方向上的真实加速度是，则（3）（）（）其中，、、分别指航向角度、前进速度和加速度的测量值，、、分别指航向角度、前进速度和加速度的测量偏差值，、、分别指航向角度、前进速度和加速度的随机测量误差。

3、基于牛顿第二定律，建立导航控制点在2D平面坐标系下的运动方程：将（3）式代入上述表达式，得到xck=xck-1+vkcosψkdt-vkεψksinψkdtyck=yck-1+vksinψkdt+vkεψkcosψkdt将上述等式以卡尔曼滤波器状态转移方程的形式表示为：其中，，表示状态空间向量；，是状态转移矩阵，由陀螺仪累积航向角度和前进速度的测量值实时更新；，，是状态转移方程的白噪声序列；系统过程噪声协方差矩阵为，用于表示状态转移方程的误差大小，本发明中设定为常数矩阵，在仿真和实验过程中整定矩阵参数。以GPS天线在大地导航坐标系下的定位坐标作为观测向量，

4、结合坐标转换公式（1），得到卡尔曼滤波器的测量方程如下：其中，，，，将（3）式代入（1）式，=，表示OEMGPS板卡定位在水平面坐标系下的随机定位误差。测量向量的噪声方差矩阵为：其中，，分别为，的方差统计值。综合上述推导，采用线性离散卡尔曼滤波器的递归差分方程进行状态向量预测和测量向量校正：预测方程组为：校正方程组为：