加载中…
个人资料
GreenSim团队
GreenSim团队
  • 博客等级:
  • 博客积分:0
  • 博客访问:448,622
  • 关注人气:680
  • 获赠金笔:0支
  • 赠出金笔:0支
  • 荣誉徽章:
正文 字体大小:

GPS和基站TDOA联合定位的仿真

(2014-05-10 21:13:36)
标签:

杂谈

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

GPS和基站TDOA联合定位的仿真

%% 设置仿真环境

%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);

0

阅读 收藏 喜欢 打印举报/Report
  

新浪BLOG意见反馈留言板 欢迎批评指正

新浪简介 | About Sina | 广告服务 | 联系我们 | 招聘信息 | 网站律师 | SINA English | 产品答疑

新浪公司 版权所有