1+ #pragma once
2+
3+ #include "app_math.h"
4+
5+ /**
6+ * General Extended Kalman Filter API
7+ *
8+ * How to use:
9+ *
10+ * 1) Include in your file like this along with the dimensions
11+ * #define DIM x
12+ * #include "app_ekf.h"
13+ *
14+ * 2) Define the functions in the function pointers
15+ *
16+ * 3) Define your measurement functions in your estimator api
17+ * (They are not stored in the ekf struct which will allow multiple
18+ * measurement functions and thus multiple sensors if required)
19+ *
20+ * Note: If you have more than 2 sensors for the update/measurement step,
21+ * usually you will need the same amount of measurement functions.
22+ * Thus, the user must swap the measurement functions in the ekf struct
23+ * before calling the update step again with the new measurements
24+ * Explain why velocity estimator is an exception
25+ */
26+
27+ typedef struct
28+ {
29+ // estimations
30+ Matrix state_estimate ;
31+ Matrix covariance_estimate ;
32+ // internal variables
33+ Matrix F_jacobian ;
34+ Matrix H_jacobian ;
35+ Matrix measurement_pred ;
36+ // user implementable functions functions
37+ void (* state_transition )(Matrix * state_estimate , Matrix * prev_state , Matrix * control_input );
38+ void (* state_jacobian )(Matrix * F_jacobian , Matrix * prev_state , Matrix * control_input );
39+ void (* measurement_jacobian )(Matrix * H_jacobian , Matrix * prev_state );
40+ void (* measurement_func )(Matrix * measurement_pred , Matrix * prev_state );
41+ } EKF ;
42+
43+ static inline void predict (EKF * ekf , Matrix * prev_state , Matrix * control_input , Matrix * process_noise_cov )
44+ {
45+ Matrix temp_cov = { .mat = (float [DIM * DIM ]){ 0.0f }, .rows = DIM , .cols = DIM };
46+ Matrix F_transpose = { .mat = (float [DIM * DIM ]){ 0.0f }, .rows = DIM , .cols = DIM };
47+
48+ // state prediction
49+ ekf -> state_transition (& ekf -> state_estimate , prev_state , control_input );
50+ ekf -> state_jacobian (& ekf -> F_jacobian , prev_state , control_input );
51+
52+ // prediction covariance prediction
53+ mult (& temp_cov , & ekf -> F_jacobian , & ekf -> covariance_estimate );
54+ transpose (& F_transpose , & ekf -> F_jacobian );
55+ mult (& ekf -> covariance_estimate , & temp_cov , & F_transpose );
56+ add (& ekf -> covariance_estimate , & ekf -> covariance_estimate , process_noise_cov );
57+ }
58+
59+ /**
60+ * Updates the prediction, gives the final, corrected state and covariance estimate
61+ * with measured values given previous state and noise covariance
62+ *
63+ * measurement pred is obtained from your measurement function
64+ */
65+ static inline void update (EKF * ekf , Matrix * measurement , Matrix * prev_state , Matrix * measurement_noise_cov )
66+ {
67+ Matrix PH_transpose = { .mat = (float [DIM * DIM ]){ 0.0f }, .rows = DIM , .cols = DIM };
68+ Matrix Ky = { .mat = (float [DIM * 1U ]){ 0.0f }, .rows = DIM , .cols = 1 };
69+ Matrix H_transpose = { .mat = (float [DIM * DIM ]){ 0.0f }, .rows = DIM , .cols = DIM };
70+ Matrix S = { .mat = (float [DIM * DIM ]){ 0.0f }, .rows = DIM , .cols = DIM };
71+ Matrix S_inv = { .mat = (float [DIM * DIM ]){ 0.0f }, .rows = DIM , .cols = DIM };
72+ Matrix K = { .mat = (float [DIM * DIM ]){ 0.0f }, .rows = DIM , .cols = DIM };
73+ Matrix residual = { .mat = (float [DIM * 1U ]){ 0.0f }, .rows = DIM , .cols = 1 };
74+ Matrix KH = { .mat = (float [DIM * DIM ]){ 0.0f }, .rows = DIM , .cols = DIM };
75+ Matrix KHP = { .mat = (float [DIM * DIM ]){ 0.0f }, .rows = DIM , .cols = DIM };
76+
77+ // measurement prediction
78+ ekf -> measurement_func (& ekf -> measurement_pred , prev_state );
79+ ekf -> measurement_jacobian (& ekf -> H_jacobian , prev_state );
80+
81+ // measurement covariance prediction
82+ transpose (& H_transpose , & ekf -> H_jacobian );
83+ mult (& PH_transpose , & ekf -> covariance_estimate , & H_transpose );
84+ mult (& S , & ekf -> H_jacobian , & PH_transpose );
85+ add (& S , & S , measurement_noise_cov );
86+
87+ // kalman gain calculation
88+ inverse2x2 (& S_inv , & S );
89+ mult (& K , & PH_transpose , & S_inv );
90+
91+ // measurement residual calculation
92+ sub (& residual , measurement , & ekf -> measurement_pred );
93+
94+ // state estimation
95+ mult (& Ky , & K , & residual );
96+ add (& ekf -> state_estimate , prev_state , & Ky );
97+
98+ // covariance estimation
99+ mult (& KH , & K , & ekf -> H_jacobian );
100+ mult (& KHP , & KH , & ekf -> covariance_estimate );
101+ sub (& ekf -> covariance_estimate , & ekf -> covariance_estimate , & KHP );
102+ }
0 commit comments