# Positioning and Trilateration

This post shows how it is possible to find the position of an object in space, using a technique called trilateration. The traditional approach to this problem relies on three measurements only. This tutorial addresses how to it is possible to take into account more measurements to improve the precision of the final result. This algorithm is robust and can work even with inaccurate measurements.

If you are unfamiliar with the concepts of latitude and longitude, I suggest you read the first post in this series: Understanding Geographical Coordinates.

#### Introduction

Finding the position in space of an object is hard. What makes such a simple request so complex, is the fact that no traditional sensor is able to locate an object. What most sensors do, usually, is estimating the distance from the target object. Sonars and radars send out sound waves and calculate the distance by detecting how long it takes for their signal to bounce back. Satellites basically do the the same, bur using radio waves. Estimating the distance from an object is often relatively easy to do.

The solution to locate an object is to rely on several distance measurements, taken by different sensors (or by the same sensor at different times). Sensor fusion is a very active field of research, and allows to reconstruct complex information from very simple measurement. The rest of this post will introduce the concept of trilateration, which is one of the most common techniques used to calculate the position of an object given distance measurements.

#### Geometrical Interpretation

Let’s imagine that we want to find the location of an object. Our first try relies on a beacon, or station, sitting at a known position . Both and are expressed with latitude and longitude values. The station cannot locate directly, but can estimate its relative distance .

With only one station available, we cannot identify the exact position of . What we know, however, is how close it is. Each point that is at distance from is a potential candidate for . This means that with only one beacon, our guess of is limited to a circle of radius around (below, in red).

We can improve the situation by using not one but two beacons, and (below). Our object  can only be along the circumference of the red circle. But for the same reason, it can only be along the circumference of the green circle. This means that it has to be on the intersections of the two circles. This suddenly restricts our guess to only two possible locations (in grey).

If we want to be absolutely precise, we need a third station, . The three circles will meet in only one point, which is indeed the correct location of .

In order to do that, we have a set of  beacons or stations, each one at a known locations . Each beacon can sense its distance from , which is called .

#### Mathematical Interpretation

A point on the Cartesian plane lies on a circle of radius centred at if and only if is a solution to this equation:

With the same reasoning, we can derive equations for the circles generated by the beacons. Each one has its own position, expressed with latitude and longitudes coordinates, , and , respectively.

The problem of trilateration is solved mathematically by finding the point that simultaneously satisfies the equations of these three circles.

There are several ways to solve these equations. If you are interested in the actual derivation, Wikipedia has a nice page explaining each step.

#### Optimisation Algorithm

While it is definitely true that trilateration can be seen (and solved) as a geometrical problem, this is often impractical. Relying on the mathematical modelling seen in the previous section requires us to have an incredibly high accuracy on our measurements. Worst case scenario: if the circles do not meet in a single point, the set of equations will have no solution. This leaves us with nothing. Even assuming we do have perfect precision, the mathematical approach does not scale nicely. What is we have not three, but four points? What if we have one hundred?

The problem of trilateration can be approached from an optimisation point of view. Ignoring circles and intersections, which is the point that provides us with the best approximation to the actual position ?

Given a point , we can estimate how well it replaces . We can do this simply by calculating its distance from each beacon . If those distances perfectly match with their respective distances , then is indeed . The more deviates from these distances, the further it is assumed from .

Under this new formulation, we can see trilateration as an optimisation problem. We need to find the pint that minimise a certain error function. For our , we have not one by three sources of error: one for each beacon:

A very common way to merge these different contributions is to average their squares. This takes away the for possibility of negative and positive errors to cancel each others out, as squares are always positive. The quantity obtained is known as mean squared error:

What is really nice about this solution is that it can be used to take into account an arbitrary number of points. The piece of code below calculates the mean square error of a point x, given a list of locations and their relative distances from the actual target.

# Mean Square Error
# locations: [ (lat1, long1), ... ]
# distances: [ distance1, ... ]
def mse(x, locations, distances):
mse = 0.0
for location, distance in zip(locations, distances):
distance_calculated = great_circle_distance(x[0], x[1], location[0], location[1])
mse += math.pow(distance_calculated - distance, 2.0)
return mse / len(distances)

What’s left now is to find the point x that minimises the mean square error. Luckily, scipy comes with several optimisation algorithms that we can use.

# initial_location: (lat, long)
# locations: [ (lat1, long1), ... ]
# distances: [ distance1,     ... ]
result = minimize(
mse,                         # The error function
initial_location,            # The initial guess
args=(locations, distances), # Additional parameters for mse
method='L-BFGS-B',           # The optimisation algorithm
options={
'ftol':1e-5,         # Tolerance
'maxiter': 1e+7      # Maximum iterations
})
location = result.x
❓ Can we use absolute values instead of squares?
Yes; using absolute values instead of squares results in a different metric called mean absolute error:

Both MSE and MAE considers both positive and negative errors. While MAE gives the same importance to each error, MSE weights larger errors more. If we are dealing with real distances, MSE is expressed in kilometres squared, while MAE would be simply in kilometres.

Mathematically speaking, working with the MSE is more convenient as squares can be derived easier compared to absolute values. In the field of Statistics, there are other properties that make MSE often a better choice compared to MAE.

For this particular application, both metrics could work just fine.

❓ How many beacons should I use?
As long as you have at least three readings from three different beacons, a good optimisation algorithms will eventually converge to the right point.
If your measure of distance is inaccurate, you can be tempted to add multiple readings to calculate the MSE. While this can be a good idea in general, it can also lead to nasty behaviours. Adding measurements from a beacon twice will make it twice as important for the optimisation algorithm.

What will definitely result in a mistake is to trying to fine using distance readings coming from a single beacon. When that is the case, all the circles are concentric and there are infinite many solutions. Your optimisation algorithm will indeed converge to a point, but it is unlikely to be the one you want.

While optimisations algorithms can solve many problems, it is unrealistic to expect them to perform well if we provide little to no additional information to them. One of the most important aspect is the initial guess. For a problem this straightforward, any relatively good optimisation algorithm should converge to a reasonable solution. However, providing a good starting point could reduce its execution time significantly.

There are several initial guesses that one could use. A sensible one is to use the centre of sensor which detected the closest distance.

	# Initial point: the point with the closest distance
min_distance     = float('inf')
closest_location = None
for member in data:
# A new closest point!
if member['distance'] < min_distance:
min_distance = member['distance']
closest_location = member['location']
initial_location = closest_location



Alternatively, one could average the locations of the sensors. If they are arranged in a grid fashion, this could work well. In the end, you should test which strategy works best for you.

❓ My optimisation algorithm is not converging to an optimal. Why?
Depending on how your circles are arranged, there could be multiple favourable solutions available.

#### Conclusion

This post concludes the series on localisation and trilateration.

##### 💖 Support this blog

This websites exists thanks to the contribution of patrons on Patreon. If you think these posts have either helped or inspired you, please consider supporting this blog.

You will be notified when a new tutorial is relesed!

##### 📝 Licensing

You are free to use, adapt and build upon this tutorial for your own projects (even commercially) as long as you credit me.

You are not allowed to redistribute the content of this tutorial on other platforms. Especially the parts that are only available on Patreon.

If the knowledge you have gained had a significant impact on your project, a mention in the credit would be very appreciated. ❤️🧔🏻

1. mau

Hi. very nice article. unfortunately trying to test it several problems related to data & py config raised. a run sample could be useful. thks.

2. Ab

Why not computing a first derivative of mean squared error, resolving for mSE = 0 and then looking for minima points? (Excluding maxima )

3. Jon

Hi,

Nice walkthrough, thanks. There are quite a few typos and inconsistencies though, for example, in the equations for the errors e1, e2, e3 you switch the subscript notation for d (di, then d2, d3…)

FWIW I implemented this in Matlab and used the fminsearch function to find the (x,y) position that minimised the MSE.

• Thank you for the corrections!
If you have more please let me know and I’ll fix them as soon as possible.

I am glad that the post helped you.
I’d be very curious to see what you have been using it for!

• Catalin Duta

Hi Jon,
Could you share the matlab code, please

4. Such a great article, I wish all of the paper i read is like this comprehensive and intuitive.

• Thank you so much! <3

5. Rudy Deschepper

Is there an import or install needed to activate great_circle_distance in:
distance_calculated = great_circle_distance(x[0], x[1], location[0], location[1])
On my Raspberry pi I get a lot of errors:
Traceback (most recent call last):
File “optimisation_algorithm.py”, line 46, in
‘maxiter’: 1e+7 # Maximum iterations
File “/usr/lib/python3/dist-packages/scipy/optimize/_minimize.py”, line 603, in minimize
callback=callback, **options)
File “/usr/lib/python3/dist-packages/scipy/optimize/lbfgsb.py”, line 335, in _minimize_lbfgsb
File “/usr/lib/python3/dist-packages/scipy/optimize/lbfgsb.py”, line 280, in func_and_grad
f = fun(x, *args)
File “/usr/lib/python3/dist-packages/scipy/optimize/optimize.py”, line 293, in function_wrapper
return function(*(wrapper_args + args))
File “optimisation_algorithm.py”, line 20, in mse
distance_calculated = great_circle_distance(x[0], x[1], location[0], location[1])
NameError: name ‘great_circle_distance’ is not defined

• I defined the great_circle_distance function as the Euclidean distance:

def euclidean_distance(x1, y1, x2, y2):
p1 = np.array((x1 ,y1))
p2 = np.array((x2, y2))
return np.linalg.norm(p1 – p2)

# Mean Square Error
# locations: [ (lat1, long1), … ]
# distances: [ distance1, … ]
def mse(x, locations, distances):
mse = 0.0
for location, distance in zip(locations, distances):
distance_calculated = euclidean_distance(x[0], x[1], location[0], location[1])
mse += math.pow(distance_calculated – distance, 2.0)
return mse / len(distances)

6. Rudy Deschepper

In the mean time “great_circle_distance” algorithm is find.
How do I define “data” in “return mse / len(data)” because NameError: name ‘data’ is not defined ?
see errors on Raspberry pi :
Traceback (most recent call last):
File “optimisation_algorithm.py”, line 87, in
‘maxiter’: 1e+7 # Maximum iterations
File “/usr/lib/python3/dist-packages/scipy/optimize/_minimize.py”, line 603, in minimize
callback=callback, **options)
File “/usr/lib/python3/dist-packages/scipy/optimize/lbfgsb.py”, line 335, in _minimize_lbfgsb
File “/usr/lib/python3/dist-packages/scipy/optimize/lbfgsb.py”, line 280, in func_and_grad
f = fun(x, *args)
File “/usr/lib/python3/dist-packages/scipy/optimize/optimize.py”, line 293, in function_wrapper
return function(*(wrapper_args + args))
File “optimisation_algorithm.py”, line 22, in mse
return mse / len(data)
NameError: name ‘data’ is not defined

7. Hi i am interested in your python code for trilateration.Where could we get a copy of the script so we can use for our trilateration please?
Thanks

### Webmentions

• Tutorial Series - Alan Zucconi March 7, 2021

[…] Part 2. Localisation and Trilateration […]

• Understanding Geographical Coordinates - Alan Zucconi March 7, 2021

[…] coordinates, and how they can be effectively manipulated. The second post in the series, Localisation and Trilateration, will cover the actual techniques used to identify the position of an object given independent […]