北航卡尔曼滤波实验报告-GPS静动态滤波实验

1、三尔曼滤波实验报告2014年4月GPS静/动态滤波实验一、实验要求1、 分别建立 GPS静态及动态卡尔曼滤波模型，编写程序对静态和动态GPS数据进行Kalman 滤波。2、对比滤波前后导航轨迹图。3、画出滤波过程中估计均方差 (P阵对角线元素开根号)的变化趋势。4、思考： 简述动态模型与静态模型的区别与联系； R阵、Q阵，P0阵的选取对滤波精度及收敛速度有何影响，取值时应注意什么；本滤波问题是否可以用最小二乘方法解决，如果可以，请阐述最小二乘方法与Kalman滤波方法的优劣对比。二、实验原理2.1 GPS静态滤波选取系统的状态变量为X L hT ,其中L为纬度(deg), 为经度(deg)，h

2、为高度(m)。设w(t)为零均值高斯白噪声，则系统的状态方程为:031w(t)所以离散化的状态模型为:Xkk,k 1Xk 1Wk 1式中，k,k 1为3 3单位阵，Wk为系统噪声序列。测量数据包括：纬度静态量测值、经度静态量测值和高度构成 可以表示为：3 1矩阵Z，量测方程Zk HXk Vk式中，H为3 3单位阵，V为量测噪声序列。系统的状态模型是十分准确的，所以系统模型噪声方差阵可以取得十分小，取Q阵零矩阵。系统测量噪声方差阵 R由测量确定，由于位置量测精度为 5m,采用克拉索夫斯基地球椭球模型，长半径 R.为6378245m，短半径Rp为6356863m。所以R阵为:5 18O、2(R(5

3、 180)2(Re COS(L)0522.2 GPS动态滤波动态滤波基于当前统计模型，X x Vx ax x y Vy ay y z Vz az在地球坐标系下解算。选取系统的状态变量为T其中Z5 丿、Ix, vx, ax, x依次为地球坐标系下x轴上的位置、速度、加速度和位置误差分量, y,z轴同理。系统的状态模型可以表示为:戏(t) AX (t) U(t) W(t)式中，位置误差视为有色噪声，为一阶马尔科夫过程，可表示为:Wx&yWy&zWz其中，i(i x, y, z)为对应马尔科夫过程的相关时间常数,Wi(ix,y,z)为零均值高斯白噪声。系统矩阵A可表示为:Ax04 4

4、04 404 4Ay04 404 404 4Az0其中，Ai 01/(i=x, y,z)ai1/输入量U可表示为:(10)Z = HX +V(11)U(t)0 0邑axay0 00 0aya 0az(8)式中，ai(i x,y,z)为机动加速度的当前均值，其自适应确定方法为:ax,k?/k 1 ，同理可得Oyfaz,k。系统噪声为:W (t)0 0 Wax Wx 0 0 Wa Wy 0 0 Waz Wz量测量为纬度动态量测值、经度动态量测值、高度和三向速度量测值。(9)由于滤波在地球坐标系下进行，为了简便首先将纬度、经度和高度转化为三轴位置坐标值，转化方式如下：x (Re h)cos(L)co

5、s( y (Re h)cos( L)sin( z (Re h)sin( L)所以，滤波的量测量为三轴位置坐标值和三轴速度测量值,Z xyzVxVyVzT。量测方程为:0(13)式中，H0 ，V为零均值高斯白噪声。综上，离散化的Kalma n滤波方程为:X k 1/k k 1/k X kXk 1X k 1/k Kk 1(Zk 1 Hk1X k 1/k )Kk 1Pk 1/k H k 1 (H k 1Pk 1/kHRk1)1(12)Pk 1/k k 1/k Pk k 1/k Q kPk 1(I Kk1H k 1 )Pk 1/k式中，k1/kX k 1/k0T2/20z1/k， k 1/k0 (iT

6、/ ix, y, z), k 1/k木X k 1/k001/k0aiT/ a2ai0 k 1/k1/k离散化的系统噪声协方差阵为:机动加速度自适应确定方法为:2ax2ax4n aLmaxn4 na maxn纂 1/kXk2离散化量测噪声协方差阵为:diag(1三、实验结果3.1 GPS静态滤波e")T/ aai(ix,y,z)2ay0eT/i2az4n aL Umaxn?当前”加速度x&2“当前”加速度0diag R2 Ry R； R： R： r2纬度经度时间/S度经 116.3468140116.3468116.3468116.3468116.3468116.3468116

7、.3468116.3468116.3468116.3468时间/S200估计咼度测量高度1高度12010080604050010002000250030001500时间/Sx 105-1均方误差1一，相对误差宀-5432105001000200025001500时间/S03000时间/S-5x 1020-100均方误差1相对误差严4r0-20-40-60-8005001000200025001500时间/S30003.2 GPS动态滤波图1 GPS静态滤波前后导航轨迹图和估计误差40.12度纬40.140.0840.0640.0440.02400纬度经度F度高W高度1ftni111O5估计高度

8、' I测量高度W599O589uro七 iSliiiinrrTW11 ir100200400500600300时间/S天向速度*$度速北向速度时间/sO70.10.080.060.040.020-0.02-0.04-0.06-0.08-0.1II;估计速度测量速度-V EI V- : I；W ML' l f!'ir"ii;II bLti liJn/HI. Im H门 皿山 lUlilMim欄 MMT*0100200300时间/S400500600差误计估度纬O O相对误差1-.I 拆舌 扌早圭 1.100200400500-7108 7 6 53 212300

9、时间/S600-20七-h_-_.l1-«相对误差1 均方误差-7x 101086420100200400500300时间/S600-0.01相对误差10 04 HiiHI均方误差0 03r0 020.01.d II,L1J 1,±L_JilLLiJi.J,0-0.02-0.01-0.02-0.03 I11110100200300400500600时间/S叩叩ITIF 附EINNWpqE 血d価血拥M山川h也仙皿-0.03 L0100200300400500600时间/S图2 GPS动态滤波前后导航轨迹图和估计误差四、实验讨论模型误差较小，量测信采用当前统计模型建模， 相比

10、 静态模型是动态模型在速度和加1 .简述动态模型与静态模型的区别与联系。静态模型的速度和加速度均为0,系统静止，状态模型比较准确,息只有位置信息。动态模型系统的速度和加速度均发生变化， 之下，系统模型的误差较大， 量测信息由位置和速度信息。速度均为0时的特殊情况。2. R阵、Q阵，P0阵的选取对滤波精度及收敛速度有何影响，取值时应注意什么。R阵的取值对滤波精度的影响很大，当R取得太大，系统就不能有效的利用量测信息对状态进行修正，因此滤波精度较低；相反，R取得太小，系统过分依赖量测信息，无法利用状态模型有效的去除有害的量测噪声，同样降低滤波的精度。Q阵的取值对滤波精度的影响也很大：Q取得太大，系

11、统就不能有效的利用状态模型对测量噪声进行修正，因此滤波精度就较低；反之，Q取得太小，系统就会过分的依赖状态模型的精度，以致量测信息无法对 状态进行有效的修正，也会降低滤波精度；只有当R和Q的取值恰好与使用的状态模型的精度相吻合时，才能使状态模型和量测信息都能有效的发挥作用，互相补充，得到最高的滤波精度。P0阵的取值对于可观测性良好的系统，只影响开始的滤波精度，对收敛精度影响 不大，但影响收敛速度。3）本滤波问题是否可以用最小二乘方法解决，如果可以，请阐述最小二乘方法与 Kalman滤波方法的优劣对比。本滤波问题可以用最小二乘方法解决。最小二乘方法的最大优点是算法简单，特别是对一般的最小二乘估计， 根本不必知道量测误差的统计信息。但又存在使用上的局限性，该方法只能估计确定性的常值向量，而无法估计随机向量的时间过程