Bancroft's method implementation

Question

Background

I've written an algorithm to solve the three-dimensional Time Difference of Arrival (TDoA) multi-lateration problem. That is, given the coordinates of n nodes, the velocity v of some signal, and the time of signal arrival at each node, we want to determine the coordinates of the signal source. In my case, the problem was further complicated by the fact that only four nodes are available.

I tried numerous approaches, and found the most success implementing Bancroft's method. With it, I'm able to obtain remarkable accuracy and precision, as well as excellent efficiency. I leave mathematical commentary to the linked page, however I'll try to step through my code as clearly as possible.

---

def find():

The find() method of the Vertexer class does the actual leg-work. When a Vertexer object is constructed, the coordinates of the nodes to be used are passed as a NumPy array to the constructor. That array looks like:

[[x_1, y_1, z_1],
 [x_2, y_2, z_2],
 [x_3, y_3, z_3],
 [x_4, y_4, z_4]]

The times of arrival are passed as a NumPy array to the find() method. That array looks like:

[[t_1],
 [t_2],
 [t_3],
 [t_4]]

The math (as per above) is performed, then we iterate through (typically two) possible solutions, and append each solution to a list, solution.

        # Iterate
        for Lambda in np.roots([ lorentzInner(oneA, oneA),
                                (lorentzInner(oneA, invA) - 1) * 2,
                                 lorentzInner(invA, invA),
                                ]):
            # Obtain solution
            X, Y, Z, T = M @ np.linalg.solve(A, Lambda * np.ones(4) + b)

            # Append solution as NumPy array to `solution`
            solution.append(np.array([X,Y,Z]))

The final solution is obtained by taking the min from solution, with the key being an error function which gives the sum of squared residuals (RSS error) of the time of arrival of the predicted point at each node, minus the given time of arrival.

---

def __post_init__():

In my application, it could be that times inputted are "invalid" (don't correspond to an actual localization, even though the code will try to find one). I compute the minimum possible time of arrival difference, and the maximum possible time of arrival difference.

The minimum possible time of arrival difference is given by a source at the centroid of the nodes. I calculate the centroid with centroid = np.average(self.nodes, axis = 0). Then, I find the time of arrival at each node and take the minimum.

The maximum possible time of arrival difference is given by the two furthest nodes in the network. I find this using a simple O(n^2) brute-force "algorithm". As of now, the code doesn't do much with this, except print the bounds.

Goals

1. Readability/Brevity: Of utmost importance is the readability/brevity of my code. Please, feel free to offer criticism of even the most minor grievances with regards to readability.

2. Efficiency

Code

from dataclasses import dataclass
import numpy as np
from random import randrange
import math

M = np.diag([1, 1, 1, -1])

@dataclass
class Vertexer:

    nodes: np.ndarray

    # Defaults
    v = 299792

    def __post_init__(self):
        # Calculate valid input range
        max = 0
        min = 1E+10
        centroid = np.average(self.nodes, axis = 0)
        for n in self.nodes:
            dist = np.linalg.norm(n - centroid)
            if dist < min:
                min = dist

            for p in self.nodes:
                dist = np.linalg.norm(n - p)

                if dist > max:
                    max = dist

        max /= self.v
        min /= self.v

        print(min, max)

    def errFunc(self, point, times):
        # Return RSS error
        error = 0

        for n, t in zip(self.nodes, times):
            error += ((np.linalg.norm(n - point) / self.v) - t)**2

        return error

    def find(self, times):
        def lorentzInner(v, w):
            # Return Lorentzian Inner-Product
            return np.sum(v * (w @ M), axis = -1)

        A = np.append(self.nodes, times * self.v, axis = 1)

        b = 0.5 * lorentzInner(A, A)
        oneA = np.linalg.solve(A, np.ones(4))
        invA = np.linalg.solve(A, b)

        solution = []
        for Lambda in np.roots([ lorentzInner(oneA, oneA),
                                (lorentzInner(oneA, invA) - 1) * 2,
                                 lorentzInner(invA, invA),
                                ]):
            X, Y, Z, T = M @ np.linalg.solve(A, Lambda * np.ones(4) + b)
            solution.append(np.array([X,Y,Z]))
    
        return min(solution, key = lambda err: self.errFunc(err, times))

Sample Input/Output

This (quite crude) code, which you can append directly to the code above, will "simulate" a source/node network, print the actual source coordinates, and pass the times of arrival/node coordinates to the algorithm. We work with light signals, and in kilometers.

Note that I'm aware this bit could be done much better. I'm interested only in improving the algorithm above, this code is just to demonstrate the algorithm's efficacy.

# Simulate sources to test code
#

# 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(np.array([[x_1, y_1, z_1],[x_2, y_2, z_2],[x_3, y_3, z_3],[x_4, y_4, z_4]]))
print(myVertexer.find(np.array([[t_1], [t_2], [t_3], [t_4]])))

Answer 1

Dataclass types

nodes has a type but v doesn't - it should probably be : int.

Vectorization

First of all, you should probably do dimension (shape) checking on self.nodes. You probably have assumptions about the shape of this array that we can't see.

Also, this:

    for n in self.nodes:
        dist = np.linalg.norm(n - centroid)
        if dist < min:
            min = dist

        for p in self.nodes:
            dist = np.linalg.norm(n - p)

            if dist > max:
                max = dist

really smells as if it can be further vectorized. Reading the documentation for norm, it does accept an axis, so at the very least you should be able to make a dist array in one step without the need to put it in a loop. min can similarly be applied over a specific axis on an entire ndarray.

You should be able to eliminate the inner loop using broadcasting. However, I can't be more specific than that as I don't know the shape of self.nodes.

Forced output

Don't do this -

    print(min, max)

in your post-constructor. It's not useful for headless or unsupervised execution. Instead, move all of your min/max code to a method that calculates and returns these values, so that the caller can decide what to do with them. Among other reasons, this will make unit testing nicer.

Names

errFunc should be err_func by PEP8; lorentz_inner and so on.

Tests

You're so close to having a useful unit test. The only thing missing is assert. For this to work, you may have to give up calling randrange and instead choose known, easily-verifiable values.