#include <extended-kalman-filter.hpp>

Inheritance diagram for stateObservation::ExtendedKalmanFilter:

Collaboration diagram for stateObservation::ExtendedKalmanFilter:

Classes
struct	Optimization

Public Member Functions
	ExtendedKalmanFilter (Index n, Index m)
	ExtendedKalmanFilter (Index n, Index m, Index p, bool directInputOutputFeedthrough=true, bool directInputStateProcessFeedthrough=true)
	ExtendedKalmanFilter (Index n, Index nt, Index m, Index mt, Index p, bool directInputOutputFeedthrough, bool directInputStateProcessFeedthrough)
void	setFunctor (DynamicalSystemFunctorBase *f)
DynamicalSystemFunctorBase *	getFunctor (void) const
DynamicalSystemFunctorBase *	functor () const
	Gets a pointer to the functor.
void	clearFunctor ()
void	setDirectInputOutputFeedthrough (bool b=true)
void	setDirectInputStateFeedthrough (bool b=true)
virtual Amatrix	getAMatrixFD (const Vector &dx)
virtual Cmatrix	getCMatrixFD (const Vector &dx)
virtual void	reset ()
	Reset the extended kalman filter (call also the reset function of the dynamics functor).
Public Member Functions inherited from stateObservation::KalmanFilterBase
	KalmanFilterBase ()
	Default constructor.
	KalmanFilterBase (Index n, Index m, Index p=0)
	KalmanFilterBase (Index n, Index nt, Index m, Index mt, Index p)
virtual void	setA (const Amatrix &A)
	Set the value of the jacobian df/dx.
virtual Matrix	getA () const
virtual void	clearA ()
	Clear the jacobian df/dx.
virtual void	setC (const Cmatrix &C)
	Set the value of the Jacobian dh/dx.
virtual Matrix	getC () const
virtual void	clearC ()
	Clear the jacobian dh/dx.
virtual void	setR (const Rmatrix &R)
	Set the measurement noise covariance matrix.
void	setMeasurementCovariance (const Rmatrix &R)
virtual Matrix	getR () const
	Set the measurement noise covariance matrix.
Matrix	getMeasurementCovariance () const
virtual void	clearR ()
	Clear the measurement noise covariance matrix.
virtual void	setQ (const Qmatrix &Q)
	Set the process noise covariance matrix.
void	setProcessCovariance (const Qmatrix &Q)
virtual Matrix	getQ () const
	Set the process noise covariance matrix.
Matrix	getProcessCovariance () const
virtual void	clearQ ()
	Clear the process noise covariance matrix.
virtual void	setStateCovariance (const Pmatrix &P)
	Set the covariance matrix of the current time state estimation error.
virtual void	clearStates ()
	clears the state and the state covariance
virtual void	clearStateCovariance ()
virtual Pmatrix	getStateCovariance () const
	Get the covariance matrix of the current time state estimation.
Amatrix	getAmatrixConstant (double c) const
	Get a matrix having the size of the A matrix having "c" values.
Amatrix	getAmatrixRandom () const
	Get a matrix having the size of the A matrix having random values.
Amatrix	getAmatrixZero () const
	Get a matrix having the size of the A matrix having zero values.
Amatrix	getAmatrixIdentity () const
	Get an identity matrix having the size of the A matrix.
bool	checkAmatrix (const Amatrix &) const
	checks whether or not a matrix has the dimensions of the A matrix
Cmatrix	getCmatrixConstant (double c) const
	Get a matrix having the size of the C matrix having "c" values.
Cmatrix	getCmatrixRandom () const
	Get a matrix having the size of the C matrix having random values.
Cmatrix	getCmatrixZero () const
	Get a matrix having the size of the C matrix having zero values.
bool	checkCmatrix (const Cmatrix &) const
	checks whether or not a matrix has the dimensions of the C matrix
Qmatrix	getQmatrixConstant (double c) const
	Get a matrix having the size of the Q matrix having "c" values.
Qmatrix	getQmatrixRandom () const
	Get a matrix having the size of the Q matrix having random values.
Qmatrix	getQmatrixZero () const
	Get a matrix having the size of the Q matrix having zero values.
Qmatrix	getQmatrixIdentity () const
	Get an identity matrix having the size of the Q matrix.
bool	checkQmatrix (const Qmatrix &) const
	checks whether or not a matrix has the dimensions of the Q matrix
Rmatrix	getRmatrixConstant (double c) const
	Get a matrix having the size of the R matrix having "c" values.
Rmatrix	getRmatrixRandom () const
	Get a matrix having the size of the R matrix having random values.
Rmatrix	getRmatrixZero () const
	Get a matrix having the size of the R matrix having zero values.
Rmatrix	getRmatrixIdentity () const
	Get an identity matrix having the size of the R matrix.
bool	checkRmatrix (const Rmatrix &) const
	checks whether or not a matrix has the dimensions of the R matrix
Pmatrix	getPmatrixConstant (double c) const
	Get a matrix having the size of the P matrix having "c" values.
Pmatrix	getPmatrixRandom () const
	Get a matrix having the size of the P matrix having random values.
Pmatrix	getPmatrixZero () const
	Get a matrix having the size of the P matrix having zero values.
Pmatrix	getPmatrixIdentity () const
	Get an identity matrix having the size of the P matrix.
bool	checkPmatrix (const Pmatrix &) const
	Checks whether or not a matrix has the dimensions of the P matrix.
virtual StateVectorTan	stateTangentVectorConstant (double c) const
	Gives a vector of state tangent vector size having duplicated "c" value.
virtual StateVectorTan	stateTangentVectorRandom () const
	Gives a vector of state tangent vector size having random values.
virtual StateVectorTan	stateTangentVectorZero () const
	Gives a vector of state tangent vector size having zero values.
virtual bool	checkStateTangentVector (const StateVectorTan &v) const
	Tells whether or not the vector has the dimensions of a state tangent vector.
virtual MeasureVectorTan	measureTangentVectorConstant (double c) const
	Gives a vector of measurement tangent vector size having duplicated "c" value.
virtual MeasureVectorTan	measureTangentVectorRandom () const
	Gives a vector of measurement tangent vector size having random values.
virtual MeasureVectorTan	measureTangentVectorZero () const
	Gives a vector of measurement tangent vector size having zero values.
virtual bool	checkMeasureTangentVector (const MeasureVectorTan &) const
	Tells whether or not the vector has the dimensions of a measurement tangent vector.
virtual void	setStateSize (Index n)
virtual void	setStateSize (Index n, Index nt)
virtual void	setMeasureSize (Index m)
virtual void	setMeasureSize (Index m, Index mt)
virtual MeasureVector	getSimulatedMeasurement (TimeIndex k)
	Get simulation of the measurement y_k using the state estimation.
virtual const StateVector &	getInnovation ()
	Get the last vector of innovation of the Kalman filter.
const StateVector &	updateStatePrediction ()
MeasureVector	updateStateAndMeasurementPrediction ()
const StateVector &	getLastPrediction () const
	get the last predicted state
const MeasureVector &	getLastPredictedMeasurement () const
	get the last predicted measurement
const MeasureVector &	getLastMeasurement () const
	get the last measurement
const Matrix &	getLastGain () const
	get the last Kalman gain matrix
void	setStateArithmetics (StateVectorArithmetics *arith)
Public Member Functions inherited from stateObservation::ZeroDelayObserver
	ZeroDelayObserver (Index n, Index m, Index p=0)
	ZeroDelayObserver ()
	Default constructor (default values for n,m,p are zero).
virtual	~ZeroDelayObserver ()
	Destructor.
virtual void	setState (const ObserverBase::StateVector &x_k, TimeIndex k)
	Set the value of the state vector at time index k.
virtual void	setCurrentState (const ObserverBase::StateVector &x_k)
	Modify the value of the state vector at the current time.
bool	stateIsSet () const
	Returns if the state is set or not. The state being set is mandatory to start the estimator.
virtual void	setMeasurement (const ObserverBase::MeasureVector &y_k, TimeIndex k)
virtual void	pushMeasurement (const ObserverBase::MeasureVector &y_k)
	Sets the measurement value at the next time index.
virtual void	clearMeasurements ()
	Remove all the given values of the measurements.
virtual void	setInput (const ObserverBase::InputVector &u_k, TimeIndex k)
virtual void	pushInput (const ObserverBase::InputVector &u_k)
	Set the input value at the next time indext.
virtual void	clearInputs ()
virtual void	clearInputsAndMeasurements ()
	Remove all the given values of the inputs and measurements.
virtual TimeIndex	estimateState ()
	run the observer until the measurement vector is depleted.
virtual ObserverBase::StateVector	getEstimatedState (TimeIndex k)
	getestimated State
virtual ObserverBase::StateVector	getCurrentEstimatedState () const
	Get the Current Estimated State.
virtual TimeIndex	getCurrentTime () const
	Get the value of the time index of the current state estimation.
Vector	getInput (TimeIndex k) const
	Get the value of the input of the time index k.
virtual TimeSize	getInputsNumber () const
	Get the number of available inputs.
virtual TimeIndex	getInputTime () const
	Get the time index of the last given input.
Vector	getMeasurement (TimeIndex k) const
	Get the measurement of the time index k.
virtual TimeIndex	getMeasurementTime () const
	Get the time index of the last given measurement.
virtual TimeSize	getMeasurementsNumber () const
	Gets the number of regitered measurements.
virtual void	setInputSize (Index p)
	changes the size of the input vector: reset the stored input vectors
Public Member Functions inherited from stateObservation::ObserverBase
	ObserverBase (Index n, Index m, Index p=0)
	ObserverBase ()
	default constructor (default values for n,m,p are zero)
virtual	~ObserverBase ()
	Destructor.
virtual Index	getStateSize () const
	gets the size of the state vector
virtual Index	getMeasureSize () const
	gets the size of the measurement vector
virtual Index	getInputSize () const
	gets the size of the input vector
virtual StateVector	stateVectorConstant (double c) const
virtual StateVector	stateVectorRandom () const
	Gives a vector of state vector size having random values.
virtual StateVector	stateVectorZero () const
	Gives a vector of state vector size having zero values.
virtual bool	checkStateVector (const StateVector &v) const
	Tells whether or not the vector has the dimensions of a state vector.
virtual MeasureVector	measureVectorConstant (double c) const
	Gives a vector of measurement vector size having duplicated "c" value.
virtual MeasureVector	measureVectorRandom () const
	Gives a vector of measurement vector size having random values.
virtual MeasureVector	measureVectorZero () const
	Gives a vector of measurement vector size having zero values.
virtual bool	checkMeasureVector (const MeasureVector &) const
	Tells whether or not the vector has the dimensions of a measurement vector.
virtual InputVector	inputVectorConstant (double c) const
	Gives a vector of input vector size having duplicated "c" value.
virtual InputVector	inputVectorRandom () const
	Gives a vector of input vector size having random values.
virtual InputVector	inputVectorZero () const
	Gives a vector of input vector size having zero values.
virtual bool	checkInputVector (const InputVector &) const
	Tells whether or not the vector has the dimensions of a input vector.

Protected Member Functions
virtual StateVector	prediction_ (TimeIndex k)
	simulate the dynamics of the state using the functor
virtual MeasureVector	simulateSensor_ (const StateVector &x, TimeIndex k)
	simulate the dynamic of the measurement using the functor
virtual MeasureVector	predictSensor_ (TimeIndex k)
	predicts the measurement using the functor, assumed that the predicted state is up-to-date
Protected Member Functions inherited from stateObservation::KalmanFilterBase
virtual StateVector	oneStepEstimation_ ()
	The Kalman filter loop.
Protected Member Functions inherited from stateObservation::StateVectorArithmetics
virtual void	stateSum (const Vector &stateVector, const Vector &tangentVector, Vector &sum)
virtual void	stateDifference (const Vector &stateVector1, const Vector &stateVector2, Vector &difference)
virtual void	measurementDifference (const Vector &measureVector1, const Vector &measureVector2, Vector &difference)

Protected Attributes
bool	directInputOutputFeedthrough_
	boolean that provides if theris a need of not for input for the masurement
bool	directInputStateProcessFeedthrough_
	boolean that provides if theris a need of not for input for the state dynamics
DynamicalSystemFunctorBase *	f_
	pointer on the dynamics functor
struct stateObservation::ExtendedKalmanFilter::Optimization	opt
Protected Attributes inherited from stateObservation::KalmanFilterBase
Index	nt_
	the size of tangent space of the state space
Index	mt_
	the size of tangent space of the measurement space
Matrix	a_
	Containers for the jacobian matrix of the process.
Matrix	c_
	Containers for the jacobian matrix of the measurement.
Matrix	q_
	Container for the process noise covariance matrice.
Matrix	r_
	Container for the measurement noise covariance matrice.
Matrix	pr_
	Container for the covariance matrix of the estimation error.
IndexedVector	xbar_
	container for the prediction
IndexedVector	ybar_
	container for the prediction of the sensor
Vector	innovation_
	Vector containing the inovation of the Kalman filter.
struct stateObservation::KalmanFilterBase::optimizationContainer	oc_
StateVectorArithmetics *	arithm_
Protected Attributes inherited from stateObservation::ZeroDelayObserver
IndexedVector	x_
	while the measurements and iputs are put in lists
IndexedVectorArray	y_
	Container for the measurements.
IndexedVectorArray	u_
	Container for the inputs.
Protected Attributes inherited from stateObservation::ObserverBase
Index	n_
	stateSize is the size of the state vector
Index	m_
	measureSize is the size of measurements vector
Index	p_
	inputSize is the size of the input vector

Additional Inherited Members
Public Types inherited from stateObservation::KalmanFilterBase
typedef Matrix	Amatrix
	The type of the jacobian df/dx.
typedef Matrix	Cmatrix
	The type of the jacobian dh/dx.
typedef Matrix	Qmatrix
	The type of the covariance matrix of the process noise v.
typedef Matrix	Rmatrix
	The type of the covariance matrix of the measurement noise w.
typedef Matrix	Pmatrix
	The type of the covariance matrix of the state estimation error.
typedef Eigen::LLT< Pmatrix >	LLTPMatrix
typedef Vector	StateVectorTan
	StateVector is the type of state tangent vector.
typedef Vector	MeasureVectorTan
	MeasureVector is the type of measurement tanegnt vector.
Public Types inherited from stateObservation::ObserverBase
typedef Vector	StateVector
	StateVector is the type of state vector.
typedef Vector	MeasureVector
	MeasureVector is the type of measurements vector.
typedef Vector	InputVector
	InputVector is the type of the input vector.
Protected Types inherited from stateObservation::KalmanFilterBase
typedef Matrix	Kmatrix
	The type of Kalman gain matrix.

Detailed Description

  The class to intanciate to use an extended Kalman filter.
  To use this class, one needs to provide a pointer on a functor
  that describes the state dynamics and the measurement dynamics.
  The functor type needs to be a derived class from the class
  DynamicsFunctorBase.

  x_{k+1}=f(x_k,u_k)+v_k

  y_k=h(x_k,u_k)+w_k

Definition at line 42 of file extended-kalman-filter.hpp.

Constructor & Destructor Documentation

◆ ExtendedKalmanFilter() [1/3]

stateObservation::ExtendedKalmanFilter::ExtendedKalmanFilter	(	Index	n,
		Index	m )

The constructor.

n : size of the state vector
m : size of the measurements vector

◆ ExtendedKalmanFilter() [2/3]

stateObservation::ExtendedKalmanFilter::ExtendedKalmanFilter	(	Index	n,
		Index	m,
		Index	p,
		bool	directInputOutputFeedthrough = true,
		bool	directInputStateProcessFeedthrough = true )

The constructor.

n : size of the state vector
m : size of the measurements vector
p : size of the input vector
The parameter directInputOutputFeedthrough defines whether (true) or not (false) the measurement y_k requires the input u_k
The parameter directInputStateProcessFeedthrough defines whether (true) or not (false) the state x_{k+1} requires the input u_k

◆ ExtendedKalmanFilter() [3/3]

stateObservation::ExtendedKalmanFilter::ExtendedKalmanFilter	(	Index	n,
		Index	nt,
		Index	m,
		Index	mt,
		Index	p,
		bool	directInputOutputFeedthrough,
		bool	directInputStateProcessFeedthrough )

The constructor.

n : size of the state vector
nt: size of the state tengent vector representation (usually nt<=n)
m : size of the measurements vector
p : size of the input vector
The parameter directInputOutputFeedthrough defines whether (true) or not (false) the measurement y_k requires the input u_k
The parameter directInputStateProcessFeedthrough defines whether (true) or not (false) the state x_{k+1} requires the input u_k

Member Function Documentation

◆ setFunctor()

void stateObservation::ExtendedKalmanFilter::setFunctor ( DynamicalSystemFunctorBase * f )

Set a pointer to the functor that defines the dynamics of the states and the measurement the user is responsible for the validity of the pointer during the execution of the kalman filter

◆ clearFunctor()

void stateObservation::ExtendedKalmanFilter::clearFunctor ( )

Clear the value of the functor Does not destroy the pointed object

◆ setDirectInputOutputFeedthrough()

void stateObservation::ExtendedKalmanFilter::setDirectInputOutputFeedthrough ( bool b = true )

Precise whether (true) or not (false) the measurement y_k requires the input u_k

◆ setDirectInputStateFeedthrough()

void stateObservation::ExtendedKalmanFilter::setDirectInputStateFeedthrough ( bool b = true )

Precise whether (true) or not (false) the estimation of the state x_{k+1} requires the input u_k

◆ getAMatrixFD()

virtual Amatrix stateObservation::ExtendedKalmanFilter::getAMatrixFD ( const Vector & dx )

virtual

Give an estimation of A matrix using finite difference method (the forward difference method) the parameter dx is the step vector for derivation it must have the size nt (tangent vector)

◆ getCMatrixFD()

virtual Cmatrix stateObservation::ExtendedKalmanFilter::getCMatrixFD ( const Vector & dx )

virtual

Give an estimation of C matrix using finite difference method (the forward difference method) the parameter dx is the step vector for derivation it must have the size nt (tangent vector)

◆ reset()

virtual void stateObservation::ExtendedKalmanFilter::reset ( )

virtual

Reset the extended kalman filter (call also the reset function of the dynamics functor).

Reimplemented from stateObservation::KalmanFilterBase.

◆ prediction_()

virtual StateVector stateObservation::ExtendedKalmanFilter::prediction_ ( TimeIndex k )

protectedvirtual

simulate the dynamics of the state using the functor

Implements stateObservation::KalmanFilterBase.

◆ simulateSensor_()

virtual MeasureVector stateObservation::ExtendedKalmanFilter::simulateSensor_	(	const StateVector &	x,
		TimeIndex	k )

protectedvirtual

simulate the dynamic of the measurement using the functor

Implements stateObservation::KalmanFilterBase.

◆ predictSensor_()

virtual MeasureVector stateObservation::ExtendedKalmanFilter::predictSensor_ ( TimeIndex k )

protectedvirtual

predicts the measurement using the functor, assumed that the predicted state is up-to-date

Reimplemented from stateObservation::KalmanFilterBase.

Member Data Documentation

◆ directInputOutputFeedthrough_

bool stateObservation::ExtendedKalmanFilter::directInputOutputFeedthrough_

protected

boolean that provides if theris a need of not for input for the masurement

Definition at line 130 of file extended-kalman-filter.hpp.

◆ directInputStateProcessFeedthrough_

bool stateObservation::ExtendedKalmanFilter::directInputStateProcessFeedthrough_

protected

boolean that provides if theris a need of not for input for the state dynamics

Definition at line 133 of file extended-kalman-filter.hpp.

◆ f_

DynamicalSystemFunctorBase* stateObservation::ExtendedKalmanFilter::f_

protected

pointer on the dynamics functor

Definition at line 136 of file extended-kalman-filter.hpp.

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

/build/source/include/state-observation/observer/extended-kalman-filter.hpp

Classes

Public Member Functions

Protected Member Functions

Protected Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ ExtendedKalmanFilter() [1/3]

◆ ExtendedKalmanFilter() [2/3]

◆ ExtendedKalmanFilter() [3/3]

Member Function Documentation

◆ setFunctor()

◆ clearFunctor()

◆ setDirectInputOutputFeedthrough()

◆ setDirectInputStateFeedthrough()

◆ getAMatrixFD()

◆ getCMatrixFD()

◆ reset()

◆ prediction_()

◆ simulateSensor_()

◆ predictSensor_()

Member Data Documentation

◆ directInputOutputFeedthrough_

◆ directInputStateProcessFeedthrough_

◆ f_