function gdop_doa(t,range)
clc;
%t为各接收站坐标二维数组,各数组中包含接收站的x,y,z坐标和测量值φ的标准差
%range数组包含仿真区域的经纬度范围及分辨率
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% [t1]=[0.0,-1000.0,7.0,1];
% [t2]=[0,1000,7,1];
% % [t3]=[0,0,7,1];
% [range]=[-2000,2000,100,-2000,2000,100];
% t=[t1;t2];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[T_Num,~] = size(t);
%T_Num = length(t);
liny = linspace(range(1),range(2),range(3));%用于产生x1,x2之间的N点行矢量。其中x1、x2、N分别为起始值、终止值、元素个数
linx = linspace(range(4),range(5),range(6));
[linY,linX] = meshgrid(liny,linx);%meshgrid返回两个y*x的矩阵
xn = length(linx);
yn = length(liny);
Cd = zeros;
for i = 1:T_Num %matlab中的下标都是从1开始的
Cd(i,i)=(t(i,4)*pi/180).^2;%测量误差协方差矩阵
end
Rdt = Cd;
GDOP=zeros;
for ii=1:xn
for jj = 1:yn
x0=linx(ii);
y0=liny(jj);
x = [x0,y0];
C=Jacobi_doa(x,t);%各点处的雅可比矩阵
if T_Num < 3
Pdx = C\Rdt/(C');
GDOP(ii,jj) = sqrt(Pdx(1,1) + Pdx(2,2)) ;
else
CTC = C'*C;
Pdx = CTC\C'*Rdt*C/(CTC');
GDOP(ii,jj) = sqrt(Pdx(1,1) + Pdx(2,2));
end
end
end
%%二维
%v=[0;5;10;15;20;25;30;35;40;35;50;55;60;65;70;75;80;];
figure;
%[C,h]=contourf(linX,linY,GDOP,v);
%clabel(C,h);
pcolor(linX,linY,GDOP)
shading interp;
colormap(colorcube)
caxis([0,5000])
hold on;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
h=colorbar;
set(get(h,'title'),'string','单位(m)','fontsize',20);
set(h,'fontsize',20);
set(gca,'fontsize',20);
for j = 1:T_Num
scatter(t(j,1),t(j,2),1500,'r','filled','p');
text(t(j,1),t(j,2),'T');hold on;
end
xlabel('x-axis(m)');ylabel('y-axis(m)')
title(sprintf('标准差为%d度,%d站DOA定位(二维空间)GDOP',t(2,4),T_Num))
%%%%%%%%%