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

• Coupcoup: Have you looked at numba? It limits the numpy functions you can use (I don't know off the top of my head if the ones you use would work as-is or not) but it provides Just-in-time compilation to C code that can execute significantly faster than native python code. Also your x_1...x_4 and t_1...t_4 generation could be done in one line each if you vectorize it. I didn't look in depth but guessing your for loops could be be vectorised as well if you're applying the same function to each node
• 10GeV: @Coupcoup Thanks for the suggestions! numba looks interesting, I'll have to look into it further.
• Coupcoup: As a heads up if numba works for you I wouldn't worry about vectorizing the inner for loops (still do the x_n though), numba runs basically as fast with for loops as with vectorized functions because it's converting everything to vectorized c code already
• Mark H: Your value for the speed of light is a billion times too fast. The unit for that value is km/sec.
• Eman Yalpsid: Try using distance_matrix in your __post_init__.

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.