Minimizing this error function, using NumPy

Question

Background

I've been working for some time on attempting to solve the (notoriously painful) Time Difference of Arrival (TDoA) multi-lateration problem, in 3-dimensions and using 4 nodes. If you're unfamiliar with the problem, it is to determine the coordinates of some signal source (X,Y,Z), given the coordinates of n nodes, the time of arrival of the signal at each node, and the velocity of the signal v.

My solution is as follows:

For each node, we write (X-x_i)**2 + (Y-y_i)**2 + (Z-z_i)**2 = (v(t_i - T)**2

Where (x_i, y_i, z_i) are the coordinates of the ith node, and T is the time of emission.

We have now 4 equations in 4 unknowns. Four nodes are obviously insufficient. We could try to solve this system directly, however that seems next to impossible given the highly nonlinear nature of the problem (and, indeed, I've tried many direct techniques... and failed). Instead, we simplify this to a linear problem by considering all i/j possibilities, subtracting equation i from equation j. We obtain (n(n-1))/2 =6 equations of the form:

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)

Which look like Xv_1 + Y_v2 + Z_v3 + T_v4 = b. We try now to apply standard linear least squares, where the solution is the matrix vector x in A^T Ax = A^T b. Unfortunately, if you were to try feeding this into any standard linear least squares algorithm, it'll choke up. So, what do we do now?

...

The time of arrival of the signal at node i is given (of course) by:

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

This equation implies that the time of arrival, T, is 0. If we have that T = 0, we can drop the T column in matrix A and the problem is greatly simplified. Indeed, NumPy's linalg.lstsq() gives a surprisingly accurate & precise result.

...

So, what I do is normalize the input times by subtracting from each equation the earliest time. All I have to do then is determine the dt that I can add to each time such that the residual of summed squared error for the point found by linear least squares is minimized.

I define the error for some dt to be the squared difference between the arrival time for the point predicted by feeding the input times + dt to the least squares algorithm, minus the input time (normalized), summed over all 4 nodes.

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

My problem:

I was able to do this somewhat satisfactorily by using brute-force. I started at dt = 0, and moved by some step up to some maximum # of iterations OR until some minimum RSS error is reached, and that was the dt I added to the normalized times to obtain a solution. The resulting solutions were very accurate and precise, but quite slow.

In practice, I'd like to be able to solve this in real time, and therefore a far faster solution will be needed. I began with the assumption that the error function (that is, dt vs error as defined above) would be highly nonlinear-- offhand, this made sense to me.

Since I don't have an actual, mathematical function, I can automatically rule out methods that require differentiation (e.g. Newton-Raphson). The error function will always be positive, so I can rule out bisection, etc. Instead, I try a simple approximation search. Unfortunately, that failed miserably. I then tried Tabu search, followed by a genetic algorithm, and several others. They all failed horribly.

So, I decided to do some investigating. As it turns out the plot of the error function vs dt looks a bit like a square root, only shifted right depending upon the distance from the nodes that the signal source is:

Where dt is on horizontal axis, error on vertical axis

And, in hindsight, of course it does!. I defined the error function to involve square roots so, at least to me, this seems reasonable.

What to do?

So, my issue now is, how do I determine the dt corresponding to the minimum of the error function?

My first (very crude) attempt was to get some points on the error graph (as above), fit it using numpy.polyfit, then feed the results to numpy.root. That root corresponds to the dt. Unfortunately, this failed, too. I tried fitting with various degrees, and also with various points, up to a ridiculous number of points such that I may as well just use brute-force.

How can I determine the dt corresponding to the minimum of this error function?

Since we're dealing with high velocities (radio signals), it's important that the results be precise and accurate, as minor variances in dt can throw off the resulting point.

I'm sure that there's some infinitely simpler approach buried in what I'm doing here however, ignoring everything else, how do I find dt?

My requirements:

1. Speed is of utmost importance
2. I have access only to pure Python and NumPy in the environment where this will be run

EDIT:

Here's my code. Admittedly, a bit messy. Here, I'm using the polyfit technique. It will "simulate" a source for you, and compare results:

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

It seems like the Bancroft method applies to this problem? Here's a pure NumPy implementation.

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