-
Notifications
You must be signed in to change notification settings - Fork 24
/
Copy pathekf.cpp
148 lines (129 loc) · 6.38 KB
/
ekf.cpp
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
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
/*************************************************************************************************************
* Class for Discrete Extended Kalman Filter
* The system to be estimated is defined as a discrete nonlinear dynamic dystem:
* x(k) = f[x(k-1), u(k-1)] + v(k) ; x = Nx1, u = Mx1
* y(k) = h[x(k)] + n(k) ; y = Zx1
*
* Where:
* x(k) : State Variable at time-k : Nx1
* y(k) : Measured output at time-k : Zx1
* u(k) : System input at time-k : Mx1
* v(k) : Process noise, AWGN assumed, w/ covariance Qn : Nx1
* n(k) : Measurement noise, AWGN assumed, w/ covariance Rn : Nx1
*
* f(..), h(..) is a nonlinear transformation of the system to be estimated.
*
***************************************************************************************************
* Extended Kalman Filter algorithm:
* Initialization:
* x(k=0|k=0) = Expected value of x at time-0 (i.e. x(k=0)), typically set to zero.
* P(k=0|k=0) = Identity matrix * covariant(P(k=0)), typically initialized with some
* big number.
* Q, R = Covariance matrices of process & measurement. As this implementation
* the noise as AWGN (and same value for every variable), this is set
* to Q=diag(QInit,...,QInit) and R=diag(RInit,...,RInit).
*
*
* EKF Calculation (every sampling time):
* Calculate the Jacobian matrix of f (i.e. F):
* F = d(f(..))/dx |x(k-1|k-1),u(k-1) ...{EKF_1}
*
* Predict x(k) through nonlinear function f:
* x(k|k-1) = f[x(k-1|k-1), u(k-1)] ...{EKF_2}
*
* Predict P(k) using linearized f (i.e. F):
* P(k|k-1) = F*P(k-1|k-1)*F' + Q ...{EKF_3}
*
* Calculate the Jacobian matrix of h (i.e. C):
* C = d(h(..))/dx |x(k|k-1) ...{EKF_4}
*
* Predict residual covariance S using linearized h (i.e. H):
* S = C*P(k|k-1)*C' + R ...{EKF_5}
*
* Calculate the kalman gain:
* K = P(k|k-1)*C'*(S^-1) ...{EKF_6}
*
* Correct x(k) using kalman gain:
* x(k|k) = x(k|k-1) + K*[y(k) - h(x(k|k-1))] ...{EKF_7}
*
* Correct P(k) using kalman gain:
* P(k|k) = (I - K*C)*P(k|k-1) ...{EKF_8}
*
*
* *Additional Information:
* - Pada contoh di atas X~(k=0|k=0) = [0]. Untuk mempercepat konvergensi bisa
* digunakan informasi plant-spesific. Misal pada implementasi Kalman Filter
* untuk sensor IMU (Inertial measurement unit) dengan X = [quaternion], dengan
* asumsi IMU awalnya menghadap ke atas tanpa rotasi: X~(k=0|k=0) = [1, 0, 0, 0]'
*
*
* See https://github.com/pronenewbits for more!
************************************************************************************************************/
#include "ekf.h"
EKF::EKF(const Matrix& XInit, const Matrix& P, const Matrix& Q, const Matrix& R,
bool (*bNonlinearUpdateX)(Matrix&, const Matrix&, const Matrix&),
bool (*bNonlinearUpdateY)(Matrix&, const Matrix&, const Matrix&),
bool (*bCalcJacobianF)(Matrix&, const Matrix&, const Matrix&),
bool (*bCalcJacobianH)(Matrix&, const Matrix&, const Matrix&))
{
/* Initialization:
* x(k=0|k=0) = Expected value of x at time-0 (i.e. x(k=0)), typically set to zero.
* P (k=0|k=0) = Identity matrix * covariant(P(k=0)), typically initialized with some
* big number.
* Q, R = Covariance matrices of process & measurement. As this implementation
* the noise as AWGN (and same value for every variable), this is set
* to Q=diag(QInit,...,QInit) and R=diag(RInit,...,RInit).
*/
this->X_Est = XInit;
this->P = P;
this->Q = Q;
this->R = R;
this->bNonlinearUpdateX = bNonlinearUpdateX;
this->bNonlinearUpdateY = bNonlinearUpdateY;
this->bCalcJacobianF = bCalcJacobianF;
this->bCalcJacobianH = bCalcJacobianH;
}
void EKF::vReset(const Matrix& XInit, const Matrix& P, const Matrix& Q, const Matrix& R)
{
this->X_Est = XInit;
this->P = P;
this->Q = Q;
this->R = R;
}
bool EKF::bUpdate(const Matrix& Y, const Matrix& U)
{
/* Run once every sampling time */
/* =============== Calculate the Jacobian matrix of f (i.e. F) =============== */
/* F = d(f(..))/dx |x(k-1|k-1),u(k-1) ...{EKF_1} */
if (!bCalcJacobianF(F, X_Est, U)) {
return false;
}
/* =========================== Prediction of x & P =========================== */
/* x(k|k-1) = f[x(k-1|k-1), u(k-1)] ...{EKF_2} */
if (!bNonlinearUpdateX(X_Est, X_Est, U)) {
return false;
}
/* P(k|k-1) = F*P(k-1|k-1)*F' + Q ...{EKF_3} */
P = F*P*(F.Transpose()) + Q;
/* =============== Calculate the Jacobian matrix of h (i.e. H) =============== */
/* H = d(h(..))/dx |x(k|k-1) ...{EKF_4} */
if (!bCalcJacobianH(H, X_Est, U)) {
return false;
}
/* =========================== Correction of x & P =========================== */
/* S = H*P(k|k-1)*H' + R ...{EKF_5} */
S = (H*P*(H.Transpose())) + R;
/* K = P(k|k-1)*H'*(S^-1) ...{EKF_6} */
Gain = P*(H.Transpose())*(S.Invers());
if (!Gain.bMatrixIsValid()) {
return false;
}
/* x(k|k) = x(k|k-1) + K*[y(k) - h(x(k|k-1))] ...{EKF_7} */
if (!bNonlinearUpdateY(Y_Est, X_Est, U)) {
return false;
}
X_Est = X_Est + (Gain * (Y - Y_Est));
/* P(k|k) = (I - K*H)*P(k|k-1) ...{EKF_8} */
P = (MatIdentity(SS_X_LEN) - (Gain*H))*P;
return true;
}