上篇博文： 【 Notes 】NLS ALGORITHMS of TOA - Based Positioning说到了三种局部迭代算法进行TOA定位，分别为：
- Newton – Raphson
- Gauss – Newton
- steepest descent method
这篇博文，针对 Newton – Raphson 这种迭代方法进行MATLAB编程（给出部分程序），对TOA定位进行仿真，仿真内容包括：
- 一次定位示意图
- 牛顿—拉夫森 迭代方法收敛性讨论
- 定位误差与信噪比之间的关系
Illustration of NLS Approach for TOA - Based Positioning in a Single Trial
Consider a 2 - D geometry of L = 4 receivers with known coordinates at (0, 0), (0,10), (10, 0), and (10, 10), while the unknown source position is ( x , y ) = (2, 3).
Note that the source is located inside the square bounded by the four receivers. For presentation simplicity, the range error variance，，is assigned proportional to .
and we define the signal - to - noise ratio ( SNR ) as .
- 如下图示，菱形表示基站位置，空心圆表示目标位置，实心圆表示估计出来的位置，发现二者几乎重合。说明可以进行定位，但有一定的误差。
2. Examine the convergence rates of the Newton – Raphson for the NLS approach in a single trial at SNR = 30 dB.
从上图看出收敛情况，该方法很快就可以收敛。貌似可以设置迭代次数为10，甚至更小，但话虽如此，毕竟这只是一次实验的收敛情况，所以迭代次数还是稍微大点为妙。取30保险。
对应的代码部分为：
clc
clear
close all
% == generating range measurements == %
%We use x1, x2, x3 and x4 to assign the
%receiver positions and X is a 4x1 matrix to store
%their coordinates while the source position to be determined is
%x. For 2D positioning, the dimension is L=2. The noise-free
%range vector is d and its noisy version is r. The noise
%component in r is a zero-mean white Gaussian vector with
%variances given by sigma2. Moreover, both SNR in dB and number
%of iterations in the local search schemes, denoted by dB and
%iter, respectively, are set to 30.x1 = [0,0];
x2 = [0,10];
x3 = [10,0];
x4 = [10,10];
X = [x1;x2;x3;x4]'; % matrix for receiver positions
x = [2,3]'; % source position to be determined
L = size(X,2); % number of receivers
d = (sqrt(sum((x*ones(1,L)-X).^2,1))).'; %noise-free ranges
dB = 30; % SNR (in dB) which is defined as the mean of squared distance over noise variance
sigma2 = d.^2/10^(dB/10); % sigma2--square of sigma, here we use: SNR_dB = 10log(d^2/sigma^2)
r = d + randn(L,1).*sqrt(sigma2);
iter = 30;x1 = x; % copy x for plot% == position estimation == %
% == NLS Newton-Raphson algorithm == %
x_nr = zeros(iter,2);x=[3,2]'; %initial guess value
for i=1:iterH=hessian_nls(X,x,r);g=grad_nls(X,x,r);x=x-inv(H)*g;x_nr(i,:)=x;
end%convegence analysis
iter_no = 1:iter;
figure(1)
plot(iter_no,x_nr(:,1));
xlabel('iter times');ylabel('x');
figure(3)
plot(iter_no,x_nr(:,2));
xlabel('iter times');ylabel('y');下面分析定位误差，也就是均方根误差，实验次数为100次，观测信噪比从-5dB 到30dB时的均方根误差rmse。
如果，所有的单位为km，则信噪比为30时的定位误差是282m，还行；信噪比为20时候的定位误差888m，看要求的标准吧。
用到的函数：
function g = grad_nls(X,x,r)
% NLS gradient computation
% --------------------------------
% g = grad_nls(X,x,r);
% g = gradient vector
% X = matrix for receiver positions
% x = 2D position estimate
% r = TOA measurement vector
%
L = size(X,2); % number of receivers
t1 = 0;
t2 = 0;
ds = sum((x*ones(1,L)-X).^2,1);
ds = ds';
for i=1:Lt1 = t1 + (r(i)-ds(i)^(0.5))*(x(1)-X(1,i))/ds(i)^(0.5);t2 = t2 + (r(i)-ds(i)^(0.5))*(x(2)-X(2,i))/ds(i)^(0.5);
end
g=-2.*[t1; t2];function H = hessian_nls(X,x,r)
% NLS Hessian matrix computation
% --------------------------------
% H = hessian_nls(X,x,r);
% H = Hessian matrix
% X = matrix for receiver positions
% x = 2D position estimate
% r = TOA measurement vector
%
L = size(X,2); % number of receivers
t1 = 0;
t2 = 0;
t3 =0;
ds = sum((x*ones(1,L)-X).^2,1);
ds = ds';
for i=1:Lt1 = t1 + (x(1)-X(1,i))^2/ds(i)-(r(i)-ds(i)^(0.5))*(x(2)-X(2,i))^2/ds(i)^(1.5);t2 = t2 + (x(2)-X(2,i))^2/ds(i)-(r(i)-ds(i)^(0.5))*(x(1)-X(1,i))^2/ds(i)^(1.5);t3 = t3 + r(i)*(x(1)-X(1,i))*(x(2)-X(2,i))/ds(i)^(1.5);
end
H=2.*[t1 t3;t3 t2];