Implementation of the Unscented Kalman filter [3]. More...

#include <ukf.h>

Inheritance diagram for mrs_lib::UKF< n_states, n_inputs, n_measurements >:

Collaboration diagram for mrs_lib::UKF< n_states, n_inputs, n_measurements >:

Classes
struct	inverse_exception
	is thrown when taking the inverse of a matrix fails during kalman gain calculation More...

struct	square_root_exception
	is thrown when taking the square root of a matrix fails during sigma generation More...

Public Types
using	x_t = typename Base_class::x_t
	state vector n*1 typedef

using	u_t = typename Base_class::u_t
	input vector m*1 typedef

using	z_t = typename Base_class::z_t
	measurement vector p*1 typedef

using	P_t = typename Base_class::P_t
	state covariance n*n typedef

using	R_t = typename Base_class::R_t
	measurement covariance p*p typedef

using	Q_t = typename Base_class::Q_t
	process covariance n*n typedef

using	W_t = typename Eigen::Matrix< double, w, 1 >
	weights vector (2n+1)*1 typedef

using	statecov_t = typename Base_class::statecov_t
	typedef of a helper struct for state and covariance

using	transition_model_t = typename std::function< x_t(const x_t &, const u_t &, double)>
	function of the state transition model typedef

using	observation_model_t = typename std::function< z_t(const x_t &)>
	function of the observation model typedef

Public Member Functions
	UKF ()
	Convenience default constructor.

	UKF (const transition_model_t &transition_model, const observation_model_t &observation_model, const double alpha=1e-3, const double kappa=1, const double beta=2)
	The main constructor.

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).

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).

void	setConstants (const double alpha, const double kappa, const double beta)
	Changes the Unscented Transform parameters.

void	setTransitionModel (const transition_model_t &transition_model)
	Changes the transition model function.

void	setObservationModel (const observation_model_t &observation_model)
	Changes the observation model function.

Protected Types
using	Base_class = KalmanFilter< n, m, p >
	Base class of this class.

using	X_t = typename Eigen::Matrix< double, n, w >
	State sigma points matrix.

using	Z_t = typename Eigen::Matrix< double, p, w >
	Measurement sigma points matrix.

using	Pzz_t = typename Eigen::Matrix< double, p, p >
	Pzz helper matrix.

using	K_t = typename Eigen::Matrix< double, n, p >
	Kalman gain matrix.

Protected Member Functions
void	computeWeights ()

X_t	computeSigmas (const x_t &x, const P_t &P) const

P_t	computePaSqrt (const P_t &P) const

Pzz_t	computeInverse (const Pzz_t &Pzz) const

virtual K_t	computeKalmanGain (const x_t &x, const z_t &inn, const K_t &Pxz, const Pzz_t &Pzz) const

Protected Attributes
double	m_alpha

double	m_kappa

double	m_beta

double	m_lambda

W_t	m_Wm

W_t	m_Wc

transition_model_t	m_transition_model

observation_model_t	m_observation_model

Static Protected Attributes
static constexpr int	n = n_states
	Length of the state vector of the system.

static constexpr int	m = n_inputs
	Length of the input vector of the system.

static constexpr int	p = n_measurements
	Length of the measurement vector of the system.

static constexpr int	w = 2 * n + 1
	Number of sigma points/weights.

Detailed Description

template<int n_states, int n_inputs, int n_measurements>
class mrs_lib::UKF< n_states, n_inputs, n_measurements >

Implementation of the Unscented Kalman filter [3].

The Unscented Kalman filter (abbreviated UKF, [3]) is a variant of the Kalman filter, which may be used for state filtration or estimation of non-linear systems as opposed to the Linear Kalman Filter (which is implemented in LKF). The UKF tends to be more accurate than the simpler Extended Kalman Filter, espetially for highly non-linear systems. However, it is generally less stable than the LKF because of the extra matrix square root in the sigma points calculation, so it is recommended to use LKF for linear systems.

The UKF C++ class itself is templated. This has its advantages and disadvantages. Main disadvantage is that it may be harder to use if you're not familiar with C++ templates, which, admittedly, can get somewhat messy, espetially during compilation. Another disadvantage is that if used unwisely, the compilation times can get much higher when using templates. The main advantage is compile-time checking (if it compiles, then it has a lower chance of crashing at runtime) and enabling more effective optimalizations during compilation. Also in case of Eigen, the code is arguably more readable when you use aliases to the specific Matrix instances instead of having Eigen::MatrixXd and Eigen::VectorXd everywhere.

Template Parameters

n_states	number of states of the system (length of the $\mathbf{x}$ vector).
n_inputs	number of inputs of the system (length of the $\mathbf{u}$ vector).
n_measurements	number of measurements of the system (length of the $\mathbf{z}$ vector).

Constructor & Destructor Documentation

◆ UKF() [1/2]

template<int n_states, int n_inputs, int n_measurements>

mrs_lib::UKF< n_states, n_inputs, n_measurements >::UKF ( )

Convenience default constructor.

This constructor should not be used if applicable. If used, the main constructor has to be called afterwards, otherwise the UKF object is invalid (not initialized).

◆ UKF() [2/2]

template<int n_states, int n_inputs, int n_measurements>

mrs_lib::UKF< n_states, n_inputs, n_measurements >::UKF	(	const transition_model_t &	transition_model,
		const observation_model_t &	observation_model,
		const double	alpha = `1e-3`,
		const double	kappa = `1`,
		const double	beta = `2`
	)

The main constructor.

Parameters

alpha	Scaling parameter of the sigma generation (a small positive value, e.g. 1e-3).
kappa	Secondary scaling parameter of the sigma generation (usually set to 0 or 1).
beta	Incorporates prior knowledge about the distribution (for Gaussian distribution, 2 is optimal).
transition_model	State transition model function.
observation_model	Observation model function.

Member Function Documentation

◆ correct()

template<int n_states, int n_inputs, int n_measurements>

UKF< n_states, n_inputs, n_measurements >::statecov_t mrs_lib::UKF< n_states, n_inputs, n_measurements >::correct	(	const statecov_t &	sc,
		const z_t &	z,
		const R_t &	R
	)		const

overridevirtual

Implements the state correction step (measurement update).

Parameters

sc	Previous estimate of the state and covariance.
z	Measurement vector.
R	Measurement covariance matrix.

Returns: The state and covariance after applying the correction step.

Implements mrs_lib::KalmanFilter< n_states, n_inputs, n_measurements >.

◆ predict()

template<int n_states, int n_inputs, int n_measurements>

UKF< n_states, n_inputs, n_measurements >::statecov_t mrs_lib::UKF< n_states, n_inputs, n_measurements >::predict	(	const statecov_t &	sc,
		const u_t &	u,
		const Q_t &	Q,
		double	dt
	)		const

overridevirtual

Implements the state prediction step (time update).

Parameters

sc	Previous estimate of the state and covariance.
u	Input vector.
Q	Process noise covariance matrix.
dt	Duration since the previous estimate.

Returns: The state and covariance after applying the correction step.

Implements mrs_lib::KalmanFilter< n_states, n_inputs, n_measurements >.

◆ setConstants()

template<int n_states, int n_inputs, int n_measurements>

void mrs_lib::UKF< n_states, n_inputs, n_measurements >::setConstants	(	const double	alpha,
		const double	kappa,
		const double	beta
	)

Changes the Unscented Transform parameters.

Parameters

alpha	Scaling parameter of the sigma generation (a small positive value - e.g. 1e-3).
kappa	Secondary scaling parameter of the sigma generation (usually set to 0 or 1).
beta	Incorporates prior knowledge about the distribution (for Gaussian distribution, 2 is optimal).

◆ setObservationModel()

template<int n_states, int n_inputs, int n_measurements>

void mrs_lib::UKF< n_states, n_inputs, n_measurements >::setObservationModel ( const observation_model_t & observation_model )

Changes the observation model function.

Parameters

observation_model the new observation model

◆ setTransitionModel()

template<int n_states, int n_inputs, int n_measurements>

void mrs_lib::UKF< n_states, n_inputs, n_measurements >::setTransitionModel ( const transition_model_t & transition_model )

Changes the transition model function.

Parameters

transition_model the new transition model

The documentation for this class was generated from the following files:

include/mrs_lib/ukf.h
include/mrs_lib/impl/ukf.hpp

Classes

Public Types

Public Member Functions

Protected Types

Protected Member Functions

Protected Attributes

Static Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ UKF() [1/2]

◆ UKF() [2/2]

Member Function Documentation

◆ correct()

◆ predict()

◆ setConstants()

◆ setObservationModel()

◆ setTransitionModel()