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;
copyright@ 2008-2022 冰豆网网站版权所有
经营许可证编号:鄂ICP备2022015515号-1