locate.m

TDOA定位中的模糊研究

distance=input('Please input the distance between main station and assistant stations A&B:');
angle=input('Please input the angle of two assistant stations:');
x0=0;
y0=0;
x1=abs(distance*sin(angle*pi*0.5/180));
x2=-x1;
y1=abs(distance*cos(angle*pi*0.5/180));
y2=y1;
A=(x0-x1)*(y0-y2)-(x0-x2)*(y0-y1);
a11=(y0-y2)/A;
a12=-(y0-y1)/A;
a21=-(x0-x2)/A;
a22=(x0-x1)/A;
xmax=50;
ymax=50;
x=-xmax;
y=ymax;
while(1)
while(y~=-ymax)
    r0=sqrt((x-x0)^2+(y-y0)^2);
    r1=sqrt((x-x1)^2+(y-y1)^2);
    r2=sqrt((x-x2)^2+(y-y2)^2);
    dr1=r1-r0;dr2=r2-r0;
    k1=0.5*[dr1^2+(x0^2+y0^2)-(x1^2+y1^2)];
    k2=0.5*[dr2^2+(x0^2+y0^2)-(x2^2+y2^2)];
    m1=a11*k1+a12*k2;
    m2=a21*k1+a22*k2;
    n1=a11*dr1+a12*dr2;
    n2=a21*dr1+a22*dr2;
    a=n1^2+n2^2-1;
    b=(m1-x0)*n1+(m2-y0)*n2;
    c=(m1-x0)^2+(m2-y0)^2;
    if (b^2>a*c)
        if (((-b+sqrt(b^2-a*c))/a)>0&((-b-sqrt(b^2-a*c))/a)>0)
            plot(x,y,'k.');
            hold on;
        end
    end
    y=y-1;
end
if(x~=xmax)
    x=x+1;
    y=ymax;
else break;
end
plot(x0,y0,'ro',x1,y1,'ro',x2,y2,'ro');

end