实施 TDOA 三边测量 Fang 算法的问题

Question

我一直在关注 this paper (notably the section on Fang's Method) in an attempt to achieve a solution to the problem of trilateration using the TDOA technique。

希望有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 等于发射器的 x 坐标 (tx.x == 0.7)，但令人沮丧的是结果是 ≈0.237.

我的确切问题的轮廓（及其解决方案，两条双曲线相交的地方）可以在下图中以几何方式查看：

任何帮助将不胜感激！

Answer 1

论文用方法给出了h的如下计算：

您的代码错误地平方了整个 (1 - (p1.x / r1)) 表达式，而不仅仅是 (p1.x / r1) 部分。此外，您使用了错误的值（p2 和 p1）而不是正确的值（p3 和 p2）。要修复，只需将 h 更改为：

double h = (sqr(p3.x) + sqr(p3.y) - sqr(r3) + r3 * r2 * (1 - sqr(p2.x / r2))) / (2 * p3.y);

实施 TDOA 三边测量 Fang 算法的问题

Issues implementing the Fang Algorithm for TDOA Trilateration

c

algebra

trilateration