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

1、假设AHRS 测得的拖拉机三轴姿态角横滚角度、俯仰角度和航向角度分别为：，则拖拉机车体坐标系下的空间矢量向大地导航坐标系转换的转换矩阵R b 2n 为：R b 2n =R R R Z Y X00cos -sin 0cos 0sin 100cos -sin =sin cos 010010-sin 0cos 0sin cos 00cos cos -sin cos sin 1 =sin cos cos sin sin 0cos -sin 0cos -sin 0sin cos cos cos -sin cos +cos sin sin sin sin +cos sin cos =sin cos cos

2、 cos +sin sin sin -cos sin +sin sin cos cos sin cos cos -sin (1假设导航控制点到GPS 天线的空间矢量在车体坐标系下的表达形式为aTGPS 天线在大地导航坐标系下的坐标为x g b h ，y gT导航控制点z g ，在大地导航坐标系下的坐标为x cy cz c ，则有：Tx c x g a y =y -R b （2） b 2n c g z c h z g The definition used for 'Eulerangles' here is equivalent to 'roll, pitch, yaw/

3、heading' (also known as Cardan. The Eulerangles are of XYZ Earth fixed type (subsequent rotation around global X, Y and Z axis, also known as aerospace sequence. From MTi manual.卡尔曼滤波器的推导及工作流程设定k 时刻拖拉机本体的真实航向角度为k ' ，拖拉机本体纵向方向上的真实前进速度是v k ' ，拖拉机本体纵向方向上的真实加速度是a k ' ，则k ' =k +k +k （

4、3）v k ' =v k +vk +vk （）a k ' =a k +ak +ak （）其中，前进速度和加速度的测量值，k 、vk 、k 、v k 、a k 分别指航向角度、ak 分别指航向角度、前进速度和加速度的测量偏差值，k 、vk 、ak 分别指航向角度、前进速度和加速度的随机测量误差。基于牛顿第二定律，建立导航控制点 在2D 平面坐标系下的运动方程：x ck =x ck -1+v k ' cos k ' dt y ck =y ck -1+v k ' sin k ' dt将（3）式代入上述表达式，得到xck=xck1+vkcoskdtvkk

5、sinkdt yck=yck1+vksinkdt+vkkcoskdt将上述等式以卡尔曼滤波器状态转移方程的形式表示为：X k =A k X k -1+b k +u k其中，X k =x ck , y ck , k ，表示状态空间向量；10-v k sin k dt x ck y ，是状态转移矩阵，由陀螺仪累积航向角度A k =01v cos dt k k ck 100k 和前进速度的测量值实时更新；v k cos k dt ，b k =v sin dt k k u k =0, 0, k ，是状态转移方程的白噪声序列；系统过程0噪声协方差矩阵为Q k ，用于表示状态转移方程的误差大小，本发明中Q

6、 k 设定为常数矩阵，在仿真和实验过程中整定矩阵参数。以GPS 天线在大地导航坐标系下的定位坐标作为观测向量，结合坐标转换公式（1），得到卡尔曼滤波器的测量方程如下：Z gk =H k X k +d k +k10xk xk gxk x gk d =其中，Z gk =，H k =，k k01y yk yk gyk gk 将（3）式代入（1）式，cos cos sin cos -sin -sin cos +cos sin sin cos cos +sin sin sin cos sin sin sin +cos sin cos -cos sin +sin sin cos =cos cos ksin

7、(+ sin +cos(+ sin cos -cos(+ sin +sin(+ sin cos =cos cos k+ cos cos(sin(+ cos -sin -sin(+ cos +cos(+ sin sin cos(+ cos +sin(+ sin sin cos sin (cos-sin cos (sin+cos cos -(sin+cos cos +(cos-sin sin sin (cos-sin cos +(sin+cos sin sin (sin+cos sin +(cos-sin sin cos -(cos-sin sin +(sin+cos sin cos =-sin

8、cos sin cos cos -sin cos -sin cos +cos sin sin -cos cos -sin sin sin sin cos +cos cos cos cos +sin sin sin -sin cos +cos sin sin -sin cos sin xk =-a cos k sin k -b sin k sin k sin k -b cos k cos k +h sin k cos k-h sin sin k cos k kxk =a cos k cos k +b sin k sin k cos k -b cos k sin k +h sin k sin k+

9、h sin k cos k cos kyk =a cos k cos k +b sin k sin k cos k -b cos k sin k +h sin k sin k+h sin k cos k cos kyk =a cos k sin k +b sin k sin k sin k +b cos k cos k -h sin k cos k+h sin k cos k sin kgxk ，gyk 表示OEM GPS板卡定位在水平面坐标系下的随机定位误差。cos cos ksin sin +cos sin cos +cos sin -sin sin cos -cos sin +sin sin cos +sin sin +cos sin cos cos cos k测量向量的噪声方差矩阵为：r xk 2R k =002 r yk 其中，r x 2，r y 2分别为gxk ，gyk 的方差统计值。综合上述推导，采用线性离散卡尔曼滤波器的递归差分方程进行状态向量预测和测量向量校正： 预