#include <extended_kalman_filter.h>

Inheritance diagram for bitrl::estimation::ExtendedKalmanFilter< MotionModelTp, ObservationModelTp >:

Collaboration diagram for bitrl::estimation::ExtendedKalmanFilter< MotionModelTp, ObservationModelTp >:

Public Types
typedef MotionModelTp	motion_model_type

typedef ObservationModelTp	observation_model_type

typedef motion_model_type::input_type	motion_model_input_type

typedef motion_model_type::matrix_type	matrix_type

typedef motion_model_type::state_type	state_type

typedef observation_model_type::input_type	observation_model_input_type

Public Member Functions
	ExtendedKalmanFilter ()
	Constructor.

	ExtendedKalmanFilter (motion_model_type &motion_model, const observation_model_type &observation_model)
	Constructor. Initialize with a writable reference of the motion model and a read only reference of the observation model.

	~ExtendedKalmanFilter ()
	Destructor.

void	estimate (const std::tuple< motion_model_input_type, observation_model_input_type > &input)
	Estimate the state. This function simply wraps the predict and update steps described by the functions below.

void	predict (const motion_model_input_type &input)
	Predicts the state vector x and the process covariance matrix P using the given input control u accroding to the following equations.

void	update (const observation_model_input_type &z)
	Updates the gain matrix K, the state vector x and covariance matrix P using the given measurement z_k according to the following equations.

void	set_motion_model (motion_model_type &motion_model)
	Set the motion model.

void	set_observation_model (const observation_model_type &observation_model)
	Set the observation model.

void	set_matrix (const std::string &name, const matrix_type &mat)
	Set the matrix used by the filter.

bool	has_matrix (const std::string &name) const
	Returns true if the matrix with the given name exists.

const state_type &	get_state () const
	Returns the state.

state_type &	get_state ()
	Returns the state.

real_t	get (const std::string &name) const
	Returns the state property with the given name.

const DynMat< real_t > &	operator[] (const std::string &name) const
	Returns the name-th matrix.

DynMat< real_t > &	operator[] (const std::string &name)
	Returns the name-th matrix.

ExtendedKalmanFilter &	with_matrix (const std::string &name, const matrix_type &mat)
	Set the matrix and return *this.

Protected Attributes
motion_model_type *	motion_model_ptr_
	pointer to the function that computes f

const observation_model_type *	observation_model_ptr_
	pointer to the function that computes h

std::map< std::string, matrix_type >	matrices_
	Matrices used by the filter internally.

Detailed Description

template<typename MotionModelTp, typename ObservationModelTp>
class bitrl::estimation::ExtendedKalmanFilter< MotionModelTp, ObservationModelTp >

Implements the Extended Kalman filter algorithm. The class expects a number of inputs in order to be useful. Concretely the application must specity

MotionModelTp a motion model ObservationModelTp observation model

The MotionModelTp should expose the following functions

evaluate(MotionModelTp input)-->State& get_matrix(const std::string)—>DynMat

In particular, the class expects the L, F matrices to be supplied via the get_matix function of the motion model.

Similarly, the ObservationModelTp should expose the following functions

evaluate(ObservationModelTp& input)—>DynVec get_matrix(const std::string)—>DynMat

In particular, the class expects the M, H matrices to be supplied via the get_matix function of the observation model.

Finally, the application should supply the P, Q, R matrices associated with the process

Member Typedef Documentation

◆ matrix_type

template<typename MotionModelTp , typename ObservationModelTp >

typedef motion_model_type::matrix_type bitrl::estimation::ExtendedKalmanFilter< MotionModelTp, ObservationModelTp >::matrix_type

◆ motion_model_input_type

template<typename MotionModelTp , typename ObservationModelTp >

typedef motion_model_type::input_type bitrl::estimation::ExtendedKalmanFilter< MotionModelTp, ObservationModelTp >::motion_model_input_type

◆ motion_model_type

template<typename MotionModelTp , typename ObservationModelTp >

typedef MotionModelTp bitrl::estimation::ExtendedKalmanFilter< MotionModelTp, ObservationModelTp >::motion_model_type

◆ observation_model_input_type

template<typename MotionModelTp , typename ObservationModelTp >

typedef observation_model_type::input_type bitrl::estimation::ExtendedKalmanFilter< MotionModelTp, ObservationModelTp >::observation_model_input_type

◆ observation_model_type

template<typename MotionModelTp , typename ObservationModelTp >

typedef ObservationModelTp bitrl::estimation::ExtendedKalmanFilter< MotionModelTp, ObservationModelTp >::observation_model_type

◆ state_type

template<typename MotionModelTp , typename ObservationModelTp >

typedef motion_model_type::state_type bitrl::estimation::ExtendedKalmanFilter< MotionModelTp, ObservationModelTp >::state_type

Constructor & Destructor Documentation

◆ ExtendedKalmanFilter() [1/2]

template<typename MotionModelTp , typename ObservationModelTp >

bitrl::estimation::ExtendedKalmanFilter< MotionModelTp, ObservationModelTp >::ExtendedKalmanFilter ( )

Constructor.

◆ ExtendedKalmanFilter() [2/2]

template<typename MotionModelTp , typename ObservationModelTp >

bitrl::estimation::ExtendedKalmanFilter< MotionModelTp, ObservationModelTp >::ExtendedKalmanFilter	(	motion_model_type &	motion_model,
		const observation_model_type &	observation_model
	)

Constructor. Initialize with a writable reference of the motion model and a read only reference of the observation model.

◆ ~ExtendedKalmanFilter()

template<typename MotionModelTp , typename ObservationModelTp >

bitrl::estimation::ExtendedKalmanFilter< MotionModelTp, ObservationModelTp >::~ExtendedKalmanFilter ( )

Destructor.

Member Function Documentation

◆ estimate()

template<typename MotionModelTp , typename ObservationModelTp >

void bitrl::estimation::ExtendedKalmanFilter< MotionModelTp, ObservationModelTp >::estimate ( const std::tuple< motion_model_input_type, observation_model_input_type > & input )

Estimate the state. This function simply wraps the predict and update steps described by the functions below.

◆ get()

template<typename MotionModelTp , typename ObservationModelTp >

real_t bitrl::estimation::ExtendedKalmanFilter< MotionModelTp, ObservationModelTp >::get ( const std::string & name ) const

inline

Returns the state property with the given name.

◆ get_state() [1/2]

template<typename MotionModelTp , typename ObservationModelTp >

state_type & bitrl::estimation::ExtendedKalmanFilter< MotionModelTp, ObservationModelTp >::get_state ( )

inline

Returns the state.

◆ get_state() [2/2]

template<typename MotionModelTp , typename ObservationModelTp >

const state_type & bitrl::estimation::ExtendedKalmanFilter< MotionModelTp, ObservationModelTp >::get_state ( ) const

inline

Returns the state.

◆ has_matrix()

template<typename MotionModelTp , typename ObservationModelTp >

bool bitrl::estimation::ExtendedKalmanFilter< MotionModelTp, ObservationModelTp >::has_matrix ( const std::string & name ) const

Returns true if the matrix with the given name exists.

◆ operator[]() [1/2]

template<typename MotionModelTp , typename ObservationModelTp >

DynMat< real_t > & bitrl::estimation::ExtendedKalmanFilter< MotionModelTp, ObservationModelTp >::operator[] ( const std::string & name )

Returns the name-th matrix.

◆ operator[]() [2/2]

template<typename MotionModelTp , typename ObservationModelTp >

const DynMat< real_t > & bitrl::estimation::ExtendedKalmanFilter< MotionModelTp, ObservationModelTp >::operator[] ( const std::string & name ) const

Returns the name-th matrix.

◆ predict()

template<typename MotionModelTp , typename ObservationModelTp >

void bitrl::estimation::ExtendedKalmanFilter< MotionModelTp, ObservationModelTp >::predict ( const motion_model_input_type & input )

Predicts the state vector x and the process covariance matrix P using the given input control u accroding to the following equations.

\hat{x}_{k = f(x_{k-1}, u_{k}, w_k) \hat{P}_{k} = F_{k-1} * P_{k-1} * F_{k-1}^T + L_{k-1} * Q_{k-1} * L_{k-1}^T

where x_{k-1} is the state at the previous step, u_{k} is the control signal and w_k is the error associated with the control signal. In input argument passed to the function is meant to model in a tuple all the arguments needed. F, L are Jacobian matrices and Q is the covariance matrix associate with the control signal

make a state prediction using the motion model

◆ set_matrix()

template<typename MotionModelTp , typename ObservationModelTp >

void bitrl::estimation::ExtendedKalmanFilter< MotionModelTp, ObservationModelTp >::set_matrix	(	const std::string &	name,
		const matrix_type &	mat
	)

Set the matrix used by the filter.

◆ set_motion_model()

template<typename MotionModelTp , typename ObservationModelTp >

void bitrl::estimation::ExtendedKalmanFilter< MotionModelTp, ObservationModelTp >::set_motion_model ( motion_model_type & motion_model )

inline

Set the motion model.

◆ set_observation_model()

template<typename MotionModelTp , typename ObservationModelTp >

void bitrl::estimation::ExtendedKalmanFilter< MotionModelTp, ObservationModelTp >::set_observation_model ( const observation_model_type & observation_model )

inline

Set the observation model.

◆ update()

template<typename MotionModelTp , typename ObservationModelTp >

void bitrl::estimation::ExtendedKalmanFilter< MotionModelTp, ObservationModelTp >::update ( const observation_model_input_type & z )

Updates the gain matrix K, the state vector x and covariance matrix P using the given measurement z_k according to the following equations.

K_k = \hat{P}_{k} * H_{k}^T * (H_k * \hat{P}_{k} * H_{k}^T + M_k * R_k * M_k^T)^{-1} x_k = \hat{x}_{k} + K_k * (z_k - h( \hat{x}_{k}, 0)) P_k = (I - K_k * H_k) * \hat{P}_{k}

update the covariance matrix

◆ with_matrix()

template<typename MotionModelTp , typename ObservationModelTp >

ExtendedKalmanFilter< MotionModelTp, ObservationModelTp > & bitrl::estimation::ExtendedKalmanFilter< MotionModelTp, ObservationModelTp >::with_matrix	(	const std::string &	name,
		const matrix_type &	mat
	)

Set the matrix and return *this.

Member Data Documentation

◆ matrices_

template<typename MotionModelTp , typename ObservationModelTp >

std::map<std::string, matrix_type> bitrl::estimation::ExtendedKalmanFilter< MotionModelTp, ObservationModelTp >::matrices_

protected

Matrices used by the filter internally.

◆ motion_model_ptr_

template<typename MotionModelTp , typename ObservationModelTp >

motion_model_type* bitrl::estimation::ExtendedKalmanFilter< MotionModelTp, ObservationModelTp >::motion_model_ptr_

protected

pointer to the function that computes f

◆ observation_model_ptr_

template<typename MotionModelTp , typename ObservationModelTp >

const observation_model_type* bitrl::estimation::ExtendedKalmanFilter< MotionModelTp, ObservationModelTp >::observation_model_ptr_

protected

pointer to the function that computes h

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

libs/bitrl/src/bitrl/estimation/extended_kalman_filter.h

Public Types

Public Member Functions

Protected Attributes

Detailed Description

Member Typedef Documentation

◆ matrix_type

◆ motion_model_input_type

◆ motion_model_type

◆ observation_model_input_type

◆ observation_model_type

◆ state_type

Constructor & Destructor Documentation

◆ ExtendedKalmanFilter() [1/2]

◆ ExtendedKalmanFilter() [2/2]

◆ ~ExtendedKalmanFilter()

Member Function Documentation

◆ estimate()

◆ get()

◆ get_state() [1/2]

◆ get_state() [2/2]

◆ has_matrix()

◆ operator[]() [1/2]

◆ operator[]() [2/2]

◆ predict()

◆ set_matrix()

◆ set_motion_model()

◆ set_observation_model()

◆ update()

◆ with_matrix()

Member Data Documentation

◆ matrices_

◆ motion_model_ptr_

◆ observation_model_ptr_