Localization (multilateration) code producing the same coordinates every time? - c++

Question

EDIT: The localization is always about x=1980.000032943933547358028590679168701171875, y=3191.99997642902735606185160577297210693359375
I've written the following code to solve the Time Delay of Arrival problem. That is, given the location of three observers, the velocity of some signal, and the time at which each receiver "saw" the signal, I want to localize the source. Here, we assume that the source and the observers are in a 2-dimensional plane (a planar Euclidean space).
My solution is as follows:
Given the time at which each observer saw the signal, I elect one receiver to be the "baseline", which I take to be t=0. I then subtract that time from the Time of Arrival at the other two observers. Now, I assign to each observer a circle, with the radius given by this difference (the "baseline" observer starts with r=0), and I slowly increment the radius of each circle until all three intersect at some point.
In reality, it's unlikely that they'll ever precisely intersect at a single point, as, for one, I can only increase the radius by a discrete amount with each iteration, and we also assume, but do not known, that the observer's clocks are precisely synchronized.
To solve this, I adapted Paul Bourke's code: http://paulbourke.net/geometry/circlesphere/tvoght.c (reasoning here: http://paulbourke.net/geometry/circlesphere/). My adaptation was more-or-less identical to this one: https://stackoverflow.com/a/19724186/14073182.
My problem is, the code always produces the same localization, to within a few tenths of a unit. I tried generating some psuedodata (i.e. pick some location, compute the expected delay, feed that into the algorithm and see if it reconstructs the correct localization), and the algorithm still produces roughly the same localization... namely, fairly close to the center of the triangle formed by the three receivers.
Any idea what I'm doing wrong here? I've talked with a few people (physicists working in GIS, geodesics, similar fields, and a mathematician), and haven't gotten anywhere.
My code is below (it's fairly brief, all things considered). To localize some source, call localizeSource(a,b,c,&xf,&xy). Where a, b, and c are the delays (radii), and xf, xy are where the coordinates of the localization will be stored.
#define EPSILON 0.0001 // Define some threshold
#define x0 3000.00
#define y0 3600.00
#define x1 2100.00
#define y1 2100.00
#define x2 0960.00
#define y2 3600.00
bool findIntersection(double r0, double r1, double r2, double *xf, double *yf){
double a, dx, dy, d, h, rx, ry;
double point2_x, point2_y;
dx = x1 - x0;
dy = y1 - y0;
d = sqrt((dy*dy) + (dx*dx));
if (d > (r0 + r1))
{
return false;
}
a = ((r0*r0) - (r1*r1) + (d*d)) / (2.0 * d) ;
point2_x = x0 + (dx * a/d);
point2_y = y0 + (dy * a/d);
h = sqrt((r0*r0) - (a*a));
rx = -dy * (h/d);
ry = dx * (h/d);
double intersectionPoint1_x = point2_x + rx;
double intersectionPoint2_x = point2_x - rx;
double intersectionPoint1_y = point2_y + ry;
double intersectionPoint2_y = point2_y - ry;
dx = intersectionPoint1_x - x2;
dy = intersectionPoint1_y - y2;
double d1 = sqrt((dy*dy) + (dx*dx));
dx = intersectionPoint2_x - x2;
dy = intersectionPoint2_y - y2;
double d2 = sqrt((dy*dy) + (dx*dx));
if(fabs(d1 - r2) < EPSILON) {
std::cout << std::setprecision(100) << intersectionPoint1_x << ", " << intersectionPoint1_y << "\n";
*xf = intersectionPoint1_x; *yf = intersectionPoint1_y;
return true;
}
else if(fabs(d2 - r2) < EPSILON) {
std::cout << std::setprecision(100) << intersectionPoint2_x << ", " << intersectionPoint2_y << "\n";
*xf = intersectionPoint2_x; *yf = intersectionPoint2_y;
return true;
}
else {
return false;
}
}
void localizeSource(double r0, double r1, double r2, double *xf, double *yf){
bool foundSource = false;
while(foundSource == false){
foundSource = findIntersection(r0, r1, r2, xf, yf);
r0 += 0.0001; r1 += 0.0001; r2 += 0.0001;
}
}
int main(){
double xf, xy;
localizeSource(1000,3000,0,&xf,&xy);
}

Answer 1

It's not super clear what you are trying to solve... Your question talks about "time delay [sic] of arrival" but then you link to the "intersection of circles" algorithm. TDoA algorithm uses parabolas not circles.
Time Difference of Arrival algorithm:
Someone on SO already wrote sample code using Fang's Method
Issues implementing the Fang Algorithm for TDOA Trilateration
Hyperbolic Position Location Estimation in the Multipath Propagation Environment
Time Difference of Arrival (TDoA) Localization Combining Weighted Least Squares and Firefly Algorithm
If you're just looking for the Triangulation / Trilateration formula:
A = 2*x2 - 2*x1
B = 2*y2 - 2*y1
C = r1*r1 - r2*r2 - x1*x1 + x2*x2 - y1*y1 + y2*y2
D = 2*x3 - 2*x2
E = 2*y3 - 2*y2
F = r2*r2 - r3*r3 - x2*x2 + x3*x3 - y2*y2 + y3*y3
x = (C*E - F*B) / (E*A - B*D)
y = (C*D - A*F) / (B*D - A*E)