基于DOA联合TDOA时间积累下二维平面GDOP

%-----------------------------------------------------------------------

%  --- 外辐射源基于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