turn_left.m

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

%左转弯
function [X,Z,z]=turn_left(XX,t)
% 转弯机动(飞机向原点逼近),XX=[x,vx,ax,y,vy,ay]',t为机动转弯时间;
% 输出[X,Z,z]分别为[xy真实值，极坐标真实值，测量值(加噪)];
% 飞机在转弯时，其向心加速度一般为a=2g~3g左右，
% 且有公式：R=v*v/a=v/w,  a=w*v;  w为角速度，R为半径，v为切向速度
% 取4维状态向量X=(x vx ax  y vy ay)';

T=1;
w=0.08;     %角速度
v=sqrt(XX(2)^2+XX(5)^2);    %飞机的机动初合速度
alpha=atan2(XX(5),XX(2));   %飞机的机动初速度方向
a=XX(1)+v/w*cos(90*pi/180+alpha);  %圆心的坐标（a,b）;
b=XX(4)+v/w*sin(90*pi/180+alpha);

X(:,1)=[a+v/w*cos(w*T+alpha-90*pi/180)
              v*cos(w*T+alpha)
         (v*cos(w*T+alpha)-XX(2))/T
        b+v/w*sin(w*T+alpha-90*pi/180)
              v*sin(w*T+alpha)
         (v*sin(w*T+alpha)-XX(5))/T ];

for i=2:t/T
    X(1,i)=a+v/w*cos(w*i*T+alpha-90*pi/180);
    X(2,i)=v*cos(w*i*T+alpha);
    X(3,i)=(X(2,i)-X(2,i-1))/T;
    X(4,i)=b+v/w*sin(w*i*T+alpha-90*pi/180);
    X(5,i)=v*sin(w*i*T+alpha);
    X(6,i)=(X(5,i)-X(5,i-1))/T;
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,t)-1)*delta_weiyi;  %x测量位移
z(2,:)=Z(2,:)+(2*rand(1,t)-1)*delta_sudu;   %x测量速度
z(3,:)=Z(3,:)+(2*rand(1,t)-1)*delta_weiyi;  %y测量位移
z(4,:)=Z(4,:)+(2*rand(1,t)-1)*delta_sudu;   %y测量速度
% Z(1,:)=sqrt(X(1,:).^2+X(4,:).^2);   %转为极坐标值Z
% Z(2,:)=atan2(X(4,:),X(1,:));
% 
% %极坐标值加噪z
% delta_r=50;     %径向距离测量误差协方差
% delta_a=0.005;  %方位角测量误差的协方差
% z(1,:)=Z(1,:)+randn(1,t/T)*delta_r;    
% z(2,:)=Z(2,:)+randn(1,t/T)*delta_a;