in Maths, Python, Tutorial

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.


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 P of an object. Our first try relies on a beacon, or station, sitting at a known position L. Both P and L are expressed with latitude and longitude values. The station cannot locate P directly, but can estimate its relative distance d.

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

We can improve the situation by using not one but two beacons, L_1 and L_2 (below). Our object P 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, L_3. The three circles will meet in only one point, which is indeed the correct location of P.

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

Mathematical Interpretation

A point \left(x,y\right) on the Cartesian plane lies on a circle of radius r centred at \left(c_x, c_y\right) if and only if is a solution to this equation:

    \[{\left(x - c_x\right)}^2 + {\left(y- c_y\right)}^2={d_1}^2\]

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, \left(\phi_1, \lambda_1 \right), \left(\phi_2, \lambda_2 \right) and \left(\phi_3, \lambda_3 \right), respectively.

The problem of trilateration is solved mathematically by finding the point P=\left(\phi, \lambda\right) that simultaneously satisfies the equations of these three circles.

    \[{\left(\phi - \phi_1\right)}^2 + {\left(\lambda- \lambda_1\right)}^2={d_1}^2\]

    \[{\left(\phi - \phi_2\right)}^2 + {\left(\lambda- \lambda_2\right)}^2={d_2}^2\]

    \[{\left(\phi - \phi_3\right)}^2 + {\left(\lambda- \lambda_3\right)}^2={d_3}^2\]

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 X=\left(\phi_x, \lambda_x\right) that provides us with the best approximation to the actual position P?

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

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

    \[e_1 = d_1 - dist\left(X, L_1\right)\]

    \[e_2= d_2 - dist\left(X, L_2\right)\]

    \[e_3 = d_3 - dist\left(X, L_3\right)\]

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:

    \[\frac{\sum { \left[d_i  -dist\left(X,L_i,\right)\right] }^2 }{N}\]

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
		'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:

    \[\frac{\sum { \left|d_i  -dist\left(X,L_i,\right)\right| } }{N}\]

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.
❓ Can I add multiple readings from the same beacon?
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 X 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.

	# 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.


This post concludes the series on localisation and trilateration.

💖 Support this blog

This website 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.

Patreon Patreon_button

📧 Stay updated

You will be notified when a new tutorial is released!

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

Write a Comment



  1. 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. Why not computing a first derivative of mean squared error, resolving for mSE = 0 and then looking for minima points? (Excluding maxima )

  3. 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.

  4. 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 “”, line 46, in
    ‘maxiter’: 1e+7 # Maximum iterations
    File “/usr/lib/python3/dist-packages/scipy/optimize/”, line 603, in minimize
    callback=callback, **options)
    File “/usr/lib/python3/dist-packages/scipy/optimize/”, line 335, in _minimize_lbfgsb
    f, g = func_and_grad(x)
    File “/usr/lib/python3/dist-packages/scipy/optimize/”, line 280, in func_and_grad
    f = fun(x, *args)
    File “/usr/lib/python3/dist-packages/scipy/optimize/”, line 293, in function_wrapper
    return function(*(wrapper_args + args))
    File “”, 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)

  5. 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 “”, line 87, in
    ‘maxiter’: 1e+7 # Maximum iterations
    File “/usr/lib/python3/dist-packages/scipy/optimize/”, line 603, in minimize
    callback=callback, **options)
    File “/usr/lib/python3/dist-packages/scipy/optimize/”, line 335, in _minimize_lbfgsb
    f, g = func_and_grad(x)
    File “/usr/lib/python3/dist-packages/scipy/optimize/”, line 280, in func_and_grad
    f = fun(x, *args)
    File “/usr/lib/python3/dist-packages/scipy/optimize/”, line 293, in function_wrapper
    return function(*(wrapper_args + args))
    File “”, line 22, in mse
    return mse / len(data)
    NameError: name ‘data’ is not defined


  • Reto Maker - Triangulación para posicionamiento en interiores 3D - Zaragoza MakerSpace August 15, 2022

    […] este enlace encontrarás la solución para el espacio de dos […]

  • Tutorial Series - Alan Zucconi August 15, 2022

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

  • Understanding Geographical Coordinates - Alan Zucconi August 15, 2022

    […] 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 […]