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

统计信号处理实验三《统计信号处理》实验三目的：掌握卡尔曼滤波滤波器的原理；内容：已知二维运动目标的CV运动方程为：1111122222(1)()0100(1)()()0100(1)()0001(1)()()0001skskTvkvkukskskTvkvkuk11112222(1)(1)(1)(1)1000(1)(1)(1)0010(1)skykvkwkykskwkvk这里1()wk、2()wk、1()vk、2()vk是相互不相关的运动噪声和观测噪声。1）试用滤波法对信号进行处理，并通过计算机模拟对其跟踪过程进行验证。2）试求其Kalman滤波方程，并通过计算机模拟对其跟踪过程进行验证。3）假设目标在运动过程中发生了机动（速度在某个时刻突然发生了改变），试观测此时的滤波和Kalman滤波结果，并对结果进行解释。要求：1)设计仿真计算的Matlab程序，给出软件清单；2)完成实验报告，对实验结果进行描述，并给出实验结果，对实验数据进行分析。实验过程：1()sk、1()vk和1()yk分别为径向距离、速度和观测值，而2()sk、2()vk和2()yk分别为横向距离、速度和观测值。1()uk和2()uk是状态噪声，是目标速度的波动；1()wk和2()wk是观测噪声；四种噪声的均值都为0，呈高斯分布，互不相关。T是雷达扫描一次的时间，此处设为1.0秒。假设目标距离雷达约500m左右，径向初速度设为150m/s，并且在远离雷达，横向初速度设为0m/s。这样它的径向速度波动大，而横向速度波动小，所以我们假设1()uk的方差21u为150m/s，2()uk的方差22u为11.210m/s。鉴于雷达的观测误差，我们假设观测噪声1()wk和2()wk的方差21w和22w均为50m。卡尔曼滤波：我们要想完成计算，就必须知道A、C和Q、R，观测数据()ky是必须提供的数据。A、C已经知道，现在寻找Q、R的确定值。Q是状态噪声协方差矩阵：2111222200000000()()0()0()00000000()uTuukQkEEukukukuuR是观测噪声协方差矩阵：21112222()0()()()()0Tww下一步就是确定初始值ˆ(2)x和(2)P，作为递推的初始数据。()kP是预测误差协方差矩阵：()()()()()TkEkkkkPxxxx(2)(2)(2)(2)(2)TEPxxxx其中：1122(2)(2)(2)(2)(2)svsvx，111222221(2)221yyyTyyyTx所以：1111122222(2)221(2)(2)(2)(2)221(2)syyyvTsyyyvTxx1111111111111122222222222222(2)(2)(2)(2)(2)(1)(2)(2)(1)(1)(2)(1)11(2)(2)(2)(2)(2)(1)(2)(2)(1)(1)(2)(1)11sswwsssws(2)(2)(2)(2)(2)TEPxxxx1111111122222222(2)(2)(2)(1)(2)(1)11(2)(2)(2)(1)(2)(1)11T22112221112222222222220020000200在推导上式时，()iwk和(1)iwk是随机过程中不同时刻的两个随机变量，我们认为这两个随机变量统计独立，而且()kw是平稳随机过程，其不同时刻的方差相同。两个时刻的时差T为雷达扫描一圈的时间。这样我们就有了初始值ˆ(2)x和(2)P，可以开始进行递推计算，首先计算k＝3时的状态变量。状态预测方程：ˆˆ(3|2)(2)(2)xAx状态预测误差协方差矩阵：(3|2)(2)(2)(2)(2)TPAPAQ最佳增益方程：1(3)(3|2)(3)(3)(3|2)(3)(3)TTKPCCPCR滤波估值方程：ˆˆˆ(3)(3|2)(3)(3)(3)(3|2)xxKyCx滤波估值误差协方差方程：(3)(3)(3)(3|2)PIKCP然后计算k＝4时的状态变量，如此递推，直到波形估计结束。实验结果：1）未机动的卡尔曼滤波：从图中可以看到，滤波之后波形变得更平滑了，在观测信号尖峰的位置比较明显。2）机动目标的卡尔曼滤波：从图像中可以看出，当目标状态突变时，卡尔曼滤波能较好的跟踪目标机动，即具有自适应性，并且卡尔曼滤波具有较好的自适应性。程序：%卡尔曼滤波clearall;C=[1000;0010;];A=[1100;0100;0011;0001;];Q=[0000;015000;0000;0000.12];R=[500;050];I=[1000;0100;0010;0001;];v1t=150;X=zeros(4,100);Y=zeros(2,100);wt=random('norm',0,sqrt(1000),1,100);u1t=random('norm',0,sqrt(150),1,100);u2t=random('norm',0,sqrt(0.12),1,100);Y(:,1)=[500+wt(1,1)wt(1,1)]';Y(:,2)=[500-v1t+wt(1,2)wt(1,2)]';X(:,2)=[Y(1,1)Y(1,2)-Y(1,1)Y(2,2)Y(2,2)-Y(2,1)]';P2=[505000;5025000;005050;0050100.12;];Pi=P2;fori=3:100v1t=150+u1t(1,i);v2t=u2t(1,i);Y(:,i)=[500+v1t*(i-1)+wt(1,i)v2t*i+wt(1,i)]';x1(:,i)=A*X(:,i-1);%状态预测估值pi=A*Pi*A'+Q;%状态预测误差协方差矩阵Ki=pi*C'*inv(C*pi*C'+R);%最佳增益方程temp1=C*x1(:,i);temp2(1,1)=Y(1,i)-temp1(1,1);temp2(2,1)=Y(2,i)-temp1(2,1);X(:,i)=x1(:,i)+Ki*temp2;%滤波估值方程Pi=(I-Ki*C)*pi;%滤波估值误差协方差方程endfigure(1);i=1:100;plot(i,Y(1,:),i,X(1,:),'r');h=legend('真实值','估计值');grid;title('卡尔曼滤波');%机动目标卡尔曼滤波clearall;C=[1000;0010;];A=[1100;0100;0011;0001;];Q=[0000;015000;0000;0000.12];R=[500;050];I=[1000;0100;0010;0001;];v1t=150;X=zeros(4,100);Y=zeros(2,100);wt=random('norm',0,sqrt(1000),1,100);u1t=random('norm',0,sqrt(150),1,100);u2t=random('norm',0,sqrt(0.12),1,100);Y(:,1)=[500+wt(1,1)wt(1,1)]';Y(:,2)=[500-v1t+wt(1,2)wt(1,2)]';X(:,2)=[Y(1,1)Y(1,2)-Y(1,1)Y(2,2)Y(2,2)-Y(2,1)]';P2=[505000;5025000;005050;0050100.12;];Pi=P2;fori=3:49v1t=150+u1t(1,i);v2t=u2t(1,i);Y(:,i)=[500-v1t*(i-1)+wt(1,i)v2t*i+wt(1,i)]';x1(:,i)=A*X(:,i-1);%状态预测估值pi=A*Pi*A'+Q;%状态预测误差协方差矩阵Ki=pi*C'*inv(C*pi*C'+R);%最佳增益方程temp1=C*x1(:,i);temp2(1,1)=Y(1,i)-temp1(1,1);temp2(2,1)=Y(2,i)-temp1(2,1);X(:,i)=x1(:,i)+Ki*temp2;%滤波估值方程Pi=(I-Ki*C)*pi;%滤波估值误差协方差方程endfori=50:60v1t=500+u1t(1,i);v2t=u2t(1,i);Y(:,i)=[500+v1t*(i-1)+wt(1,i)v2t*i+wt(1,i)]';x1(:,i)=A*X(:,i-1);%状态预测估值pi=A*Pi*A'+Q;%状态预测误差协方差矩阵Ki=pi*C'*inv(C*pi*C'+R);%最佳增益方程temp1=C*x1(:,i);temp2(1,1)=Y(1,i)-temp1(1,1);temp2(2,1)=Y(2,i)-temp1(2,1);X(:,i)=x1(:,i)+Ki*temp2;%滤波估值方程Pi=(I-Ki*C)*pi;%滤波估值误差协方差方程endfori=61:99v1t=150+u1t(1,i);v2t=u2t(1,i);Y(:,i)=[500-v1t*(i-1)+wt(1,i)v2t*i+wt(1,i)]';x1(:,i)=A*X(:,i-1);%状态预测估值pi=A*Pi*A'+Q;%状态预测误差协方差矩阵Ki=pi*C'*inv(C*pi*C'+R);%最佳增益方程temp1=C*x1(:,i);temp2(1,1)=Y(1,i)-temp1(1,1);temp2(2,1)=Y(2,i)-temp1(2,1);X(:,i)=x1(:,i)+Ki*temp2;%滤波估值方程Pi=(I-Ki*C)*pi;%滤波估值误差协方差方程endfigure(1);i=1:100;plot(i,Y(1,:),i,X(1,:),'r');h=legend('真实值','估计值');grid;title('卡尔曼滤波');