Python approximation search algorithm not producing expected results

  algorithm, approximation, c++, physics, python

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 the Cling interpreter, 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 Python code 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++

LEAVE A COMMENT