mrs_lib
Various reusable classes, functions and utilities for use in MRS projects
Loading...
Searching...
No Matches
ukf.hpp
Go to the documentation of this file.
1// clang: MatousFormat
2
3#ifndef UKF_HPP
4#define UKF_HPP
5
12#ifndef UKF_H
13#include <mrs_lib/ukf.h>
14#endif
15
16#include <iostream>
17
18namespace mrs_lib
19{
20 /* constructor //{ */
21
22 template <int n_states, int n_inputs, int n_measurements>
26
27 template <int n_states, int n_inputs, int n_measurements>
28 UKF<n_states, n_inputs, n_measurements>::UKF(const transition_model_t& transition_model, const observation_model_t& observation_model, const double alpha,
29 const double kappa, const double beta)
30 : m_alpha(alpha),
31 m_kappa(kappa),
32 m_beta(beta),
33 m_Wm(W_t::Zero()),
34 m_Wc(W_t::Zero()),
35 m_transition_model(transition_model),
36 m_observation_model(observation_model)
37 {
38 assert(alpha > 0.0);
39 computeWeights();
40 }
41
42 //}
43
44 /* computeWeights() //{ */
45
46 template <int n_states, int n_inputs, int n_measurements>
48 {
49 // initialize lambda
50 /* m_lambda = double(n) * (m_alpha * m_alpha - 1.0); */
51 m_lambda = m_alpha * m_alpha * (double(n) + m_kappa) - double(n);
52
53 // initialize first terms of the weights
54 m_Wm(0) = m_lambda / (double(n) + m_lambda);
55 m_Wc(0) = m_Wm(0) + (1.0 - m_alpha * m_alpha + m_beta);
56
57 // initialize the rest of the weights
58 for (int i = 1; i < w; i++)
59 {
60 m_Wm(i) = (1.0 - m_Wm(0)) / (w - 1.0);
61 m_Wc(i) = m_Wm(i);
62 }
63 }
64
65 //}
66
67 /* setConstants() //{ */
68
69 template <int n_states, int n_inputs, int n_measurements>
70 // update the UKF constants
71 void UKF<n_states, n_inputs, n_measurements>::setConstants(const double alpha, const double kappa, const double beta)
72 {
73 m_alpha = alpha;
74 m_kappa = kappa;
75 m_beta = beta;
76
77 computeWeights();
78 }
79
80 //}
81
82 /* setTransitionModel() method //{ */
83
84 template <int n_states, int n_inputs, int n_measurements>
86 {
87 m_transition_model = transition_model;
88 }
89
90 //}
91
92 /* setObservationModel() method //{ */
93
94 template <int n_states, int n_inputs, int n_measurements>
96 {
97 m_observation_model = observation_model;
98 }
99
100 //}
101
102 /* computePaSqrt() method //{ */
103
104 template <int n_states, int n_inputs, int n_measurements>
106 {
107 // calculate the square root of the covariance matrix
108 const P_t Pa = (double(n) + m_lambda) * P;
109
110 /* Eigen::SelfAdjointEigenSolver<P_t> es(Pa); */
111 Eigen::LLT<P_t> llt(Pa);
112 if (llt.info() != Eigen::Success)
113 {
114 P_t tmp = Pa + (double(n) + m_lambda) * 1e-9 * P_t::Identity();
115 llt.compute(tmp);
116 if (llt.info() != Eigen::Success)
117 {
118 std::cout << "UKF: taking the square root of covariance during sigma point generation failed." << std::endl;
119 throw square_root_exception();
120 }
121 }
122
123 const P_t Pa_sqrt = llt.matrixL();
124 return Pa_sqrt;
125 }
126 //}
127
128 /* computeInverse() method //{ */
129 template <int n_states, int n_inputs, int n_measurements>
130 typename UKF<n_states, n_inputs, n_measurements>::Pzz_t UKF<n_states, n_inputs, n_measurements>::computeInverse(const Pzz_t& Pzz) const
131 {
132 Eigen::ColPivHouseholderQR<Pzz_t> qr(Pzz);
133 if (!qr.isInvertible())
134 {
135 // add some stuff to the tmp matrix diagonal to make it invertible
136 Pzz_t tmp = Pzz + 1e-9 * Pzz_t::Identity(Pzz.rows(), Pzz.cols());
137 qr.compute(tmp);
138 if (!qr.isInvertible())
139 {
140 // never managed to make this happen except for explicitly putting NaNs in the input
141 std::cout << "UKF: could not compute matrix inversion!!! Fix your covariances (the measurement's is probably too low...)" << std::endl;
142 throw inverse_exception();
143 }
144 std::cout << "UKF: artificially inflating matrix for inverse computation! Check your covariances (the measurement's might be too low...)" << std::endl;
145 }
146 Pzz_t ret = qr.inverse();
147 return ret;
148 }
149 //}
150
151 /* computeKalmanGain() method //{ */
152 template <int n_states, int n_inputs, int n_measurements>
153 typename UKF<n_states, n_inputs, n_measurements>::K_t UKF<n_states, n_inputs, n_measurements>::computeKalmanGain([[maybe_unused]] const x_t& x,
154 [[maybe_unused]] const z_t& inn,
155 const K_t& Pxz, const Pzz_t& Pzz) const
156 {
157 const Pzz_t Pzz_inv = computeInverse(Pzz);
158 const K_t K = Pxz * Pzz_inv;
159 return K;
160 }
161 //}
162
163 /* computeSigmas() method //{ */
164 template <int n_states, int n_inputs, int n_measurements>
165 typename UKF<n_states, n_inputs, n_measurements>::X_t UKF<n_states, n_inputs, n_measurements>::computeSigmas(const x_t& x, const P_t& P) const
166 {
167 // calculate sigma points
168 // fill in the middle of the elipsoid
169 X_t S;
170 S.col(0) = x;
171
172 const P_t P_sqrt = computePaSqrt(P);
173 const auto xrep = x.replicate(1, n);
174
175 // positive sigma points
176 S.template block<n, n>(0, 1) = xrep + P_sqrt;
177
178 // negative sigma points
179 S.template block<n, n>(0, n + 1) = xrep - P_sqrt;
180
181 /* std::cout << "x: " << std::endl << x << std::endl; */
182 /* std::cout << "S rowmean: " << std::endl << S.rowwise().mean() << std::endl; */
183
184 return S;
185 }
186 //}
187
188 /* predict() method //{ */
189
190 template <int n_states, int n_inputs, int n_measurements>
192 const Q_t& Q, double dt) const
193 {
194 const auto& x = sc.x;
195 const auto& P = sc.P;
196 statecov_t ret;
197
198 const X_t S = computeSigmas(x, P);
199
200 // propagate sigmas through the transition model
201 X_t X;
202 for (int i = 0; i < w; i++)
203 {
204 X.col(i) = m_transition_model(S.col(i), u, dt);
205 }
206
207 /* std::cout << "x: " << std::endl << x << std::endl; */
208 /* std::cout << "X rowmean: " << std::endl << X.rowwise().mean() << std::endl; */
209 /* std::cout << "m_Wm sum: " << m_Wm.sum() << std::endl; */
210
211 // recompute the state vector
212 ret.x = x_t::Zero();
213 for (int i = 0; i < w; i++)
214 {
215 // TODO: WHY DOES THIS SHIT WORK IF I SUBSTITUTE m_Wm(i) FOR 1.0/w ??
216 x_t tmp = 1.0 / w * X.col(i);
217 /* x_t tmp = m_Wm(i) * X.col(i); */
218 ret.x += tmp;
219 }
220
221 // recompute the covariance
222 ret.P = P_t::Zero();
223 for (int i = 0; i < w; i++)
224 {
225 ret.P += m_Wc(i) * (X.col(i) - ret.x) * (X.col(i) - ret.x).transpose();
226 }
227 ret.P += Q;
228
229 return ret;
230 }
231
232 //}
233
234 /* correct() method //{ */
235
236 template <int n_states, int n_inputs, int n_measurements>
238 const R_t& R) const
239 {
240 const auto& x = sc.x;
241 const auto& P = sc.P;
242 const X_t S = computeSigmas(x, P);
243
244 // propagate sigmas through the observation model
245 Z_t Z_exp(z.rows(), w);
246 for (int i = 0; i < w; i++)
247 {
248 Z_exp.col(i) = m_observation_model(S.col(i));
249 }
250
251 // compute expected measurement
252 z_t z_exp = z_t::Zero(z.rows());
253 for (int i = 0; i < w; i++)
254 {
255 z_exp += m_Wm(i) * Z_exp.col(i);
256 }
257
258 // compute the covariance of measurement
259 Pzz_t Pzz = Pzz_t::Zero(z.rows(), z.rows());
260 for (int i = 0; i < w; i++)
261 {
262 Pzz += m_Wc(i) * (Z_exp.col(i) - z_exp) * (Z_exp.col(i) - z_exp).transpose();
263 }
264 Pzz += R;
265
266 // compute cross covariance
267 K_t Pxz = K_t::Zero(n, z.rows());
268 for (int i = 0; i < w; i++)
269 {
270 Pxz += m_Wc(i) * (S.col(i) - x) * (Z_exp.col(i) - z_exp).transpose();
271 }
272
273 // compute Kalman gain
274 const z_t inn = (z - z_exp); // innovation
275 const K_t K = computeKalmanGain(sc.x, inn, Pxz, Pzz);
276
277 // check whether the inverse produced valid numbers
278 if (!K.array().isFinite().all())
279 {
280 std::cout << "UKF: inverting of Pzz in correction update produced non-finite numbers!!! Fix your covariances (the measurement's is probably too low...)" << std::endl;
281 throw inverse_exception();
282 }
283
284 // correct
285 statecov_t ret;
286 ret.x = x + K * inn;
287 ret.P = P - K * Pzz * K.transpose();
288 return ret;
289 }
290
291 //}
292
293} // namespace mrs_lib
294
295#endif
Implementation of the Unscented Kalman filter .
Definition ukf.h:40
typename Eigen::Matrix< double, p, w > Z_t
Measurement sigma points matrix.
Definition ukf.h:51
typename Base_class::R_t R_t
measurement covariance p*p typedef
Definition ukf.h:67
typename Eigen::Matrix< double, n, w > X_t
State sigma points matrix.
Definition ukf.h:50
void setObservationModel(const observation_model_t &observation_model)
Changes the observation model function.
Definition ukf.hpp:95
virtual statecov_t predict(const statecov_t &sc, const u_t &u, const Q_t &Q, double dt) const override
Implements the state prediction step (time update).
Definition ukf.hpp:191
typename Base_class::x_t x_t
state vector n*1 typedef
Definition ukf.h:59
typename Base_class::statecov_t statecov_t
typedef of a helper struct for state and covariance
Definition ukf.h:73
void setConstants(const double alpha, const double kappa, const double beta)
Changes the Unscented Transform parameters.
Definition ukf.hpp:71
typename Base_class::u_t u_t
input vector m*1 typedef
Definition ukf.h:61
void setTransitionModel(const transition_model_t &transition_model)
Changes the transition model function.
Definition ukf.hpp:85
typename Base_class::z_t z_t
measurement vector p*1 typedef
Definition ukf.h:63
typename std::function< x_t(const x_t &, const u_t &, double)> transition_model_t
function of the state transition model typedef
Definition ukf.h:75
typename Base_class::Q_t Q_t
process covariance n*n typedef
Definition ukf.h:69
typename Base_class::P_t P_t
state covariance n*n typedef
Definition ukf.h:65
typename std::function< z_t(const x_t &)> observation_model_t
function of the observation model typedef
Definition ukf.h:77
typename Eigen::Matrix< double, w, 1 > W_t
weights vector (2n+1)*1 typedef
Definition ukf.h:71
typename Eigen::Matrix< double, n, p > K_t
Kalman gain matrix.
Definition ukf.h:53
UKF()
Convenience default constructor.
Definition ukf.hpp:23
virtual statecov_t correct(const statecov_t &sc, const z_t &z, const R_t &R) const override
Implements the state correction step (measurement update).
Definition ukf.hpp:237
typename Eigen::Matrix< double, p, p > Pzz_t
Pzz helper matrix.
Definition ukf.h:52
All mrs_lib functions, classes, variables and definitions are contained in this namespace.
Definition attitude_converter.h:24
is thrown when taking the inverse of a matrix fails during kalman gain calculation
Definition ukf.h:90
Defines UKF - a class implementing the Unscented Kalman Filter .