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

三维TOA/AOA定位的泰勒级数展开最小二乘算法MATLAB源代码

(2012-09-01 20:12:54)
标签:

it

杂谈

知识/探索

    各种体制的无线定位问题问题,通常都可以归结为非线性参数估计,泰勒级数展开最小二乘法则是解决非线性参数估计的一种常用方法。
三维TOA/AOA定位的泰勒级数展开最小二乘算法MATLAB源代码
三维TOA/AOA定位的泰勒级数展开最小二乘算法MATLAB源代码
clc
clear
close all
%% 设置基站个数和坐标
%  GreenSim团队——专业级算法设计&代写程序
%  欢迎访问GreenSim团队主页→http://blog.sina.com.cn/greensim
%设置基站,矩阵每一行带表一个基站的三维坐标(米),矩阵的行数代表基站个数
BSxyz=[
          0,   0,   0;
       1000,   0,   0;
          0,1000,   0;
          0,   0,1000;
       ];
N=size(BSxyz,1);%基站个数
%% 设置移动台真实坐标
xr=1000*rand;
yr=1000*rand;
zr=1000*rand;
%% 设置观测误差水平
%移动台到达角AOA观测误差,标准差,单位:度
SigmaAlpha=0.1;
%基站离开角AOD观测误差,标准差,单位:度
SigmaBeta=0.1;
%TOA观测误差,标准差,折算成距离(米)
SigmaRho=10;
%% 随机产生观测量
Alpha=zeros(N,1);%方位角
Beta=zeros(N,1);%俯仰角
Rho=zeros(N,1);%到达时间
for i=1:N
    xi=BSxyz(i,1);
    if xr==xi
        xr=xi+0.1;%为防止移动台和基站重叠而做的处理,下同
    end
    yi=BSxyz(i,2);
    if yr==yi
        yr=yi+0.1;
    end
    zi=BSxyz(i,3);
    if zr==zi
        zr=zi+0.1;
    end
    Alpha(i,1)=atan((yr-yi)/(xr-xi))+normrnd(0,2*pi*SigmaAlpha/360,1,1);
    Beta(i,1)=atan((zr-zi)/(sqrt((xr-xi)^2+(yr-yi)^2)))+normrnd(0,2*pi*SigmaBeta/360,1,1);
    Rho(i,1)=sqrt((xr-xi)^2+(yr-yi)^2+(zr-zi)^2)+normrnd(0,SigmaRho,1,1);
end
%% 设置迭代初始值和停止条件
XR=[xr;yr;zr];
X0=XR+500*(rand(size(XR))-0.5);
Delta=0.001;%收敛精度
K=50;%最大迭代次数
%% 调用定位算法
tic
[X,AllX,Alldxyz]=Taylor_TOA_AOA_3D(X0,Alpha,Beta,Rho,BSxyz,Delta,K);
toc
%% 绘图
%收敛曲线
figure
plot(Alldxyz,'.-');
xlabel('迭代次数','FontName','Times New Roman','FontSize',10);
ylabel('误差(均方根误差)','FontName','Times New Roman','FontSize',10);
title('收敛曲线','FontName','Times New Roman','FontSize',10);
%%
%收敛轨迹
figure
plot3(BSxyz(:,1),BSxyz(:,2),BSxyz(:,3),'pk','MarkerEdgeColor','k','MarkerFaceColor','r','MarkerSize',12);
hold on
text(BSxyz(:,1),BSxyz(:,2),BSxyz(:,3),'Base Station','FontName','Times New Roman','FontSize',8,'BackgroundColor',[1 1 .6]);
plot3(BSxyz(:,1),BSxyz(:,2),BSxyz(:,3),'pk','MarkerEdgeColor','k','MarkerFaceColor','r','MarkerSize',12);
plot3([xr,xr],[yr,yr],[zr,zr],'^k','MarkerEdgeColor','k','MarkerFaceColor','r','MarkerSize',10);
text(xr,yr,zr,'Move Station','FontName','Times New Roman','FontSize',8,'BackgroundColor',[1 1 .6]);
plot3([xr,xr],[yr,yr],[zr,zr],'^k','MarkerEdgeColor','k','MarkerFaceColor','r','MarkerSize',10);
RK=size(AllX,1);
plot3(AllX(:,1),AllX(:,2),AllX(:,3),'ko-','MarkerFaceColor','r','MarkerSize',5);
xlabel('X轴','FontName','Times New Roman','FontSize',10);
ylabel('Y轴','FontName','Times New Roman','FontSize',10);
zlabel('Z轴','FontName','Times New Roman','FontSize',10);
title('收敛轨迹','FontName','Times New Roman','FontSize',10);
grid on

0

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

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

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

新浪公司 版权所有