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

Kalman滤波在目标跟踪应用仿真研究李金磊09040023卫杨勇09042165郑成波09041153卡尔曼滤波在目标跟踪应用仿真研究（李金磊，卫杨勇，郑成波NationnalUniversityofDefenseTechnology，Changsha,China）【摘要】卡尔曼滤波器是一个“optimalrecursivedataprocessingalgorithm”，它广泛也应用于军事方面的雷达系统以及导弹追踪领域。雷达在搜索到目标并记录目标的位置数据，对测量到的目标位置数据（称为点迹）进行处理，自动形成航迹，并对目标在下一时刻的位置进行预测。本文简要讨论了用Kalman滤波方法对单个目标航迹进行预测，并借助于Matlab仿真工具，对实验的效果分析。【关键词】：Kalman滤波、目标跟踪、Matlab仿真一、实验目的目标跟踪是卡尔曼滤波的主要应用领域。通过本实验，进一步加深对卡尔曼滤波算法的理解，了解卡尔曼滤波算法的基本特点，掌握卡尔曼滤波算法应用研究的基本步骤和方法。二、情景设定假设有一二坐标雷达对一平面上运动的目标进行观测，目标在t=0-600秒沿x轴作恒速直线运动，运动速度为15米/秒，目标的起始点为（-10000米,2000米）。雷达扫描周期T=2秒，x和y独立地进行观测，观测噪声标准差均为100米。要求滤波误差标准差的方差压缩系数为0.5。三、算法分析跟踪滤波的目的是根据已经获得的目标观测数据对目标的状态进行精确的估计。在二维平面内当目标做匀速运动时，我们可以建立在笛卡尔坐标系下的目标离散运动模型和观测模型为：X(k+1)=QX(k)+TW(k)(3-1)Z(k)=C(k)X(k)+V(k)(3-2)本文只考虑匀速直线运动，所以我们只建立匀速直线运动模型。1．建立模型Kalman滤波在目标跟踪应用仿真研究李金磊09040023卫杨勇09042165郑成波09041153当目标做匀速直线运动，取状态变量为：X=[yxVx](3-3)状态方程为：𝑋(𝑘+1)=𝛷𝑋(𝑘)(3-4)观测方程为：𝑍(𝑘)=𝐻𝑋(𝑘)+𝑈(𝑘)(3-5)其中：Z=[ZyZx]U=[UyUx]H=[100010]Φ=[10001T001]对目标位置和速度的最佳滤波和最佳预测如下：预测：𝑋̂(𝑘|𝑘−1)=𝛷𝑋̂(𝑘−1|𝑘−1)(3-6)预测误差协方差：𝑃(𝑘|𝑘−1)=𝛷𝑃(𝑘−1|𝑘−1)𝛷𝑇(3-7)卡尔曼增益：K(k)=P(k|k−1)HT[HP(k|k−1)HT+R]−1(3-8)滤波：X̂(k|k)=X̂(k|k−1)+K(k)[Z(k)−HX̂(k|k−1)](3-9)滤波协方差为：𝑃(𝑘|𝑘)=[𝐼−𝐾(𝑘)𝐻]𝑃(𝑘|𝑘−1)(3-10)其中：R=[σy200σx2]2.初始化实际上我们常常无法得知目标的初始状态，这时我们可以利用前几个测量值建立状态的起始估计。现在我们用两点起始法：Kalman滤波在目标跟踪应用仿真研究李金磊09040023卫杨勇09042165郑成波09041153X̂(2|2)=[Zy(2)Zx(2)Zx(2)−Zx(1)T]P(2|2)=[σy2000σx2σx2T0σx2T2σx2T]滤波误差均值：ex(k)̅̅̅̅̅̅̅=1N∑[xi(k)−xî(k|k)]Ni=1滤波误差标准差：σx̂=√1N∑[xi(k)−xî(k|k)]2Ni=1−ex(k)̅̅̅̅̅̅̅2四、仿真分析-我们已经把卡尔曼滤波的算法叙述的很清楚，由3-6到3-10的5个公式就很容易实现卡尔曼滤波算法。在计算机仿真中，我们采用Matlab编写程序，利用蒙特卡洛的方法对跟踪滤波器进行仿真分析，次数为1000次。以下给出仿真图和结果分析。图4-1图4-1是目标的真实轨迹和测量轨迹，测量轨迹是真实轨迹数据添加方差和Kalman滤波在目标跟踪应用仿真研究李金磊09040023卫杨勇09042165郑成波09041153均值固定的随机测量噪声得到的，目标沿y=2000米做匀速直线运动。给出的测量轨迹用于滤波后与滤波轨迹作比较分析。从图中可以看出，测量轨迹围绕真实轨迹作上下浮动。图4-2图4-2是滤波轨迹和滤波均值轨迹。从图中可以看出，滤波开始时误差较大，但是随着时间的推移，滤波误差降低，估计值逐步逼近真实轨迹。采用蒙特卡洛方法，多次观测取均值，滤波更为接近真实曲线。图4-3Kalman滤波在目标跟踪应用仿真研究李金磊09040023卫杨勇09042165郑成波09041153图4-3是y方向滤波估计误差均值及误差标准差。滤波开始时误差较大，随着采样点的增加，误差逐渐减小，误差的标准差也具有同样的特性。达到了滤波误差标准差的方差压缩系数为0.5的要求。图4-4图4-4是x方向滤波估计误差均值及误差标准差。与y方向的估计误差均值相比，x方向的估计误差均值波动较大，这是由于在x方向上有速度分量的缘故，同时其方向上滤波估计误差也有一定波动。也达到了其压缩系数为0.5的要求。Kalman滤波在目标跟踪应用仿真研究李金磊09040023卫杨勇09042165郑成波09041153图4-5X方向速度估计曲线见图4-5。单次滤波速度与实际值有差距，但是1000次滤波取均值后速度滤波已经于实际值，但是滤波开始时仍有很大偏差，这随着采样点的增加，误差逐渐减小。五、参考文献[1]刘福声罗鹏飞，统计信号处理，国防科技大学出版社，1999[2]万建伟，信号处理实验，国防科技大学出版社六、附录实验源程序：clearall;clc;%===============================仿真场景===================================sigma=10000;T=2;t=300;Vx=15;H=[100;010];Q=[100;01T;001];eXk(:,t)=[000]';eXz(:,t)=[000]';eeXz(:,t)=[00]';N=10;%蒙特卡洛次数fori=1:Nforj=1:tZk(:,j)=[2000+wgn(1,1,40);-10000+Vx*T*(j-1)+wgn(1,1,40)];endforj=1:300ifj==1Xk(:,1)=[Zk(1,1),Zk(2,1),0]';Xk1(:,1)=Xk(:,1);Xk(:,2)=[Zk(1,2),Zk(2,2),(Zk(2,2)-Zk(2,1))/T]';Xk1(:,2)=Xk(:,2);Pk=[sigma,0,0;0,sigma,sigma/T;0,sigma/T,2*sigma/T];elseifj2Xk1(:,j)=Q*Xk(:,j-1);%预测Pk1=Q*Pk*Q';%预测误差协方差Kk=Pk1*H'*inv(H*Pk1*H'+sigma*eye(2));%kalman增益Xk(:,j)=Xk1(:,j)+Kk*(Zk(:,j)-H*Xk1(:,j));%滤波Pk=(eye(3)-Kk*H)*Pk1;%滤波协方差endKalman滤波在目标跟踪应用仿真研究李金磊09040023卫杨勇09042165郑成波09041153end%%%1000次求平均eXk(:,j)=eXk(:,j)+Xk(:,j)/N;%滤波eXz(:,j)=eXz(:,j)+([2000;-10000+Vx*(j-1)*T;0]-Xk(:,j))/N;%滤波误差均值eeXz(:,j)=eeXz(:,j)+[(2000-Xk(1,j))^2;(-10000+Vx*(j-1)*T-Xk(2,j))^2]/N;%滤波误差标准差endend%===========================绘图====================================%真实轨迹和测量轨迹subplot(2,1,1);j=0:0.1:t;plot(-10000+Vx*(j-1)*T,2000);title('目标真实轨迹');xlabel('X(米)');ylabel('Y(米)');subplot(2,1,2);plot(Zk(2,:),Zk(1,:));title('测量轨迹');xlabel('X(米)');ylabel('Y(米)');%滤波单次仿真和蒙特卡洛仿真figure;%subplot(2,1,1);plot(Xk(2,:),Xk(1,:),'g');title('单次滤波数据曲线');xlabel('X(米)');ylabel('Y(米)');holdon%subplot(2,1,2);plot(eXk(2,:),eXk(1,:));title('1000次滤波数据曲线');xlabel('X(米)');ylabel('Y(米)');%j=1:t;figure;subplot(211);plot(j,eXz(2,:));title('X滤波误差均值曲线');xlabel('采样次数');ylabel('Y(米)');subplot(212);forj=1:teeXz(1,j)=sqrt(eeXz(1,j)-eXz(1,j)^2);eeXz(2,j)=sqrt(eeXz(2,j)-eXz(2,j)^2);endj=1:t;plot(j,eeXz(2,:));title('x滤波误差标准差曲线');xlabel('采样次数');ylabel('Y(米)');figure;subplot(211);plot(j,eXz(1,:));title('y滤波误差均值曲线');xlabel('采样次数');ylabel('Y(米)');subplot(212);plot(j,eeXz(1,:));title('y滤波误差标准差曲线');xlabel('采样次数');ylabel('Y(米)');figure;subplot(211);plot(j,Xk(3,:));title('单次滤波速度估计');subplot(212);plot(j,eXk(3,:));title('1000次滤波速度估计');xlabel('采样次数');ylabel('Y(米)');