Problem Description

Given the 3D coordinate locations of four sensors and their known distance from a target, calculate the location of the target.

Background & Techniques

We have sensors at known locations in 3D space. Each sensor can supply distance information to a target but knows nothing about the target's direction. Alternatively, the sensors are Satellites and the target is a GPS receiver which reads very accurate time stamps transmitted by the satellites and calculates distances based on time offsets between its clock when a clock time message is received and satellites' clock time when the message was sent (as contained in the message).

Given the sensor (X, Y, Z) coordinates and their distance-to-target values, we'll use Gaussian Elimination to solve a set of linear equations describing the problem and derived as follows:

The distance Di to target at (X_t, Y_t,  Z_t) for sensor "i" located at (A_i, B_i, C_i) is given by

Sqr(D_i)=Sqr(A_i - X_t) + Sqr(B_i - Y_t) + Sqr(C_i - -Z_t)

after expanding:

D_i² = A_i² - 2A_iX_t + X_t² +B_i² - 2B_iY_t +Y_t² +C_i² - 2C_iZ_t + Z_t²

Solving this system of quadratic equations can be tricky, but we can we can subtract the Sensor1 equation from each of the other 3 to obtain linear equations like the following example for Sensor2::

2(A₁-A₂)X_t + 2(B₁-B₂)Y_t + 2(_C1-C₂)Z_t = D₂² - A₂² - B₂² - C₂² - D₁²

The resulting 3 equations in 3 unknowns form a system of linear equations which are solved here using Gaussian Elimination to find the (X_t, Y_t, Z_t) target coordinates.

Note that the original problem requires  4 equations to resolve the  3 unknowns (the x,y, and z coordinates of the target) . Two sensors can narrow target location down to a circle (the intersection of 2 spheres with target distances as radii), the 3rd sensor narrows the location down to two possible points, (the intersection of the circle with the 3rd sensor's sphere). The 4th sensor should resolve which of those two points is the target. Any error in specified locations or distances would either have no solution or require that some input values be adjusted.  The techniques applied here result in reported distances being adjusted to produce a solution. Differences between input and calculated distances are listed as part of the solution.

The Solve button returns a position which may be at distances different from those in the original distance equations but do satisfy the distances relative to Sensor 1.   That is part of what we sacrifice by eliminating one of the equations and converting quadratics to linear.  A better solution in the GPS case might be to incorporate dummy 4th variable representing the distance error due to clock synchronization errors between the satellites and the GPS receiver.   The multivariate Newton-Raphson algorithm is a possible way to solve using 4 quadratic equations in 4 unknowns.   Sounds like a good candidate for our next project!

April 26, 2012:  Version 5 posted today adds a second method for locating the target.  Trilateration  uses the locations of three sensors to exactly narrow the target location down to at most two possibilities.  See this Wikipedia article for an excellent discussion and derivation of the math involved.  I translated a C code implementation  to Delphi for testing.  By solving the 4 combination using 3 of the 4 sensors and saving the two solutions from each case, it seems that we have good luck in identifying the single solution.  It is much more robust the the Gaussian Elimination version, easily  solving the case submitted by a user which led to the current investigation: Using notation (x, y, z, r) to represent the x, y, z, coordinates and r, the measured distance to the target. the test case is defined by  (0, 0, 0, 10), (10, 0, 0, 10), (0, 10, 0, 10), and (10, 10, 0, 10).  Gaussian Elimination finds no solution or and incorrect solution if a small increment is added to the z coordinate of one of the sensors to remove the singularity .  Trilateration correctly indentifies the solution as (5, 5, 7.07)  or (5, 5, -7.07) which is also valid.  Assigning a non-zero z coordinate to move any of the sensors slightly above or below the xy plane will produce the correct single solution..    

One other small change in Version 5 - European (,) and US (.) decimal separators should  now both be honored when cases are saved or loaded.  I'm sure my European friends will let me know if it doesn't work. J   

Non-programmers are welcome to read on, but may want to skip to the bottom of this page to download executable version of the program.

Notes for Programmers

I developed the algebraic technique described above after spending a week trying to get the geometric approach to work.  Geometrically, each of the 4 sensors and its target distance define a sphere upon which the target must lie, and that should be the common point of intersection of all 4 spheres.    My plan was to find the circle defined by the intersection of spheres 1 and 2, then find the 2 points where that circle intersected sphere 3 and then choose which of those two points came closes to the surface of sphere 4.   I never did track down some little bug in the process and thus the switch of approaches. 

There are 19 TEdit controls for user input; 4 for each of the 4 sensors plus 3 if the user wants to enter target values.  The target values were convenient when debugging the code with sets of points with known solutions.  In order to simplify the code, I defined an array, Sensors,  of  TSensorEdits records, each containing 4 TEdit pointers (object references are always pointers), plus the numeric version of the X, Y, Z, and R (distance)  values represent by the edits for that sensor.     

Unit UMatrix is the unit from the old Borland Turbo Pascal  Numeric Toolbox which contains the GuussianElimination procedure used here among other matrix operations.  

Load and Save buttons use Ini file types to save and reload problem cases.

Running/Exploring the Program 

Suggestions for Further Explorations

Implementing a multivariate Newton-Raphson to solve the 4 distance equations would be an interesting exercise.