室内定位算法（牛顿迭代）matlab

%% 设置参数

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)