资源描述：
《考虑两辆车道路上同向行驶,在O-16s时,两车均保持匀.doc》由会员上传分享，免费在线阅读，更多相关内容在行业资料-天天文库。
1、考虑两辆车在道路上同向行驶，在O-16s时，两车均保持匀速直线运动，由安装在后车上的车载毫米波雷达检测出与前车的距离为150m，相对速度为-3m/s，方位角。在16-20s时，前车向右偏转，与后车的相对角加速度为。后车加速，与前车的纵向相对加速度为。雷达的扫描周期为T=0.1s，系统噪声为，。量测误差为。functiongenzongT=0.1;%雷达扫描周期N=100;var1=0.3;var2=0.3;var3=1;var4=0.5;var5=0.2;R=[var3^200;0var4^20;00var5^2];
2、A1=[1TT^2/200;01T00;00100;0001T;00001];H1=[10000;01000;00010];G1=[T^2/40;T/20;10;0T/2;01];A2=[1TT^2/2000;01T000;001000;0001TT^2/2;00001T;000001];H2=[100000;010000;000100];G2=[T^2/40;T/20;0T^2/4;0T^2/4;0T/2;01];Q1=[var1^20;0var2^2];Q2=[var1^20;0var2^2];P=[0.950.
3、05;0.050.95];%模型转移矩阵U=[10]';%模拟目标真实运动x0=150;vx0=-5;y0=1;ax1=0;ay1=0;ax2=-1.5;ay2=1;t1=16;K1=t1/T;t2=20;K2=(t2-t1)/T;%0--16s,两车相对速度为零，相对角度为零k1=1:K1;x1=x0+vx0*(k1*T);vx1=vx0*ones(1,length(k1));y1=y0*ones(1,length(k1));%16--20s,两车相对速度增大，前车向右偏转k2=1:K2;x2=x1(K1)+vx0
4、*(k2*T)+ax2*(k2*T).^2/2;vx2=vx1(K1)+ax2*(k2*T);y2=y1(K1)+ay2*(k2*T).^2/2;x=[x1,x2];vx=[vx1,vx2];y=[y1,y2];K=K1+K2;xx=zeros(1,K);vxx=zeros(1,K);yy=zeros(1,K);vyy=zeros(1,K);ex1=zeros(1,K);ex2=zeros(1,K);evx1=zeros(1,K);evx2=zeros(1,K);ey1=zeros(1,K);ey2=zeros(1,
5、K);%进行N次仿真fori=1:N%产生观测数据zx=randn(1,K)*3+x;zvx=randn(1,K)*0.2+vx;zy=randn(1,K)*0.1+y;Z=[zx;zvx;zy];%两点起始确定初值LX=[zx(2)zvx(2)(zvx(2)-zvx(1))/Tzy(2)(zy(2)-zy(1))/T]';LP=[var3^202*var3^2/T^2000var4^20002*var3^2/T^208*var3^2/T^4+4*var4^2/T^2+var1^2/4+1000000000var5^
6、2var5^2/T000(var5^2)/T2*(var5^2)/T^2+(var2^2)*(T^2)/16];xx(1)=xx(1)+zx(1);vxx(1)=vxx(1)+zvx(1);yy(1)=yy(1)+zy(1);vyy(1)=vyy(1);ex1(2)=ex1(1)+x(1)-zx(1);ex2(2)=ex2(1)+(x(1)-zx(1))^2;evx1(2)=evx1(1)+vx(1)-zvx(1);evx2(2)=evx2(1)+(vx(1)-zvx(1))^2;ey1(1)=ey1(1)+y(1)
7、-zy(1);ey2(1)=ey2(1)+(y(1)-zy(1))^2;%从第M＋1次采样开始采用2模型的IMM算法LX1=[LX',0]';LX2=[LX',0]';LP1=[LP,zeros(5,1);zeros(1,6)];LP2=[LP,zeros(5,1);zeros(1,6)];fork=2:Kxx(k)=xx(k)+LX(1);vxx(k)=vxx(k)+LX(2);vyy(k)=LX(5);ex1(k)=ex1(k)+x(k)-LX(1);ex2(k)=ex2(k)+(x(k)-LX(1))^2;ev
8、x1(k)=evx1(k)+vx(k)-LX(2);evx2(k)=evx2(k)+(vx(k)-LX(2))^2;yy(k)=yy(k)+LX(4);ey1(k)=ey1(k)+y(k)-LX(4);ey2(k)=ey2(k)+(y(k)-LX(4))^2;%形成混合初始条件C_=P'*U;U=P.*(U*ones(1,2))./(ones(
