使用 NumPy 最小化此误差函数

Question

背景

我一直在努力解决（众所周知的痛苦）到达时间差 (TDoA) 多点定位问题，在节点中3-dimensions并使用4节点。如果你不熟悉这个问题，那就是确定某个信号源(X,Y,Z)的坐标，给定n节点的坐标，信号到达每个节点的时间，以及信号的速度v。

我的解决方案如下：

对于每个节点，我们写 (X-x_i)**2 + (Y-y_i)**2 + (Z-z_i)**2 = (v(t_i - T)**2

哪里(x_i, y_i, z_i)是ith节点坐标，T是发射时间。

我们现在4有4未知数的方程。四个节点显然不够用。我们可以尝试直接解决这个系统，但是鉴于问题的高度非线性性质，这似乎几乎是不可能的（事实上，我已经尝试了许多直接技术……但都失败了）。相反，我们通过考虑所有i/j可能性，i从方程中减去方程，将其简化为线性问题j。我们得到(n(n-1))/2 =6以下形式的方程：

2*(x_j - x_i)*X + 2*(y_j - y_i)*Y + 2*(z_j - z_i)*Z + 2 * v**2 * (t_i - t_j) = v**2 ( t_i**2 - t_j**2) + (x_j**2 + y_j**2 + z_j**2) - (x_i**2 + y_i**2 + z_i**2)

哪个看起来像Xv_1 + Y_v2 + Z_v3 + T_v4 = b。现在，我们尝试运用标准线性最小二乘法，那里的解决方案是矩阵向量x在A^T Ax = A^T b。不幸的是，如果您尝试将其输入到任何标准的线性最小二乘算法中，它就会窒息。所以我们现在怎么办？

...

信号到达节点的时间i（当然）由下式给出：

sqrt( (X-x_i)**2 + (Y-y_i)**2 + (Z-z_i)**2 ) / v

这个方程意味着到达时间T， 是0。如果我们有那个T = 0，我们可以删除T矩阵中的列，A问题就大大简化了。事实上，NumPy's linalg.lstsq()给出了令人惊讶的准确和精确的结果。

...

所以，我所做的是通过从每个方程中减去最早的时间来标准化输入时间。然后我要做的就是确定dt每次可以添加的值，以便最小化线性最小二乘法找到的点的总和平方误差的残差。

我将某些误差定义为dt通过将input times+馈送dt到最小二乘算法预测的点的到达时间之间的平方差减去输入时间（归一化），对所有4节点求和。

for node, time in nodes, times:
    error += ( (sqrt( (X-x_i)**2 + (Y-y_i)**2 + (Z-z_i)**2 ) / v) - time) ** 2

我的问题：

通过使用蛮力，我能够令人满意地做到这一点。我从 开始dt = 0，然后逐步提高到某个最大迭代次数或直到达到某个最小 RSS 错误，这就是dt我添加到归一化时间以获得解决方案。由此产生的解决方案非常准确和精确，但速度很慢。

在实践中，我希望能够实时解决这个问题，因此需要一个更快的解决方案。我首先假设误差函数（即上面定义的dtvs error）将是高度非线性的——这对我来说很有意义。

由于我没有实际的数学函数，我可以自动排除需要微分的方法（例如Newton-Raphson）。误差函数总是正的，所以我可以排除bisection等。相反，我尝试了一个简单的近似搜索。不幸的是，这不幸地失败了。然后我尝试了禁忌搜索，然后是遗传算法和其他几个算法。他们都失败了。

所以，我决定做一些调查。事实证明，误差函数与 dt 的关系图看起来有点像平方根，只是根据与信号源所在节点的距离向右移动：

其中dt是在水平轴上，误差在垂直轴

而且，事后看来，当然可以！. 我将误差函数定义为涉及平方根，所以，至少对我来说，这似乎是合理的。

该怎么办？

那么，我现在的问题是，如何确定dt对应于误差函数的最小值？

我的第一个（非常粗略的）尝试是在错误图上获取一些点（如上），使用 拟合它numpy.polyfit，然后将结果提供给numpy.root. 该根对应于dt. 不幸的是，这也失败了。我尝试使用各种degrees，以及各种点进行拟合，最多可达到可笑的点数，这样我也可以只使用蛮力。

如何确定dt对应于该误差函数的最小值？

由于我们正在处理高速（无线电信号），因此结果精确和准确很重要，因为 的微小差异dt可能会导致结果点偏离。

我敢肯定，我在这里所做的事情中隐藏着一些无限简单的方法，但忽略其他所有内容，我如何找到dt？

我的要求：

1. 速度是最重要的
2. 我只能访问 purePython并且NumPy在将要运行的环境中

编辑：

这是我的代码。不可否认，有点乱。在这里，我正在使用该polyfit技术。它将为您“模拟”一个来源，并比较结果：

from numpy import poly1d, linspace, set_printoptions, array, linalg, triu_indices, roots, polyfit
from dataclasses import dataclass
from random import randrange
import math

@dataclass
class Vertexer:

    receivers: list

    # Defaults
    c = 299792

    # Receivers:
    # [x_1, y_1, z_1]
    # [x_2, y_2, z_2]
    # [x_3, y_3, z_3]

    # Solved:
    # [x, y, z]

    def error(self, dt, times):
        solved = self.linear([time + dt for time in times])

        error = 0
        for time, receiver in zip(times, self.receivers):
            error += ((math.sqrt( (solved[0] - receiver[0])**2 + 
                (solved[1] - receiver[1])**2 +
                (solved[2] - receiver[2])**2 ) / c ) - time)**2

        return error

    def linear(self, times):
        X = array(self.receivers)
        t = array(times)

        x, y, z = X.T   
        i, j = triu_indices(len(x), 1)

        A = 2 * (X[i] - X[j])
        b = self.c**2 * (t[j]**2 - t[i]**2) + (X[i]**2).sum(1) - (X[j]**2).sum(1)

        solved, residuals, rank, s = linalg.lstsq(A, b, rcond=None)

        return(solved)

    def find(self, times):
        # Normalize times
        times = [time - min(times) for time in times]
        
        # Fit the error function

        y = []
        x = []
        dt = 1E-10
        for i in range(50000):
            x.append(self.error(dt * i, times))
            y.append(dt * i)    

        p = polyfit(array(x), array(y), 2)
        r = roots(p)
        
        return(self.linear([time + r for time in times]))




# SIMPLE CODE FOR SIMULATING A SIGNAL

# Pick nodes to be at random locations
x_1 = randrange(10); y_1 = randrange(10); z_1 = randrange(10)
x_2 = randrange(10); y_2 = randrange(10); z_2 = randrange(10)
x_3 = randrange(10); y_3 = randrange(10); z_3 = randrange(10)
x_4 = randrange(10); y_4 = randrange(10); z_4 = randrange(10)

# Pick source to be at random location
x = randrange(1000); y = randrange(1000); z = randrange(1000)

# Set velocity
c = 299792 # km/ns

# Generate simulated source
t_1 = math.sqrt( (x - x_1)**2 + (y - y_1)**2 + (z - z_1)**2 ) / c
t_2 = math.sqrt( (x - x_2)**2 + (y - y_2)**2 + (z - z_2)**2 ) / c
t_3 = math.sqrt( (x - x_3)**2 + (y - y_3)**2 + (z - z_3)**2 ) / c
t_4 = math.sqrt( (x - x_4)**2 + (y - y_4)**2 + (z - z_4)**2 ) / c

print('Actual:', x, y, z)

myVertexer = Vertexer([[x_1, y_1, z_1],[x_2, y_2, z_2],[x_3, y_3, z_3],[x_4, y_4, z_4]])
solution = myVertexer.find([t_1, t_2, t_3, t_4])
print(solution)

Answer 1

班克罗夫特方法似乎适用于这个问题？这是一个纯 NumPy 实现。

# Implementation of the Bancroft method, following
# https://gssc.esa.int/navipedia/index.php/Bancroft_Method
M = np.diag([1, 1, 1, -1])


def lorentz_inner(v, w):
    return np.sum(v * (w @ M), axis=-1)


B = np.array(
    [
        [x_1, y_1, z_1, c * t_1],
        [x_2, y_2, z_2, c * t_2],
        [x_3, y_3, z_3, c * t_3],
        [x_4, y_4, z_4, c * t_4],
    ]
)
one = np.ones(4)
a = 0.5 * lorentz_inner(B, B)
B_inv_one = np.linalg.solve(B, one)
B_inv_a = np.linalg.solve(B, a)
for Lambda in np.roots(
    [
        lorentz_inner(B_inv_one, B_inv_one),
        2 * (lorentz_inner(B_inv_one, B_inv_a) - 1),
        lorentz_inner(B_inv_a, B_inv_a),
    ]
):
    x, y, z, c_t = M @ np.linalg.solve(B, Lambda * one + a)
    print("Candidate:", x, y, z, c_t / c)