getpolar.m

序惯最小二乘法

function  rgm=getpolar(x,y,K_num)
K=K_num;
Range_a=zeros(1,K);
Azimuth_a=zeros(1,K);
Range_b=zeros(1,K);
Azimuth_b=zeros(1,K);
u=100;
v=10;
for k=1:K
    Range_a(k)=sqrt(x(k)^2+y(k)^2);
    Azimuth_a(k)=atan(x(k)/y(k));
    Range_b(k)=sqrt((x(k)-u)^2+(y(k)-v)^2);
    Azimuth_b(k)=atan((x(k)-u)/(y(k)-v));
end
rgm=[Range_a;Azimuth_a;Range_b;Azimuth_b];