alpha-beta-tracker/main.cpp at main · hexpad/alpha-beta-tracker · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
#include <iostream>
#include <fstream>

class AlphaBetaTracker
{
private:
    double alpha, beta;

    double x, y;
    double vx, vy;

    double lastT;
    bool started;

public:
    AlphaBetaTracker(double a, double b)
        : alpha(a),
        beta(b),
        x(0.0),
        y(0.0),
        vx(0.0),
        vy(0.0),
        lastT(0.0),
        started(false)
    {

    }

    void update(double t, double mx, double my)
    {
        // first data
        if (!started)
        {
            x = mx;
            y = my;
            lastT = t;
            started = true;
            return;
        }

        double dt = t - lastT;
        if (dt <= 0)
            return;

        // prediction
        double px = x + vx * dt;
        double py = y + vy * dt;

        // error
        double ex = mx - px;
        double ey = my - py;

        // update position
        x = px + alpha * ex;
        y = py + alpha * ey;

        // update velocity
        vx = vx + (beta / dt) * ex;
        vy = vy + (beta / dt) * ey;

        lastT = t;
    }

    void predict(double sec, double& fx, double& fy)
    {
        fx = x + vx * sec;
        fy = y + vy * sec;
    }

    double getX() { return x; }
    double getY() { return y; }
    double getVx() { return vx; }
    double getVy() { return vy; }
};

int main()
{
    double alpha, beta;
    std::cout << "alpha: ";
    std::cin >> alpha;
    std::cout << "beta: ";
    std::cin >> beta;

    AlphaBetaTracker tracker(alpha, beta);

    std::ifstream file("measurements.txt");
    if (!file)
    {
        std::cout << "file error\n";
        return 1;
    }

    std::ofstream out("tracker_output.csv");
    out << "t,mx,my,x,y,vx,vy,fx,fy\n";

    double t, mx, my;

    while (file >> t >> mx >> my)
    {
        tracker.update(t, mx, my);

        double fx, fy;
        tracker.predict(5.0, fx, fy);

        out << t << ","
            << mx << "," << my << ","
            << tracker.getX() << "," << tracker.getY() << ","
            << tracker.getVx() << "," << tracker.getVy() << ","
            << fx << "," << fy << "\n";
    }

    std::cout << "done\n";
    return 0;
}