python - 不断缩放 3 个圆直到它们相交

Question

我已经实现了trilateration Python中的定位算法，到目前为止，结果看起来很不正常，因为计算的距离受到信号干扰的影响，所以它看起来像这样:

当它应该看起来像这样时:

因此，我正在考虑使用常数因子同时缩放圆，直到它们全部相交于一点(这将是最佳的)或直到它们的相互距离之和最小。给定 2D 空间中三个圆的 XY 坐标，以及它们的 FSPL -计算距引用点(其中一个圆的中心)的距离，该函数应返回最小化误差的最佳缩放因子。应该是这样的:

def find_best_scaling_constant(p1, p2, p3, r1, r2, r3):
  # some magic here
  return scalingConstant

find_best_scaling_constant((0.00, 0.00), (3.15, -0.47), (4.90, 7.00), 1.12, 1.77, 0.18)

我不是数学家，所以我不知道这个逻辑是否有意义，但如果有人有评论或更好的想法，请分享。这会有很大的帮助!

Answer 1

让圆有中心和坐标:

并让您计算出的相应半径为:

分别。所以看起来您正在寻找点 以及具有以下属性的缩放因子:

同样，我们需要找到一个点 在三个圆的公共(public)交点平面上，通过将原始圆的半径重新缩放公因子 k 获得，或者用数学符号表示，我们需要求解该方程组

很明显，上面的系统和系统之前写的属性是等价的。

为了简化事情，请对系统中每个方程的两边进行平方:

根据毕达哥拉斯定理，写出

这就是为什么，在显式公式中，上面的三个二次方程组实际上是二次方程组:

将项从每个方程的右侧移至左侧后，变为:

展开每个方程中的所有平方差并对项重新排序:

为了简化这个系统，从第一个方程中减去第二个方程，然后从第二个方程中减去第三个方程并保留其中一个二次方程，假设保留第一个二次方程:

寻找该系统解决方案的想法如下:

为了简化符号和表达式，我们可以使用一些线性代数的符号。定义以下二乘二矩阵和二乘一列向量:

当我们将后面的矩阵方程乘以M的逆矩阵时:

让我们也用矩阵表示法来写

最后，在使用适当的缩放因子进行缩放后，找到三个圆的交点的算法可以表述如下:

观察到二次方程有两个 z 解。我选择的带负号的点是当三个圆在每个圆的外部并且具有初始不相交半径时的第一个交点。还有第二个交点，它对应于 z 的正解。如果您有来自第四个塔的可用信息，那么您将能够选择正确的点，甚至可能能够完全线性化问题。但仅凭这些可用数据，您就有两种解决方案。

我用以下手工制作的示例测试了该算法:

x1 = 0;  y1 = 0;  r1 = sqrt(13)/3;
x2 = 5;  y2 = 1;  r2 = sqrt(13)/3;
x3 = 3;  y3 = 7;  r3 = sqrt(17)/3;

它会输出正确的位置

x = 2;  y = 3;  

和缩放因子k = 3。

我在 Matlab/Octave 中实现了它，因为我对那里的线性代数很满意:

function [xy, k] = location_scaled(x1, y1, r1, x2, y2, r2, x3, y3, r3)
        
        M = 2*[x2 - x1  y2 - y1; 
               x3 - x2  y3 - y2];
        A = [r1^2 - r2^2; 
             r2^2 - r3^2];
        B = [x2^2 + y2^2 - x1^2 - y1^2; 
             x3^2 + y3^2 - x2^2 - y2^2];
        A = M\A;
        B = M\B;
        
        a = A'*A;
        b = 2*B'*A - 2*[x1  y1]*A - r1^2;
        c = [x1 y1]*[x1; y1] - 2*[x1  y1]*B + B'*B;
        
        k = (- b - sqrt(b^2 - 4*a*c)) / (2*a);
        
        xy = k*A + B;
        k = sqrt(k);
        
end

这是 python 版本:

import numpy as np

def location_scaled(x1, y1, r1, x2, y2, r2, x3, y3, r3):

        M = 2*np.array([[x2 - x1,  y2 - y1], 
                        [x3 - x2,  y3 - y2]])
        A = np.array([r1**2 - r2**2, 
                      r2**2 - r3**2])
        B = np.array([x2**2 + y2**2 - x1**2 - y1**2, 
                      x3**2 + y3**2 - x2**2 - y2**2])

        M = np.linalg.inv(M)
        A = M.dot(A)
        B = M.dot(B)
        x1_y1 = np.array([x1, y1])

        a = A.dot(A)
        b = 2*B.dot(A) - 2*x1_y1.dot(A) - r1**2
        c = x1*x1 + y1*y1 - 2*x1_y1.dot(B) + B.dot(B)

        k = (- b - np.sqrt(b*b - 4*a*c)) / (2*a);

        return k*A + B, np.sqrt(k)