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.

- Introduction
- Part 1. Geometrical Interpretation
- Part 2. Mathematical Interpretation
- Part 3. Optimisation Algorithm
- Part 4. Additional Considerations
- Conclusion

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.

1 2 3 4 5 6 7 8 9 |
# 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.

1 2 3 4 5 6 7 8 9 10 11 12 13 |
# 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?

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.

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.

#### Additional Considerations

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.

1 2 3 4 5 6 7 8 9 |
# 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?#### Conclusion

This post concludes the series on localisation and trilateration.

- Part 1. Understanding Geographical Coordinates
**Part 2. Positioning 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.

##### 📧 Stay updated

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. ❤️🧔🏻

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

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

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!

Hi Jon,

Could you share the matlab code, please

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

Thank you so much! <3

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

f, g = func_and_grad(x)

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

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

f, g = func_and_grad(x)

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