【原创】TDOA定位的泰勒级数展开最小二乘定位算法Matlab源码

[greensim]

定位估计问题通常归结为非线性参数估计，泰勒级数展开最小二乘法是解决非线性估计的通用方法。本源码由GreenSim团队原创，转载请注明，有意购买源码或代写相关程序，请与GreenSim团队联系。

function [x,y]=Taylor_TDOA(x0,y0,Lambda,BSxy,Delta)
%% TDOA定位泰勒级数展开最小二乘定位算法
% GreenSim团队原创作品，转载请注明
% 欢迎访问GreenSim——算法仿真团队→http://blog.sina.com.cn/greensim
%% 输入参数列表
% (x0,y0) 迭代初始值（米）
% Lambda TDOA观测值（米）
% BSxy 基站坐标（米），N×2矩阵
% Delta 迭代停止的门限（米）
%% 输出参数列表
% (x,y) 定位结果

%%
dxy=Inf;
N=size(BSxy,1);%基站个数
H=zeros(N-1,2);%H矩阵初始化
x=x0;
y=y0;
while dxy>Delta
%构造H矩阵
d=zeros(N,1);%d向量初始化，移动台到各基站的距离
for i=1:N
d(i)=sqrt((x-BSxy(i,1))^2+(y-BSxy(i,2))^2);
if d(i)<0.01%避免分母为零
d(i)=0.01;
end
end
for i=1:(N-1)
H(i,1)=(x-BSxy(i+1,1))/d(i)-(x-BSxy(1,1))/d(1);
end
%构造Y矩阵
Y=Lambda-(d(2:end,1)-d(1));
%使用线性最小二乘求解Y=H*X
dxy=sqrt(sum(X.^2));
x=x+X(1);
disp([x,y]);
end