%-----------------------------------------------------------------------
% --- 外辐射源基于DOA联合TDOA时间积累下二维平面GDOP分析 ---一发一收体制----
%-----------------------------------------------------------------------
%------------------ 定位模型 --------------------
clc;clear;
close all;
c=3e8; % 传播速度
% % 郊区
% xo=-10e3;yo=0; % 接收站1的位置 %郊区
% xa=-xo;ya=0; % 发射站1的位置 %郊区
% % 目标位置
% xt=-41000:1002:51000;
% yt=-51000:1002:61000;
% % 市区
xo=-0.25e3;yo=0; % 接收站1的位置 %市区
xa=0.25e3;ya=0; % 发射站1的位置 %市区
% 目标位置
xt=-2000:53:3000;
yt=-2000:53:2000;
da=pi/180; % 方位测量误差标准差
dtao=1e-7; % 时差测量误差标准差
R=[(da)^2 0;0 (dtao)^2]; % 测量误差协方差矩阵
for l=1:length(xt)
for j=1:length(yt)
a=(sqrt((xt(l)-xo)*(xt(l)-xo)+(yt(j)-yo)*(yt(j)-yo))+sqrt((xt(l)-xa)*(xt(l)-xa)+(yt(j)-ya)*(yt(j)-ya)))/2;
tao=2*(a-xo)/c;
afa_diff_x =-(yt(j)-yo)/(xt(l)-xo)^2/(1+(yt(j)-yo)^2/(xt(l)-xo)^2);
afa_diff_y =1/(xt(l)-xo)/(1+(yt(j)-yo)^2/(xt(l)-xo)^2);
f_diff_x =2*xt(l)/(xo+c*tao)^2;
f_diff_y =2*yt(j)/(1/4*c^2*tao^2+c*tao*xo);
f_diff_tao
=-2*xt(l)^2/(xo+c*tao)^3*c-yt(j)^2/(1/4*c^2*tao^2+c*tao*xo)^2*(1/2*c^2*tao+c*xo);
H=[afa_diff_x afa_diff_y;-f_diff_x/f_diff_tao -f_diff_y/f_diff_tao];
Px=pinv(H)*R*pinv(H)';
GDOP(l,j)=sqrt(Px(1,1)+Px(2,2));
end
end
% V=[0.1 0.3 0.4 0.5 0.6 0.8 1 1.5 2 3 4 5 6 7 8 9 10 11 12 13 15 17 20 23 25
28 30 35 40 45 50 60 80 100];%郊区
% V=[0.08 0.1 0.11 0.12 0.125 0.13 0.135 0.14 0.145 0.147 0.15 0.155 0.16
0.165 0.17 0.175 0.18 0.2 0.3 0.5 0.7 1];%市区 %1e-6
V=[0.005 0.007 0.01 0.015 0.02 0.025 0.03 0.035 0.04 0.045 0.05 0.055 0.06
0.08 0.10 0.15 0.2 0.3 0.5 1];%市区 %1e-7
figure(1);
[pic1]=contour(xt./1000,yt./1000,GDOP.'/1000,V);
clabel(pic1);xlabel('X(Km) ');ylabel('Y(Km) ');title( 'CONTOUR of valleys of
GDOP in T_1R_1 (Km)' );
hold on;
plot(xo/1000,yo/1000,'gP',xa./1000,ya./1000,'rP');
axis([-1.25,1.75,-1.5,1.5]);%市区
% axis([-30,50,-40,40]);%郊区
grid on