GPS和基站TDOA联合定位的仿真
2014-05-10 21:13阅读：
当GPS卫星不足4颗时，使用基站的TDOA观测量与GPS伪距观测量，组成混合定位模型实现定位。
%%
设置仿真环境
%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);
