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 (Xt, Yt,
Zt) for sensor "i" located at (Ai,
Bi, Ci) is given by
Sqr(Di)=Sqr(Ai - Xt) + Sqr(Bi -
Yt) + Sqr(Ci - -Zt)
after expanding:
Di2 = Ai2 - 2AiXt
+ Xt2 +Bi2 - 2BiYt
+Yt2 +Ci2 - 2CiZt
+ Zt2
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(A1-A2)Xt + 2(B1-B2)Yt
+ 2(C1-C2)Zt = D22 -
A22 - B22 - C22
- D12
The resulting 3 equations in 3 unknowns form a system of linear equations
which are solved here using Gaussian Elimination to find the (Xt, Yt,
Zt) 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