各种体制的无线定位问题问题，通常都可以归结为非线性参数估计，泰勒级数展开最小二乘法则是解决非线性参数估计的一种常用方法。

http://s9/middle/4b425443xc8a2d6e48c18&690
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