11.10. High-level interface: Indoor localization (MATLAB & Python)¶

The indoor localization problem is to estimate the position of a target by measurements from various anchors with known location. Outdoors, this well known as GPS, while indoors other frequency bands (and less accurate clocks) are usually used. In this example, we show how to generate code for a position estimator that relies on time-of-flight (TOF) measurements (GPS uses time-difference-of-arrival, TDOA). The latter can be easily implemented with FORCESPRO as well with only minor changes to the code below.

Figure 11.44 Indoor localization example GUI.

You can download the Matlab code of this example to try it out for yourself by clicking here (MATLAB) or here (Python).

Running the code will produce an interactive window like in Figure 11.44.

11.10.1. Time of flight measurements¶

Given $$N$$ anchors with known positions $$(x_i^a,y_i^a)$$, $$i=1,\dots,N$$, the distance to the target with unknown position $$(x,y)$$ is given by:

$d_i = ct_i = \sqrt{(x-x_i^a)^2 + (y-y_i^a)^2}$

where $$t_i$$ is the time the signal from anchor $$i$$ travels at the speed $$c=299\,792\,458\,\mathrm{m/s}$$

11.10.2. Estimation error¶

Instead of the real distance, we work with squared distances to define the estimation error:

$e_i = (x-x_i^a)^2 + (y-y_i^a)^2 - d_i^2$

11.10.3. Minimize the error¶

The objective is a least-squares error function:

$\min_{x,y} \sum_{i=1}^N e_i^2$

11.10.4. Implementation¶

The following Matlab/Python code generates C-code for implementing an optimizer for minimizing the least-squares error function from above. It takes the anchor positions and the distance measurements, and returns the estimated position of the target.

%% This function generates the estimator
function generateEstimator(numberOfAnchors,xlimits,ylimits)
% Generates 2D decoding code for localization using FORCES NLP
% na: number of anchors
global na
na = numberOfAnchors;

%% NLP problem definition
% no need to change anything below
model.N = 1;      % number of distance measurements
model.nvar = 2;   % number of variables (use 3 if 3D)
model.npar = numberOfAnchors*3; % number of parameters: coordinates of anchors in 2D, plus measurements
model.objective = @objective;
model.lb = [xlimits(1) ylimits(1)]; % lower bounds on (x,y)
model.ub = [xlimits(2) ylimits(2)]; % upper bounds on (x,y)

%% codesettings
codesettings = getOptions('localizationDecoder');
codesettings.printlevel = 0; % set to 2 to see some prints
% codesettings.server = 'http://winner10:2470';
codesettings.maxit = 50; % maximum number of iterations

%% generate code
FORCES_NLP(model, codesettings);
end

%% This function implements the objective
% We assume that the parameter vector p is ordered as follows:
% p(1:na)        - x-coordinates of the anchors
% p(na+(1:na))   - y-coordinates of the anchors
% p(2*na+(1:na)) - distance measurements of the anchors
function obj = objective( z,p )
global na
obj=0;
for i = 1:na
obj = obj + ( (p(i)-z(1))^2 + (p(i+na)-z(2))^2 - p(i+2*na)^2 )^2;
end
end

def generate_estimator(number_of_anchors, xlimits, ylimits):
"""
Generates and returns a FORCESPRO solver that esimates a position based on
noisy measurement inputs.
"""

# NLP problem definition
# ----------------------

model = forcespro.nlp.SymbolicModel(1)  # number of distance measurements
model.nvar = 2  # number of variables (use 3 if 3D)
model.npar = number_of_anchors * 3  # number of parameters: coordinates of anchors in 2D, plus measurements
model.objective = objective  # objective is defined as it's own function below
model.lb = np.array([xlimits[0], ylimits[0]])  # lower bounds on (x,y)
model.ub = np.array([xlimits[1], ylimits[1]])  # upper bounds on (x,y)

# FORCESPRO solver settings
# --------------------------

codesettings = forcespro.CodeOptions()
codesettings.printlevel = 0  # set to 2 to see some prints
codesettings.maxit = 50  # maximum number of iterations

# Generate a solver
# -----------------
solver = model.generate_solver(codesettings)

return solver

def objective(z, p):
"""
This function implements the objective to be minimized.

We assume that the parameter vector p is ordered as follows:

- p[0:(na-1)]        - x-coordinates of the anchors
- p[na:(2*na-1)]     - y-coordinates of the anchors
- p[(2*na):(3*na-1)] - distance measurements of the anchors
"""
obj = 0
for i in range(n):
obj += ((p[i] - z[0])**2 + (p[i + n] - z[1])**2 - p[i + 2*n]**2)**2
return obj

def distance(xa, xtrue, ya, ytrue):
return np.sqrt((xa - xtrue)**2 + (ya - ytrue)**2)