Background.
I've written a Python implementation of an algorithm I've formulated to solve the Time Difference of Arrival (TDoA) multilateration problem in the case of $3$ dimensions and $3$ receivers ($\mathrm{3D/3}$). That is, given the coordinates of $3$ receivers, the velocity of some signal, and the time of signal arrival at each receiver, I wish to determine the coordinates of the source.
While a solution is relatively trivial in the $\mathrm{2D/3}$ or $\mathrm{3D/4}$ case, determining a solution in the $\mathrm{3D/3}$ case is difficult as the underlying system of equations is underdetermined. You may wish to read more here or here.
My Solution.
In my solution, I begin by modeling the setup as the system of equations:
$$[1] \;\;\;\;(x_i - x)^2 + (y_i - y)^2 + (z_i - z)^2 - v^2(t_i - t)^2 = (x_j - x)^2 + (y_j - y)^2 + (z_j - z)^2 + v^2(t_j - t)^2$$
Where $\langle x,y,z\rangle$ denotes the coordinates of the source, $\langle x_r,y_r,z_r\rangle$ the coordinates of receiver $r$, $t_r$ the time of arrival at $r$, $v$ the magnitude of the signal velocity, and $t$ the "true" emission time.
With three receivers, we have three independent equations of the form $[1]$ ($1/2$, $1/3$, and $2/3$).
I pass these equations to the SymPy package's symbolic solver solve(). While an exact (or even approximal/numerical) solution to the above system is not possible, I am able to constrain the solution to a plane, given as $x$ in terms of $y$ and $z$. I'm also able to determine $t$ as a function of $x,y$ and $z$.
I then simply iterate over all possible points in the plane, compute the time taken for a signal from that point to arrive at each receiver, then calculate the $RSS$ error, which is defined as:
$$(a_1 - t_1)^2 + (a_2 - t_2)^2 + (a_3 - t_3)^2$$
...where $a_i$ denotes the computed time of arrival at receiver $i$.
That point with least $RSS$ error gives the solution.
Problem.
Qualitatively, this works surprisingly well $1/2$ to $3/4$ of the time, giving solutions that I can eyeball as reasonably close to or virtually precisely the true receiver location, in both simulated and experimental data. The rest of the time, however, it's not even close.
I began to look into this further. In these cases, the $RSS$ error of the solution is significantly higher than the $RSS$ error computed for a signal arriving from the known receiver position. For "simulated sources", at least, (where I simply choose a random point in space, compute the time taken for a signal from that point to reach each receiver, and feed those times to the algorithm) intuitively, it seems to me that the $RSS$ error should be "$0$", no (there will be an offset due to $t$)?
For example, for a "simulated source" at coordinates $\langle 50, 51, 82\rangle$, the code finds the solution $\langle 7.94985768512192, 7, 4 \rangle$ with $RSS$ error $4e-10$. The $RSS$ error in this case for a signal sent from the known receiver coordinates is $1.5e-7$... significantly larger than I would've anticipated.
In those cases where a solution is found successfully, the $RSS$ error is typically on the order of $1e-11$ to $1e-13$.
What can be done to improve the efficacy of this algorithm? Perhaps there's a better way to define my $RSS$ error?
I can verify that, in those cases where a "good" solution is not found, the plane determined by SymPy does indeed include the true receiver location. Hence, the issue seems to be with minimizing the $RSS$ error.
Code.
If it is somehow useful, here's the code in question. It is dependent upon SymPy, and some standard Python modules. It is admittedly still a bit messy.
import sympy as sp
import math
from dataclasses import dataclass
x,y,z,t = sp.symbols('x,y,z,t', real=True)
c = 299792
@dataclass
class Vertexer:
roc: list
def err(self, x, y, z, data):
arrival_at_i = math.sqrt((x - self.roc[0][0])**2 + (y - self.roc[0][1])**2 + (z - self.roc[0][2])**2) / c
arrival_at_j = math.sqrt((x - self.roc[1][0])**2 + (y - self.roc[1][1])**2 + (z - self.roc[1][2])**2) / c
arrival_at_k = math.sqrt((x - self.roc[0][0])**2 + (y - self.roc[0][1])**2 + (z - self.roc[0][2])**2) / c
return (arrival_at_i - data[0])**2 + (arrival_at_j - data[1])**2 + (arrival_at_k - data[2])**2
def find(self, data, xm, ym):
eq1=sp.Eq((self.roc[0][0] - x)**2 + (self.roc[0][1] - y)**2 + (self.roc[0][2] - z)**2 - c**2 * (data[0] - t)**2, (self.roc[1][0] - x)**2 + (self.roc[1][1] - y)**2 + (self.roc[1][2] - z)**2 - c**2 * (data[1] - t)**2)
eq2=sp.Eq((self.roc[0][0] - x)**2 + (self.roc[0][1] - y)**2 + (self.roc[0][2] - z)**2 - c**2 * (data[0] - t)**2, (self.roc[2][0] - x)**2 + (self.roc[2][1] - y)**2 + (self.roc[2][2] - z)**2 - c**2 * (data[2] - t)**2)
eq3=sp.Eq((self.roc[1][0] - x)**2 + (self.roc[1][1] - y)**2 + (self.roc[1][2] - z)**2 - c**2 * (data[1] - t)**2, (self.roc[2][0] - x)**2 + (self.roc[2][1] - y)**2 + (self.roc[2][2] - z)**2 - c**2 * (data[2] - t)**2)
soln = sp.solve((eq1,eq2,eq3))
fx = 0; fy = 0; fz = 0; px = 0; err1 = 100; err2 = 0
for i in range(-100, 100):
for j in range(-100, 100):
px = soln[x].subs([(y, i), (z, j)])
err2 = self.err(soln[x].subs([(y, i), (z, j)]), i, j, data)
if(i == xm and j == ym):
print(err2)
if(err2 < err1):
err1 = err2
fx = px; fy = i; fz = j
print('soln', fx,fy,fz, err1)
Usage:
myVertexer = Vertexer(myReceiverCoordinatesList)
myVertexer.find(myArrivalTimeList, knownSourceX, knownSourceY)
You have $4$ unknowns, which are $x, y, z, t$, and you have three equations:
$ (x - x_1)^2 + (y - y_1)^2 + (z - z_1)^2 = v^2 (t_1 - t)^2 \hspace{48pt}(1)$
$ (x - x_2)^2 + (y - y_2)^2 + (z - z_2)^2 = v^2 (t_2 - t)^2 \hspace{48pt}(2)$
$ (x - x_3)^2 + (y - y_3)^2 + (z - z_3)^2 = v^2 (t_3 - t)^2 \hspace{48pt}(3)$
Subtract $(2)$ from $(1)$ and $(3)$ from $(1)$:
$ -2 x (x_1 - x_2) - 2 y (y_1 - y_2) - 2 z (z_1 - z_2) = v^2 (t_1^2 - t_2^2) - 2 v^2 t (t_1 - t_2) \hspace{48pt}(4)$
$ -2 x (x_1 - x_3) - 2 y (y_1 - y_3) - 2 z (z_1 - z_3) = v^2 (t_1^2 - t_3^2) - 2 v^2 t (t_1 - t_3) \hspace{48pt}(4)$
A standard linear algebra package will solve this linear system of equations of $2$ equations in $4$ variables, and give out the solution as
$ (x, y, z, t) = V_0 + \alpha V_1 + \beta V_2 = V_0 + V u \hspace{48pt}(6) $
where $V = [V_1, V_2] $ and $u = [\alpha, \beta]^T $
Now, equation $(1)$ can be written compactly as
$ (r - r_0)^T Q (r - r_0) = 0 \hspace{48pt} (7)$
Where, $r = [x, y, z, t]^T$, $r_0 = [x_1, y_1, z_1, t_1]^T $ and
$Q = \begin{bmatrix} 1 && 0 && 0 && 0 \\ 0 && 1 && 0 && 0 \\ 0 &&0 && 1 && 0 \\ 0 && 0 && 0 && -v^2 \end{bmatrix} $
Substituting $(6)$ into $(7)$, one obtains,
$ (V_0 - r_0 + V u)^T Q (V_0 - r_0 + V u) = 0 \hspace{48pt} (8)$
And this equation relates $\alpha$ and $\beta$ the free parameters of $(6)$.
There is an infinite number of possible solutions and they are equally good. I don't think one can find the true emission time or the true source location.