trajectory3.m

雷达数据处理中的重要模型算法之一

%clear,clc
T=1;
X(:,1)=[120000 -426 0 2000 0 0];
for k=2:31   %cv
    X(:,k)=[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]*X(:,k-1);
end
X([3 6],31)=[5 -10]';
for k=32:38   %ca
    X(:,k)=[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]*X(:,k-1);
end
X([3 6],38)=[-8 18]';
for k=39:49   %ca
    X(:,k)=[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]*X(:,k-1);
end
X([3 6],49)=[10 -20]';
for k=50:61   %ca
    X(:,k)=[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]*X(:,k-1);
end
X([3 6],61)=[0 30]';
for k=62:65   %ca
    X(:,k)=[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]*X(:,k-1);
end
X([3 6],65)=[-10 -8]';
X(:,66)=[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]*X(:,65);
X([3 6],66)=[-5 0]';
for k=67:81   %ca
    X(:,k)=[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]*X(:,k-1);
end
X([3 6],81)=[5 -10]';
for k=82:90   %ca
    X(:,k)=[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]*X(:,k-1);
end


Z(1,:)=X(1,:);%x位置
Z(2,:)=X(2,:);%x速度
Z(3,:)=X(4,:);%y位置
Z(4,:)=X(5,:);%y速度

delta_weiyi=200;     %位移误差
delta_sudu=10;       %速度误差
%加噪
z(1,:)=Z(1,:)+(2*rand(1,90/T)-1)*delta_weiyi;  %x测量位移
z(2,:)=Z(2,:)+(2*rand(1,90/T)-1)*delta_sudu;   %x测量速度
z(3,:)=Z(3,:)+(2*rand(1,90/T)-1)*delta_weiyi;  %y测量位移
z(4,:)=Z(4,:)+(2*rand(1,90/T)-1)*delta_sudu;   %y测量速度
% 
% figure; %x位移
% plot(X(1,:)/1000,'k'); %真实
% hold on
% plot(z(1,:)/1000,'b:'); %测量
% title('X轴位移');
% xlabel('t/（秒）');ylabel('位移/（米）');
% legend('真实','测量');
% 
% figure; %x速度
% plot(X(2,:),'k'); %真实
% hold on
% plot(z(2,:),'b:'); %测量
% title('X轴速度');
% xlabel('t/（秒）');ylabel('速度/（米）');
% legend('真实','测量');
% 
% figure; %y位移
% plot(X(4,:),'k'); %真实
% hold on
% plot(z(3,:),'b:'); %测量
% title('y轴位移');
% xlabel('t/（秒）');ylabel('位移/（米）');
% legend('真实','测量');
% 
% figure; %y速度
% plot(X(5,:),'k'); %真实
% hold on
% plot(z(4,:),'b:'); %测量
% title('y轴速度');
% xlabel('t/（秒）');ylabel('速度/（米）');
% legend('真实','测量');
% % 
% figure; %XY平面上的目标轨迹
% plot(X(1,:)/1000,X(4,:)/1000,'k'); %真实
% hold on
% plot(z(1,:)/1000,z(3,:)/1000,'b:'); %测量
% title('XY平面上的目标轨迹');
% xlabel('x/（米）');ylabel('y/（米）');
% legend('真实','测量');
% 


%非线性
% Z(1,:)=sqrt(X(1,:).^2+X(4,:).^2);
% Z(2,:)=atan2(X(4,:),X(1,:));
% save data X Z;
% 
% figure
% plot(X(1,:),X(4,:));
% title('XY平面上的目标轨迹');
% xlabel('x/（米）');ylabel('y/（米）');