# Position Estimation using 2D multilateration for non-intersecting distances

I am trying to estimate the position of a point $P$ in Matlab. I have $n$ access points (AP) at known positions ($n>2$) as well as the distances to the point $P$ from each AP. These distances all have a distance error. I know how to solve this using multilateration if the circles from the known nodes would intersect, but they don't.

The distances are computed from RSSI measurements (received signal strength indication) and might be weighted (larger weight if RSSI is high).

See code below. Note that I'm not weighting the RSSI measurements. I'm creating an error function, where $xp$ and $yp$ are the point's unknown coordinates. $x$ and $y$ are known coordinates of the APs. I want to sum all the errors and find the $xp$ and $yp$ that minimize the error function, but not sure how to do this in Matlab.

    for n=1:N           % N - number of APs
err=@(px,py) sqrt((x(n)-px).^2+(y(n)-py).^2)-d(n);
end
err=sum(err);    % this doesn't work obviously
x0 = [xp_true(k),yp_true(k)];
out= fminunc(err,x0);
xp(k,sample)=out(1);
yp(k,sample)=out(2);


The problem you describe is an example of a nonlinear least squares problem. You can find documentation for the MATLAB function to solve such a problem along with several references describing the mathematics and numerical methods for solution here: lsqnonlin

The idea behind least squares problems is that you have more data points (access point locations and distances in your case) than unknowns (x and y locations of the unknown point in your case).

The simple MATLAB code to solve this problem is

  % function to calculate the error in distance between point and access point
% x, y, and d are known vectors of length > 2
fun = @(xyp) distResidual(xyp, x, y, d);
xy0 = [0 0]; % initial guess at the point's x and y location
xy = lsqnonlin(fun, xy0)


where x, y and d are the known x, y, and distance vectors to the point for the set of access points. The definition of the distResidual function is

function F=distResidual(xy, x, y, d)
F = (x-xy(1)).^2 + (y-xy(2)).^2 - d.^2;
end

• You could also supply the gradient and hessian, because they can be calculated relatively easily. Also you can use a weighted average of the points, with the inverse of the distances as weights as an other and possibly better initial guess. I also wonder if the problem is convex when the points are spaced out sufficiently and the errors are not too big. Apr 27 '17 at 15:01