22 template <
int n_states,
int n_inputs,
int n_measurements>
27 template <
int n_states,
int n_inputs,
int n_measurements>
29 const double kappa,
const double beta)
35 m_transition_model(transition_model),
36 m_observation_model(observation_model)
46 template <
int n_states,
int n_inputs,
int n_measurements>
51 m_lambda = m_alpha * m_alpha * (double(n) + m_kappa) -
double(n);
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);
58 for (
int i = 1; i < w; i++)
60 m_Wm(i) = (1.0 - m_Wm(0)) / (w - 1.0);
69 template <
int n_states,
int n_inputs,
int n_measurements>
84 template <
int n_states,
int n_inputs,
int n_measurements>
87 m_transition_model = transition_model;
94 template <
int n_states,
int n_inputs,
int n_measurements>
97 m_observation_model = observation_model;
104 template <
int n_states,
int n_inputs,
int n_measurements>
108 const P_t Pa = (double(n) + m_lambda) * P;
111 Eigen::LLT<P_t> llt(Pa);
112 if (llt.info() != Eigen::Success)
114 P_t tmp = Pa + (double(n) + m_lambda) * 1e-9 * P_t::Identity();
116 if (llt.info() != Eigen::Success)
118 std::cout <<
"UKF: taking the square root of covariance during sigma point generation failed." << std::endl;
119 throw square_root_exception();
123 const P_t Pa_sqrt = llt.matrixL();
129 template <
int n_states,
int n_inputs,
int n_measurements>
132 Eigen::ColPivHouseholderQR<Pzz_t> qr(Pzz);
133 if (!qr.isInvertible())
136 Pzz_t tmp = Pzz + 1e-9 * Pzz_t::Identity(Pzz.rows(), Pzz.cols());
138 if (!qr.isInvertible())
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();
144 std::cout <<
"UKF: artificially inflating matrix for inverse computation! Check your covariances (the measurement's might be too low...)" << std::endl;
146 Pzz_t ret = qr.inverse();
152 template <
int n_states,
int n_inputs,
int n_measurements>
154 [[maybe_unused]]
const z_t& inn,
155 const K_t& Pxz,
const Pzz_t& Pzz)
const
157 const Pzz_t Pzz_inv = computeInverse(Pzz);
158 const K_t K = Pxz * Pzz_inv;
164 template <
int n_states,
int n_inputs,
int n_measurements>
172 const P_t P_sqrt = computePaSqrt(P);
173 const auto xrep = x.replicate(1, n);
176 S.template block<n, n>(0, 1) = xrep + P_sqrt;
179 S.template block<n, n>(0, n + 1) = xrep - P_sqrt;
190 template <
int n_states,
int n_inputs,
int n_measurements>
192 const Q_t& Q,
double dt)
const
194 const auto& x = sc.x;
195 const auto& P = sc.P;
198 const X_t S = computeSigmas(x, P);
202 for (
int i = 0; i < w; i++)
204 X.col(i) = m_transition_model(S.col(i), u, dt);
213 for (
int i = 0; i < w; i++)
216 x_t tmp = 1.0 / w * X.col(i);
223 for (
int i = 0; i < w; i++)
225 ret.P += m_Wc(i) * (X.col(i) - ret.x) * (X.col(i) - ret.x).transpose();
236 template <
int n_states,
int n_inputs,
int n_measurements>
240 const auto& x = sc.x;
241 const auto& P = sc.P;
242 const X_t S = computeSigmas(x, P);
245 Z_t Z_exp(z.rows(), w);
246 for (
int i = 0; i < w; i++)
248 Z_exp.col(i) = m_observation_model(S.col(i));
252 z_t z_exp = z_t::Zero(z.rows());
253 for (
int i = 0; i < w; i++)
255 z_exp += m_Wm(i) * Z_exp.col(i);
259 Pzz_t Pzz = Pzz_t::Zero(z.rows(), z.rows());
260 for (
int i = 0; i < w; i++)
262 Pzz += m_Wc(i) * (Z_exp.col(i) - z_exp) * (Z_exp.col(i) - z_exp).transpose();
267 K_t Pxz = K_t::Zero(n, z.rows());
268 for (
int i = 0; i < w; i++)
270 Pxz += m_Wc(i) * (S.col(i) - x) * (Z_exp.col(i) - z_exp).transpose();
274 const z_t inn = (z - z_exp);
275 const K_t K = computeKalmanGain(sc.x, inn, Pxz, Pzz);
278 if (!K.array().isFinite().all())
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;
287 ret.P = P - K * Pzz * K.transpose();
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 .