Pagr From 4 Sensors

[Home]   [Puzzles & Projects]    [Delphi Techniques]   [Math topics]   [Library]   [Utilities]

 

Search

Search WWW

Search DelphiForFun.org

As of October, 2016, Embarcadero is offering a free release of Delphi (Delphi 10.1 Berlin Starter Edition ).     There are a few restrictions, but it is a welcome step toward making more programmers aware of the joys of Delphi.  They do say "Offer may be withdrawn at any time", so don't delay if you want to check it out.  Please use the feedback link to let me know if the link stops working.

 

Support DFF - Shop

 If you shop at Amazon anyway,  consider using this link. We receive a few cents from each purchase.   Thanks.


Support DFF - Donate

 If you benefit from the website,  in terms of knowledge, entertainment value, or something otherwise useful, consider making a donation via PayPal  to help defray the costs.  (No PayPal account necessary to donate via credit card.)  Transaction is secure.

Contact

Feedback:  Send an e-mail with your comments about this program (or anything else).

Search DelphiForFun.org only

 

 

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, YtZt) 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 

Suggestions for Further Explorations

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

 

Original Date: January 15, 2009 

Modified: February 18, 2016

 

  [Feedback]   [Newsletters (subscribe/view)] [About me]
Copyright 2000-2017, Gary Darby    All rights reserved.