radarbest.m

双基地雷达空间定位精度分析

clear;clc;clf;
a=input('Please input the half value of Transmitter and Receiver distance (a):');
b=input('Please input the half value of X and Y direction range (b):');
dserr=input('Please input the value of distance squred error (dserr):');
aserr=input('Please input the value of angle squred error (aserr):');
xmax=2*b;ymax=2*b;zmax=0.4;n=0;
step=2;stepz=0.2;count=0;nf=4*b/step+1;nh=zmax/stepz;
x=-xmax;y=ymax;z=0.2;k(1)=0;
while(1)
   while(x<=xmax)
        while(y>=(-ymax))
            n=n+1;
            P11=zeros(3);P12=zeros(3);P13=zeros(3);P21=zeros(3);P22=zeros(3);P23=zeros(3);P31=zeros(3);P32=zeros(3);P33=zeros(3);
            R11=zeros(3);R12=zeros(3);R13=zeros(3);R21=zeros(3);R22=zeros(3);R23=zeros(3);R31=zeros(3);R32=zeros(3);R33=zeros(3);
            W11=zeros(3);W12=zeros(3);W13=zeros(3);W21=zeros(3);W22=zeros(3);W23=zeros(3);W31=zeros(3);W32=zeros(3);W33=zeros(3);
            A11=zeros(3);A12=zeros(3);A13=zeros(3);A21=zeros(3);A22=zeros(3);A23=zeros(3);A31=zeros(3);A32=zeros(3);A33=zeros(3);
            addW=zeros(3);Rx13=zeros(3);Rx132=zeros(3);Rx12=zeros(3);
            P=[P11 P12 P13;P21 P22 P23;P31 P32 P33];
            W=[W11 W12 W13;W21 W22 W23;W31 W32 W33];
            A=[A11 A12 A13;A21 A22 A23;A31 A32 A33];
            distanceR1=sqrt((x-2*a)^2+y^2+z^2);
            distanceR2=sqrt((x+2*a)^2+y^2+z^2);
            distanceT=sqrt(x^2+y^2+z^2);
            distanceR1o=sqrt((x-2*a)^2+y^2);
            distanceR2o=sqrt((x+2*a)^2+y^2);
            distanceTo=sqrt(x^2+y^2);
            angleTu=asin(z/distanceT);
            angleR1u=asin(z/distanceR1);
            angleR2u=asin(z/distanceR2);
            angleR1o=pi-acos((2*a-x)/distanceR1o);
            angleR2o=acos((2*a+x)/distanceR2o);
            if (x>=0)
                angleTo=acos(x/distanceTo);
            else
                angleTo=pi-acos(abs(x)/distanceTo);
            end         
            A11(1,:)=[cos(angleR1u)*cos(angleR1o),-distanceR1*sin(angleR1u)*cos(angleR1o),-distanceR1*cos(angleR1u)*sin(angleR1o)];
            A11(2,:)=[cos(angleR1u)*sin(angleR1o),-distanceR1*sin(angleR1u)*sin(angleR1o),distanceR1*cos(angleR1u)*cos(angleR1o)];
            A11(3,:)=[sin(angleR1u),distanceR1*cos(angleR1u),0];
            A22(1,:)=[cos(angleR2u)*cos(angleR2o),-distanceR2*sin(angleR2u)*cos(angleR2o),-distanceR2*cos(angleR2u)*sin(angleR2o)];
            A22(2,:)=[cos(angleR2u)*sin(angleR2o),-distanceR2*sin(angleR2u)*sin(angleR2o),distanceR2*cos(angleR2u)*cos(angleR2o)];
            A22(3,:)=[sin(angleR2u),distanceR2*cos(angleR2u),0];
            A33(1,:)=[cos(angleTu)*cos(angleTo),-distanceT*sin(angleTu)*cos(angleTo),-distanceT*cos(angleTu)*sin(angleTo)];
            A33(2,:)=[cos(angleTu)*sin(angleTo),-distanceT*sin(angleTu)*sin(angleTo),distanceT*cos(angleTu)*cos(angleTo)];
            A33(3,:)=[sin(angleTu),distanceT*cos(angleTu),0];
            P11=[dserr 0 0;0 aserr 0;0 0 aserr];
            P12=[0 0 0;0 0 0;0 0 0];
            P13=[dserr 0 0;0 0 0;0 0 0];
            P21=[0 0 0;0 0 0;0 0 0];
            P22=[dserr 0 0;0 aserr 0;0 0 aserr];
            P23=[0 0 0;0 0 0;0 0 0];
            P31=[dserr 0 0;0 0 0;0 0 0];
            P32=[0 0 0;0 0 0;0 0 0];
            P33=[dserr 0 0;0 aserr 0;0 0 aserr];
            R11=A11*P11*A11';R12=A11*P12*A11';R13=A11*P13*A11';R21=A22*P21*A22';R22=A22*P22*A22';R23=A22*P23*A22';R31=A33*P31*A33';R32=A33*P32*A33';R33=A33*P33*A33';
            R1=[R11 R13;R31 R33];
            W1=inv(R1);
            W11(1,:)=[W1(1,1),W1(1,2),W1(1,3)];W13(1,:)=[W1(1,4),W1(1,5),W1(1,6)];
            W11(2,:)=[W1(2,1),W1(2,2),W1(2,3)];W13(2,:)=[W1(2,4),W1(2,5),W1(2,6)];
            W11(3,:)=[W1(3,1),W1(3,2),W1(3,3)];W13(3,:)=[W1(3,4),W1(3,5),W1(3,6)];
            W31(1,:)=[W1(4,1),W1(4,2),W1(4,3)];W33(1,:)=[W1(4,4),W1(4,5),W1(4,6)];
            W31(2,:)=[W1(5,1),W1(5,2),W1(5,3)];W33(2,:)=[W1(5,4),W1(5,5),W1(5,6)];
            W31(3,:)=[W1(6,1),W1(6,2),W1(6,3)];W33(3,:)=[W1(6,4),W1(6,5),W1(6,6)];            
            addW=W11+W13+W31+W33;
            Rx13=inv(addW);Rx132=inv(inv(Rx13)+inv(R22));
            E2=pi*sqrt(det(Rx132));
            Rx12=inv(inv(R11)+inv(R22));
            E1=pi*sqrt(det(Rx12));
            k(n)=E1/E2;
            y=y-step;
        end
        x=x+step;
        y=ymax;
    end
    if(z<=zmax)
        z=z+stepz;x=-xmax;y=ymax;
    else break;
    end
end
x=-xmax:step:xmax;y=ymax:(-step):(-ymax);
[X,Y] = meshgrid(x,y);
for(i=1:nf)
    for(j=1:nf)
        l=i+(j-1)*nf;
        Z(i,j)=k(l);
    end
end
F=vpa(Z,3);
Z=double(F);
contour3(X,Y,Z,3000);
datacursormode on