当GPS卫星不足4颗时，使用基站的TDOA观测量与GPS伪距观测量，组成混合定位模型实现定位。

http://s14/mw690/001nrTRpgy6IM2ZIE6F6d&690

%% 设置仿真环境

%GPS卫星坐标

GPSxyz=[

         20000, 20000,20000;

        -20000, 20000,20000;

        -20000,-20000,20000;

         20000,-20000,20000;

       ];

%基站坐标

BSxyz0=[1000,0,30];

BSxyz=[

             0, 1000,   30;

        -1000,     0,   30;

             0,-1000,   30;

       ];

%目标真实坐标

xr=1000*(rand-0.5);

yr=1000*(rand-0.5);

zr=100*rand;

wr=0;

%  GreenSim团队——专业级算法设计&代写程序

%  欢迎访问GreenSim团队主页→http://blog.sina.com.cn/greensim

%% 模拟产生观测量

M=size(GPSxyz,1);%伪距观测量个数

N=size(BSxyz,1);%TDOA观测量个数

SigmaDist=20;%伪距观测误差，标准差，折算成距离（单位：米）

SigmaRho=60;%TDOA观测误差，标准差，折算成距离（单位：米）

Dist=zeros(M,1);

Rho=zeros(N,1);

for i=1:M

    xgi=GPSxyz(i,1);

    ygi=GPSxyz(i,2);

    zgi=GPSxyz(i,3);

    Dist(i)=sqrt((xr-xgi)^2+(yr-ygi)^2+(zr-zgi)^2)+normrnd(0,SigmaDist,1,1);

end

for i=1:N

    xb0=BSxyz0(1);

    yb0=BSxyz0(2);

    zb0=BSxyz0(3);

    xbi=BSxyz(i,1);

    ybi=BSxyz(i,2);

    zbi=BSxyz(i,3);

    Rho(i)=sqrt((xr-xbi)^2+(yr-ybi)^2+(zr-zbi)^2)-sqrt((xr-xb0)^2+(yr-yb0)^2+(zr-zb0)^2)+normrnd(0,SigmaRho,1,1);

end

%% 设置迭代初始值和停止条件

XR=[xr;yr;zr;wr];

X0=XR+1000*(rand(size(XR))-0.5);

Delta=0.00001;%收敛精度

K=100;%最大迭代次数

%% 调用定位算法

tic

[X,AllX,Alldxyz]=Taylor_GPS_TDOA_3D(X0,Dist,Rho,GPSxyz,BSxyz0,BSxyz,Delta,K);

toc

%% 整理结果

disp('目标真实坐标为（米）：');

disp(XR(1:3));

disp('迭代初始点坐标为（米）：');

disp(X0(1:3));

disp('定位结果为（米）：');

disp(X(1:3));

disp('定位误差为（米）：');

disp(sqrt(sum((X(1:3)-XR(1:3)).^2)));

%% 绘制收敛曲线

figure;

plot(Alldxyz,'.-');

xlabel('迭代次数','FontName','Times New Roman','FontSize',10);

ylabel('步进幅度（均方根误差）','FontName','Times New Roman','FontSize',10);

title('收敛曲线','FontName','Times New Roman','FontSize',10);

%% 计算当前GDOP

[G,PHI]=GDOP_GPS_TDOA_3D(X,GPSxyz,BSxyz0,BSxyz,SigmaDist,SigmaRho);

%%

disp('GDOP误差分布矩阵为');

disp(PHI);

disp('定位结果附近的GDOP值为（米）：');

disp(G);