Background.
I've written an algorithm to solve the Time Difference of Arrival (TDoA) multilateration problem in 3-dimensions. That is, given the known coordinates of N nodes (e.g. optical photo detectors), the velocity of some signal, and the time of signal arrival at each node, I wish to determine the cartesian coordinates of the source ("source reconstruction").
To do so, I'm largely following this discussion on Math.SE.
Use.
To perform source reconstruction, one must first construct an object of the Reconstructor class, passing a NumPy array of the form [[x_1, y_1, z_1], ... [x_n, y_n, z_n] (where x_n, y_n, z_n are the known coordinates of the nth node) and the signal velocity to the constructor. Then, call the Find() method, passing a list of signal arrival times of the form [t_1, ... t_n] (where t_n is the time of arrival at node n).
I first determine an initial estimate by solving the linearized system discussed in the linked post. There are n(n-1)/2 equations (combinatoric combinations of nodes). To construct these combinations, I do:
LHS = np.tile(system, (len(system), 1))
RHS = np.repeat(system, len(system), 0)
...the combinatoric combinations are given by corresponding rows of the arrays LHS and RHS (please note a change in notation... my RHS and LHS don't correspond directly to the RHS and LHS described in the linked post). I then construct A and b by:
A = 2 * (LHS - RHS)
b = np.square(LHS) - np.square(RHS)
A[:, -1] = A[:, -1] * self.c**2
b[:, -1] = b[:, -1] * self.c**2
b = np.sum(b, axis=1).reshape(-1, 1)
This initial estimate is then used as an initial condition for the "full" nonlinear solver, which minimizes the objective function discussed implicitly in the first post, and in more depth here. The objective function is aptly named Objective().
Goals.
I'm satisfied with the performance of this code (execution time, accuracy of solutions, etc). I'm looking to improve code clarity and conciseness, and to identify potential bugs. I'm also open to general suggestions for improvement.
Code.
from dataclasses import dataclass
from scipy.optimize import minimize
import numpy as np
@dataclass
class Vertexer:
roc: np.ndarray
c: float
def LinEst(self, times):
system=np.hstack((self.roc, np.atleast_2d(times).T))
LHS = np.tile(system, (len(system), 1))
RHS = np.repeat(system, len(system), 0)
A = 2 * (LHS - RHS)
b = np.square(LHS) - np.square(RHS)
A[:, -1] = A[:, -1] * self.c**2
b[:, -1] = b[:, -1] * self.c**2
b = np.sum(b, axis=1).reshape(-1, 1)
return np.linalg.lstsq(A, b, rcond=None)[0]
def Objective(self, var, times):
chi2 = 0
for i in range(len(self.roc)):
recv = self.roc[i]
chi2 = chi2 + np.sqrt((var[0] - recv[0])**2 + (var[1] - recv[1])**2 + (var[2] - recv[2])**2) - self.c * (times[i] - var[3])
return chi2
def Find(self, times):
init = self.LinEst(times)
res = minimize(self.Objective, init, args=times, method='COBYLA', options={'maxiter': 1e5})
print('The transmitter is located at: ', res.x)
Example Usage.
Example usage, as requested. I've put together some (admittedly very crude) code that generates randomized nodes and transmitter, computes signal transit times, and feeds these times/node coordinates to the algorithm.
import math
from random import randrange
# Pick nodes to be at random locations
x_1 = randrange(1000); y_1 = randrange(1000); z_1 = randrange(1000)
x_2 = randrange(1000); y_2 = randrange(1000); z_2 = randrange(1000)
x_3 = randrange(1000); y_3 = randrange(1000); z_3 = randrange(1000)
x_4 = randrange(1000); y_4 = randrange(1000); z_4 = randrange(1000)
x_5 = randrange(1000); y_5 = randrange(1000); z_5 = randrange(1000)
x_6 = randrange(1000); y_6 = randrange(1000); z_6 = randrange(1000)
x_7 = randrange(1000); y_7 = randrange(1000); z_7 = randrange(1000)
# Pick source to be at random location
x = randrange(1000); y = randrange(1000); z = randrange(1000)
# Set signal velocity
c = 299792 # km/s
# 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
t_5 = math.sqrt( (x - x_5)**2 + (y - y_5)**2 + (z - z_5)**2 ) / c
t_6 = math.sqrt( (x - x_6)**2 + (y - y_6)**2 + (z - z_6)**2 ) / c
t_7 = math.sqrt( (x - x_7)**2 + (y - y_7)**2 + (z - z_7)**2 ) / c
print('Actual Transmitter Coordinates:', 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], [x_5, y_5, z_5], [x_6, y_6, z_6], [x_7, y_7, z_7]]), c)
myVertexer.Find([t_1, t_2, t_3, t_4, t_5, t_6, t_7])
Some example output:
Actual: 572 604 45
The transmitter is located at:
[[5.66459752e+02]
[6.02872674e+02]
[5.11596944e+01]
[5.33881614e-03]]
572 604 45 -> 566.4 602.8 53.3
