I’m porting a C++ approximation search algorithm to Python. The approximation search is designed to solve a localization in 2D space. For some reason, while my C++ code works great, my Python code produces incorrect results. For the example below (I’ve included both the C++ code, which will likely convey the algorithmic process better than I could hope to in words, as well as the Python code), the expected output is approximately [1417, 2905]. The output from the C++ code is close (to within a few hundredths), while the Python code produces [0, 1984.557760000002].
What I’ve tried:
Admittedly, my Python is a bit rusty. However, I’ve tried stepping through this code with both pdb and a GUI debugger, and I’m struggling to determine where the issue occurs. Everything– seems –to work as expected up to the end, yet I still get an incorrect result. It could well be that I’m missing something, though. I’m not especially experiences in Python debugging.
I’ve also tried simply sprinkling in print statements to perform value checks throughout the code. Very crude, I know, but sometimes I find it helpful. Here, again, I can’t identify anything out of the ordinary. I’m really quite stumped.
The (functioning) C++ code
#define c 299792458
#define STATN01x 3000.00
#define STATN01y 3600.00
#define STATN02x 2100.00
#define STATN02y 2100.00
#define STATN03x 960.00
#define STATN03y 3600.00
class approx{
public:
double a, aa, ai, af, da, *e, e0;
int i, n;
bool done, stop;
void init(double _ai, double _af, double _da, int _n, double *_e){
if(_ai<=_af) { ai = _ai; af = _af; }
else { ai = _af; af = _ai; }
da = fabs(_da);
n = _n ;
e = _e ;
e0 = -1.0;
i = 0; a = ai; aa=ai;
done = false; stop = false;
}
void step(){
if((e0 < 0.0) || (e0 > *e)) { e0 = *e; aa = a; }
if(stop){
i++;
if (i >= n) { done = true; a = aa; return; }
ai = aa - fabs(da);
af = aa + fabs(da);
a = ai; da *= 0.1;
ai += da; af -= da;
stop = false;
}
else{
a += da;
if(a > af) { a = af; stop = true; }
}
}
};
void compute(double toaSTATN01, double toaSTATN02, double toaSTATN03){
const int n = 3;
double receivers[n][3] = { {STATN01x, STATN01y, toaSTATN01},
{STATN02x, STATN02y, toaSTATN02},
{STATN03x, STATN03y, toaSTATN03} };
// Normalizes times into deltas from baseline
double baseline = std::min(std::min(receivers[0][2], receivers[1][2]), receivers[2][2]);
for(int i = 0; i < n; ++i){
receivers[i][2] -= baseline;
}
// Fit position
approx ax, ay;
double x, y;
double error, dt[n];
for(ax.init(0.0, 100000.0, 32.0, 6, &error); !ax.done; ax.step()){
for(ay.init(0, 100000.0, 32.0, 6, &error); !ay.done; ay.step()){
for(int i = 0; i < n; ++i){
x = receivers[i][0] - ax.a;
y = receivers[i][1] - ay.a;
baseline = sqrt( pow(x,2) + pow(y,2) );
dt[i] = baseline / c;
}
baseline = std::min(std::min(dt[0],dt[1]),dt[2]);
for(int i = 0; i < n; ++i){
dt[i] -= baseline;
}
error = 0.0;
for(int i = 0; i < n; ++i) error += fabs(receivers[i][2] - dt[i]);
}
}
std::cout << std::setprecision(500) << ax.a << " " << ay.aa << "n";
}
int main(){
compute(0.00000299321, 0.000000747190328,0);
}
Note: this
C++code is designed to be run in theClinginterpreter, where it is an entirely valid program. Some small changes may be necessary to compile this.
The Python code
import math
from dataclasses import dataclass
@dataclass
class Approximate:
ai: float
af: float
da: float
n : int
aa: float # = self.ai
a : float # = self.ai
e : float = 0
e0: float = -1.0
i : int = 0
done: bool = False
stop: bool = False
def step(self):
if (self.e0 < 0.0) or (self.e0 > self.e):
self.e0 = self.e
self.aa = self.a
if (self.stop):
self.i += 1
if (self.i >= self.n):
self.done = True
self.a = self.aa
return
self.ai = self.aa - self.da
self.af = self.aa + self.da
self.a = self.ai
self.da *= 0.1
self.ai += self.da
self.af -= self.da
self.stop = False
else:
self.a += self.da
if (self.a > self.af):
self.a = self.af
self.stop = True
def localize(recv):
ax = Approximate(0, 5000, 32, 6, 0, 0)
ay = Approximate(0, 5000, 32, 6, 0, 0)
error = 0
dt = [0, 0, 0]
x = 0
y = 0
a = 0
c = 299800000 # Speed of light
baseline = 0
while not ax.done:
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
basline = min(dt[0], dt[1], dt[2])
for j in range(3):
dt[j] -= basline
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)
# 1417 2905
localize([[3000, 3600, 0.00000299321],
[2100, 2100, 0.000000747190328],
[960, 3600, 0]])
I’ve made some simplifications here in the interest of keeping the
Pythoncode simple and brief, but nothing that should change the results, I think.
Please, let me know if I can provide any additional information that may assist in solving the problem. I’ve decided to leave it here, as I don’t want to overwhelm the reader.
Also, attribution on the way for the C++ code. While I’ve written it, it was heavily inspired by another code, which is deserving of attribution.
Source: Windows Questions C++
