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

1、卡尔曼滤波实验报告2014年4月GPS静/动向滤波实验一、实验要求1、分别成立GPS静态及动向卡尔曼滤波模型，编写程序对静态和动向GPS数据进行Kalman滤波。2、对照滤波前后导航轨迹图。3、画出滤波过程中预计均方差(P阵对角线元素开根号)的变化趋向。4、思虑：简述动向模型与静态模型的差别与联系；R阵、Q阵，P0阵的选用对滤波精度及收敛速度有何影响，取值时应注意什么；本滤波问题能否能够用最小二乘方法解决，假如能够，请论述最小二乘方法与Kalman滤波方法的好坏对照。二、实验原理2.1GPS静态滤波选用系统的状态变量为XLhT,此中L为纬度(deg),为经度(deg)，h为高度。设w(t)为零

2、均值高斯白噪声，则系统的状态方程为：&031w(t)(1)X所以失散化的状态模型为：Xkk,k1Xk1Wk1(2)式中，k,k1为33单位阵，Wk为系统噪声序列。丈量数据包含：纬度静态量测值、经度静态量测值和高度组成31矩阵Z，量测方程能够表示为：ZkHXkVk(3)式中，H为33单位阵，Vk为量测噪声序列。系统的状态模型是十分正确的，所以系统模型噪声方差阵能够获得十分小，取Q阵零矩阵。系统丈量噪声方差阵R由丈量确立，因为地点量测精度为5m，采纳克拉索夫斯基地球椭球模型，长半径Re为6378245m，短半径Rp为6356863m。所以R阵为：(5180)200RpR0(5180)20(4)Re

3、cos(L)00522.2GPS动向滤波动向滤波鉴于目前统计模型，在地球坐标系下解算。选用系统的状态变量为XxvxaxxyvyayyzvzazTx挨次为地球坐标系下xz,此中x,vx,ax,轴上的地点、速度、加快度和地点偏差重量，y,z轴同理。系统的状态模型能够表示为：X&(t)AX(t)U(t)W(t)(5)式中，地点偏差视为有色噪声，为一阶马尔科夫过程，可表示为：&1wxxxx&y1wy(6)yy&1wzzzz此中，i(ix,y,z)为对应马尔科夫过程的有关时间常数，wi(ix,y,z)为零均值高斯白噪声。系统矩阵A可表示为：Ax044044A044Ay044(7)044044Az0100

4、此中，Ai0010001/ai(i=x,y,z)00001/i输入量U可表示为：Taxay000az0(8)U(t)00000axayazaiixyzax,k?式中，(,)为灵活加快度的目前均值，其自适应确立方法为：&同理可得ay,k、az,k。系统噪声为：W(t)00waxwx00waywy00wazwzT(9)量丈量为纬度动向量测值、经度动向量测值、高度和三向速胸怀测值。因为滤波在地球坐标系下进行，为了简易第一将纬度、经度和高度转变为三轴地点坐标值，转变方式以下：x(Reh)cos(L)cos()y(Reh)cos(L)sin()(10)z(Reh)sin(L)所以，滤波的量丈量为三轴地点

5、坐标值和三轴速度丈量值，即ZxyzvxvyvzT。量测方程为：Z=HX+V(11)100100000000000010010000式中，H000000001001，V为零均值高斯白噪声。010000000000000001000000000000000100综上，失散化的Kalman滤波方程为：Xk1/kk1/kXkXk1Xk1/kKk1(Zk1Hk1Xk1/k)Kk1Pk1/kHkT1(Hk1Pk1/kHTk1Rk1)1(12)Pk1/kk1/kPkTk1/kQkPk1(IKk1Hk1)Pk1/kx001TT2/20k1/k01T0式中，k1/k0ky1/k0，ki1/k(ix,y,z)，0

6、0kz1/k0010000eT/i1T(T/a1eT/ai)a2kx00ii1/k01(1eT/ai)a0y0ik1/kk1/k，k1/ki00(ix,y,z)00kz1/k00eT/a0i000eT/i离散化的系统噪声协方差阵为：Qdiag002200220022，灵活加快度自适应确立方法为：axxayyazz24?24?2“目前”加快度axamax&xk1/kamax&xk04?(13)22“目前”加快度axamax&0 xk?失散化量测噪声协方差阵为：RdiagRx2Ry2Rz2Rv2xRv2yRv2z。三、实验结果3.1GPS静态滤波纬度116.3468经度39.9774预计纬度预计经

7、度39.9774丈量纬度116.3468丈量经度39.9774116.346839.9774116.3468g39.9774g116.3468eedd度39.9774度116.3468纬经39.9774116.346839.9774116.346839.9774116.346839.9774116.3468050010001500200025003000050010001500200025003000时间/s时间/s高度x10-51405均方偏差相对偏差120预计高度4丈量高度100g3ed/m差/80误2度计高估度60纬14002050010001500200025003000-150010

8、00150020002500300000时间/s时间/sx10-5205均方偏差均方偏差相对偏差相对偏差40g3m-20e/差差误误2-40计计估估度度高经1-600-80-150010001500200025003000-1005001000150020002500300000时间/s时间/s图1GPS静态滤波前后导航轨迹图和预计偏差3.2GPS动向滤波纬度经度40.12116.06预计纬度预计经度丈量纬度116.05丈量经度40.1116.0440.08gg116.03eedd40.06度度纬经116.0240.04116.0140.0211640115.99010020030040050

9、06000100200300400500600时间/s时间/s高度东向速度102016预计高度预计速度1015丈量高度14丈量速度12101010)m1005s8/m度/度高10006速499529900985100200300400500600-210020030040050060000时间/s时间/s北向速度天向速度210.1预计速度预计速度20丈量速度0.08丈量速度0.06190.04)0.02s18smm(0/(度17度速速-0.0216-0.04-0.0615-0.0814100200300400500600-0.110020030040050060000时间/s时间/sx10-7

10、x10-7812相对偏差相对偏差7均方偏差10均方偏差685差差6误4误计计估3估4度度纬经22100-1100200300400500600-210020030040050060000时间/s时间/s200.05相对偏差相对偏差15均方偏差0.04均方偏差100.035差0.02差误误计0.01计0估估度0度速高-5向东-0.01-10-0.02-15-0.03-20-0.0401002003004005006000100200300400500600时间/s时间/s0.050.05相对偏差相对偏差0.04均方偏差0.04均方偏差0.030.03差0.02差0.02误误计计估0.01估0.0

11、1度度速速向0向0北天-0.01-0.01-0.02-0.02-0.03100200300400500600-0.0310020030040050060000时间/s时间/s图2GPS动向滤波前后导航轨迹图和预计偏差四、实验议论1简述动向模型与静态模型的差别与联系。静态模型的速度和加快度均为0，系统静止，状态模型比较正确，模型偏差较小，量测信息只有地点信息。动向模型系统的速度和加快度均发生变化，采纳目前统计模型建模，对比之下，系统模型的偏差较大，量测信息由地点和速度信息。静态模型是动向模型在速度和加速度均为0时的特别状况。2R阵、Q阵，P0阵的选用对滤波精度及收敛速度有何影响，取值时应注意什么

12、。R阵的取值对滤波精度的影响很大，当R获得太大，系统就不可以有效的利用量测信息对状态进行修正，所以滤波精度较低；相反，R获得太小，系统过分依靠量测信息，没法利用状态模型有效的去除有害的量测噪声，相同降低滤波的精度。Q阵的取值对滤波精度的影响也很大：Q获得太大，系统就不可以有效的利用状态模型对丈量噪声进行修正，所以滤波精度就较低；反之，Q获得太小，系统就会过分的依靠状态模型的精度，致使量测信息没法对状态进行有效的修正，也会降低滤波精度；只有当R和Q的取值恰巧与使用的状态模型的精度相符合时，才能使状态模型和量测信息都能有效的发挥作用，相互增补，获得最高的滤P0不大，但影响收敛速度。3）本滤波问题能否能够用最小二乘方法解决，假如能够，请论述最小二乘方法与Kalman滤波方法的好坏对照。本滤波问题能够用最小二乘方法解决。最小二乘方法的最大长处是算法简单，特别是对一般的最小二乘预计，根本不用知道量测偏差的统计信息。但又存在使用上的限制性，该方法只好预计确立性