clear,clc
tic
N=1;
R=[1*(pi/180)^2 0;0 1*(pi/180)^2;];
% 3个观测站的位置S1,S2,S3,目标位置为T
%%%%%%%%% 单位换算成公里!!!
S1=[0 30]*1000;S2=[ 0 -30]*1000;
xt=-41000:2000:41000;
yt=-41000:2000:41000;
S=[S1;S2];
%y1=tan(fe1)*(x-S1(1))+S1(2);%S1的测向射线
%y2=tan(fe2)*(x-S2(1))+S2(2);%S2的测向射线
%y3=tan(fe3)*(x-S3(1))+S3(2);%S3的测向射线
k=1;
for l=1:length(xt)
for j=1:length(yt)
rr1_2=(xt(l)-S(1,1))^2+(yt(j)-S(1,2))^2;
rr2_2=(xt(l)-S(2,1))^2+(yt(j)-S(2,2))^2;
% rr3_2=(xt(l)-S(3,1))^2+(yt(j)-S(3,2))^2;
H(1,1)=-(yt(j)-S(1,2))/rr1_2;H(1,2)=(xt(l)-S(1,1))/rr1_2;
H(2,1)=-(yt(j)-S(2,2))/rr2_2;H(2,2)=(xt(l)-S(2,1))/rr2_2;
% H(3,1)=-(yt(j)-S(3,2))/rr3_2;H(3,2)=(xt(l)-S(3,1))/rr3_2;
Px=pinv(H)*R*pinv(H');
gdop(l,j)=sqrt(Px(1,1)+Px(2,2));
regdop(l,j)=gdop(l,j)/(sqrt(xt(l)*xt(l)+yt(j)*yt(j)));
end
end
Q=[0.5 0.6 0.7 0.8 0.9 1 1.2 1.3 1.5 2 2.5 3 5 7 10];
figure(1);
pic1=contour(xt/1000,yt/1000,gdop'/1000,Q),clabel(pic1);
xlabel('X（Km）');ylabel('Y（Km）');title( 'GDOP(T_1R_2)（Km） ' );hold on ;
plot(S1(1,1)/1000,S1(1,2)/1000,'rP',S2(1,1)/1000,S2(1,2)/1000,'rP');
axis([-40,40,-40,40]);
grid on
W=[0.01 0.02 0.025 0.03 0.035 0.04 0.045 0.05 0.06 0.07 0.08 0.1 0.2 0.5 1 2];
figure(2);
pic2=contour(xt/1000,yt/1000,regdop',W),clabel(pic2);
xlabel('X（Km）');ylabel('Y（Km）');title( '相对GDOP(T_1R_2)' );hold on ;
plot(S1(1,1)/1000,S1(1,2)/1000,'rP',S2(1,1)/1000,S2(1,2)/1000,'rP');
axis([-40,40,-40,40]);
grid on
toc
D152
