卡尔曼滤波在目标跟踪中应用仿真研究

卡尔曼滤波在目标跟踪中应用仿真研究【摘要】目标跟踪问题的应用背景是雷达数据处理，即雷达在搜索到目标并记录目标的位置数据，对测量到的目标位置数据（称为点迹）进行处理，自动形成航迹，并对目标在下一时刻的位置进行预测。本文简要讨论了用KALMAN滤波方法对单个目标航迹进行预测，并借助于MATLAB仿真工具，对实验的效果进行评估。关键词KALMAN滤波、目标跟踪、MATLAB仿真1情景假设假定有一个二座标雷达对一平面上运动的目标进行观测，目标在04T秒沿Y轴作恒速直线运动，运动速度为15米/秒，目标的起始点为2000米,10000米，在406T秒向轴方向做的慢转弯，加速度均为0075米/秒2，完成慢转弯后加X09速度将降为零，从秒开始做的快转弯，加速度为03米/秒2，在660秒结束转1T弯，加速度降至零。雷达扫描周期秒，和独立地进行观测，观测噪声的标准差2TXY均为100米。2KALMAN滤波算法分析为了简单起见，仅对轴方向进行考虑。X首先，目标运动沿轴方向的运动可以用下面的状态方程描述MERGEFORMAT2121/XXKTKUKU用矩阵的形式表述为，MERGEFORMAT22XKKW在上式中，。XK10T21XKU考虑雷达的观测，得出观测方程为ZKCXKVMERGEFORMAT23在MERGEFORMAT23中，10，为零均值的噪声序列，方差已知。对目标进行预测，由相关理论可得到下面的迭代式/XKKMERGEFORMAT24在MERGEFORMAT24中，11|KEXZ，反映了由前1K各观测值对目前状态的估计。而预测的误差协方差可由下式表出，/1/11TTXXPKKQKMERGEFORMAT25对于最佳滤波，迭代表达式为/KKKZKCXKMERGEFORMAT26在式MERGEFORMAT26中，为KALMAN增益。而滤波误差的协方差为，MERGEFORMAT27/1XXPKIKKCPK在应用上面的公式进行KALMAN滤波时，需要指定初值。由于实际中通常无法得到目标的初始状态，我们可以利用前几个观测值建立状态的初始估计，比如采用前两个观测值，MERGEFORMAT282/21/TXXXZZ此时，估计误差为MERGEFORMAT2/2TXXXVTXVU29而误差协方差矩阵为，MERGEFORMAT21022/XXXTP3仿真计算与结果为了真实地反映出KALMAN滤波的效果，采用了MONTECARLO方法，采用多次实验取均值的方法进行研究，可以计算出估计的误差均值和方差，其表达式为MERGEFORMAT2111|MXIIIEKXK而误差的标准差可以表示为MERGEFORMAT21|MXIIXIXKEK212在MERGEFORMAT211和MERGEFORMAT212中，就是进行MONTECARLO仿M真的次数，而为取样点数。当仿真的次数越多时，实验的效果越接近于实际，但是计算K的速度会明显变慢。在仿真时，需要根据实际适当选取。在本程序中，取。50另外，在仿真过程中，为了进一步研究目标的航迹，在660秒后又进行了一段时间的匀速运动仿真。下面是仿真的结果，仿真源文件分别为TRAJECTORYM，KALMAN_FILTERM和FILTER_RESULTM，分别产生原始目标航迹、卡尔曼滤波估计和最终的输出，为了方便观察，将原始航迹、观测数据和经过滤波后的估计航迹反映在一张图上。并且，画出了给定MONTECARLO仿真的次数情况下，沿轴方向和轴方向估计误差值的均值和标准差随着采XY样点数的增多而发生变化的情况。首先给出原始航迹，31原始航迹图上图中可以看出目标运动的真实轨迹，首先沿轴方向运动，并经过两个转弯。Y接下来经过KALMAN滤波后的估计的X轴方向上的航迹和理论值为32滤波输出X轴方向航迹图而对应地，估计的Y轴方向上的航迹和理论值为33滤波输出Y轴方向航迹图将测量数据和理论值对比，得出的图形为34理论航迹与观测值而将输出的X和Y方向的航迹估计显示在直角坐标系下，为下图所示35理论航迹与滤波估计值下面对滤波估计值的误差均值和方差进行简单分析，对X和Y方向的航迹估计进行分别讨论，假定MONTECARLO仿真的次数50。由MERGEFORMAT211和MMERGEFORMAT212式就可以求出实际滤波输出每时刻误差的均值和标准差。36估计误差均值曲线（左X轴右Y轴）对应的标准差曲线为37估计误差标准差曲线（上X轴下Y轴）经过上面的仿真分析，可以看出KALMAN滤波算法对于动态目标的跟踪有着比较好的效果，而且可以较好地抑止环境中的噪声影响。附录1TRAJECTORYMFUNCTIONX,YTRAJECTORYTS,OFFTIME产生真实航迹X,Y，并在直角坐标系下显示出TS为雷达扫描周期，每隔TS秒取一个观测数据最初做匀速运动，接下来进行两个90度的机动转弯IFNARGIN2ERROR输入的变量过多，请检查ENDIFOFFTIME3ERRORTOOMANYINPUTARGUMENTSENDOFFTIME800产生理论的航迹X,YTRAJECTORYTS,OFFTIMEPVDDNCEILOFFTIME/TSRANDNSTATE,SUM100CLOCK设置随机数发生器FORI1NVXIDRANDN1观测噪声，两者独立VYIDRANDN1ZXIXIVXI实际观测值ZYIYIVYIEND产生观测数据FORN1MON用卡尔曼滤波得到估计的航迹XEKALMAN_FILTERTS,OFFTIME,D,0YEKALMAN_FILTERTS,OFFTIME,D,1误差矩阵XER1N,NX1NXE1NYER1N,NY1NYE1NEND滤波误差的均值XERBMEANXER,2YERBMEANYER,2滤波误差的标准差XSTDSTDXER,1,2计算有偏的估计值，FLAG1YSTDSTDYER,1,2作图FIGUREPLOTX,Y,RHOLDONPLOTZX,ZY,GHOLDONPLOTXE,YE,BHOLDOFFAXIS15005000100010000,GRIDONLEGEND真实轨迹,观测数据,滤波估计FIGURESUBPLOT2,2,1PLOTXERBAXIS05005050XLABEL观测次数YLABELX方向滤波误差均值,GRIDONSUBPLOT2,2,2PLOTYERBAXIS05005050XLABEL观测次数YLABELY方向滤波误差均值,GRIDONSUBPLOT2,2,3PLOTXSTDAXIS05000150XLABEL观测次数YLABELX方向滤波误差标准值,GRIDONSUBPLOT2,2,4PLOTYSTDAXIS05000150XLABEL观测次数YLABELY方向滤波误差标准值,GRIDONXXERYYER附录KALMAN_FILTERMFUNCTIONXEKALMAN_FILTERTS,OFFTIME,D,FLAGKALMAN_FILTER采用KALMAN滤波方法，从观测数值中得到航迹的估计XE输出X轴方向上的误差TS采样时间，即雷达工作周期OFFTIME仿真截止时间D噪声的标准差值FLAG判断计算X轴或Y轴数据,0X,1YIFNARGIN4ERROR输入的变量过多，请检查ENDIFOFFTIME600ERROR仿真时间必须大于600S，请重新输入ENDPVDD噪声的功率NCEILOFFTIME/TS采样点数SIGMA10加速度方向的的扰动SWITCHFLAGCASE0AZEROS1,4000075ONES1,200ZEROS1,1003ONES1,50ZEROS1,OFFTIME660对不同时段的加速度进行描述CASE1AZEROS1,4000075ONES1,200ZEROS1,1003ONES1,50ZEROS1,OFFTIME660OTHERWISEERROR输入仅能为0或1END定义系统的状态方程PHI1,TS0,1GAMMATSTS/2TSC10RPVQSIGMA2WRANDNSTATE,SUM100CLOCK设置随机数发生器FORN0TSOFFTIME1WN/TS1AN1SIGMARANDN1,1ENDXESTZEROS2,1用前K1时刻的输出值估计K时刻的预测值XFLIZEROS2,1K时刻KALMAN滤波器的输出值XESZEROS2,1预测输出误差XEFZEROS2,1滤波后输出的误差PXEZEROS2,1预测输出误差均方差矩阵PXZEROS2,1滤波输出误差均方差矩阵XEZEROS1,N得到最终的滤波输出值，仅仅考虑距离分量X,YTRAJECTORYTS,OFFTIME产生理论的航迹FORI1NVXIDRANDN1观测噪声，两者独立VYIDRANDN1ZXIXIVXI实际观测值ZYIYIVYIENDSWITCHFLAGCASE0XFLIZX2ZX2ZX1/TS利用前两个观测值来对初始条件进行估计XEFVX2TSW1/2VX1VX2/TSPXPV,PV/TSPV/TS,2PV/TSTSTSQ/4FORK3NXESTPHIXFLI更新该时刻的预测值XESPHIXEFGAMMAWK1预测输出误差PXEPHIPXPHIGAMMAQGAMMA预测误差的协方差阵KPXECINVCPXECRKALMAN滤波增益XFLIXESTKZXKCXESTXEFEYE2KCXESKVXKPXEYE2KCPXEXEKXFLI1,1ENDXE1ZX1XE2ZX2CASE1XFLIZY2ZY2ZY1/TS利用前两个观测值来对初始条件进行估计XEFVY2TSW1/2VY1VY2/TSPXPV,PV/TSPV/TS,2PV/TSTSTS