Talk:Trilateration

From Wikipedia, the free encyclopedia
Jump to: navigation, search
WikiProject Mathematics (Rated Start-class, Mid-importance)
WikiProject Mathematics
This article is within the scope of WikiProject Mathematics, a collaborative effort to improve the coverage of Mathematics on Wikipedia. If you would like to participate, please visit the project page, where you can join the discussion and see a list of open tasks.
Mathematics rating:
Start Class
Mid Importance
 Field: Geometry

Example C program[edit]

This is an enhanced version of the example program I submitted earlier. It is a lightly-tested implementation of the trilateration algorithm.

The program reads the coordinates and the radius for each sphere from standard input, then calculates the intersection point, if any.

This version handles correctly the situation where the three spheres are colinear. For example,

{\bar p}_1 = (0, \  0, \  0), \  r_1 = 1
{\bar p}_2 = (3, \  0, \  0), \  r_2 = 2
{\bar p}_3 = (9, \  0, \  0), \  r_3 = 8

which intersect at (1, \ 0, \  0).

(I tested the function with six billion intersecting but otherwise random spheres, and maxzero = 0.000000000001 \ min(r_1, r_2, r_3). All solutions were found, no false negatives were reported. The maximum relative error in the distances (compared to radii) in the solutions was less than 0.0000000001. The tests did not include any non-intersecting sphere configurations. Nominal animal (talk) 16:09, 19 June 2010 (UTC))

#include <stdio.h>
#include <math.h>

/* No rights reserved (CC0, see http://wiki.creativecommons.org/CC0_FAQ).
 * The author has waived all copyright and related or neighboring rights
 * to this program, to the fullest extent possible under law.
*/

/* Largest nonnegative number still considered zero */
#define   MAXZERO  0.0

typedef struct vec3d	vec3d;
struct vec3d {
	double	x;
	double	y;
	double	z;
};
 
/* Return the difference of two vectors, (vector1 - vector2). */
vec3d vdiff(const vec3d vector1, const vec3d vector2)
{
	vec3d v;
	v.x = vector1.x - vector2.x;
	v.y = vector1.y - vector2.y;
	v.z = vector1.z - vector2.z;
	return v;
}
 
/* Return the sum of two vectors. */
vec3d vsum(const vec3d vector1, const vec3d vector2)
{
	vec3d v;
	v.x = vector1.x + vector2.x;
	v.y = vector1.y + vector2.y;
	v.z = vector1.z + vector2.z;
	return v;
}
 
/* Multiply vector by a number. */
vec3d vmul(const vec3d vector, const double n)
{
	vec3d v;
	v.x = vector.x * n;
	v.y = vector.y * n;
	v.z = vector.z * n;
	return v;
}
 
/* Divide vector by a number. */
vec3d vdiv(const vec3d vector, const double n)
{
	vec3d v;
	v.x = vector.x / n;
	v.y = vector.y / n;
	v.z = vector.z / n;
	return v;
}
 
/* Return the Euclidean norm. */
double vnorm(const vec3d vector)
{
	return sqrt(vector.x * vector.x + vector.y * vector.y + vector.z * vector.z);
}
 
/* Return the dot product of two vectors. */
double dot(const vec3d vector1, const vec3d vector2)
{
	return vector1.x * vector2.x + vector1.y * vector2.y + vector1.z * vector2.z;
}
 
/* Replace vector with its cross product with another vector. */
vec3d cross(const vec3d vector1, const vec3d vector2)
{
	vec3d v;
	v.x = vector1.y * vector2.z - vector1.z * vector2.y;
	v.y = vector1.z * vector2.x - vector1.x * vector2.z;
	v.z = vector1.x * vector2.y - vector1.y * vector2.x;
	return v;
}

/* Return zero if successful, negative error otherwise.
 * The last parameter is the largest nonnegative number considered zero;
 * it is somewhat analoguous to machine epsilon (but inclusive).
*/
int trilateration(vec3d *const result1, vec3d *const result2,
                  const vec3d p1, const double r1,
                  const vec3d p2, const double r2,
                  const vec3d p3, const double r3,
                  const double maxzero)
{
	vec3d	ex, ey, ez, t1, t2;
	double	h, i, j, x, y, z, t;
 
	/* h = |p2 - p1|, ex = (p2 - p1) / |p2 - p1| */
	ex = vdiff(p2, p1);
	h = vnorm(ex);
	if (h <= maxzero) {
		/* p1 and p2 are concentric. */
		return -1;
	}
	ex = vdiv(ex, h);
 
	/* t1 = p3 - p1, t2 = ex (ex . (p3 - p1)) */
	t1 = vdiff(p3, p1);
	i = dot(ex, t1);
	t2 = vmul(ex, i);
 
	/* ey = (t1 - t2), t = |t1 - t2| */
	ey = vdiff(t1, t2);
	t = vnorm(ey);
	if (t > maxzero) {
		/* ey = (t1 - t2) / |t1 - t2| */
		ey = vdiv(ey, t);
 
		/* j = ey . (p3 - p1) */
		j = dot(ey, t1);
	} else
		j = 0.0;

	/* Note: t <= maxzero implies j = 0.0. */
	if (fabs(j) <= maxzero) {
		/* p1, p2 and p3 are colinear. */

		/* Is point p1 + (r1 along the axis) the intersection? */
		t2 = vsum(p1, vmul(ex, r1));
		if (fabs(vnorm(vdiff(p2, t2)) - r2) <= maxzero &&
		    fabs(vnorm(vdiff(p3, t2)) - r3) <= maxzero) {
			/* Yes, t2 is the only intersection point. */
			if (result1)
				*result1 = t2;
			if (result2)
				*result2 = t2;
			return 0;
		}

		/* Is point p1 - (r1 along the axis) the intersection? */
		t2 = vsum(p1, vmul(ex, -r1));
		if (fabs(vnorm(vdiff(p2, t2)) - r2) <= maxzero &&
		    fabs(vnorm(vdiff(p3, t2)) - r3) <= maxzero) {
			/* Yes, t2 is the only intersection point. */
			if (result1)
				*result1 = t2;
			if (result2)
				*result2 = t2;
			return 0;
		}

		return -2;
	}
 
	/* ez = ex x ey */
	ez = cross(ex, ey);
 
	x = (r1*r1 - r2*r2) / (2*h) + h / 2;
	y = (r1*r1 - r3*r3 + i*i) / (2*j) + j / 2 - x * i / j;
	z = r1*r1 - x*x - y*y;
	if (z < -maxzero) {
		/* The solution is invalid. */
		return -3;
	} else
	if (z > 0.0)
		z = sqrt(z);
	else
		z = 0.0;
 
	/* t2 = p1 + x ex + y ey */
	t2 = vsum(p1, vmul(ex, x));
	t2 = vsum(t2, vmul(ey, y));
 
	/* result1 = p1 + x ex + y ey + z ez */
	if (result1)
		*result1 = vsum(t2, vmul(ez, z));
 
	/* result1 = p1 + x ex + y ey - z ez */
	if (result2)
		*result2 = vsum(t2, vmul(ez, -z));
 
	return 0;
}

int main(void)
{
	vec3d	p1, p2, p3, o1, o2;
	double	r1, r2, r3;
	int	result;
 
	while (fscanf(stdin, "%lg %lg %lg %lg %lg %lg %lg %lg %lg %lg %lg %lg",
	                     &p1.x, &p1.y, &p1.z, &r1,
	                     &p2.x, &p2.y, &p2.z, &r2,
	                     &p3.x, &p3.y, &p3.z, &r3) == 12) {
		printf("Sphere 1: %g %g %g, radius %g\n", p1.x, p1.y, p1.z, r1);
		printf("Sphere 2: %g %g %g, radius %g\n", p2.x, p2.y, p2.z, r2);
		printf("Sphere 3: %g %g %g, radius %g\n", p3.x, p3.y, p3.z, r3);
		result = trilateration(&o1, &o2, p1, r1, p2, r2, p3, r3, MAXZERO);
		if (result)
			printf("No solution (%d).\n", result);
		else {
			printf("Solution 1: %g %g %g\n", o1.x, o1.y, o1.z);
			printf("  Distance to sphere 1 is %g (radius %g)\n", vnorm(vdiff(o1, p1)), r1);
			printf("  Distance to sphere 2 is %g (radius %g)\n", vnorm(vdiff(o1, p2)), r2);
			printf("  Distance to sphere 3 is %g (radius %g)\n", vnorm(vdiff(o1, p3)), r3);
			printf("Solution 2: %g %g %g\n", o2.x, o2.y, o2.z);
			printf("  Distance to sphere 1 is %g (radius %g)\n", vnorm(vdiff(o2, p1)), r1);
			printf("  Distance to sphere 2 is %g (radius %g)\n", vnorm(vdiff(o2, p2)), r2);
			printf("  Distance to sphere 3 is %g (radius %g)\n", vnorm(vdiff(o2, p3)), r3);
		}
	}
 
	return 0;
}

Note that the trilateration() function takes an extra parameter, maxzero, which is the largest nonnegative number still considered zero.

By default maxzero = 0, which is too small to find the solution in some cases. Something like min(r_1, r_2, r_3) \cdot 0.0000001 should work well with double precision floating-point numbers.

If best precision is desired, the trilateration() function should be modified to return min(h,fabs(j)) instead of zero when successful. Call the function with each of the six sphere order permutations, with maxzero = 0, and select the result which corresponds the largest return value; this should have the least numerical instability.

If all six calls return negative, set maxzero to, say, maxzero = min(r_1, r_2, r_3) / 100, and retry. If these calls also fail, there is no solution. Otherwise, use e.g. binary search (halving maxzero each round) to find the smallest working maxzero. Select the result with corresponds to the largest return value (using the smallest working maxzero).

The function is unoptimized, but still rather cheap: on an Athlon64, each call to the trilateration() function takes roughly 2000 - 6000 clock cycles. Nominal animal (talk) 18:58, 18 June 2010 (UTC)

Real improvement to preliminary computation section by User:Nominal animal[edit]

Nominal animal's recent edit to the Preliminary computation section of the trilateration article appears to be a very good simplification. Thank you for a useful contribution. RHB100 (talk) 21:54, 17 June 2010 (UTC)

Thanks! Also, thank you for the language cleanup; English is obviously not my native language. Nominal animal (talk) 16:04, 19 June 2010 (UTC)

Substitution[edit]

Just a minor point, shouldn't rather

z^2=r_1^2-x^2-y^2

than

y^2+z^2=r_1^2-x^2

be substituted in the third equation? 129.247.247.238 (talk) 10:47, 19 November 2012 (UTC)

Since there were no objections, I will change the respective line. 129.247.247.238 (talk) 13:50, 28 November 2012 (UTC)

Broken link?[edit]

The link "Efficient Solution and Performance Analysis of 3-D Position Estimation by Trilateration" in the External Links section gives me a 404 error. daviddoria (talk) 14:34, 12 February 2013 (UTC)


As of 30 May 2014 the link points to a file hoster that shows porn when cklicking the download button. The link should probably be removed.--Jmknaup (talk) 11:28, 30 May 2014 (UTC)

Wrong generalisation that 3/4 spheres are enough?[edit]

The article states that:

"If it is known that the point lies on the surface of a fourth sphere then knowledge of this sphere's center along with its radius is sufficient to determine the one unique location."

I'm fairly sure that this need not always be true. If you have found 2 points, that all lie on the first three spheres, then there still is an infinite number of other spheres, that also have both points on their surface. Knowing the position and radius of one of these spheres would not help to discriminate between one of the two points. To visualize: take any 4th sphere and rotate it around the axis that is defined by the two points. All spheres that are thus generated do not provide any further information. So, a 4th sphere CAN help, but it need NOT ALWAYS be enough. Or, even: there is no number N such that N spheres are guaranteed to define a single point in space via the intersection of their surfaces. One needs additional constraints as to how the spheres are positioned relative to each other. At the very least the sentence in question should be weakened to something like "is usually sufficient", if that is the case in real-world-scenarios (e.g. GPS). — Preceding unsigned comment added by 94.198.62.204 (talk) 15:21, 18 June 2013 (UTC)

When I think about it, even the statement about 3 spheres narrowing down the position to 2 points in 3D, is not correct:

"In three-dimensional geometry, when it is known that a point lies on three surfaces such as the surfaces of three spheres then the centers of the three spheres along with their radii provide sufficient information to narrow the possible locations down to no more than two."

There is actually an infinity number of different spheres in 3D that share a common "boundary-intersection-circle" (their centers lie on a straight line). I will delete the whole paragraph, because it is factually wrong and I'm not sure what it is supposed to say. --94.198.62.204 (talk) 12:41, 15 July 2013 (UTC)

The generalization holds as long as no more than 2 reference points are colinear and no more than 3 are coplanar.--Jmknaup (talk) 11:55, 30 May 2014 (UTC)

Statistical Lateration[edit]

This has probably been addressed before, but I think it is worth reconsidering: the statistical (i.e. in presence of noise) use of (tri)lateration should be emphasized and I think it will greatly improve the article. The current "multilateration" page is essentially a TDOA article (a bit too much, in fact). But the problem of lateration is slightly more abstract (it doesn't concern itself from where the measurements come from, in fact it is also heuristically used in RSS-based localization). I am no expert on the field, thus I can't write about it, but I think the general problem should (at least) be cited. In its statistical usage, "tri" (in the word "trilateration") is omitted, since you usually have N nodes (centers of the spheres), with N sufficiently high, and the radii are known within a plus or minus "delta" (note that, inside this delta, there are also included the uncertainties in the coordinates of the centers, leaving the centers error-free; in other words, all the errors are put in the radii's uncertainties, simplifying the notation). Note that the \delta_i, with i=1,\dots,N, are generally different (it cannot be assumed \delta_i = \delta for all i). The "spheres" (spherical shells: r_i - \delta_i, r_i + \delta_i) typically intersects in a region (the "grey zone" or something like that) and then, for example, the centroid (center of mass) is taken. Obviously, the algorithm discards the "spheres" which do not intersect at all. Can someone with more expertize write about this or is there already an article (except multilateration/TDOA) regarding these topics? — Preceding unsigned comment added by 93.148.154.1 (talk) 13:57, 2 April 2014 (UTC)