东南大学统计信号处理实验实验三

统计信号处理实验三目的：掌握卡尔曼滤波滤波器的原理；内容：用雷达跟踪目标，目标的运动可以看成是在径向和横向内的二维运动，其运动方程和观测方程分别为：1111122222(1)()0100(1)()()0100(1)()0001(1)()()0001skskTvkvkukskskTvkvkuk11112222(1)(1)(1)(1)1000(1)(1)(1)0010(1)skykvkwkykskwkvk1()sk、1()vk和1()yk分别为径向距离、速度和观测值，而2()sk、2()vk和2()yk分别为横向距离、速度和观测值。1()uk和2()uk是状态噪声，是目标速度的波动；1()wk和2()wk是观测噪声；四种噪声的均值都为0，呈高斯分布，互不相关。T是雷达扫描一次的时间，此处设为1.0秒。假设目标距离雷达约160Km左右，径向初速度设为300m/s，并且在向雷达靠近，横向初速度设为0m/s。这样它的径向速度波动大，而横向速度波动小，所以我们假设1()uk的方差21u为300m/s，2()uk的方差22u为11.210m/s。鉴于雷达的观测误差，我们假设观测噪声1()wk和2()wk的方差21w和22w均为1.0Km。其中21u，22u，21w和22w的初始值不是最佳的，学生完全可自己修改以上参数，并观察计算结果的变化，给出最好的滤波效果。任务：1）试用滤波法对信号进行处理，并通过计算机模拟对其跟踪过程进行验证；2）试求其Kalman滤波方程，并通过计算机模拟对其跟踪过程进行验证；3）假设目标在运动过程中发生了机动（速度在某个时刻突然发生了改变），试观测此时的滤波和Kalman滤波结果，并对结果进行解释。要求：1）设计仿真计算的Matlab程序，给出软件清单；2）完成实验报告，给出实验结果，并对实验数据进行分析。(1)滤波法对信号进行处理。clear;alfa=0.6;beta=0.4;sigma_u1=0.3;sigma_u2=0.2;sigma_w=0.1;T=1;s1=[12zeros(1,499)];s2=[7zeros(1,499)];v1=[15zeros(1,499)];v2=[4zeros(1,499)];y1=[zeros(1,500)];y2=[zeros(1,500)];A=[1T00;0100;001T;0001];C=[1000;0010];X=[s1;v1;s2;v2];X0=[11.8zeros(1,499);13.8zeros(1,499);6.8zeros(1,499);3.9zeros(1,499)];Y=[y1;y2];Y0=[y1;y2];u1=sigma_u1*randn(1,500);u2=sigma_u2*randn(1,500);w1=sigma_w*randn(1,500);w2=sigma_w*randn(1,500);M=500;fori=1:M-1;X(:,i+1)=A*X(:,i)+[0;u1(i);0;u2(i)];Y(:,i+1)=C*X(:,i+1)+[w1(i+1)w2(i+1)]';ends1=X(1,:);v1=X(2,:);s2=X(3,:);v2=X(4,:);K=[alfa0;beta/T0;0alfa;0beta/T];M=500;fori=1:M-1;X1(:,i+1)=A*X0(:,i);X0(:,i+1)=X1(:,i+1)+K*[Y(1,i+1)-X1(1,i+1);Y(2,i+1)-X1(3,i+1)];Y0(:,i+1)=C*X0(:,i+1);endt=0:499;figure(1);plot(X(3,:),X(1,:),'b',X0(3,:),X0(1,:),'r');h=legend('真实值','估计值');xlabel('s2');ylabel('s1');title('alfa-beta滤波');gridon;figure(2);plot(t,X(2,:),'b',t,X0(2,:),'r');h=legend('真实值','估计值');xlabel('t');ylabel('v1');title('alfa-beta滤波v1');gridon;figure(3);plot(t,X(4,:),'b',t,X0(4,:),'r');h=legend('真实值','估计值');xlabel('t');ylabel('v2');title('alfa-beta滤波v2');gridon;取值alfa=0.8；beta=0.2；第二个取值下的估计效果较差。2)试求其Kalman滤波方程，并通过计算机模拟对其跟踪过程进行验证；clear;clc;sigma_u1=0.3;sigma_u2=0.2;sigma_w=0.1;T=1;s1=[12zeros(1,499)];s2=[7zeros(1,499)];v1=[15zeros(1,499)];v2=[4zeros(1,499)];y1=[zeros(1,500)];y2=[zeros(1,500)];A=[1T00;0100;001T;0001];C=[1000;0010];Q=[0000;0sigma_u100;0000;000sigma_u2];R=[sigma_w0;0sigma_w];I=[1000;0100;0010;0001];P2=[sigma_wsigma_w/T00;sigma_w/Tsigma_u1+2*sigma_w/(T^2)00;00sigma_wsigma_w/T;00sigma_w/Tsigma_u2+2*sigma_w/(T^2)];X=[s1;v1;s2;v2];X0=[11.8zeros(1,499);13.8zeros(1,499);6.8zeros(1,499);3.9zeros(1,499)];Y=[y1;y2];Y0=[y1;y2];u1=sigma_u1*randn(1,500);u2=sigma_u2*randn(1,500);w1=sigma_w*randn(1,500);w2=sigma_w*randn(1,500);M=500;fori=1:M-1;X(:,i+1)=A*X(:,i)+[0;u1(i);0;u2(i)];Y(:,i+1)=C*X(:,i+1)+[w1(i+1)w2(i+1)]';ends1=X(1,:);v1=X(2,:);s2=X(3,:);v2=X(4,:);fori=1:M-1;ifi==1P1=A*P2*A'+Q;elseP1=A*P0*A'+Q;endK=P1*C'*inv(C*P1*C'+R);X1(:,i+1)=A*X0(:,i)X0(:,i+1)=X1(:,i+1)+K*(Y(:,i+1)-C*X1(:,i+1));Y0(:,i+1)=C*X0(:,i+1);P0=(I-K*C)*P1;endt=1:500;figure(1);plot(X(3,:),X(1,:),'b',X0(3,:),X0(1,:),'r');h=legend('真实值','估计值');xlabel('s2');ylabel('s1');title('Kalman滤波的距离');gridon;figure(2);plot(t,X(2,:),'b',t,X0(2,:),'r');h=legend('真实值','估计值');xlabel('t');ylabel('v1');title('Kalman滤波的v1');gridon;figure(3);plot(t,X(4,:),'b',t,X0(4,:),'r');h=legend('真实值','估计值');xlabel('t');ylabel('v2');title('Kalman滤波的v2');gridon;3)假设目标在运动过程中发生了机动（速度在某个时刻突然发生了改变），试观测此时的滤波和Kalman滤波结果，并对结果进行解释。①滤波clear;clc;alfa=0.6;beta=0.4;sigma_u1=0.3;sigma_u2=0.2;sigma_w=0.1;T=1;s1=[12zeros(1,499)];s2=[7zeros(1,499)];v1=[15zeros(1,499)];v2=[4zeros(1,499)];y1=[zeros(1,500)];y2=[zeros(1,500)];A=[1T00;0100;001T;0001];C=[1000;0010];X=[s1;v1;s2;v2];X0=[11.8zeros(1,499);13.8zeros(1,499);6.8zeros(1,499);3.9zeros(1,499)];Y=[y1;y2];Y0=[y1;y2];u1=sigma_u1*randn(1,500);u2=sigma_u2*randn(1,500);w1=sigma_w*randn(1,500);w2=sigma_w*randn(1,500);M=500;fori=1:M-1;X(:,i+1)=A*X(:,i)+[0;u1(i);0;u2(i)];ifi==199X(2,i+1)=X(2,i+1)+10;X(4,i+1)=X(4,i+1)+10;endY(:,i+1)=C*X(:,i+1)+[w1(i+1)w2(i+1)]';ends1=X(1,:);v1=X(2,:);s2=X(3,:);v2=X(4,:);K=[alfa0;beta/T0;0alfa;0beta/T];M=500;fori=1:M-1;X1(:,i+1)=A*X0(:,i);X0(:,i+1)=X1(:,i+1)+K*[Y(1,i+1)-X1(1,i+1);Y(2,i+1)-X1(3,i+1)];Y0(:,i+1)=C*X0(:,i+1);endt=0:499;figure(1);plot(X(3,:),X(1,:),'b',X0(3,:),X0(1,:),'r');h=legend('真实值','估计值');xlabel('s2');ylabel('s1');title('alfa-beta滤波的距离');gridon;figure(2);plot(t,X(2,:),'b',t,X0(2,:),'r');h=legend('真实值','估计值');xlabel('t');ylabel('v1');title('alfa-beta滤波的v1');gridon;figure(3);plot(t,X(4,:),'b',t,X0(4,:),'r');h=legend('真实值','估计值');xlabel('t');ylabel('v2');title('alfa-beta滤波的v2');gridon;②卡尔曼滤波clear;clc;sigma_u1=0.3;sigma_u2=0.2;sigma_w=0.1;T=1;s1=[12zeros(1,499)];s2=[7zeros(1,499)];v1=[15zeros(1,499)];v2=[4zeros(1,499)];y1=[zeros(1,500)];y2=[zeros(1,500)];A=[1T00;0100;001T;0001];C=[1000;0010];Q=[0000;0sigma_u100;0000;000sigma_u2];R=[sigma_w0;0sigma_w];I=[1000;0100;0010;0001];P2=[sigma_wsigma_w/T00;sigma_w/Tsigma_u1+2*sigma_w/(T^2)00;00sigma_wsigma_w/T;00sigma_w/Tsigma_u2+2*sigma_w/(T^2)];X=[s1;v1;s2;