Untitled.zip_GDOP_GDOP的matlab实现_TDOA GDOP matlab_Untitled_gdop m

clc; clear; x2=-12.99; x3=12.99; xt=0; x1=0;%%%星型d=15km y2=7.5; y3=7.5; yt=-15; y1=0; z2=0.01; z3=0; zt=0.01; z1=0.01; % % x1=-25.98; x2=25.98; x3=0; xt=0;%%%星型d=30km % y1=15; y2=15; y3=-30; yt=0; % z1=0.2; z2=0.2; z3=0.2; zt=0.25; % xt=-12.99; x2=12.99; x3=0; x1=0;%%%菱形型d=15km % yt=7.5; y2=7.5; y3=15; y1=0; % zt=0; z2=0; z3=0; z1=0.1; % x1=-25.98; x2=25.98; x3=0; xt=0;%%%菱形型d=30km % y1=15; y2=15; y3=30; yt=0; % z1=0; z2=0; z3=0; zt=0.1; % x1=-10.6; x2=10.6; x3=21.2; xt=0;%%%平行角型形型d=15km % y1=10.6; y2=10.6; y3=0; yt=0; % z1=0; z2=0; z3=0; zt=0.1; % x1=-21.2; x2=21.2; x3=42.4; xt=0;%%%平行型形型d=30km % y1=21.2; y2=21.2; y3=0; yt=0; % z1=0; z2=0; z3=0; zt=0.1; % % x1=-30; x2=0; x3=30; xt=0;%%%倒三形型d=30km % y1=30; y2=30; y3=30; yt=0; % z1=0; z2=0; z3=0; zt=0.1; z=15; y=-200:1:200;x=-200:1:200; % for i=1:401 for j=1:401 % for i=-70:70 % for j=-70:70 m=x(i); n=y(j); r1=((m-x1).^2+(n-y1).^2+(z-z1).^2).^(1/2); r2=((m-x2).^2+(n-y2).^2+(z-z2).^2).^(1/2); r3=((m-x3).^2+(n-y3).^2+(z-z3).^2).^(1/2); %r4=((m-x4).^2+(n-y4).^2+(z-z4).^2).^(1/2); rt=((m-xt).^2+(n-yt).^2+(z-zt).^2).^(1/2); c11=(m-x1)/r1;c21=(m-x2)/r2;c31=(m-x3)/r3;ct1=(m-xt)/rt; c12=(n-y1)/r1;c22=(n-y2)/r2;c32=(n-y3)/r3;ct2=(n-yt)/rt; c13=(z-z1)/r1;c23=(z-z2)/r2;c33=(z-z3)/r3;ct3=(z-zt)/rt; c=[(-ct1+c11) (-ct2+c12) (-ct3+c13);(-ct1+c21) (-ct2+c22) (-ct3+c23);(-ct1+c31) (-ct2+c32) (-ct3+c33)]; b=inv(c'*c)*(c'); b11=b(1); b12=b(4); b13=b(7); b21=b(2); b22=b(5); b23=b(8); b31=b(3); b32=b(6); b33=b(9); % b14=b(10); % b24=b(11); % b34=b(12); sigmap=0.5;%测量误差的标准差 sigmas=0.0075;%测时误差 eta=0.35;%Ri站距离和测量之间的相关系数 sigma11=sigmap.^2+sigmas.^2*2; sigma22=sigmap.^2+sigmas.^2*2; sigma33=sigmap.^2+sigmas.^2*2; sigma44=sigmap.^2+sigmas.^2*2; sigma12=eta*sigmap.^2+sigmas.^2; sigma13=eta*sigmap.^2+sigmas.^2; sigma14=eta*sigmap.^2+sigmas.^2; sigma21=eta*sigmap.^2+sigmas.^2; sigma23=eta*sigmap.^2+sigmas.^2; sigma24=eta*sigmap.^2+sigmas.^2; sigma31=eta*sigmap.^2+sigmas.^2; sigma32=eta*sigmap.^2+sigmas.^2; sigma34=eta*sigmap.^2+sigmas.^2; sigma41=eta*sigmap.^2+sigmas.^2; sigma42=eta*sigmap.^2+sigmas.^2; sigma43=eta*sigmap.^2+sigmas.^2; sigmax2=b11*b11*sigma11+b11*b12*sigma12+b11*b13*sigma13+b12*b11*sigma21+b12*b12*sigma22+b12*b13*sigma23+b13*b11*sigma31+b13*b12*sigma32+b13*b13*sigma33; sigmay2=b21*b21*sigma11+b21*b22*sigma12+b21*b23*sigma13+b22*b21*sigma21+b22*b22*sigma22+b22*b23*sigma23+b23*b21*sigma31+b23*b22*sigma32+b23*b23*sigma33; sigmaz2=b31*b31*sigma11+b31*b32*sigma12+b31*b33*sigma13+b32*b31*sigma21+b32*b32*sigma22+b32*b33*sigma23+b33*b31*sigma31+b33*b32*sigma32+b33*b33*sigma33; gdop(j,i)=(sigmax2+sigmay2+sigmaz2).^(1/2); end end figure(1); [c,handle]=contour(x,y,gdop,25);%[c,handle]=contour(gdop,25); clabel(c,handle); % hold on; % plot(0,0,0.1,'rpentagram','linewidth',2); % text(0,0,1,'T'); % plot(-30,0,0,'rpentagram','linewidth',2); % text(-31,0,0,'R1'); % plot(30,0,0,'rpentagram','linewidth',2); % text(31,0,0,'R2'); % plot(0,-5,0,'rpentagram','linewidth',2); % text(0,-6,0,'R3'); % plot(0,5,0,'rpentagram','linewidth',2); % text(0,6,1,'R4'); % hold off; xlabel('x方向(单位:km)'); ylabel('y方向(单位:km)'); % title('GDOP图');