I've ported an approximation search algorithm from C++ to Python (the logic and very nice original implementation attributable to). I then wrote a script to use this algorithm in solving a 2-dimensional localization problem (the Time Difference of Arrival problem). The 2-dimensional solution worked great. When I nest to 3-dimensions, however, the script doesn't produce expected localizations.
Note that this almost certainly isn't an issue of the mathematics of solvability. In theory, the algorithm should be extendable to any n dimensions given n+1 receivers, so long as not all n+1 receivers lie in an n-1 dimensional space. In this case, I have 4 receivers for a 3 dimensional solution.
The approximation search algorithm can be found here. I've excluded it from this post as the issue almost certainly doesn't lie with this part of the code.
What I've tried:
I've tried stepping through this with pdb and a GUI debugger. I've also tried sprinkling in print statements to perform value checks. Unfortunately, due in part to the fact that there is so much going on in terms of calculations, I'm struggling to identify precisely where the issue occurs. I have a hunch it may have something to do with where I've placed ax = Appr... and ay = Appr..., however it's only a hunch (and, I've tried many combinations of placement with no success).
(Functioning) 2-Dimensional Solution:
def localize(recv):
ax = Approximate(0, 5000, 32, 10)
ay = Approximate(0, 5000, 32, 10)
#az = Approximate(0, 5000, 32, 6)
error = 0
dt = [0, 0, 0]
c = 299800000 # Speed of light
baseline = 0
while not ax.done:
ay = Approximate(0, 5000, 32, 10)
while not ay.done:
for i in range(3):
x = recv[i][0] - ax.a
y = recv[i][1] - ay.a
baseline = math.sqrt((x * x) + (y * y))
dt[i] = baseline / c
# Normalize times into deltas from zero
baseline = min(dt[0], dt[1], dt[2])
for j in range(3):
dt[j] -= baseline
error = 0.0
error = 0.0
for k in range(3):
error += math.fabs(recv[k][2] - dt[k])
ay.e = error; ax.e = error
ay.step()
ax.step()
# Found solution
print(ax.aa)
print(ay.aa)
(Problematic) 3-Dimensional Solution:
def localize(recv):
ax = Approximate(0, 5000, 32, 10)
ay = Approximate(0, 5000, 32, 10)
az = Approximate(0, 5000, 32, 10)
error = 0
dt = [0, 0, 0, 0]
c = 299800000 # Speed of light
baseline = 0
while not ax.done:
ay = Approximate(0, 5000, 32, 10)
while not ay.done:
az = Approximate(0, 5000, 32, 10)
while not az.done:
for i in range(4):
x = recv[i][0] - ax.a
y = recv[i][1] - ay.a
z = recv[i][2] - az.a
baseline = math.sqrt((x * x) + (y * y) + (z * z))
dt[i] = baseline / c
# Normalize times into deltas from zero
basline = min(dt[0], dt[1], dt[2], dt[3])
for j in range(4):
dt[j] -= basline
error = 0.0
for k in range(4):
error += math.fabs(recv[k][3] - dt[k])
ay.e = error; ax.e = error; az.e = error
az.step()
ay.step()
ax.step()
# Found solution
print(ax.aa)
print(ay.aa)
print(az.aa)
#Localization
# x = 1765 y = 2313 z = 1753
localize([[120,145,90,0.0000002468378075465656],
[20,450,37,0.0000002433879002368936],
[10,-50,324,0.0000002433879002368936],
[67,903,45,0.0000002314851840328957]])
Predicted localization: 975.6857619200017, 811.0280894080021, 1278.482239584
Expected behavior: 1765, 2313, 753
(to within a fair degree of precision (C++ algorithm provides precision on the order of a fraction of a unit)).
Also, apologies for the messy code. It is in need of some streamlining.
EDIT:
As @jack pointed out below, the issue is almost certainly with my calculation of the error. I'm not sure what mistake I could possibly be making, however. It shouldn't be all that different from the 2-dimensional error calculation, it's a very basic minimization of summed squared errors problem. Not sure if it's a mathematical issue, or some coding issue that I've missed.
