Trilateration using 3 latitude/longitude points, and 3 distances?

Question

I want to find out an unknown target location (latitude and longitude co-ordinates). There are 3 known points (latitude and longitude co-ordinate pairs) and for each point a distance in kilometers to the target location. How can I calculate the co-ordinates of the target location?

For example, say I have the following data points

37.418436,-121.963477   0.265710701754km
37.417243,-121.961889   0.234592423446km
37.418692,-121.960194   0.0548954278262km

What I'd like is what is the math for a function that takes that as input and returns 37.417959,-121.961954 as output.

I understand how to calculate the distance between two points, from http://www.movable-type.co.uk/scripts/latlong.html I understand the general principle that with three circles like these you get exactly one point of overlap. What I'm hazy on is the math needed to calculate that point with this input.

Answer 1

After some looking around at Wikipedia and the same question/answer at StackOverflow, I figured I would take a stab at it, and try to fill in the gaps.

First off, Not sure where you got the output, but it appears to be wrong. I plotted the points in ArcMap, buffered them to the distances specified, ran intersect to on the buffers, and then captured the vertex of intersection to get the solutions. Your proposed output is the point in green. I calculated the value in the callout box, which is about 3 meters of what ArcMap gave for solution derived from the intersect.

The math on the wikipedia page isn't too bad, just need to covert your geodetic coordinates to the cartesian ECEF, which can be found here. the a/x +h terms can be replaced by the authalic sphere radius, if you aren't using an ellipsoid.

Probably easiest just give you some well(?) documented code, so here it is in python

import math
import numpy

#assuming elevation = 0
earthR = 6371
LatA = 37.418436
LonA = -121.963477
DistA = 0.265710701754
LatB = 37.417243
LonB = -121.961889
DistB = 0.234592423446
LatC = 37.418692
LonC = -121.960194
DistC = 0.0548954278262

#using authalic sphere
#if using an ellipsoid this step is slightly different
#Convert geodetic Lat/Long to ECEF xyz
#   1. Convert Lat/Long to radians
#   2. Convert Lat/Long(radians) to ECEF
xA = earthR *(math.cos(math.radians(LatA)) * math.cos(math.radians(LonA)))
yA = earthR *(math.cos(math.radians(LatA)) * math.sin(math.radians(LonA)))
zA = earthR *(math.sin(math.radians(LatA)))

xB = earthR *(math.cos(math.radians(LatB)) * math.cos(math.radians(LonB)))
yB = earthR *(math.cos(math.radians(LatB)) * math.sin(math.radians(LonB)))
zB = earthR *(math.sin(math.radians(LatB)))

xC = earthR *(math.cos(math.radians(LatC)) * math.cos(math.radians(LonC)))
yC = earthR *(math.cos(math.radians(LatC)) * math.sin(math.radians(LonC)))
zC = earthR *(math.sin(math.radians(LatC)))

P1 = numpy.array([xA, yA, zA])
P2 = numpy.array([xB, yB, zB])
P3 = numpy.array([xC, yC, zC])

#from wikipedia
#transform to get circle 1 at origin
#transform to get circle 2 on x axis
ex = (P2 - P1)/(numpy.linalg.norm(P2 - P1))
i = numpy.dot(ex, P3 - P1)
ey = (P3 - P1 - i*ex)/(numpy.linalg.norm(P3 - P1 - i*ex))
ez = numpy.cross(ex,ey)
d = numpy.linalg.norm(P2 - P1)
j = numpy.dot(ey, P3 - P1)

#from wikipedia
#plug and chug using above values
x = (pow(DistA,2) - pow(DistB,2) + pow(d,2))/(2*d)
y = ((pow(DistA,2) - pow(DistC,2) + pow(i,2) + pow(j,2))/(2*j)) - ((i/j)*x)

# only one case shown here
z = numpy.sqrt(pow(DistA,2) - pow(x,2) - pow(y,2))

#triPt is an array with ECEF x,y,z of trilateration point
triPt = P1 + x*ex + y*ey + z*ez

#convert back to lat/long from ECEF
#convert to degrees
lat = math.degrees(math.asin(triPt[2] / earthR))
lon = math.degrees(math.atan2(triPt[1],triPt[0]))

print lat, lon

Answer 2

I'm not sure if I'm being naive, but, if you buffer each point by size, and then intersect all three circles that would get you the correct location?

You can compute the intersection using spatial APIs. Examples:

• GeoScript
• Java Topology Suite
• NET Topology Suite
• GEOS

Answer 3

The following notes use planarithmic geometry (i.e. you would have to project your coordinates into an appropriate local coordinate system).

My reasoning, with a worked example in Python, follows:

Take 2 of the data-points (call them a and b). Call our target point x. We already know the distances ax and bx. We can calculate the distance ab using Pythagoras' theorem.

>>> import math
>>> a = (1, 4)
>>> b = (3, 6)
>>> dist_ax = 3
>>> dist_bx = 5.385
# Pythagoras's theorem
>>> dist_ab = math.sqrt(abs(a[0]-b[0])**2 + abs(a[1]-b[1])**2)
>>> dist_ab
2.8284271247461903

Now, you can work out the angles of these lines:

>>> angle_abx = math.acos((dist_bx * dist_bx + dist_ab * dist_ab - dist_ax * dist_ax)/(2 * dist_bx * dist_ab))
>>> math.degrees(angle_abx)
23.202973815040256
>>> angle_bax = math.acos((dist_ax * dist_ax + dist_ab * dist_ab - dist_bx * dist_bx)/(2 * dist_ax * dist_ab))
>>> math.degrees(angle_bax)
134.9915256259537
>>> angle_axb = math.acos((dist_ax * dist_ax + dist_bx * dist_bx - dist_ab * dist_ab)/(2 * dist_ax * dist_bx))
>>> math.degrees(angle_axb)
21.805500559006095

Unfortunately I am short on time to complete the answer for you, However, now you know the angles, you can calculate two possible locations for x. Then, using the third point c you can calculate which location is correct.

Answer 4

This might work. Quickly again in python, you could put this in the body of a function xN,yN = coordinates of points, r1 & r2 = radius values

dX = x2 - x1
dY = y2 - y1

centroidDistance = math.sqrt(math.pow(e,2) + math.pow(dY,2)) #distance from centroids
distancePL = (math.pow(centroidDistance,2) + (math.pow(r1,2) - math.pow(r2,2))) / (2 * centroidDistance) #distance from point to a line splitting the two centroids

rx1 = x1 + (dX *k)/centroidDistance + (dY/centroidDistance) * math.sqrt(math.pow(r1,2) - math.pow(distancePL,2))
ry1 = y1 + (dY*k)/centroidDistance - (dX /centroidDistance) * math.sqrt(math.pow(r1,2) - math.pow(distancePL,2))

rx2 = x1 + (dX *k)/centroidDistance - (dY/centroidDistance) * math.sqrt(math.pow(r1,2) - math.pow(distancePL,2))
ry2 = y1 + (dY*k)/centroidDistance + (dX /centroidDistance) * math.sqrt(math.pow(r1,2) - math.pow(distancePL,2))

rx & ry values are the return values (should be in an array) of the two intersection points on a circle, if that helps clarify things.

Do this for the first 2 circles, then again for the first and last. If either of the results from the first iteration compare with the results from the second (within some tolerance, probably), then you have the point of intersection. It's not a great solution especially when you start adding more than points into the process, but is the simplest I can see without going to solve a system of equations.

Answer 5

You may use spatial API from postgis (St_Intersection, St_buffer functions). As fmark noticed, you must also remember that Postgis uses planar algorithms, but for small areas, using equi-distant prjection does not introduce much error.

Answer 6

Do it in PHP language:

//assuming elevation = 0
$earthR = 6371; // in km ( = 3959 in miles)

$LatA = 37.418436;
$LonA = -121.963477;
$DistA = 0.265710701754;

$LatB = 37.417243;
$LonB = -121.961889;
$DistB = 0.234592423446;

$LatC = 37.418692;
$LonC = -121.960194;
$DistC = 0.0548954278262;

/*
#using authalic sphere
#if using an ellipsoid this step is slightly different
#Convert geodetic Lat/Long to ECEF xyz
#   1. Convert Lat/Long to radians
#   2. Convert Lat/Long(radians) to ECEF
*/
$xA = $earthR *(cos(deg2rad($LatA)) * cos(deg2rad($LonA)));
$yA = $earthR *(cos(deg2rad($LatA)) * sin(deg2rad($LonA)));
$zA = $earthR *(sin(deg2rad($LatA)));

$xB = $earthR *(cos(deg2rad($LatB)) * cos(deg2rad($LonB)));
$yB = $earthR *(cos(deg2rad($LatB)) * sin(deg2rad($LonB)));
$zB = $earthR *(sin(deg2rad($LatB)));

$xC = $earthR *(cos(deg2rad($LatC)) * cos(deg2rad($LonC)));
$yC = $earthR *(cos(deg2rad($LatC)) * sin(deg2rad($LonC)));
$zC = $earthR *(sin(deg2rad($LatC)));

/*
INSTALL:
sudo pear install Math_Vector-0.7.0
sudo pear install Math_Matrix-0.8.7
*/
// Include PEAR::Math_Matrix
//  /usr/share/php/Math/Matrix.php
//  include_path=".:/usr/local/php/pear/"
require_once 'Math/Matrix.php';
require_once 'Math/Vector.php';
require_once 'Math/Vector3.php';


$P1vector = new Math_Vector3(array($xA,$yA,$zA));
$P2vector = new Math_Vector3(array($xB,$yB,$zB));
$P3vector = new Math_Vector3(array($xC,$yC,$zC));

#from wikipedia: http://en.wikipedia.org/wiki/Trilateration
#transform to get circle 1 at origin
#transform to get circle 2 on x axis

// CALC EX
$P2minusP1 = Math_VectorOp::substract($P2vector, $P1vector);
$l = new Math_Vector($P2minusP1);
$P2minusP1_length = $l->length();
$norm = new Math_Vector3(array($P2minusP1_length,$P2minusP1_length,$P2minusP1_length));
$d = $norm; //save calc D
$ex = Math_VectorOp::divide($P2minusP1, $norm);
//echo "ex: ".$ex->toString()."\n";
$ex_x = floatval( $ex->_tuple->getData()[0] ) ;
$ex_y = floatval( $ex->_tuple->getData()[1] ) ;
$ex_z = floatval( $ex->_tuple->getData()[2] ) ;
$ex = new Math_Vector3(array($ex_x,$ex_y,$ex_z));

// CALC i
$P3minusP1 = Math_VectorOp::substract($P3vector, $P1vector);
$P3minusP1_x = floatval( $P3minusP1->_tuple->getData()[0] ) ;
$P3minusP1_y = floatval( $P3minusP1->_tuple->getData()[1] ) ;
$P3minusP1_z = floatval( $P3minusP1->_tuple->getData()[2] ) ;
$P3minusP1 = new Math_Vector3(array($P3minusP1_x,$P3minusP1_y,$P3minusP1_z));
$i = Math_VectorOp::dotProduct($ex, $P3minusP1);
//echo "i = $i\n";

// CALC EY
$iex = Math_VectorOp::scale($i, $ex);
//echo " iex = ".$iex->toString()."\n";
$P3P1iex = Math_VectorOp::substract($P3minusP1, $iex);
//echo " P3P1iex = ".$P3P1iex->toString()."\n";
$l = new Math_Vector($P3P1iex);
$P3P1iex_length = $l->length();
$norm = new Math_Vector3(array($P3P1iex_length,$P3P1iex_length,$P3P1iex_length));
//echo "norm: ".$norm->toString()."\n";
$ey = Math_VectorOp::divide($P3P1iex, $norm);
//echo " ey = ".$ey->toString()."\n";
$ey_x = floatval( $ey->_tuple->getData()[0] ) ;
$ey_y = floatval( $ey->_tuple->getData()[1] ) ;
$ey_z = floatval( $ey->_tuple->getData()[2] ) ;
$ey = new Math_Vector3(array($ey_x,$ey_y,$ey_z));

// CALC EZ
$ez = Math_VectorOp::crossProduct($ex, $ey);
//echo " ez = ".$ez->toString()."\n";

// CALC D
// do it before
$d = floatval( $d->_tuple->getData()[0] ) ;
//echo "d = $d\n";

// CALC J
$j = Math_VectorOp::dotProduct($ey, $P3minusP1);
//echo "j = $j\n";

#from wikipedia
#plug and chug using above values
$x = (pow($DistA,2) - pow($DistB,2) + pow($d,2))/(2*$d);
$y = ((pow($DistA,2) - pow($DistC,2) + pow($i,2) + pow($j,2))/(2*$j)) - (($i/$j)*$x);

# only one case shown here
$z = sqrt( pow($DistA,2) - pow($x,2) - pow($y,2) );

//echo "x = $x - y = $y  - z = $z\n";

#triPt is an array with ECEF x,y,z of trilateration point
$xex = Math_VectorOp::scale($x, $ex);
$yey = Math_VectorOp::scale($y, $ey);
$zez = Math_VectorOp::scale($z, $ez);

// CALC $triPt = $P1vector + $xex + $yey + $zez;
$triPt = Math_VectorOp::add($P1vector, $xex);
$triPt = Math_VectorOp::add($triPt, $yey);
$triPt = Math_VectorOp::add($triPt, $zez);
//echo " triPt = ".$triPt->toString()."\n";
$triPt_x = floatval( $triPt->_tuple->getData()[0] ) ;
$triPt_y = floatval( $triPt->_tuple->getData()[1] ) ;
$triPt_z = floatval( $triPt->_tuple->getData()[2] ) ;


#convert back to lat/long from ECEF
#convert to degrees
$lat = rad2deg(asin($triPt_z / $earthR));
$lon = rad2deg(atan2($triPt_y,$triPt_x));

echo $lat .','. $lon;