%第一步將三組雷達測得的數據分別存入zu1,zu2,zu3變量，方法是用所給Excel數據表格直接復制到Matlab數據變量中
%主程序,作圖，含雷達分布、每三個雷達求得的目標位置、畫球法所畫的球、最小二乘法求得的目標位置……
%本程序調用了兩個函數leida2.m和lsq2.m，如需改動限制角度，三個程序里的角度值都必須該
global zu1 zu2 zu3;  %定義為全局變量
x=zu2(:,1);  %取出第二組中的雷達x軸坐標
y=zu2(:,2);  %取出第二組中的雷達y軸坐標
r=zu2(:,4);  %取出第二組中的雷達測得的雷達與目標距離
a=1;
for i=1:length(zu2(:,1)-2)
    for j=i+1:length(zu2(:,1)-1)
        for k=j+1:length(zu2(:,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
[O,F,M]=fminimax('leida2',[-28148,4324.9,23926]);   %應用畫球法求解目標位置，O為所畫球的球心，M為所畫球的半徑
plot3(zu2(:,1),zu2(:,2),zu2(:,3),'*')    %畫出雷達的分布位置，用‘*’表示(不需要時可以去掉該句程序)
title('雷達及目標相對位置');
xlabel('X軸');
ylabel('Y軸');
zlabel('Z軸');
hold on
plot3(ok(:,1),ok(:,2),ok(:,3),'.k')  %畫出雷達測得的目標的分布位置，用‘.’表示
x0=[-28148,4324.9,23926];
[X,RESNORM,RESIDUAL,EXITFLAG]=lsqnonlin('lsq2',x0);   %應用最小二乘法求解目標位置，X為所求得的目標位置
plot3(X(1),X(2),X(3),'pr')   %畫出應用最小二乘法求解得的目標位置
box on
hold on
plot3(O(1),O(2),O(3),'or')   %畫出應用畫球法求解得的目標位置
hold on
ro=M^(1/2);
[xo,yo,zo]=sphere(25);
mesh(ro*(xo+O(1)/ro),ro*(yo+O(2)/ro),ro*(zo+O(3)/ro))  %畫出畫球法求得的球
hidden off
axis equal