mrs_lib
Various reusable classes, functions and utilities for use in MRS projects
Loading...
Searching...
No Matches
nckf.h
1// clang: MatousFormat
6#ifndef NCKFSYSTEMMODELS_H
7#define NCKFSYSTEMMODELS_H
8
9#include <mrs_lib/lkf.h>
10#include <mrs_lib/ukf.h>
11
12namespace mrs_lib
13{
14
15 /* class NCLKF //{ */
27 template <int n_states, int n_inputs, int n_measurements>
28 class NCLKF : public LKF<n_states, n_inputs, n_measurements>
29 {
30 public:
31 /* NCLKF definitions (typedefs, constants etc) //{ */
32 static const int n = n_states;
33 static const int m = n_inputs;
34 static const int p = n_measurements;
37 using x_t = typename Base_class::x_t;
38 using u_t = typename Base_class::u_t;
39 using z_t = typename Base_class::z_t;
40 using P_t = typename Base_class::P_t;
41 using R_t = typename Base_class::R_t;
42 using Q_t = typename Base_class::Q_t;
45 using A_t = typename Base_class::A_t;
46 using B_t = typename Base_class::B_t;
47 using H_t = typename Base_class::H_t;
48 using K_t = typename Base_class::K_t;
49 //}
50
51 public:
58 NCLKF(){};
59
68 NCLKF(const A_t& A, const B_t& B, const H_t& H, const double l) : Base_class(A, B, H), l(l){};
69
70 protected:
71 double l;
72
73 protected:
74 /* computeKalmanGain() method //{ */
75 virtual K_t computeKalmanGain(const statecov_t& sc, const z_t& z, const R_t& R, const H_t& H) const override
76 {
77 const R_t W = H * sc.P * H.transpose() + R;
78 const R_t W_inv = Base_class::invert_W(W);
79 const K_t K_orig = sc.P * H.transpose() * W_inv;
80
81 const z_t inn = z - (H * sc.x); // innovation
82 const x_t x = sc.x + K_orig * inn;
83 const double inn_scale = inn.transpose() * W_inv * inn;
84
85 const double x_norm = x.norm();
86 const K_t K = K_orig + (l / x_norm - 1.0) * x * (inn.transpose() * W_inv) / inn_scale;
87
88 return K;
89 }
90 //}
91 };
92 //}
93
94 /* class NCLKF_partial //{ */
95
110 template <int n_states, int n_inputs, int n_measurements, int n_norm_constrained_states>
111 class NCLKF_partial : public NCLKF<n_states, n_inputs, n_measurements>
112 {
113 public:
114 /* NCLKF_partial definitions (typedefs, constants etc) //{ */
115 static const int n = n_states;
116 static const int m = n_inputs;
117 static const int p = n_measurements;
118 static const int nq = n_norm_constrained_states;
121 using x_t = typename Base_class::x_t;
122 using u_t = typename Base_class::u_t;
123 using z_t = typename Base_class::z_t;
124 using P_t = typename Base_class::P_t;
125 using R_t = typename Base_class::R_t;
126 using Q_t = typename Base_class::Q_t;
129 using A_t = typename Base_class::A_t;
130 using B_t = typename Base_class::B_t;
131 using H_t = typename Base_class::H_t;
132 using K_t = typename Base_class::K_t;
134 using indices_t = std::array<int, nq>;
135 using xq_t = Eigen::Matrix<double, nq, 1>;
136 using Hq_t = Eigen::Matrix<double, p, nq>;
137 using Kq_t = Eigen::Matrix<double, nq, p>;
138 //}
139
140 public:
148
158 NCLKF_partial(const A_t& A, const B_t& B, const H_t& H, const double l, const indices_t& norm_constrained_indices)
159 : Base_class(A, B, H, l), norm_indices(norm_constrained_indices)
160 {
161 if (n >= 0)
162 {
163 for (auto& idx : norm_indices)
164 {
165 if (idx > n)
166 {
167 printf("[NCLKF_partial]: Index of a norm-constrained state (%d) cannot be higher than the number of states (%d)! Setting it to zero.", idx, n);
168 idx = 0;
169 }
170 if (idx < 0)
171 {
172 printf("[NCLKF_partial]: Index of a norm-constrained state (%d) cannot be less than zero! Setting it to zero.", idx);
173 idx = 0;
174 }
175 }
176 }
177 };
178
179 private:
180 indices_t norm_indices;
181
182 /* helper methods for Eigen matrix subscripting //{ */
183
184 template <typename T, int rows, int cols, size_t out_rows, size_t out_cols>
185 Eigen::Matrix<T, out_rows, out_cols> select_indices(const Eigen::Matrix<T, rows, cols>& mat, const std::array<int, out_rows>& row_indices,
186 const std::array<int, out_cols>& col_indices) const
187 {
188 Eigen::Matrix<T, out_rows, cols> tmp;
189 for (int it = 0; it < (int)out_rows; it++)
190 {
191 const auto row = row_indices[it];
192 assert(rows < 0 || row < rows);
193 tmp.row(it) = mat.row(row);
194 }
195
196 Eigen::Matrix<T, out_rows, out_cols> ret;
197 for (int it = 0; it < (int)out_cols; it++)
198 {
199 const auto col = col_indices[it];
200 assert(cols < 0 || col < cols);
201 tmp.col(it) = tmp.col(col);
202 }
203
204 return ret;
205 }
206
207 template <typename T, int rows, int cols, size_t out_rows>
208 Eigen::Matrix<T, out_rows, cols> select_rows(const Eigen::Matrix<T, rows, cols>& mat, const std::array<int, out_rows>& row_indices) const
209 {
210 Eigen::Matrix<T, out_rows, cols> ret;
211 for (int it = 0; it < (int)out_rows; it++)
212 {
213 const auto row = row_indices[it];
214 assert(rows < 0 || row < rows);
215 ret.row(it) = mat.row(row);
216 }
217 return ret;
218 }
219
220 template <typename T, int rows, int cols, size_t out_cols>
221 Eigen::Matrix<T, rows, out_cols> select_cols(const Eigen::Matrix<T, rows, cols>& mat, const std::array<int, out_cols>& col_indices) const
222 {
223 Eigen::Matrix<T, rows, out_cols> ret;
224 for (int it = 0; it < (int)out_cols; it++)
225 {
226 const auto col = col_indices[it];
227 assert(cols < 0 || col < cols);
228 ret.col(it) = mat.col(col);
229 }
230 return ret;
231 }
232
233 template <typename T, int rows, size_t out_rows>
234 Eigen::Matrix<T, out_rows, 1> select_indices(const Eigen::Matrix<T, rows, 1>& vec, const std::array<int, out_rows>& row_indices) const
235 {
236 return select_rows(vec, row_indices);
237 }
238
239 template <typename T, int rows, int cols, size_t n_rows>
240 void set_rows(const Eigen::Matrix<T, (size_t)n_rows, cols>& from_mat, Eigen::Matrix<T, rows, cols>& to_mat,
241 const std::array<int, n_rows>& row_indices) const
242 {
243 for (int rit = 0; rit < (int)n_rows; rit++)
244 {
245 const auto ridx = row_indices.at(rit);
246 assert(rows < 0 || ridx < rows);
247 to_mat.row(ridx) = from_mat.row(rit);
248 }
249 }
250
251 //}
252
253 protected:
254 /* computeKalmanGain() method //{ */
255 virtual K_t computeKalmanGain(const statecov_t& sc, const z_t& z, const R_t& R, const H_t& H) const override
256 {
257 const R_t W = H * sc.P * H.transpose() + R;
258 const R_t W_inv = Base_class::invert_W(W);
259 K_t K = sc.P * H.transpose() * W_inv;
260
261 // calculate the kalman gain for the norm-constrained states
262 {
263 const Kq_t K_orig = select_rows(K, norm_indices);
264 const xq_t xq = select_indices(sc.x, norm_indices);
265 const Hq_t Hq = select_cols(H, norm_indices);
266 const z_t inn = z - (Hq * xq); // innovation
267 const xq_t x = xq + K_orig * inn;
268 const double inn_scale = inn.transpose() * W_inv * inn;
269
270 const double x_norm = x.norm();
271 const Kq_t Kq = K_orig + (Base_class::l / x_norm - 1.0) * x * (inn.transpose() * W_inv) / inn_scale;
272 set_rows(Kq, K, norm_indices);
273 }
274
275 return K;
276 }
277 //}
278 };
279 //}
280
281 /* class NCUKF //{ */
282 template <int n_states, int n_inputs, int n_measurements>
283 class NCUKF : public UKF<n_states, n_inputs, n_measurements>
284 {
285 public:
286 /* LKF definitions (typedefs, constants etc) //{ */
287 static const int n = n_states;
288 static const int m = n_inputs;
289 static const int p = n_measurements;
290 using Base_class = UKF<n, m, p>;
291
292 using x_t = typename Base_class::x_t; // state vector n*1
293 using u_t = typename Base_class::u_t; // input vector m*1
294 using z_t = typename Base_class::z_t; // measurement vector p*1
295 using P_t = typename Base_class::P_t; // state covariance n*n
296 using R_t = typename Base_class::R_t; // measurement covariance p*p
297 using Q_t = typename Base_class::Q_t; // measurement covariance p*p
298 using statecov_t = typename Base_class::statecov_t; // helper struct for state and covariance
299
300 using transition_model_t = typename Base_class::transition_model_t;
301 using observation_model_t = typename Base_class::observation_model_t;
302
303 using X_t = typename Base_class::X_t; // state sigma points matrix n*w
304 using Z_t = typename Base_class::Z_t; // measurement sigma points matrix p*w
305 using Pzz_t = typename Base_class::Pzz_t; // Pzz helper matrix p*n
306 using K_t = typename Base_class::K_t; // kalman gain n*p
307 //}
308
309 public:
310 NCUKF(){};
311
312 NCUKF(const transition_model_t& transition_model, const observation_model_t& observation_model, const double l, const double alpha = 1e-3,
313 const double kappa = 1, const double beta = 2)
314 : Base_class(transition_model, observation_model, alpha, kappa, beta), l(l){};
315
316 public:
317 /* correct() method //{ */
318 virtual statecov_t correct(const statecov_t& sc, const z_t& z, const R_t& R) const override
319 {
320 const auto& x = sc.x;
321 const auto& P = sc.P;
322 const X_t S = Base_class::computeSigmas(x, P);
323
324 // propagate sigmas through the observation model
325 Z_t Z_exp;
326 for (int i = 0; i < Base_class::w; i++)
327 {
328 Z_exp.col(i) = Base_class::m_observation_model(S.col(i));
329 }
330
331 // compute expected measurement
332 z_t z_exp = z_t::Zero();
333 for (int i = 0; i < Base_class::w; i++)
334 {
335 z_exp += Base_class::m_Wm(i) * Z_exp.col(i);
336 }
337
338 // compute the covariance of measurement
339 Pzz_t Pzz = Pzz_t::Zero();
340 for (int i = 0; i < Base_class::w; i++)
341 {
342 Pzz += Base_class::m_Wc(i) * (Z_exp.col(i) - z_exp) * (Z_exp.col(i) - z_exp).transpose();
343 }
344 Pzz += R;
345
346 // compute cross covariance
347 K_t Pxz = K_t::Zero();
348 for (int i = 0; i < Base_class::w; i++)
349 {
350 Pxz += Base_class::m_Wc(i) * (S.col(i) - x) * (Z_exp.col(i) - z_exp).transpose();
351 }
352
353 // compute Kalman gain
354 const z_t inn = (z - z_exp); // innovation
355 const K_t K = computeKalmanGain(sc.x, inn, Pxz, Pzz);
356
357 // check whether the inverse produced valid numbers
358 if (!K.array().isFinite().all())
359 {
360 printf("UKF: inverting of Pzz in correction update produced non-finite numbers!!! Fix your covariances (the measurement's is probably too low...)");
361 throw typename Base_class::inverse_exception();
362 }
363
364 // correct
365 statecov_t ret;
366 ret.x = x + K * inn;
367 ret.P = P - K * Pxz.transpose() - Pxz * K.transpose() + K * Pzz * K.transpose();
368 return ret;
369 }
370 //}
371
372 protected:
373 double l;
374
375 protected:
376 /* computeKalmanGain() method //{ */
377 virtual K_t computeKalmanGain(const x_t& x, const z_t& inn, const K_t& Pxz, const Pzz_t& Pzz) const override
378 {
379 const Pzz_t Pzz_inv = Base_class::computeInverse(Pzz);
380 const K_t K_orig = Pxz * Pzz_inv;
381
382 const x_t x_pred = x + K_orig * inn;
383 const double inn_scale = inn.transpose() * Pzz_inv * inn;
384
385 const double x_norm = x_pred.norm();
386 const K_t K = K_orig + (l / x_norm - 1.0) * x_pred * (inn.transpose() * Pzz_inv) / inn_scale;
387
388 return K;
389 }
390 //}
391 };
392 //}
393
394} // namespace mrs_lib
395
396#endif // NCKFSYSTEMMODELS_H
397
Implementation of the Linear Kalman filter .
Definition lkf.h:39
A_t A
The system transition matrix .
Definition lkf.h:148
B_t B
The input to state mapping matrix .
Definition lkf.h:149
Eigen::Matrix< double, n, p > K_t
Kalman gain matrix type .
Definition lkf.h:58
typename Base_class::statecov_t statecov_t
Helper struct for passing around the state and its covariance in one variable.
Definition lkf.h:53
H_t H
The state to measurement mapping matrix .
Definition lkf.h:150
Eigen::Matrix< double, n, m > B_t
Input to state mapping matrix type .
Definition lkf.h:56
typename Base_class::R_t R_t
Measurement noise covariance matrix type .
Definition lkf.h:51
typename Base_class::P_t P_t
State uncertainty covariance matrix type .
Definition lkf.h:50
typename Base_class::Q_t Q_t
Process noise covariance matrix type .
Definition lkf.h:52
Eigen::Matrix< double, p, n > H_t
State to measurement mapping matrix type .
Definition lkf.h:57
typename Base_class::u_t u_t
Input vector type .
Definition lkf.h:48
Eigen::Matrix< double, n, n > A_t
System transition matrix type .
Definition lkf.h:55
typename Base_class::x_t x_t
State vector type .
Definition lkf.h:47
typename Base_class::z_t z_t
Measurement vector type .
Definition lkf.h:49
This class implements the partially norm-constrained linear Kalman filter .
Definition nckf.h:112
typename Base_class::H_t H_t
State to measurement mapping matrix type .
Definition nckf.h:131
static const int nq
Number of states to which the norm constraint applies.
Definition nckf.h:118
NCLKF_partial(const A_t &A, const B_t &B, const H_t &H, const double l, const indices_t &norm_constrained_indices)
The main constructor.
Definition nckf.h:158
static const int p
Length of the measurement vector of the system.
Definition nckf.h:117
typename Base_class::Q_t Q_t
Process noise covariance matrix type .
Definition nckf.h:126
typename Base_class::statecov_t statecov_t
Helper struct for passing around the state and its covariance in one variable.
Definition nckf.h:127
Eigen::Matrix< double, nq, p > Kq_t
Norm-constrained kalman gain type .
Definition nckf.h:137
typename Base_class::K_t K_t
Kalman gain matrix type .
Definition nckf.h:132
Eigen::Matrix< double, nq, 1 > xq_t
Norm-constrained states vector type .
Definition nckf.h:135
typename Base_class::u_t u_t
Input vector type .
Definition nckf.h:122
typename Base_class::A_t A_t
System transition matrix type .
Definition nckf.h:129
Eigen::Matrix< double, p, nq > Hq_t
Norm-constrained measurement mapping type .
Definition nckf.h:136
static const int m
Length of the input vector of the system.
Definition nckf.h:116
typename Base_class::B_t B_t
Input to state mapping matrix type .
Definition nckf.h:130
typename Base_class::R_t R_t
Measurement noise covariance matrix type .
Definition nckf.h:125
std::array< int, nq > indices_t
Indices of the norm-constrained states type.
Definition nckf.h:134
NCLKF_partial()
Convenience default constructor.
Definition nckf.h:147
typename Base_class::z_t z_t
Measurement vector type .
Definition nckf.h:123
typename Base_class::P_t P_t
State uncertainty covariance matrix type .
Definition nckf.h:124
static const int n
Length of the state vector of the system.
Definition nckf.h:115
typename Base_class::x_t x_t
State vector type .
Definition nckf.h:121
This class implements the norm-constrained linear Kalman filter .
Definition nckf.h:29
typename Base_class::P_t P_t
State uncertainty covariance matrix type .
Definition nckf.h:40
static const int m
Length of the input vector of the system.
Definition nckf.h:33
typename Base_class::R_t R_t
Measurement noise covariance matrix type .
Definition nckf.h:41
NCLKF()
Convenience default constructor.
Definition nckf.h:58
typename Base_class::z_t z_t
Measurement vector type .
Definition nckf.h:39
typename Base_class::u_t u_t
Input vector type .
Definition nckf.h:38
static const int n
Length of the state vector of the system.
Definition nckf.h:32
typename Base_class::x_t x_t
State vector type .
Definition nckf.h:37
typename Base_class::H_t H_t
State to measurement mapping matrix type .
Definition nckf.h:47
NCLKF(const A_t &A, const B_t &B, const H_t &H, const double l)
The main constructor.
Definition nckf.h:68
typename Base_class::A_t A_t
System transition matrix type .
Definition nckf.h:45
typename Base_class::Q_t Q_t
Process noise covariance matrix type .
Definition nckf.h:42
typename Base_class::statecov_t statecov_t
Helper struct for passing around the state and its covariance in one variable.
Definition nckf.h:43
static const int p
Length of the measurement vector of the system.
Definition nckf.h:34
typename Base_class::K_t K_t
Kalman gain matrix type .
Definition nckf.h:48
typename Base_class::B_t B_t
Input to state mapping matrix type .
Definition nckf.h:46
Definition nckf.h:284
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
static constexpr int w
Number of sigma points/weights.
Definition ukf.h:46
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
typename Base_class::u_t u_t
input vector m*1 typedef
Definition ukf.h:61
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, n, p > K_t
Kalman gain matrix.
Definition ukf.h:53
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 .