Extended Kalman Filter implementation. For an algorithmic overview, see also https://en.wikipedia.org/wiki/Extended_Kalman_filter. More...

#include <ExtendedKalmanFilter.h>

Inheritance diagram for ct::optcon::ExtendedKalmanFilter< STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR >:

Public Types
using	Base = EstimatorBase< STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR >

using	control_vector_t = ct::core::ControlVector< CONTROL_DIM, SCALAR >

using	output_matrix_t = ct::core::OutputMatrix< OUTPUT_DIM, SCALAR >

using	output_vector_t = ct::core::OutputVector< OUTPUT_DIM, SCALAR >

using	state_matrix_t = ct::core::StateMatrix< STATE_DIM, SCALAR >

using	state_vector_t = ct::core::StateVector< STATE_DIM, SCALAR >

Public Types inherited from ct::optcon::EstimatorBase< STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR >
using	control_vector_t = ct::core::ControlVector< CONTROL_DIM, SCALAR >

using	state_vector_t = ct::core::StateVector< STATE_DIM, SCALAR >

using	state_matrix_t = ct::core::StateMatrix< STATE_DIM, SCALAR >

using	output_vector_t = ct::core::OutputVector< OUTPUT_DIM, SCALAR >

using	output_matrix_t = ct::core::OutputMatrix< OUTPUT_DIM, SCALAR >

Public Member Functions
	ExtendedKalmanFilter (std::shared_ptr< SystemModelBase< STATE_DIM, CONTROL_DIM, SCALAR >> f, std::shared_ptr< LinearMeasurementModel< OUTPUT_DIM, STATE_DIM, SCALAR >> h, const state_matrix_t &Q, const output_matrix_t &R, const state_vector_t &x0=state_vector_t::Zero(), const state_matrix_t &P0=state_matrix_t::Zero())
	Constructor. More...

	ExtendedKalmanFilter (std::shared_ptr< SystemModelBase< STATE_DIM, CONTROL_DIM, SCALAR >> f, std::shared_ptr< LinearMeasurementModel< OUTPUT_DIM, STATE_DIM, SCALAR >> h, const ExtendedKalmanFilterSettings< STATE_DIM, SCALAR > &ekf_settings)
	Constructor from settings. More...

const state_vector_t &	predict (const control_vector_t &u, const ct::core::Time &dt, const ct::core::Time &t) override
	Estimator predict method. More...

const state_vector_t &	update (const output_vector_t &y, const ct::core::Time &dt, const ct::core::Time &t) override
	Estimator update method. More...

const state_matrix_t &	getCovarianceMatrix ()

void	setQ (const state_matrix_t &Q)
	update Q matrix More...

void	setR (const output_matrix_t &R)
	update R matrix More...

Public Member Functions inherited from ct::optcon::EstimatorBase< STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR >
	EstimatorBase (std::shared_ptr< SystemModelBase< STATE_DIM, CONTROL_DIM, SCALAR >> f, std::shared_ptr< LinearMeasurementModel< OUTPUT_DIM, STATE_DIM, SCALAR >> h, const state_vector_t &x0=state_vector_t::Zero())
	Constructor. More...

	EstimatorBase (const EstimatorBase &arg)
	Copy constructor. More...

virtual	~EstimatorBase ()=default

void	setSystemModel (std::shared_ptr< SystemModelBase< STATE_DIM, CONTROL_DIM, SCALAR >> f)
	update the system model More...

void	setMeasurementModel (std::shared_ptr< LinearMeasurementModel< OUTPUT_DIM, STATE_DIM, SCALAR >> h)
	update the measurement model More...

const state_vector_t &	getEstimate () const
	Estimate getter. More...

void	setEstimate (const state_vector_t &x)
	Estimate setter. More...

Protected Attributes
state_matrix_t	Q_
	Filter Q matrix. More...

output_matrix_t	R_
	Filter R matrix. More...

state_matrix_t	P_
	Covariance estimate. More...

Protected Attributes inherited from ct::optcon::EstimatorBase< STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR >
std::shared_ptr< SystemModelBase< STATE_DIM, CONTROL_DIM, SCALAR > >	f_
	System model for propagating the system. More...

std::shared_ptr< LinearMeasurementModel< OUTPUT_DIM, STATE_DIM, SCALAR > >	h_
	Observation model used to calculate the output error. More...

state_vector_t	x_est_
	State estimate. More...

Detailed Description

template<size_t STATE_DIM, size_t CONTROL_DIM, size_t OUTPUT_DIM, typename SCALAR = double>
class ct::optcon::ExtendedKalmanFilter< STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR >

Extended Kalman Filter implementation. For an algorithmic overview, see also https://en.wikipedia.org/wiki/Extended_Kalman_filter.

Examples:: KalmanDisturbanceFiltering.cpp, and KalmanFiltering.cpp.

Member Typedef Documentation

◆ Base

template<size_t STATE_DIM, size_t CONTROL_DIM, size_t OUTPUT_DIM, typename SCALAR = double>

using ct::optcon::ExtendedKalmanFilter< STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR >::Base = EstimatorBase<STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR>

◆ control_vector_t

template<size_t STATE_DIM, size_t CONTROL_DIM, size_t OUTPUT_DIM, typename SCALAR = double>

using ct::optcon::EstimatorBase< STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR >::control_vector_t = ct::core::ControlVector<CONTROL_DIM, SCALAR>

◆ output_matrix_t

template<size_t STATE_DIM, size_t CONTROL_DIM, size_t OUTPUT_DIM, typename SCALAR = double>

using ct::optcon::EstimatorBase< STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR >::output_matrix_t = ct::core::OutputMatrix<OUTPUT_DIM, SCALAR>

◆ output_vector_t

template<size_t STATE_DIM, size_t CONTROL_DIM, size_t OUTPUT_DIM, typename SCALAR = double>

using ct::optcon::EstimatorBase< STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR >::output_vector_t = ct::core::OutputVector<OUTPUT_DIM, SCALAR>

◆ state_matrix_t

template<size_t STATE_DIM, size_t CONTROL_DIM, size_t OUTPUT_DIM, typename SCALAR = double>

using ct::optcon::EstimatorBase< STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR >::state_matrix_t = ct::core::StateMatrix<STATE_DIM, SCALAR>

◆ state_vector_t

template<size_t STATE_DIM, size_t CONTROL_DIM, size_t OUTPUT_DIM, typename SCALAR = double>

using ct::optcon::EstimatorBase< STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR >::state_vector_t = ct::core::StateVector<STATE_DIM, SCALAR>

Constructor & Destructor Documentation

◆ ExtendedKalmanFilter() [1/2]

template<size_t STATE_DIM, size_t CONTROL_DIM, size_t OUTPUT_DIM, typename SCALAR >

ct::optcon::ExtendedKalmanFilter< STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR >::ExtendedKalmanFilter	(	std::shared_ptr< SystemModelBase< STATE_DIM, CONTROL_DIM, SCALAR >>	f,
		std::shared_ptr< LinearMeasurementModel< OUTPUT_DIM, STATE_DIM, SCALAR >>	h,
		const state_matrix_t &	Q,
		const output_matrix_t &	R,
		const state_vector_t &	x0 = `state_vector_t::Zero()`,
		const state_matrix_t &	P0 = `state_matrix_t::Zero()`
	)

Constructor.

◆ ExtendedKalmanFilter() [2/2]

template<size_t STATE_DIM, size_t CONTROL_DIM, size_t OUTPUT_DIM, typename SCALAR >

ct::optcon::ExtendedKalmanFilter< STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR >::ExtendedKalmanFilter	(	std::shared_ptr< SystemModelBase< STATE_DIM, CONTROL_DIM, SCALAR >>	f,
		std::shared_ptr< LinearMeasurementModel< OUTPUT_DIM, STATE_DIM, SCALAR >>	h,
		const ExtendedKalmanFilterSettings< STATE_DIM, SCALAR > &	ekf_settings
	)

Constructor from settings.

Member Function Documentation

◆ predict()

template<size_t STATE_DIM, size_t CONTROL_DIM, size_t OUTPUT_DIM, typename SCALAR >

auto ct::optcon::ExtendedKalmanFilter< STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR >::predict	(	const control_vector_t &	u,
		const ct::core::Time &	dt,
		const ct::core::Time &	t
	)

overridevirtual

Estimator predict method.

Implements ct::optcon::EstimatorBase< STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR >.

References dt, ct::optcon::EstimatorBase< STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR >::f_, ct::optcon::ExtendedKalmanFilter< STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR >::P_, ct::optcon::ExtendedKalmanFilter< STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR >::Q_, t, u, and ct::optcon::EstimatorBase< STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR >::x_est_.

Referenced by main().

◆ update()

template<size_t STATE_DIM, size_t CONTROL_DIM, size_t OUTPUT_DIM, typename SCALAR >

auto ct::optcon::ExtendedKalmanFilter< STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR >::update	(	const output_vector_t &	y,
		const ct::core::Time &	dt,
		const ct::core::Time &	t
	)

overridevirtual

Estimator update method.

Implements ct::optcon::EstimatorBase< STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR >.

References ct::optcon::EstimatorBase< STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR >::h_, ct::optcon::ExtendedKalmanFilter< STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR >::P_, ct::optcon::ExtendedKalmanFilter< STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR >::R_, t, and ct::optcon::EstimatorBase< STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR >::x_est_.

Referenced by main().

◆ getCovarianceMatrix()

template<size_t STATE_DIM, size_t CONTROL_DIM, size_t OUTPUT_DIM, typename SCALAR >

auto ct::optcon::ExtendedKalmanFilter< STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR >::getCovarianceMatrix ( )

References ct::optcon::ExtendedKalmanFilter< STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR >::P_.

Referenced by main().

◆ setQ()

template<size_t STATE_DIM, size_t CONTROL_DIM, size_t OUTPUT_DIM, typename SCALAR = double>

void ct::optcon::ExtendedKalmanFilter< STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR >::setQ ( const state_matrix_t & Q )

inline

update Q matrix

References ct::optcon::ExtendedKalmanFilter< STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR >::Q_.

◆ setR()

template<size_t STATE_DIM, size_t CONTROL_DIM, size_t OUTPUT_DIM, typename SCALAR = double>

void ct::optcon::ExtendedKalmanFilter< STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR >::setR ( const output_matrix_t & R )

inline

update R matrix

References ct::optcon::ExtendedKalmanFilter< STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR >::R_.

Member Data Documentation

◆ Q_

template<size_t STATE_DIM, size_t CONTROL_DIM, size_t OUTPUT_DIM, typename SCALAR = double>

state_matrix_t ct::optcon::ExtendedKalmanFilter< STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR >::Q_

protected

Filter Q matrix.

Referenced by ct::optcon::ExtendedKalmanFilter< STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR >::predict(), and ct::optcon::ExtendedKalmanFilter< STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR >::setQ().

◆ R_

template<size_t STATE_DIM, size_t CONTROL_DIM, size_t OUTPUT_DIM, typename SCALAR = double>

output_matrix_t ct::optcon::ExtendedKalmanFilter< STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR >::R_

protected

Filter R matrix.

Referenced by ct::optcon::ExtendedKalmanFilter< STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR >::setR(), and ct::optcon::ExtendedKalmanFilter< STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR >::update().

◆ P_

template<size_t STATE_DIM, size_t CONTROL_DIM, size_t OUTPUT_DIM, typename SCALAR = double>

state_matrix_t ct::optcon::ExtendedKalmanFilter< STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR >::P_

protected

Covariance estimate.

Referenced by ct::optcon::ExtendedKalmanFilter< STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR >::getCovarianceMatrix(), ct::optcon::ExtendedKalmanFilter< STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR >::predict(), and ct::optcon::ExtendedKalmanFilter< STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR >::update().

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

/home/gim2rng/ct_devel_ws/src/control-toolbox/ct_optcon/include/ct/optcon/filter/ExtendedKalmanFilter.h
/home/gim2rng/ct_devel_ws/src/control-toolbox/ct_optcon/include/ct/optcon/filter/ExtendedKalmanFilter-impl.h

Public Types

Public Member Functions

Protected Attributes

Detailed Description

template<size_t STATE_DIM, size_t CONTROL_DIM, size_t OUTPUT_DIM, typename SCALAR = double> class ct::optcon::ExtendedKalmanFilter< STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR >

Member Typedef Documentation

◆ Base

◆ control_vector_t

◆ output_matrix_t

◆ output_vector_t

◆ state_matrix_t

◆ state_vector_t

Constructor & Destructor Documentation

◆ ExtendedKalmanFilter() [1/2]

◆ ExtendedKalmanFilter() [2/2]

Member Function Documentation

◆ predict()

◆ update()

◆ getCovarianceMatrix()

◆ setQ()

◆ setR()

Member Data Documentation

◆ Q_

◆ R_

◆ P_

template<size_t STATE_DIM, size_t CONTROL_DIM, size_t OUTPUT_DIM, typename SCALAR = double>
class ct::optcon::ExtendedKalmanFilter< STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR >