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.
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,
# 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
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.
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.
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.
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))
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]])))