TDOA/AOA/AOD混合定位的非线性比较强，这里给出其最小二乘的泰勒级数展开最小二乘算法。本源码由GreenSim团队原创，转载请注明，有意购买源码或代写相关程序，请与GreenSim团队联系。
function [X,AllX,Alldxy]=Taylor_TDOA_AOA_AOD(X0,Theta,Phi,Tau,xb,yb,Delta,K)
%% TDOA/AOA/AOD定位的泰勒级数展开最小二乘算法
% 参考文献
% Ji Li, Ligen Wang, Jean-Jules Brault, Jean Conan
% Mobile Location in MIMO Communication Systems by Using Learning Machine
% 要求多径（散射体）个数：N>=3
% GreenSim团队原创作品，转载请注明
%
欢迎访问GreenSim——算法仿真团队→http://blog.sina.com.cn/greensim
%% 输入参数列表
% X0 迭代初始值（米）,(2N+2)×1列向量，排列规则为[x1,y1,x2,y2,…,xN,yN,xm,ym]
% Theta 移动台到达角观测值AOA（弧度），N×1列向量
% Phi 基站离开角观测值AOD（弧度）,N×1列向量
% Tau 传播时延差的观测值TDOA，折算成距离（米），(N-1)×1列向量
% (xb,yb) 基站坐标（米）
% Delta 迭代停止的门限（米）
% K 最大迭代次数
%% 输出参数列表
% X 输出结果，含散射体和移动台的定位结果
%%
N=length(Theta);%多径（散射体）个数
H=zeros(3*N-1,2*N+2);%H矩阵初始化
Rho=zeros(3*N-1,1);%计算观测值初始化
AllX=zeros(K,2*N+2);
Alldxy=zeros(K,1);
dxy=Inf;%收敛误差初始化
X=X0;%迭代初始点
counter=1;%迭代计数器
while dxy>Delta
xm=X(2*N+1);
ym=X(2*N+2);
x1=X(1);
y1=X(2);
for i=1:N
xi=X(2*i-1);
yi=X(2*i);
%以下是防止分母为零而采取的措施
if xi==xm
xi=xm+0.000001;
end
if yi==ym
yi=ym+0.000001;
end
if xi==xb
xi=xb+0.000001;
end
if yi==yb
yi=yb+0.000001;
end
%以下是H矩阵和Rho向量的前N行
H(i,2*i-1)=-(yi-ym)/(xi-xm)^2/(1+(yi-ym)^2/(xi-xm)^2);
H(i,2*i)=1/(xi-xm)/(1+(yi-ym)^2/(xi-xm)^2);
H(i,2*N+1)=(yi-ym)/(xi-xm)^2/(1+(yi-ym)^2/(xi-xm)^2);
H(i,2*N+2)=-1/(xi-xm)/(1+(yi-ym)^2/(xi-xm)^2);
Rho(i,1)=atan((yi-ym)/(xi-xm));
%以下是H矩阵的中间N行
H(N+i,2*i-1)=-(yi-yb)/(xi-xb)^2/(1+(yi-yb)^2/(xi-xb)^2);
H(N+i,2*i)=1/(xi-xb)/(1+(yi-yb)^2/(xi-xb)^2);
Rho(N+i,1)=atan((yi-yb)/(xi-xb));
%以下是H矩阵的后N-1行
if i>=2
H(2*N+i-1,1)=-1/2/(x1^2-2*x1*xm+xm^2+y1^2-2*y1*ym+ym^2)^(1/2)*(2*x1-2*xm)-1/2/(x1^2-2*x1*xb+xb^2+y1^2-2*y1*yb+yb^2)^(1/2)*(2*x1-2*xb);
H(2*N+i-1,2)=-1/2/(x1^2-2*x1*xm+xm^2+y1^2-2*y1*ym+ym^2)^(1/2)*(2*y1-2*ym)-1/2/(x1^2-2*x1*xb+xb^2+y1^2-2*y1*yb+yb^2)^(1/2)*(2*y1-2*yb);
H(2*N+i-1,2*i-1)=1/2/(xi^2-2*xi*xm+xm^2+yi^2-2*yi*ym+ym^2)^(1/2)*(2*xi-2*xm)+1/2/(xi^2-2*xi*xb+xb^2+yi^2-2*yi*yb+yb^2)^(1/2)*(2*xi-2*xb);
H(2*N+i-1,2*i)=1/2/(xi^2-2*xi*xm+xm^2+yi^2-2*yi*ym+ym^2)^(1/2)*(2*yi-2*ym)+1/2/(xi^2-2*xi*xb+xb^2+yi^2-2*yi*yb+yb^2)^(1/2)*(2*yi-2*yb);
H(2*N+i-1,2*N+1)=1/2/(xi^2-2*xi*xm+xm^2+yi^2-2*yi*ym+ym^2)^(1/2)*(-2*xi+2*xm)-1/2/(x1^2-2*x1*xm+xm^2+y1^2-2*y1*ym+ym^2)^(1/2)*(-2*x1+2*xm);
H(2*N+i-1,2*N+2)=1/2/(xi^2-2*xi*xm+xm^2+yi^2-2*yi*ym+ym^2)^(1/2)*(-2*yi+2*ym)-1/2/(x1^2-2*x1*xm+xm^2+y1^2-2*y1*ym+ym^2)^(1/2)*(-2*y1+2*ym);
Rho(2*N+i-1,1)=sqrt((xi-xm)^2+(yi-ym)^2)+sqrt((xi-xb)^2+(yi-yb)^2)-sqrt((x1-xm)^2+(y1-ym)^2)-sqrt((x1-xb)^2+(y1-yb)^2);
end
end
%构造Y向量
Y=[Theta;Phi;Tau]-Rho;
%收敛误差
dxy=sqrt(sum(DX.^2));
%更新X
X=X+DX;
AllX(counter,:)=X';
Alldxy(counter,1)=dxy;
%显示(xm,ym)
%disp(counter);
%disp(X(2*N+1:2*N+2));
counter=counter+1;
if counter>K
return
end
end
AllX=AllX(1:counter-1,:);
Alldxy=Alldxy(1:counter-1,1);