lsq3.m

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

function f=lsq3(X)
global zu3;
x=zu3(:,1);  %取出第三组中的雷达x轴坐标
y=zu3(:,2);  %取出第三组中的雷达y轴坐标
r=zu3(:,4);  %取出第三组中的雷达测得的雷达与目标距离
a=1;
for i=1:length(zu3(:,1)-2)
    for j=i+1:length(zu3(:,1)-1)
        for k=j+1:length(zu3(:,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
xi=ok(:,1);
yi=ok(:,2);
zi=ok(:,3);
t=1:length(xi);
f=(X(1)-xi(t)).^2+(X(2)-yi(t)).^2+(X(3)-zi(t)).^2;%目标函数，使到个点的距离和最小