TDOA定位Taylor算法
共 1817字,需浏览 4分钟
·
2022-05-27 16:54
来源:https://blog.csdn.net/gongshouxiayin/article/details/111055294
作者:Rika7777
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 ‧‧‧‧‧‧‧‧‧‧‧‧‧‧‧
关注我的微信公众号,回复“加群”按规则加入技术交流群。
点击下面图片,有星球具体介绍,新用户有新人优惠券,老用户半价优惠,期待大家一起学习一起进步。
点击“阅读原文”查看更多分享,欢迎点分享、收藏、点赞、在看。