Solving the TDoA multilateration problem in 3-dimensions

Question

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

Answer 1

I'd recommend building up your confidence when it comes to using numpy broadcasting. For instance, in your example usage code, you could greatly simplify it (as well as speed it up) by using something like this:

import numpy as np

nodes = np.random.random(size=(7, 3)) * 1000
source = np.random.random(size=(1, 3)) * 1000

# Set signal velocity
c = 299792 # km/s

# Generate simulated source
times = np.sum(np.sqrt((nodes - source)**2), axis=1) / c

print('Actual Transmitter Coordinates:', source)

myVertexer = Vertexer(nodes, c)

myVertexer.Find(times)

^Your Vertexer.Objective method could also be rewritten using broadcasting.

Your code is using odd capitalisation, which breaks the python PEP8 standard. Your methods, variables and functions should be lower_case_with_underscores, and only classes should be CapsCase.

Also, you can improve the naming here a bit. Your Vertexer has a very abstract name, when in reality it's pretty dedicated to this one task, I'd call it 'PositionEstimator' or something.

More fundamentally, if you're looking to make more complicated code using arrays, it might be worth investigating a labelled array library like xarray. Xarray is built on top of numpy, and is fully compatible with it, but provides support for labelling array dimensions, and smart broadcasting by both dimension and coordinate. This means you're no longer reliant on axes being in a certain order, and you'll get more helpful logging and error behaviour.

import xarray as xr

nodes = xr.DataArray(np.random.random(size=(7, 3)) * 1000, 
                     dims=['node_id', 'cartesian_coord'], 
                     coords={'cartesian_coord': list('xyz')})

source = xr.DataArray(np.random.random(size=3) * 1000,
                      dims=['cartesian_coord'],
                      coords={'cartesian_coord': list('xyz')})


def cartesian_distance(a, b):
    return np.sqrt((a - b)**2).sum('cartesian_coord')

times = cartesian_distance(nodes, source) / c
print("Nodes:", nodes)
print("Source:", source)
print("Nodes - Source:", nodes - source)
print("Times:", times)

Example output:

Nodes: <xarray.DataArray (node_id: 7, cartesian_coord: 3)>
array([[7.19631700e+01, 9.75777191e+02, 2.70351564e+02],
       [1.87056352e+02, 4.93631896e+02, 2.96245852e+02],
       [6.62286796e+02, 6.17374863e+02, 4.28134475e+02],
       [8.59948126e+02, 9.27239633e+01, 2.76307889e+02],
       [3.01367974e+02, 8.51093936e+02, 6.27013266e+02],
       [4.35597652e+02, 5.09113939e-01, 5.55342998e+02],
       [2.48227291e+02, 6.42566454e+02, 7.95675439e+02]])
Coordinates:
  * cartesian_coord  (cartesian_coord) <U1 'x' 'y' 'z'
Dimensions without coordinates: node_id
Source: <xarray.DataArray (cartesian_coord: 3)>
array([265.79108707, 835.0866561 , 165.15837507])
Coordinates:
  * cartesian_coord  (cartesian_coord) <U1 'x' 'y' 'z'
Nodes - Source: <xarray.DataArray (node_id: 7, cartesian_coord: 3)>
array([[-193.82791703,  140.69053451,  105.19318859],
       [ -78.73473516, -341.45476012,  131.0874766 ],
       [ 396.49570904, -217.71179329,  262.97609971],
       [ 594.15703916, -742.36269282,  111.14951415],
       [  35.57688699,   16.00728038,  461.85489058],
       [ 169.80656529, -834.57754216,  390.18462303],
       [ -17.56379568, -192.52020247,  630.51706397]])
Coordinates:
  * cartesian_coord  (cartesian_coord) <U1 'x' 'y' 'z'
Dimensions without coordinates: node_id
Times: <xarray.DataArray (node_id: 7)>
array([0.00146672, 0.00183886, 0.00292597, 0.00482891, 0.00171265,
       0.00465179, 0.00280395])
Dimensions without coordinates: node_id

Answer 2

Minor simplifications possible in your LinEst (which should be called lin_est by PEP8), such as pre-computing c**2 and multiplying it in place.

reshape(-1, 1) is not needed because the sum is already flat.

Rather than np.sqrt of a sum of squares, use np.linalg.norm.

The for in objective needs to go away, and be replaced with the vectorised equivalent.

Don't print in your business logic class.

Your linear estimation step is fast and to my eye looks reasonably accurate. The COBYLA step, by comparison, is slow, offers little change, and seems to break the fourth (t) coordinate. So I'm not sure that it offers a lot of value.

Don't write c yourself; take it from scipy.constants.

Don't use randrange; use the Numpy generator. Also, your quantities are better-represented as being continuous - from uniform() - rather than integers.

Your test data generation needs to be vectorised.

Suggested

from dataclasses import dataclass

import scipy
from numpy.random import default_rng
from scipy.optimize import minimize
import numpy as np


@dataclass
class Vertexer:
    roc: np.ndarray
    c: float

    def lin_est(self, times: np.ndarray) -> np.ndarray:
        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)
        c2 = self.c**2
        A[:, -1] *= c2
        b[:, -1] *= c2
        b = b.sum(axis=1)

        return np.linalg.lstsq(A, b, rcond=None)[0]

    def objective(self, var: np.ndarray, times: np.ndarray) -> float:
        norms = np.linalg.norm(var[:3] - self.roc, axis=1)
        addends = norms + self.c*(var[3] - times)
        chi2 = addends.sum()
        return chi2

    def find(self, init: np.ndarray, times: np.ndarray) -> np.ndarray:
        res = minimize(fun=self.objective, x0=init, args=times, method='COBYLA', options={'maxiter': 1e5})
        return res.x.flatten()


def main() -> None:
    rand = default_rng(seed=0)
    nodes = rand.uniform(high=1000, size=(7, 3))
    vel = scipy.constants.c / 1e3  # km/s
    my_vertexer = Vertexer(nodes, vel)

    source = rand.uniform(high=1000, size=3)
    print('Transmitter coordinates -')
    print('Actual:', source)


    times = np.linalg.norm(source - nodes, axis=1)/vel
    init = my_vertexer.lin_est(times)
    print('Initial-estimate:', init)

    x = my_vertexer.find(init, times)
    print('Estimated: ', x)


if __name__ == '__main__':
    main()

Output

Transmitter coordinates -
Actual: [124.2832765  670.62441469 647.18951157]
Initial-estimate: [1.23095131e+02 8.14941028e+02 7.35096887e+02 4.74328837e-03]
Estimated:  [   124.21993466    814.8694385     734.97854933 -99994.99525597]