卡尔曼滤波 matlab仿真

1、随机信号处理上机作业题目：假设有一个二坐标雷达对一平面上运动目标的进行观察，目标在t=0400秒沿y轴作恒速直线运功，运动速度为-15m/s,目标的起点为(2000m,10000m),雷达扫描周期为2秒，x和y独立地进行观察，观察噪声的标准差均为100m。试建立雷达对目标的跟踪算法，并进行仿真分析，给出仿真结果，画出目标真实轨迹、对目标的观察和滤波曲线。一、跟踪算法考虑利用卡尔曼滤波算法对目标的运动状态进行估计。由于目标在二维平面内做匀速运动，因此这里只考虑匀速运动情况。1.建立模型由于目标沿y轴作匀速直线运动，取状态变量XS=yvy状态方程：sa+1丿二asa丿(1)观测方程：Zk7=CSk

2、)+Vk丿其中，(2)0_00zvxV=xzvyyZ=对目标位置和速度的同时滤波与一步预测的方程组如下:预测估计方程:S(k/k-1)=aS(k-1)预测误差协方差：P(k/k-1)=AP(k-1)AT滤波估计增益：G2x0()CT(k/k-1)B(k)=R=Ck/k-1匕+R，其中滤波估计方程：S(k/k)=S(k/k一1)+)_Z(k)-S(/k-1)滤波误差协方差：P(k/k)=11-B(k)Cb(/k-1)2.初始化利用目标的前几个测量值建立状态的其实估计,采用两点起始法。S(2/2)=Z(2)xZ(2)Z(2)y-Z(1)yyTp2/2)=G2xG2yG2TG2t2G2yT滤波误差均

3、值：-x=二芒S(k)-Si/k)i=1滤波误差标准差：S(k)-SH/k)二.仿真分析利用MATLAB对前面建立的模型进行仿真，结果如下。图2.1图2.1是目标运动的真实轨迹和观测轨迹曲线。其中，真实轨迹显示目标在x=2000米处沿y轴方向做匀速直线运动，而观测轨迹是目标运动的真实轨迹加上方差和随机测量噪声得到的。从图中可以看出，观测轨迹围绕真实轨迹作上下浮动。单次滤波数据曲线100次滤波数据曲线图2.2图2.2是单次滤波和100次滤波后的数据曲线。从图中可以看出，滤波刚开始时误差较大，之后滤波误差逐渐降低，估计值逐步逼近真实轨迹。而随着滤波次数增加，滤波后的结果更为接近真实轨迹。图2.3图

4、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-

5、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*(

6、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)

7、;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(l,:),eSk(2,:);title(100次滤波数据曲线)；xlabel(X(米);ylabel(Y(米)；j=1:t;figure

8、;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)A2);eeSz(2,j)=sqrt(eeSz(2,j)-eSz(2,j)A2);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);