6#ifndef NCKFSYSTEMMODELS_H
7#define NCKFSYSTEMMODELS_H
9#include <mrs_lib/lkf.h>
27 template <
int n_states,
int n_inputs,
int n_measurements>
28 class NCLKF :
public LKF<n_states, n_inputs, n_measurements>
32 static const int n = n_states;
33 static const int m = n_inputs;
34 static const int p = n_measurements;
75 virtual K_t computeKalmanGain(
const statecov_t& sc,
const z_t& z,
const R_t& R,
const H_t&
H)
const override
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;
81 const z_t inn = z - (
H * sc.x);
82 const x_t x = sc.x + K_orig * inn;
83 const double inn_scale = inn.transpose() * W_inv * inn;
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;
110 template <
int n_states,
int n_inputs,
int n_measurements,
int n_norm_constrained_states>
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;
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>;
159 :
Base_class(
A,
B,
H, l), norm_indices(norm_constrained_indices)
163 for (
auto& idx : norm_indices)
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);
172 printf(
"[NCLKF_partial]: Index of a norm-constrained state (%d) cannot be less than zero! Setting it to zero.", idx);
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
188 Eigen::Matrix<T, out_rows, cols> tmp;
189 for (
int it = 0; it < (int)out_rows; it++)
191 const auto row = row_indices[it];
192 assert(rows < 0 || row < rows);
193 tmp.row(it) = mat.row(row);
196 Eigen::Matrix<T, out_rows, out_cols> ret;
197 for (
int it = 0; it < (int)out_cols; it++)
199 const auto col = col_indices[it];
200 assert(cols < 0 || col < cols);
201 tmp.col(it) = tmp.col(col);
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
210 Eigen::Matrix<T, out_rows, cols> ret;
211 for (
int it = 0; it < (int)out_rows; it++)
213 const auto row = row_indices[it];
214 assert(rows < 0 || row < rows);
215 ret.row(it) = mat.row(row);
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
223 Eigen::Matrix<T, rows, out_cols> ret;
224 for (
int it = 0; it < (int)out_cols; it++)
226 const auto col = col_indices[it];
227 assert(cols < 0 || col < cols);
228 ret.col(it) = mat.col(col);
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
236 return select_rows(vec, row_indices);
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
243 for (
int rit = 0; rit < (int)n_rows; rit++)
245 const auto ridx = row_indices.at(rit);
246 assert(rows < 0 || ridx < rows);
247 to_mat.row(ridx) = from_mat.row(rit);
255 virtual K_t computeKalmanGain(
const statecov_t& sc,
const z_t& z,
const R_t& R,
const H_t&
H)
const override
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;
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);
267 const xq_t x = xq + K_orig * inn;
268 const double inn_scale = inn.transpose() * W_inv * inn;
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);
282 template <
int n_states,
int n_inputs,
int n_measurements>
283 class NCUKF :
public UKF<n_states, n_inputs, n_measurements>
287 static const int n = n_states;
288 static const int m = n_inputs;
289 static const int p = n_measurements;
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){};
318 virtual statecov_t correct(
const statecov_t& sc,
const z_t& z,
const R_t& R)
const override
320 const auto& x = sc.x;
321 const auto& P = sc.P;
322 const X_t S = Base_class::computeSigmas(x, P);
328 Z_exp.col(i) = Base_class::m_observation_model(S.col(i));
332 z_t z_exp = z_t::Zero();
335 z_exp += Base_class::m_Wm(i) * Z_exp.col(i);
339 Pzz_t Pzz = Pzz_t::Zero();
342 Pzz += Base_class::m_Wc(i) * (Z_exp.col(i) - z_exp) * (Z_exp.col(i) - z_exp).transpose();
347 K_t Pxz = K_t::Zero();
350 Pxz += Base_class::m_Wc(i) * (S.col(i) - x) * (Z_exp.col(i) - z_exp).transpose();
354 const z_t inn = (z - z_exp);
355 const K_t K = computeKalmanGain(sc.x, inn, Pxz, Pzz);
358 if (!K.array().isFinite().all())
360 printf(
"UKF: inverting of Pzz in correction update produced non-finite numbers!!! Fix your covariances (the measurement's is probably too low...)");
367 ret.P = P - K * Pxz.transpose() - Pxz * K.transpose() + K * Pzz * K.transpose();
377 virtual K_t computeKalmanGain(
const x_t& x,
const z_t& inn,
const K_t& Pxz,
const Pzz_t& Pzz)
const override
379 const Pzz_t Pzz_inv = Base_class::computeInverse(Pzz);
380 const K_t K_orig = Pxz * Pzz_inv;
382 const x_t x_pred = x + K_orig * inn;
383 const double inn_scale = inn.transpose() * Pzz_inv * inn;
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;
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
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 .