Kalmanfilter仿真作业.docx

1、Kalmanfilter仿真作业研究生上机作业姓名：*学号:*1情景假设(学号末尾2, 6)假设有一个二坐标雷达对一平面上运动目标的进行观察， 目标在t = 0400秒沿y轴作恒速直线运功，运动速度为-15m/s ,目标的起点为(2000m 10000m),雷达扫描周期为2秒，x和y独立地进行观察，观察噪声的 标准差均为100m试建立雷达对目标的跟踪算法，并进行仿真分析，给出仿真 结果，画出目标真实轨迹、对目标的观察和滤波曲线。2.Kalman滤波算法分析为了简单起见，仅对X轴方向进行考虑。首先，目标运动沿X轴方向的运动可以用下面的状态方程描述：x(k 1) x(k) T*(k) (T2/2)

2、Ux(k)(21)X(k 1) X(k) Tux(k) .W(k)用矩阵的形式表述为，X(k 1) X(k)在上式中，X(k)景中 ux(k)=0。考虑雷达的观测，得出观测方程为：Z(k) C(k)X(k) V(k) (2.3)在(2.3)中，C(k) 1 0，V(k)为零均值的噪声序列，方差已知。对目标进行预测，由相关理论可得到下面的迭代式：Xk/k 1) Xk 1/k 1)(2.4)在(2.4)中，X?(k/k 1) EX(k)|Zk1，反映了由前k 1各观测值对目前状态的 估计。而预测的误差协方差可由下式表出，PX(k/k 1) FX(k 1/k 1) T Q(k 1) T (2.5)对

3、于最佳滤波，迭代表达式为：刃(k/k) X?(k/k 1) K(k)Z(k) C(k)Xk/k 1) (2.6)在式(2.6)中， K(k)为Kalman增益。而滤波误差的协方差为,Px(k/k) I K(k)C(k)P*(k/k 1)(2.7)在应用上面的公式进行Kalman滤波时，需要指定初值。由于实际中通常无 法得到目标的初始状态，我们可以利用前几个观测值建立状态的初始估计， 比如采用前两个观测值，刃(2/2) 乙(2) Zx(2) Zx(1)/TT(2.8)此时，估计误差为TX(2/2) vx(2) T_ ux(1)如(1)丁如(2) (2.9)而误差协方差矩阵为,3.仿真计算与结果首

4、先给出理论航迹,3.1理论航迹图由图3.1可以看出目标运动的理论航迹，即沿 y轴负方向直线运动。 假定观察噪声的标准差均为 100m，可以给出目标的观测数据和理论航迹的对比图，见图3.2。12000200011000100008000700060005000400030001500 1600 1700 1800 1900 2000 2100 2200 2300 2400 2500-理论航迹观测数据- . -90003.2观测数据和理论航迹对比图接下来经过Kalman滤波后，x轴方向的航迹图，见图3.3X轴方向航迹图3.3滤波输出x轴方向航迹图y轴方向的航迹图，见图3.4。Y轴方向航迹图3.5。

5、3.4滤波输出y轴方向航迹图将输出的x和y方向的航迹估计显示在直角坐标系下，见图ex(k)x/k) XKk| k) (3.1)i 1XOY方向航迹图3.5滤波输出xoy方向航迹图为了真实地反映出Kalman滤波的效果，采用了 Monte-Carlo方法，采用多 次实验取均值的方法进行研究，可以计算出估计的误差均值和方差，其表达式为:而误差的标准差可以表示为:在（ 3.1 ）和（3.2 ）中，M就是进行Monte-Carlo仿真的次数，而k为取样点 数。当仿真的次数越多时，实验的效果越接近于实际，但是计算的速度会明显变 慢。在仿真时，需要根据实际适当选取。在本程序中，取 M 50。下面对滤波估计

6、值的误差均值和方差进行简单分析， 对x和y方向的航迹估计进行分别讨论，假定 Monte-Carlo仿真的次数M = 50。由（3.1 ）和（3.2 式就可以求出实际滤波输出每时刻误差的均值和标准差。3.6估计误差均值和标准差曲线经过上面的仿真分析，可以看出 Kalman滤波算法对于动态目标的跟踪有着比较好的效果，而且可以较好地抑止环境中的噪声影响。 并且观测次数越多，滤波估计值的误差均值和方差越接近零，即是与理论航迹越接近。程序附录：1.Majectory.m （产生理论航迹图）function X,Y=trajectory（Ts,offtime）% 产生真实航迹 X,Y ，并在直角坐标系下显

7、示出 % Ts为雷达扫描周期，每隔Ts秒取一个观测数据 % 做匀速运动ifnargin2error（输入的变量过多，请检查）; endifofftime4error(输入的变量过多，请检查);endifofftime3error(Too many input arguments.); end offtime=400;% 产生理论的航迹x,y=trajectory(Ts,offtime);Pv=d*d;N=ceil(offtime/Ts)+1;randn(state,sum(100*clock); % 设置随机数发生器for i=1:Nvx(i)=d*randn(1); % 观测噪声，两者独立v

8、y(i)=d*randn(1);zx(i)=x(i)+vx(i); % 实际观测值zy(i)=y(i)+vy(i);end% 产生观测数据for n=1:mon% 用卡尔曼滤波得到估计的航迹XE YE=Kalman_filter1(Ts,offtime,d); %误差矩阵XER(1:N,n)=x(1:N) -(XE(1:N);YER(1:N,n)=y(1:N) -(YE(1:N); end%滤波误差的均值 XERB=mean(XER,2); YERB=mean(YER,2);%滤波误差的标准差flag=1XSTD=std(XER,1,2); % 计算有偏的估计值， YSTD=std(YER,1

9、,2);figure subplot(2,2,1) plot(XERB) axis(0 50 -50 50)xlabel(观测次数)ylabel(X 方向滤波误差均值 ),grid on; subplot(2,2,2) plot(YERB) axis(0 50 -50 50)xlabel(观测次数)ylabel(Y 方向滤波误差均值 ),grid on; subplot(2,2,3) plot(XSTD) axis(0 50 0 150)xlabel(观测次数)ylabel(X 方向滤波误差标准值 ),grid on; subplot(2,2,4) plot(YSTD) axis(0 50 0 150)xlabel(观测次数)ylabel(Y 方向滤波误差标准值 ),grid on;X=XER;Y=YER;