卡尔曼滤波-matlab仿真

随机信号处理上机作业题目：假设有一个二坐标雷达对一平面上运动目标的进行观察，目标在t＝0～400秒沿y轴作恒速直线运功，运动速度为-15m/s，目标的起点为（2000m，10000m），雷达扫描周期为2秒，x和y独立地进行观察，观察噪声的标准差均为100m。试建立雷达对目标的跟踪算法，并进行仿真分析，给出仿真结果，画出目标真实轨迹、对目标的观察和滤波曲线。一、跟踪算法考虑利用卡尔曼滤波算法对目标的运动状态进行估计。由于目标在二维平面内做匀速运动，因此这里只考虑匀速运动情况。1.建立模型由于目标沿y轴作匀速直线运动，取状态变量yvyxS状态方程：kASkS1(1)观测方程：kVkCSkZ(2)其中，10010001TA010001CyxzzZyxvvV对目标位置和速度的同时滤波与一步预测的方程组如下:预测估计方程:11/^^kAkkSS预测误差协方差：TAkAPkkP11/滤波估计增益：RCkkCkkCkBTT1/1/，其中2200yxR滤波估计方程：1/1//^^^kkkZkBkkkkSSS滤波误差协方差：1/1/kkPCkBkkP2.初始化利用目标的前几个测量值建立状态的其实估计,采用两点起始法。TZZZZSyyyx)1()2()2()2(2/2TTTPyyyyx22222200002/2滤波误差均值：kkSkSNkeINiix/)(1)(1滤波误差标准差：221)(/)(1kekkSkSNxNiIiy二.仿真分析利用MATLAB对前面建立的模型进行仿真,结果如下。19991999.21999.41999.61999.820002000.22000.42000.62000.820014000600080001000012000目标真实轨迹X(米)Y(米)1700180019002000210022002300050001000015000测量轨迹X(米)Y(米)图2.1图2.1是目标运动的真实轨迹和观测轨迹曲线。其中，真实轨迹显示目标在x=2000米处沿y轴方向做匀速直线运动，而观测轨迹是目标运动的真实轨迹加上方差和随机测量噪声得到的。从图中可以看出，观测轨迹围绕真实轨迹作上下浮动。194019601980200020202040206020804000600080001000012000单次滤波数据曲线X(米)Y(米)19921994199619982000200220044000600080001000012000100次滤波数据曲线X(米)Y(米)图2.2图2.2是单次滤波和100次滤波后的数据曲线。从图中可以看出，滤波刚开始时误差较大，之后滤波误差逐渐降低，估计值逐步逼近真实轨迹。而随着滤波次数增加，滤波后的结果更为接近真实轨迹。020406080100120140160180200-50510X滤波误差均值曲线采样次数X(米)020406080100120140160180200050100150x滤波误差标准差曲线采样次数X(米)图2.3020406080100120140160180200-20-10010y滤波误差均值曲线采样次数Y(米)020406080100120140160180200050100150y滤波误差标准差曲线采样次数Y(米/秒)图2.4图2.3,图2.4分别是x和y方向滤波估计误差均值及误差标准差曲线。从图上可以看出，滤波开始时误差较大，随着采样次数的增加，误差逐渐减小，误差的标准差也具有相同特性。另外,可以看到由于在y方向上有速度分量,因此y方向的估计误差均值比x方向的估计误差均值波动要大一些。%仿真场景sigma=10000;T=2;t=200;Vy=-15;C=[100;010];A=[100;01T;001];eSk(:,t)=[000]';eSz(:,t)=[000]';eeSz(:,t)=[00]';N=100;%蒙特卡洛次数fori=1:Nforj=1:tZk(:,j)=[2000+wgn(1,1,40);10000+Vy*T*(j-1)+wgn(1,1,40)];endforj=1:200ifj==1Sk(:,1)=[Zk(1,1),Zk(2,1),0]';Sk1(:,1)=Sk(:,1);Sk(:,2)=[Zk(1,2),Zk(2,2),(Zk(2,2)-Zk(2,1))/T]';Sk1(:,2)=Sk(:,2);Pk=[sigma,0,0;0,sigma,sigma/T;0,sigma/T,2*sigma/T];elseifj2Sk1(:,j)=A*Sk(:,j-1);%预测Pk1=A*Pk*A';%预测误差协方差Bk=Pk1*C'*inv(C*Pk1*C'+sigma*eye(2));%kalman增益Sk(:,j)=Sk1(:,j)+Bk*(Zk(:,j)-C*Sk1(:,j));%滤波Pk=(eye(3)-Bk*C)*Pk1;%滤波协方差endend%%%1000次求平均eSk(:,j)=eSk(:,j)+Sk(:,j)/N;%滤波eSz(:,j)=eSz(:,j)+([2000;10000+Vy*(j-1)*T;0]-Sk(:,j))/N;%滤波误差均值eeSz(:,j)=eeSz(:,j)+[(2000-Sk(1,j))^2;(10000+Vy*(j-1)*T-Sk(2,j))^2]/N;%滤波误差标准差endend%绘图%真实轨迹和测量轨迹subplot(2,1,1);j=0:0.1:t;plot(2000,10000+Vy*(j-1)*T);title('目标真实轨迹');xlabel('X(米)');ylabel('Y(米)');subplot(2,1,2);plot(Zk(1,:),Zk(2,:));title('测量轨迹');xlabel('X(米)');ylabel('Y(米)');%滤波单次仿真和蒙特卡洛仿真figure;subplot(2,1,1);plot(Sk(1,:),Sk(2,:));title('单次滤波数据曲线');xlabel('X(米)');ylabel('Y(米)');subplot(2,1,2);plot(eSk(1,:),eSk(2,:));title('100次滤波数据曲线');xlabel('X(米)');ylabel('Y(米)');j=1:t;figure;subplot(211);plot(j,eSz(1,:));title('X滤波误差均值曲线');xlabel('采样次数');ylabel('X(米)');subplot(212);forj=1:teeSz(1,j)=sqrt(eeSz(1,j)-eSz(1,j)^2);eeSz(2,j)=sqrt(eeSz(2,j)-eSz(2,j)^2);endj=1:t;plot(j,eeSz(1,:));title('x滤波误差标准差曲线');xlabel('采样次数');ylabel('X(米)');figure;subplot(211);plot(j,eSz(2,:));title('y滤波误差均值曲线');xlabel('采样次数');ylabel('Y(米)');subplot(212);plot(j,eeSz(2,:));title('y滤波误差标准差曲线');xlabel('采样次数');ylabel('Y(米/秒)');figure;subplot(211);plot(j,Sk(3,:));title('单次滤波速度估计');xlabel('采样次数');ylabel('Y(米/秒)');subplot(212);plot(j,eSk(3,:));title('100次滤波速度估计');xlabel('采样次数');ylabel('Y(米/秒)');