遍历求解目标坐标.m

多基时差雷达定位算法。以雷达测量误差分布规律入手

%第一步将三组雷达测得的数据分别存入zu1,zu2,zu3变量，方法是用所给Excel数据表格直接复制到Matlab数据变量中
global zu1 zu2 zu3;  %定义为全局变量
x=zu1(:,1);  %取出第一组中的雷达x轴坐标
y=zu1(:,2);  %取出第一组中的雷达y轴坐标
r=zu1(:,4);  %取出第一组中的雷达测得的雷达与目标距离
a=1;
for i=1:length(zu1(:,1)-2)
    for j=i+1:length(zu1(:,1)-1)
        for k=j+1:length(zu1(:,1))  %遍历每三个雷达点的组合
            t1=abs(atan((y(j)-y(k))/(x(j)-x(k)+eps)));
            t2=abs(atan((y(i)-y(k))/(x(i)-x(k)+eps)));
            t3=abs(atan((y(i)-y(j))/(x(i)-x(j)+eps)));  %三个雷达点两两连接相交后所在直线与x轴所形成的角度
            v(1)=abs(t1-t2);v(2)=abs(t3-t2);v(3)=abs(t1-t3);v1=pi-max(v);v0=min(v,v1);  %求出三个雷达点两两连接相交后所组成三角形的最小角v0
            if (v0>30*pi/180)        %把最小角大于30度的情况判断出来进行目标位置求解
                x1=x(i);y1=y(i);r1=r(i);
                x2=x(j);y2=y(j);r2=r(j);
                x3=x(k);y3=y(k);r3=r(k);
                A=2*[x2-x1 y2-y1
                    x3-x2 y3-y2];
                b=[r1^2-r2^2+x2^2+y2^2-x1^2-y1^2
                    r2^2-r3^2+x3^2+y3^2-x2^2-y2^2];
                ok(a,1:2)=A\b;   
                ok(a,3)=sqrt(r1^2-(ok(a,1)-x1)^2-(ok(a,2)-y1)^2);  %分别求出当选择i,j,k点时所得的目标位置坐标
                a=a+1;
            end
        end
    end
end