1
考虑两辆车在道路上同向行驶,在O-16s时,两车均保持匀速直线运动,由安装在后车上的
车载毫米波雷达检测出与前车的距离为150m,相对速度为-3m/s,方位角
2
°
。在16-20s时,前
车向右偏转,与后车的相对角加速度为
2
1 / s
°
。后车加速,与前车的纵向相对加速度为
2
a 1.8m / s= -
。雷达的扫描周期为T=0.1s,系统噪声为
0.3m / s
a
s
=
,
0.3 / s
b
s
°
=
。量测误差
为
1 2 3
1m, 0.5m / s, 0.2 / s
s s s
°
= = =
。
function genzong
T=0.1; %雷达扫描周期
N=100;
var1=0.3;var2=0.3;var3=1;var4=0.5;var5=0.2;
R=[var3^2 0 0;0 var4^2 0;0 0 var5^2];
A1=[1 T T^2/2 0 0;0 1 T 0 0;0 0 1 0 0;0 0 0 1 T;0 0 0 0 1];
H1=[1 0 0 0 0 ;0 1 0 0 0;0 0 0 1 0];
G1=[T^2/4 0;T/2 0;1 0;0 T/2;0 1];
A2=[1 T T^2/2 0 0 0;0 1 T 0 0 0;0 0 1 0 0 0;0 0 0 1 T T^2/2;0 0 0 0 1 T;0 0 0 0 0 1];
H2=[1 0 0 0 0 0;0 1 0 0 0 0;0 0 0 1 0 0];
G2=[T^2/4 0;T/2 0;0 T^2/4;0 T^2/4;0 T/2;0 1];
Q1=[var1^2 0;0 var2^2];
Q2=[var1^2 0;0 var2^2];
P=[0.95 0.05;0.05 0.95];%模型转移矩阵
U=[1 0]';
%模拟目标真实运动
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*(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,K);
%进行 N 次仿真
for i=1:N