Task
This code accomplishes the Time Difference of Arrival (TDoA) multilateration problem (see) using gradient descent (known otherwise as steepest descent).
Goal
I'm looking to:
a) Improve speed: In practice, the algorithm will need to accomplish localizations 'on the fly'. Currently, it takes about a minute or so to perform one, whereas I'd like to perform thousands to hundreds of thousands in a reasonable time frame (unfortunately, I cannot put a number on that right now).
b) Improve readability: The code will need to be read many times over, and every ounce counts. The code should be as brief as possible (without sacrificing readability, of course) and easy to follow.
And, of course, if I've overlooked some edge case, etc, that's important as well.
import random
import math
from dataclasses import dataclass
@dataclass
class Vector:
"""Simple vector class to avoid dependencies, could
easily be replaced with Numpy array, e.g."""
x: float
y: float
z: float
def __add__(self, operand):
return Vector(self.x + operand.x,
self.y + operand.y, self.z + operand.z)
def __sub__(self, operand):
return Vector(self.x - operand.x, self.y - operand.y, self.z - operand.z)
def __rmul__(self, scalar):
return Vector(self.x * scalar, self.y * scalar,
self.z * scalar)
def __truediv__(self, scalar):
return Vector(self.x / scalar, self.y / scalar,
self.z / scalar)
def __abs__(self):
return math.sqrt(pow(self.x, 2) + pow(self.y, 2) + pow(self.z, 2))
@dataclass
class Localize:
"""Newton-Raphson localization class."""
a: Vector
b: Vector
c: Vector
d: Vector
@staticmethod
def tend(y, q, dxq):
dyq = abs(y - q)
return (dxq - dyq) * (y - q) / dyq
def find(self, dxa, dxb, dxc, dxd, gamma=1e-3, precision=1e-10, max=int(1e+10)):
y = Vector(0, 0, 0)
for i in range(max):
f = self.tend(y, self.a, dxa) + self.tend(y, self.b, dxb) + self.tend(y, self.c, dxc) + self.tend(y, self.d,
dxd)
y += gamma * f
if abs(f) <= precision:
return y
raise Exception('Max. iterations insufficient.')
def random_point():
return Vector(random.random(), random.random(), random.random())
def test_error():
a = random_point()
b = random_point()
c = random_point()
d = random_point()
x = random_point()
localize = Localize(a, b, c, d)
y = localize.find(abs(x - a), abs(x - b), abs(x - c), abs(x - d))
return abs(x - y)
print(test_error())