clear
%蛇形机动运动环境仿真
s=2840; % 初始距离
f=30*pi/180; % 目标初始方位角
T=0.05; % 采样间隔
x0=s*cos(f); % 初始坐标
y0=s*sin(f);
v=300; % 速度
W=[0.5;1.0;1.5;2;2.5]; % 转弯速率数组
m=-1; % 方向系数
q=10; % 状态误差方差常数
Q=[T^2*T^2/4 T^2*T^2/4 T^2*T/2 T*T^2/2
T^2*T^2/4 T^2*T^2/4 T*T^2/2 T*T^2/2
T^2*T/2 T*T^2/2 T^2 T^2
T*T^2/2 T*T^2/2 T^2 T^2]*200; %
L=[T^2/2 T^2/2 T T];
R=0.1.*eye(2);
P=1500.*eye(4);
I=eye(4);
n=10000; % 采样次数
k=q*randn(n,2); % 白噪声矩阵
o=k(:,1)'; % x方向上的噪声
b=k(:,2)'; % y方向上的噪声
d=0; % 总滤波次数
i=0; % 观测矩阵
for j=1:length(W)
j;
m=m*(-1);
w=m*W(j); % 当前转弯速率
r=abs(v/w); % 机动半径
xr=x0-r*cos(f); % 机动中心坐标
yr=y0-r*sin(f);
for i=0:n
x2=xr+r*cos(f+w*i*T);
y2=yr+r*sin(f+w*i*T); % 机动理论值
d=d+1;
z(:,d)=[x2;y2]; % 理论矩阵
%kalman滤波
ch=pi-abs(w)*i*T; % 状态判断量
js=abs(w)*T; % 单位时间转动量
%X、Y的均方差
if (ch<js)
x0=xr-r*cos(f); % 下次机动起始坐标
y0=yr-r*sin(f);
break
end
end
end
plot(z(1,:),z(2,:))
