turn_right.m

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

%右转弯
function [X,Z,z]=turn_right(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(alpha-90*pi/180);  %圆心的坐标（a,b）;
b=XX(4)+v/w*sin(alpha-90*pi/180);

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

for i=2:t/T
    X(1,i)=a+v/w*cos(alpha+90*pi/180-w*i*T);
    X(2,i)=v*cos(alpha-w*i*T);
    X(3,i)=(X(2,i)-X(2,i-1))/T;
    X(4,i)=b+v/w*sin(alpha+90*pi/180-w*i*T);
    X(5,i)=v*sin(alpha-w*i*T);
    X(6,i)=(X(5,i)-X(5,i-1))/T;
end

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;