This repository has been archived by the owner on May 26, 2020. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 0
/
EarthPositionFilter.h
109 lines (101 loc) · 2.88 KB
/
EarthPositionFilter.h
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
#ifndef EARTH_POSITION_FILTER_H
#define EARTH_POSITION_FILTER_H
#include <Arduino.h>
#define G_ACCEL_M_S2 9.807
class EarthPositionFilter
{
protected:
#ifndef STM32_CORE_VERSION
// state variables (position, velocity), (x)
int64_t pos_mm;
double vel_mm_per_sec;
// state covariance matrix, (P)
double P[2][2];
// state transition matrix, (F)
// { {1, dt}, {0, 1} }
double dt;
// control input matrix, (B)
// { 0.5*a*dt^2, dt }
double dt_dt;
// process noise matrix, (Q)
// { {0.25*dt^4, 0.5*dt^3}, {0.5*dt^3, dt^2} } * process_variance
double dt_dt_dt_2, dt_dt_dt_dt_4;
double proc_var;
// measurement matrix, (H)
// { 1, 0 }
// measurement noise matrix, (R)
double R;
#else
// state variables (position, velocity), (x)
double pos_m;
double vel_m_per_sec;
// state covariance matrix, (P)
double P[2][2];
// state transition matrix, (F)
// { {1, dt}, {0, 1} }
double dt;
// control input matrix, (B)
// { 0.5*a*dt^2, dt }
double dt_dt;
// process noise matrix, (Q)
// { {0.25*dt^4, 0.5*dt^3}, {0.5*dt^3, dt^2} } * process_variance
double dt_dt_dt_2, dt_dt_dt_dt_4;
double proc_var;
// measurement matrix, (H)
// { 1, 0 }
// measurement noise matrix, (R)
double R;
#endif
public:
#ifndef STM32_CORE_VERSION
EarthPositionFilter() :
pos_mm{0}, vel_mm_per_sec{0.0},
P{100000000.0, 0.0, 0.0, 100000000.0},
proc_var{9000000.0}, R{25000000.0} {}
#else
EarthPositionFilter() :
pos_m{0.0}, vel_m_per_sec{0.0},
P{100.0, 0.0, 0.0, 100.0},
proc_var{9.0}, R{25.0} {}
#endif
#ifndef STM32_CORE_VERSION
// getter functions
int64_t get_pos_mm();
double get_vel_mm_per_sec();
void get_P(double (&)[2][2]);
double get_P(uint8_t, uint8_t);
double get_deltat();
double get_proc_var();
double get_R();
// setter functions
void set_pos_mm(int64_t);
void set_vel_mm_per_sec(double);
void set_P(const double (&&)[2][2]);
void set_deltat(double);
void set_proc_var(double);
void set_R(double);
// predict and update functions
void predict(double);
void update(int64_t);
#else
// getter functions
double get_pos_m();
double get_vel_m_per_sec();
void get_P(double (&)[2][2]);
double get_P(uint8_t, uint8_t);
double get_deltat();
double get_proc_var();
double get_R();
// setter functions
void set_pos_m(double);
void set_vel_m_per_sec(double);
void set_P(const double (&&)[2][2]);
void set_deltat(double);
void set_proc_var(double);
void set_R(double);
// predict and update functions
void predict(double);
void update(double);
#endif
};
#endif // EARTH_POSITION_FILTER_H