zu2作图.m

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

%第一步将三组雷达测得的数据分别存入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