TDOA定位Taylor算法

李肖遥

共 1817字,需浏览 4分钟

 ·

2022-05-27 16:54

    关注、星标公众号,直达精彩内容

来源:https://blog.csdn.net/gongshouxiayin/article/details/111055294

作者:Rika7777


基于TDOA的经典定位算法分为两类,一类是可以求出解析解的算法,如Fang算法、Chan算法的;另一类是迭代算法,如Taylor算法。

Taylor级数展开法是一种迭代算法,在Taylor级数展开的基础上,利用初始迭代值进行WLS估计,然后求解位置估计误差的局部最小二乘解,并对标签的位置进行更新。Taylor算法的前提是需要标签位置的初始估计值,而算法的主要思想是通过不断迭代来修正待定位标签位置的估计值,最后逐渐逼近标签真实的位置坐标。

References
[1] 宋洋. 超宽带室内定位技术研究[D].西安科技大学,2019.

代码实现(matlab)

X_estimate = 50;
Y_estimate = 50;
Xb = [200 200];
X = [200 0; -100 173; -100 -173];
Noise = 0.1*randn(3,1);
Real_ms = [20 20];

BSN = size(X,1);      
x = X_estimate;
y = Y_estimate;

MS = [x,y];  
iEP = MS;

%%
Rb = sqrt((Real_ms(1) - Xb(1))^2+(Real_ms(2) - Xb(2))^2);
RD = zeros(BSN,1);
for i = 1:BSN                
   RD(i) = -Rb+sqrt((Real_ms(1)- X(i,1))^2+(Real_ms(2) - X(i,2))^2)+Noise(i);
end
%%
% TDOA协方差矩阵Q
Q = 0.01*eye(BSN);  
delta = [1 1];  
kk = 0;  
% Taylor级数展开法
while ((abs(delta(1)) + abs(delta(2))) > 0.01)  
   R1 = sqrt((iEP(1) - Xb(1))^2 + (iEP(2) - Xb(2))^2);  
   R = zeros(1,BSN);
   kk = kk+1;
   for i = 1: BSN
       R(i) = sqrt((iEP(1) - X(i,1))^2 + (iEP(2) - X(i,2))^2);
   end
   
   % hi
   hi = zeros(BSN,1);
   for i = 1: BSN
       hi(i) = RD(i) - (R(i) - R1);
   end
   
   % Gi
   Gi = zeros(BSN,2);
   for i = 1: BSN
       Gi(i, 1) = (Xb(1)-iEP(1))/R1 - (X(i,1) - iEP(1))/R(i);
       Gi(i, 2) = (Xb(2)-iEP(2))/R1 - (X(i,2) - iEP(2))/R(i);
   end
   
   % delta
   delta = inv(Gi'*inv(Q)*Gi)*Gi'*inv(Q)*hi;  
   if (abs(delta(1))+abs(delta(2))) > 0.01  
       EP = iEP + delta';  
       iEP = EP;   % 更新迭代值
   end
end

% 输出
z_out = iEP;   % 标签坐标估计值
mse = sqrt((z_out(1)-Real_ms(1))^2+(z_out(2)-Real_ms(2))^2);   % 均方误差

免责声明:本文素材来源网络,版权归原作者所有。如涉及作品版权问题,请与我联系删除。

‧‧‧‧‧‧‧‧‧‧‧‧‧‧‧‧  END  ‧‧‧‧‧‧‧‧‧‧‧‧‧‧‧

关注我的微信公众号,回复“加群”按规则加入技术交流群。

点击下面图片,有星球具体介绍,新用户有新人优惠券,老用户半价优惠,期待大家一起学习一起进步。


点击“阅读原文”查看更多分享,欢迎点分享、收藏、点赞、在看。

浏览 15
点赞
评论
收藏
分享

手机扫一扫分享

分享
举报
评论
图片
表情
推荐
点赞
评论
收藏
分享

手机扫一扫分享

分享
举报