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

卡尔曼滤波实验报告2014年4月GPS静/动态滤波实验一、实验要求1、分别建立GPS静态及动态卡尔曼滤波模型，编写程序对静态和动态GPS数据进行Kalman滤波。2、对比滤波前后导航轨迹图。3、画出滤波过程中估计均方差(P阵对角线元素开根号)的变化趋势。4、思考：①简述动态模型与静态模型的区别与联系；②R阵、Q阵，P0阵的选取对滤波精度及收敛速度有何影响，取值时应注意什么；③本滤波问题是否可以用最小二乘方法解决，如果可以，请阐述最小二乘方法与Kalman滤波方法的优劣对比。二、实验原理2.1GPS静态滤波选取系统的状态变量为[]TLhX,其中L为纬度(deg),为经度(deg)，h为高度(m)。设()wt为零均值高斯白噪声，则系统的状态方程为：310()wtX(1)所以离散化的状态模型为：,111kkkkkWXX(2)式中，,1kk为33单位阵，kW为系统噪声序列。测量数据包括：纬度静态量测值、经度静态量测值和高度构成31矩阵Z，量测方程可以表示为：kkkZHXV(3)式中，H为33单位阵，kV为量测噪声序列。系统的状态模型是十分准确的，所以系统模型噪声方差阵可以取得十分小，取Q阵零矩阵。系统测量噪声方差阵R由测量确定，由于位置量测精度为5m，采用克拉索夫斯基地球椭球模型，长半径eR为6378245m，短半径pR为6356863m。所以R阵为：2225180()0051800()0cos()005peRRLR(4)2.2GPS动态滤波动态滤波基于当前统计模型，在地球坐标系下解算。选取系统的状态变量为TxxxyyyzzzXxvayvazva,其中,,,xxxxva依次为地球坐标系下x轴上的位置、速度、加速度和位置误差分量，,yz轴同理。系统的状态模型可以表示为：()()()()ttttXAXUW(5)式中，位置误差视为有色噪声，为一阶马尔科夫过程，可表示为：xxxxyyyyzzzz111(6)其中，i(,,ixyz)为对应马尔科夫过程的相关时间常数，(,,)iwixyz为零均值高斯白噪声。系统矩阵A可表示为：444444444444000000xyzAAAA(7)其中，0100001000/0000/11iiaiA(i=x,y,z)输入量U可表示为：T()000000000xyzyxzaaaaaatU(8)式中，(,,)iaixyz为机动加速度的当前均值，其自适应确定方法为：,/1ˆxkkkax，同理可得,,ykzkaa、。系统噪声为：T()000000xyzaxayaztW(9)量测量为纬度动态量测值、经度动态量测值、高度和三向速度量测值。由于滤波在地球坐标系下进行，为了简便首先将纬度、经度和高度转化为三轴位置坐标值，转化方式如下：(Re)cos()cos()(Re)cos()sin()(Re)sin()xhLyhLzhL(10)所以，滤波的量测量为三轴位置坐标值和三轴速度测量值，即[]TxyzZxyzvvv。量测方程为：Z=HX+V(11)式中，100100000000000010010000000000001001010000000000000001000000000000000100H，V为零均值高斯白噪声。综上，离散化的Kalman滤波方程为：1/1/11/1/111111/111/111/1/1/1111/()()()kkkkkkkkkkkkkTTkkkkkkkkkTkkkkkkkkkkkkkXΦXXXKZHXKPHHPHRPΦPΦQPIKHP(12)式中，1/1/1/1/000000xkkykkkkzkkΦΦΦΦ，21//1/20010(,,)0010000iikkTTTTixyzeΦ，1//1/1/000000xkkykkkkzkk1ΦΦΦΦ，/2/1///1(/1)001(1)0(,,)000000aiiiaiiaiiTaaTiakkTTTTeeixyzeeΦ离散化的系统噪声协方差阵为：222222000000xyzaxayazdiagQ，机动加速度自适应确定方法为：222max1/max22max4π4πˆˆ[][]ππ4πˆ[]? πxxakkkakaxaxax00“当前”加速度“当前”加速度(13)离散化量测噪声协方差阵为：222xyzxyzvvvdiagRRRRRR222R。三、实验结果3.1GPS静态滤波图1GPS静态滤波前后导航轨迹图和估计误差3.2GPS动态滤波05001000150020002500300039.977439.977439.977439.977439.977439.977439.977439.977439.977439.9774纬度时间/s纬度/deg估计纬度测量纬度050010001500200025003000116.3468116.3468116.3468116.3468116.3468116.3468116.3468116.3468116.3468116.3468经度时间/s经度/deg估计经度测量经度05001000150020002500300020406080100120140高度时间/s高度/m估计高度测量高度050010001500200025003000-1012345x10-5时间/s纬度估计误差/deg均方误差相对误差050010001500200025003000-1012345x10-5时间/s经度估计误差/deg均方误差相对误差050010001500200025003000-100-80-60-40-20020时间/s高度估计误差/m均方误差相对误差01002003004005006004040.0240.0440.0640.0840.140.12纬度时间/s纬度/deg估计纬度测量纬度0100200300400500600115.99116116.01116.02116.03116.04116.05116.06经度时间/s经度/deg估计经度测量经度010020030040050060098599099510001005101010151020高度时间/s高度/m估计高度测量高度0100200300400500600-20246810121416东向速度时间/s速度/(m/s)估计速度测量速度01002003004005006001415161718192021北向速度时间/s速度/(m/s)估计速度测量速度0100200300400500600-0.1-0.08-0.06-0.04-0.0200.020.040.060.080.1天向速度时间/s速度(m/s)估计速度测量速度0100200300400500600-1012345678x10-7时间/s纬度估计误差相对误差均方误差0100200300400500600-2024681012x10-7时间/s经度估计误差相对误差均方误差图2GPS动态滤波前后导航轨迹图和估计误差四、实验讨论1．简述动态模型与静态模型的区别与联系。静态模型的速度和加速度均为0，系统静止，状态模型比较准确，模型误差较小，量测信息只有位置信息。动态模型系统的速度和加速度均发生变化，采用当前统计模型建模，相比之下，系统模型的误差较大，量测信息由位置和速度信息。静态模型是动态模型在速度和加速度均为0时的特殊情况。2．R阵、Q阵，P0阵的选取对滤波精度及收敛速度有何影响，取值时应注意什么。R阵的取值对滤波精度的影响很大，当R取得太大，系统就不能有效的利用量测信息对状态进行修正，因此滤波精度较低；相反，R取得太小，系统过分依赖量测信息，无法利用状态模型有效的去除有害的量测噪声，同样降低滤波的精度。Q阵的取值对滤波精度的影响也很大：Q取得太大，系统就不能有效的利用状态模型对测量噪声进行修正，因此滤波精度就较低；反之，Q取得太小，系统就会过分的依赖状态模型的精度，以致量测信息无法对状态进行有效的修正，也会降低滤波精度；只有当R和Q的取值恰好与使用的状态模型的精度相吻合时，才能使状态模型和量测信息都能有效的发挥作用，互相补充，得到最高的滤波精度。P0阵的取值对于可观测性良好的系统，只影响开始的滤波精度，对收敛精度影响不大，但影响收敛速度。3）本滤波问题是否可以用最小二乘方法解决，如果可以，请阐述最小二乘方法与Kalman滤波方法的优劣对比。本滤波问题可以用最小二乘方法解决。最小二乘方法的最大优点是算法简单，特别是对0100200300400500600-20-15-10-505101520时间/s高度估计误差相对误差均方误差0100200300400500600-0.04-0.03-0.02-0.0100.010.020.030.040.05时间/s东向速度估计误差相对误差均方误差0100200300400500600-0.03-0.02-0.0100.010.020.030.040.05时间/s北向速度估计误差相对误差均方误差0100200300400500600-0.03-0.02-0.0100.010.020.030.040.05时间/s天向速度估计误差相对误差均方误差一般的最小二乘估计，根本不必知道量测误差的统计信息。但又存在使用上的局限性，该方法只能估计确定性的常值向量，而无法估计随机向量的时间过程；最小二乘的最优指标只保证了量测的误差平方和最小，而并未确保被估计量的估计误差达到最佳，因此该方法的估计精度不高。而卡尔曼滤波是一种线性最小方差估计，其算法是递推的，且使用状态空间法在时域内设计滤波器，适用于多维随机过程的估计；卡尔曼滤波的估计量可以是平稳的，也可以是非平稳的；卡尔曼滤波具有连续型和离散型两类算法，离散型算法可直接在数字计算机上实现。