统计信号处理实验三

《统计信号处理》实验三目的：掌握卡尔曼滤波滤波器的原理；内容：用雷达跟踪目标，目标的运动可以看成是在径向和横向内的二维运动，其运动方程和观测方程分别为：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：滤波状态方程和输出方程：(1)()()(1)(1)(1)kkkkkkxAxuyCxwk时刻对k+1时刻的状态估计为：ˆ(1|)()kkkxAx根据恒增益滤波思想，二者按照一定比例叠加，得到恒增益滤波的滤波公式：ˆˆˆ(1)(1|)(1)(1|)ˆˆ(1|)(1)(1|)kkkkkkkkkkkxxKyyxKyCx其中的K是一个4行2列的矩阵，需要自己推导。本题中，10001000010001TTA,00100001C距离新息：1()ˆ(1)(1|)(1)1001()()()(1)10(1)(1|)()TskkkkykvkskTvkykykskkvkyCx速度新息：(1)()(1)(1|)()(1)()()(1)(1|)ykskvkvkkvkTykskTvkykskkTT按照新息修正的思想，对距离新息的加权系数取，对速度新息的加权系数取，直接对状态变量预测估值ˆ(1|)kkx进行修正：)|1()1()|1()1(0000)|1()1(2211kkskykkskyTTkkxkx即求得TTK0000。二、任务2：Kalman滤波我们要想完成计算，就必须知道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时的状态变量，如此递推，直到波形估计结束。三、任务3：机动目标跟踪在目标机动的情况下，即目标运动轨迹突然变化，比较滤波器和卡尔曼滤波效果的差别。四、任务4（选作）：非平稳噪声下的目标跟踪自己建立仿真的u、w序列，控制好它们的方差Q（K）和R（K），并用于程序中。在Q（K）和R（K）改变的情况下，观测恒增益滤波的结果和卡尔曼滤波的结果，比较并解释。实验结果：一、滤波取alpha=0.8，beta=0.2，滤波结果如下：（红色曲线代表滤波后的波形）改变alpha、beta的取值，使alpha=0.6，beta=0.1，滤波结果如下：从图中可以看出，滤波之后波形变得更平滑了，在观测信号尖峰的位置比较明显。而且适当减小alpha、β后滤波效果更好了。二、Kalman滤波从图中可以看出，滤波之后波形变得更平滑了，在观测信号尖峰的位置比较明显。三、机动目标跟踪在t=50时刻，设置速度反向，t=56时刻，再次反向，得到的滤波和kalman滤波的波形分别如下：减小alpha，β后，从图像中可以看出，当目标状态突变时，滤波和kalman滤波都能较好的跟踪目标机动，即具有自适应性，而卡尔曼滤波的自适应性更好。程序清单：%alpha_beta滤波alpha=0.8;beta=0.2;x0=[1000030000]';y0=[100000]';K=[alpha0;beta0;0alpha;0beta;];C=[1000;0010;];A=[1100;0100;0011;0001;];X=zeros(4,100);Y=zeros(2,100);X(:,1)=x0;Y(:,1)=y0;wt=random('norm',0,sqrt(500),1,100);u1t=random('norm',0,sqrt(100),1,100);u2t=random('norm',0,sqrt(0.12),1,100);fori=1:99v1t=300+u1t(1,i);v2t=u2t(1,i);Y(:,i+1)=[10000-v1t*i+wt(1,i)v2t*i+wt(1,i)]'x1(:,i+1)=A*X(:,i);%x(k+1|k)temp1=C*x1(:,i+1);temp2(1,1)=Y(1,i+1)-temp1(1,1);temp2(2,1)=Y(2,i+1)-temp1(2,1);X(:,i+1)=x1(:,i+1)+K*temp2;%x(k+1)endfigure(1);i=1:100;plot(i,Y(1,:),i,X(1,:),'r');grid;title('alpha_beta滤波');%卡尔曼滤波C=[1000;0010;];A=[1100;0100;0011;0001;];Q=[0000;030000;0000;0000.12];R=[10000;01000];I=[1000;0100;0010;0001;];v1t=300;X=zeros(4,100);Y=zeros(2,100);wt=random('norm',0,sqrt(1000),1,100);u1t=random('norm',0,sqrt(300),1,100);u2t=random('norm',0,sqrt(0.12),1,100);Y(:,1)=[16000+wt(1,1)wt(1,1)]';Y(:,2)=[16000-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=[1000100000;1000230000;0010001000;0010002000.12;];Pi=P2;fori=3:100v1t=300+u1t(1,i);v2t=u2t(1,i);Y(:,i)=[16000-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');grid;title('kalman滤波');%机动情况下alpha_beta滤波alpha=0.8;beta=0.2;x0=[1000030000]';y0=[100000]';K=[alpha0;beta0;0alpha;0beta;];C=[1000;0010;];A=[1100;0100;0011;0001;];X=zeros(4,100);Y=zeros(2,10