%% 设置参数
N = 4; % 锚数量
M = 5; % 移动节点数
% 距离相关err(归一化到距离的噪声标准差)
distMeasurementErrRatio = 0.1; % 这意味着距离测量的精度是90,例如1米的测量距离的误差大约是0.1米。
networkSize = 100; % 我们考虑移动设备可以漫游的100x100的区域
anchorLoc = [0 0; % 在区域的4个顶点处设置锚点
networkSize 0;
0 networkSize;
networkSize networkSize];
% 为移动节点构建随机位置
mobileLoc = networkSize*rand(M,2);
% 计算欧氏距离
% 非常快的计算方案 :)
% distance = sqrt(sum((anchorLoc - repmat(mobileLoc,N,1)).^2 , 2));
% 易于理解的计算方案
distance = zeros(N,M);
for m = 1 : M
for n = 1 : N
distance(n,m) = sqrt((anchorLoc(n,1)-mobileLoc(m,1)).^2 + ...
(anchorLoc(n,2)-mobileLoc(m,2)).^2 );
end
end
% 可视化界面
f1 = figure(1);
clf
plot(anchorLoc(:,1),anchorLoc(:,2),'ko','MarkerSize',8,'lineWidth',2,'MarkerFaceColor','k');
grid on
hold on
plot(mobileLoc(:,1),mobileLoc(:,2),'b+','MarkerSize',8,'lineWidth',2);
% 噪声测量
distanceNoisy = distance + distance.*distMeasurementErrRatio.*(rand(N,M)-1/2);
% 用牛顿法求解
% (http://en.wikipedia.org/wiki/Gauss%E2%80%93Newton_algorithm)
numOfIteration = 5;
% 初始猜测(随机位置)
mobileLocEst = networkSize*rand(M,2);
% 重复迭代
for m = 1 : M
for i = 1 : numOfIteration
% 计算模拟距离
distanceEst = sqrt(sum((anchorLoc - repmat(mobileLocEst(m,:),N,1)).^2 , 2));
% 计算衍生品
% d0 = sqrt((x-x0)^2 + (y-y0)^2 )
% derivatives -> d(d0)/dx = (x-x0)/d0
% derivatives -> d(d0)/dy = (y-y0)/d0
distanceDrv = [(mobileLocEst(m,1)-anchorLoc(:,1))./distanceEst ... % x-coordinate
(mobileLocEst(m,2)-anchorLoc(:,2))./distanceEst]; % y-coordinate
% delta值
delta = - (distanceDrv.'*distanceDrv)^-1*distanceDrv.' * (distanceEst - distanceNoisy(:,m));
% 更新估计值
mobileLocEst(m,:) = mobileLocEst(m,:) + delta.';
end
end
plot(mobileLocEst(:,1),mobileLocEst(:,2),'ro','MarkerSize',8,'lineWidth',2);
legend('锚的位置','移动的真实位置','移动位置估计',...
'位置','最好')
% 计算均方根误差
Err = mean(sqrt(sum((mobileLocEst-mobileLoc).^2)));
title(['平均估计误差为',num2str(Err),'米'])
axis([-0.1 1.1 -0.1 1.1]*networkSize)