每次都产生相同坐标的本地化（多点定位）代码？

Posted 2023-02-23

【中文标题】每次都产生相同坐标的本地化（多点定位）代码？

【英文标题】：Localization (multilateration) code producing the same coordinates every time?

【发布时间】：2020-09-26 15:13:39

【问题描述】：

编辑：本地化总是关于x=1980.000032943933547358028590679168701171875, y=3191.99997642902735606185160577297210693359375

我编写了以下代码来解决到达时间延迟问题。也就是说，给定三个观察者的位置、某些信号的速度以及每个接收器“看到”信号的时间，我想定位源。在这里，我们假设源和观察者在一个二维平面（平面欧几里得空间）。

我的解决方法如下：

鉴于每个观察者看到信号的时间，我选择一个接收器作为“基线”，我认为它是 t=0。然后我从其他两个观察者的到达时间中减去那个时间。现在，我为每个观察者分配一个圆，半径由这个差异给出（“基线”观察者从 r=0 开始），然后我慢慢增加每个圆的半径，直到所有三个圆在某个点相交。

实际上，它们不太可能精确地在一个点相交，因为我只能在每次迭代时将半径增加一个离散量，我们还假设，但不知道观察者的时钟是精确同步的。

为了解决这个问题，我改编了 Paul Bourke 的代码：http://paulbourke.net/geometry/circlesphere/tvoght.c（此处推理：http://paulbourke.net/geometry/circlesphere/）。我的改编版或多或少与此版相同：https://***.com/a/19724186/14073182。

---

我的问题是，代码总是 产生相同的本地化，在一个单元的十分之几以内。我尝试生成一些伪数据（即选择某个位置，计算预期延迟，将其输入算法并查看它是否重建正确的定位），并且该算法仍然产生大致相同的定位......即，相当接近中心由三个接收器组成的三角形。

知道我在这里做错了什么吗？我与一些人（从事 GIS、测地线、类似领域的物理学家和一位数学家）交谈过，但都没有得到任何结果。

我的代码如下（考虑到所有因素，它相当简短）。要本地化某些来源，请致电localizeSource(a,b,c,&xf,&xy)。其中a、b 和c 是延迟（半径），xf、xy 是存储定位坐标的位置。

#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);
    

【问题讨论】：

我不知道你希望代码做什么。为什么要给出不同的结果？请阅读How to Ask 并提供minimal reproducible example，它确实显示了您的期望。顺便说一句：您在代码中犯了几个错误。尽管不是致命的，但您仍然可以从 codereview.stackexchange.com 的审核中受益。

扩展@UlrichEckhardt 的关于在代码审查上发帖的建议；当前的问题不会在那里成为主题，因为代码无法按照您想要的方式工作。如果你解决了这个问题，那么如果你提出一个新问题，它可能会成为一个好问题，检查if it is on-topic并关注how to post a good question。

@UlrichEckhardt 嗨。感谢您指出了这一点。我想我知道所有这些错误。就我而言，我对数据了解得足够多，我知道在实践中我永远不会遇到这些错误。另外，我发布了整个代码，因为我觉得这是我的 algo.logic 的一个问题，而不是我的代码中的一些错误（如果我错了，请纠正我，但我认为这仍然是 SO 的主题。我在这里看到了很多关于算法的问题。算法的解决方案/问题。逻辑）。我已经包括了裸分钟。需要计算。根据我的逻辑进行本地化。如果我删除任何东西，代码什么都不做。

您的问题未达到预期结果。给定输入 1000、3000 和 0，如您的示例所示，应该答案是什么？

"[时间] 差给出的半径" -- 使用时间单位的量作为距离单位的量的理由是什么？虽然我可以看到计算是如何被强制进行的，但这种对设置的描述有点糟糕的数学味道。

【参考方案1】：

目前还不清楚您要解决的问题...您的问题是关于“到达时间延迟 [原文如此]”，但随后您链接到“圆的交点”算法。 TDoA 算法使用抛物线而不是圆。

到达时间差算法：

SO 上的某人已经使用 Fang 的方法编写了示例代码 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

如果您只是在寻找三角测量/三边测量公式：

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)

【讨论】：

其实是双曲线的交集。不过，不断增长的圈子是一种有效的（如果是迭代的和近似的）替代方法。