trajectory.asv

编写卡尔曼滤波程序实现航迹的滤波估计

function [X,Y]=trajectory(Ts,offtime)
% 产生真实航迹[X,Y]，并在直角坐标系下显示出
% Ts为雷达扫描周期，每隔Ts秒取一个观测数据
% 最初做匀速运动，接下来进行两个90度的机动转弯

if nargin>2
    error('输入的变量过多，请检查');
end

if offtime<600
    error('仿真时间必须大于600s，请重新输入');
end

x=zeros(offtime,1);
y=zeros(offtime,1);
X=zeros(ceil(offtime/Ts),1);
Y=zeros(ceil(offtime/Ts),1);

% t=0:400s，速度vx,vy为沿x和y轴的速度分量(m/s)
x0=2000;%起始点坐标
y0=10000;
vx=0;
vy=15; % 沿y方向

for t=1:400
    x(t)=x0+vx*t;
    y(t)=y0+vy*t;
end

% t=400:600s，ax,ay为沿x和y轴的加速度分量(m/s/s)
ax=24.975;
vx=10;
%ay=0.175;

for t=0:30
    x(t+401)=x(400)+vx*t+ax*t*t/2;
    y(t+401)=y(400)+vy*t;
    vx=10+ax*t;
end
%%%%%%%%%%%%%%%%%
ax=0.975;
%ay=0.175;
for t=0:100
    x(t+431)=x(430)+vx*t+ax*t*t/2;
    y(t+431)=y(430)+vy*t;
end

% 600s以后匀速运动，一直到截止时间
for t=0:(offtime-530)
    x(t+531)=x(531)+vx*t;
    y(t+531)=y(531)+vy*t;
end

% 得到雷达的观测数据
for n=0:Ts:offtime
    X(n/Ts+1)=x(n+1);
    Y(n/Ts+1)=y(n+1);
end
for i=0:Ts:offtime
   vx(i)=d*normrnd(0,0.5,[1 800]); % 观测噪声，两者独立
   vy(i)=d*normrnd(0,0.5,[1 800]);
   zx(i)=x(i)+vx(i); % 实际观测值
   zy(i)=y(i)+vy(i);
end

%显示真实轨迹
plot(X,Y,'LineWidth',2),
axis([1000 30000 15900 16400]),grid on;
legend('目标真实航迹');
plot(zx,Y,'LineWidth',2),
axis([1000 30000 15900 16400]),grid on;
legend('目标真实航迹');