Я следовал этой статье (в частности, разделу о методе Фанга), пытаясь найти решение проблемы трилатерации, используя метод TDOA .
Я надеюсь, что кто-то, имеющий опыт в Fang / TDOA, сможет протянуть мне руку помощи. По какой-то причине моя реализация возвращает неправильные корни в последний квадратик. Вот код, который я написал до сих пор:
#include <stdio.h>
#include <math.h>
struct Point {
double x;
double y;
};
inline double sqr(double n) {
return n * n;
}
// r1 and r2 are the TDOA of the sound impulse to p1 and p2, respectively
void fang(double r1, double r2) {
// transmitter coords
Point tx = {0.7, -0.1};
// receiver coordinates
Point p0 = {0, 0};
Point p1 = {1.7320508075688772, 0};
Point p2 = {0.8660254037844388, 1.5};
// linear coefficients
double g = ((r2 * (p1.x/r1)) - p2.x) / p2.y;
double h = (sqr(p2.x) + sqr(p2.y) - sqr(r2) + r2 * r1 * sqr(1 - (p1.x / r1))) / (2 * p2.y);
// quadratic coefficents
double d = -(1 - sqr(p1.x / r1) + sqr(g));
double e = p1.x * (1 - sqr(p1.x / r1)) - (2 * g * h);
double f = (sqr(r1) / 4) * sqr(1 - sqr(p1.x / r1)) - sqr(h);
double result_x = (-e - sqrt(sqr(e) - (4 * d * f))) / (2 * d);
}
int main() {
// these values have been calculated a-priori, from the known transmitter coords
double r1 = 0.32977743096231715;
double r2 = 0.90148404145971694;
fang(r1, r2);
}
В конечном итоге я ожидал, что x_result будет равен координате х передатчика (tx.x == 0.7), но, к сожалению, результат равен ≈0.237.
Схема моей точной задачи (и ее решения, где две гиперболы пересекаются) можно геометрически увидеть на следующем графике:
Любая помощь будет принята с благодарностью!