Line data Source code
1 : /*
2 : * Copyright (C) 2024 robinAZERTY [https://github.com/robinAZERTY]
3 : *
4 : * This file is part of ESP32AlgebraFilters library.
5 : *
6 : * ESP32AlgebraFilters library is free software: you can redistribute it and/or modify
7 : * it under the terms of the GNU General Public License as published by
8 : * the Free Software Foundation, either version 3 of the License, or
9 : * (at your option) any later version.
10 : *
11 : * ESP32AlgebraFilters library is distributed in the hope that it will be useful,
12 : * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 : * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 : * GNU General Public License for more details.
15 : *
16 : * You should have received a copy of the GNU General Public License
17 : * along with ESP32AlgebraFilters library. If not, see <https://www.gnu.org/licenses/>.
18 : */
19 :
20 : #define EKF_CPP
21 : #include "ekf.hpp"
22 :
23 : template <size_t x_dim, size_t u_dim, size_t c_dim, size_t z_num, typename T>
24 20 : Ekf<x_dim, u_dim, c_dim, z_num, T>::Ekf()
25 : {
26 2 : X.fill(0);
27 2 : P.fill(0);
28 2 : Cov_U.fill(0);
29 2 : dx.fill(1e-4);
30 2 : du.fill(1e-4);
31 2 : dc.fill(1e-4);
32 2 : }
33 :
34 : template <size_t x_dim, size_t u_dim, size_t c_dim, size_t z_num, typename T>
35 4496 : inline void Ekf<x_dim, u_dim, c_dim, z_num, T>::finite_diff_Fx(const size_t i)
36 : {
37 4496 : const T eps = dx[i];
38 4496 : prev_X[i] += eps;
39 4496 : Vector<T> dX(&Fx_val_T[i * x_dim], x_dim, true);
40 4496 : dX.holdSub(*f(prev_X, U, C).release(), X);
41 4496 : dX /= eps;
42 4496 : prev_X[i] -= eps;
43 4496 : }
44 :
45 : template <size_t x_dim, size_t u_dim, size_t c_dim, size_t z_num, typename T>
46 2498 : inline void Ekf<x_dim, u_dim, c_dim, z_num, T>::finite_diff_Fu(const size_t i)
47 : {
48 2498 : const T eps = du[i];
49 2498 : U[i] += eps;
50 2498 : Vector<T> dX(&Fu_val_T[i * x_dim], x_dim, true);
51 2498 : dX.holdSub(*f(prev_X, U, C).release(), X);
52 2498 : dX /= eps;
53 2498 : U[i] -= eps;
54 2498 : }
55 :
56 : template <size_t x_dim, size_t u_dim, size_t c_dim, size_t z_num, typename T>
57 422 : inline void Ekf<x_dim, u_dim, c_dim, z_num, T>::finite_diff_H(const size_t z_idx, const size_t i)
58 : {
59 422 : const T eps = dx[i];
60 422 : X[i] += eps;
61 422 : Vector<T> dz(&H_val[z_idx][i * z_dim[z_idx]], z_dim[z_idx], true);
62 422 : dz.holdSub(*h[z_idx](X, C).release(), h_val[z_idx]);
63 422 : dz /= eps;
64 422 : X[i] -= eps;
65 422 : }
66 :
67 : template <size_t x_dim, size_t u_dim, size_t c_dim, size_t z_num, typename T>
68 5 : void Ekf<x_dim, u_dim, c_dim, z_num, T>::setMeasurementFunction(Vector_f2<T> h, size_t z_dim, size_t z_idx)
69 : {
70 5 : this->h[z_idx] = h;
71 5 : this->z_dim[z_idx] = z_dim;
72 5 : this->H_val[z_idx].resize(x_dim, z_dim);
73 5 : this->S_inv[z_idx].resize(z_dim);
74 5 : }
75 :
76 : template <size_t x_dim, size_t u_dim, size_t c_dim, size_t z_num, typename T>
77 1499 : inline void Ekf<x_dim, u_dim, c_dim, z_num, T>::predict()
78 : {
79 :
80 1499 : if (f == nullptr)
81 0 : throw "Ekf::predict() state transition function not set";
82 :
83 :
84 1499 : prev_X = X;
85 :
86 1499 : X = f(prev_X, U, C);
87 :
88 1499 : if (Fx != nullptr)
89 0 : Fx_val_T = Fx(prev_X, U, C);
90 : else
91 1499 : this->finite_diff_Fx();
92 1499 : if (Fu != nullptr)
93 0 : Fu_val_T = Fu(prev_X, U, C);
94 : else
95 1499 : this->finite_diff_Fu();
96 :
97 1499 : P.holdMul(*(Fx_val_T.T * P).release(), Fx_val_T);
98 1499 : P.addMul(*(Fu_val_T.T * Cov_U).release(), Fu_val_T);
99 1499 : }
100 :
101 : template <size_t x_dim, size_t u_dim, size_t c_dim, size_t z_num, typename T>
102 143 : inline void Ekf<x_dim, u_dim, c_dim, z_num, T>::update(const Vector<T> &Z, const symMatrix<T> &R, const size_t z_idx)
103 : {
104 143 : compute_H_P(z_idx);
105 143 : computeResidualPrecision(R, z_idx);
106 143 : computeResidual(Z, z_idx);
107 143 : updateStateCovariance(z_idx);
108 143 : }
109 :
110 : template <size_t x_dim, size_t u_dim, size_t c_dim, size_t z_num, typename T>
111 143 : void Ekf<x_dim, u_dim, c_dim, z_num, T>::compute_H_P(const size_t z_idx)
112 : {
113 143 : if (h[z_idx] == nullptr)
114 0 : throw "Ekf::update() measurement function not set";
115 :
116 143 : h_val[z_idx] = h[z_idx](X, C); // measurement prediction
117 :
118 : // measurement function Jacobian computation
119 143 : if (H[z_idx] == nullptr)
120 143 : this->finite_diff_H(z_idx);
121 : else
122 0 : H_val[z_idx] = H[z_idx](X, C);
123 :
124 143 : H_P[z_idx].holdMul(H_val[z_idx].T, P); // common expression (calculus factorisation)
125 143 : pre_S[z_idx].holdMul(H_P[z_idx], H_val[z_idx]); // pre computation of residual covariance (R not added yet)
126 143 : }
127 :
128 : template <size_t x_dim, size_t u_dim, size_t c_dim, size_t z_num, typename T>
129 143 : inline void Ekf<x_dim, u_dim, c_dim, z_num, T>::computeResidualPrecision(const symMatrix<T> &R, const size_t z_idx)
130 : {
131 : // residual precision
132 143 : S_inv[z_idx].holdAdd(pre_S[z_idx], R);
133 143 : S_inv[z_idx].holdInv(S_inv[z_idx], false);
134 143 : }
135 :
136 : template <size_t x_dim, size_t u_dim, size_t c_dim, size_t z_num, typename T>
137 143 : inline void Ekf<x_dim, u_dim, c_dim, z_num, T>::computeResidual(const Vector<T> &Z, const size_t z_idx)
138 : {
139 143 : if (z_dim[z_idx] != Z.size())
140 0 : throw "Ekf::update() measurement dimension mismatch";
141 :
142 143 : y[z_idx].holdSub(Z, h_val[z_idx]);
143 143 : }
144 :
145 : template <size_t x_dim, size_t u_dim, size_t c_dim, size_t z_num, typename T>
146 143 : inline void Ekf<x_dim, u_dim, c_dim, z_num, T>::updateStateCovariance(const size_t z_idx)
147 : {
148 143 : K.holdMul(H_P[z_idx].T, S_inv[z_idx]); // Kalman gains
149 143 : X.addMul(K, y[z_idx]); // update the state
150 143 : P.subMul(K, H_P[z_idx]); // update the state covariance
151 143 : }
|
