Classical ExtendedKalman-filter (EKF). More...

#include <StateEstimator/ExtendedKalman.hh>

Inheritance diagram for BIAS::ExtendedKalman:

Collaboration diagram for BIAS::ExtendedKalman:

Public Member Functions
void	AddDebugLevel (const long int lv)

void	AddDebugLevel (const std::string &name)

bool	DebugLevelIsSet (const long int lv) const

bool	DebugLevelIsSet (const std::string &name) const

	ExtendedKalman ()

void	GetCovariance (Matrix< double > &cov) const
	Returns the system state covariance. More...

int	GetDebugLevel () const

std::ostream &	GetDebugStream () const

void	GetDebugStream (std::ostream &os) const

void	GetState (Vector< double > &state) const
	Returns the predicted system state. More...

void	GetStatePosteriori (Vector< double > &state) const
	Returns the corrected system state, which is the update-corrected state. More...

virtual void	MeasurementFunction (const Vector< double > &state, Vector< double > &measurePred)=0
	Implement this to correlate the actual state with the 'predicted' measurement (h in [1])! It should 'extract' the measurement from the state! More...

long int	Name2DebugLevel (const std::string &name) const
	looks up a debuglevel in the internal map, returns 0 if not found More...

long int	NewDebugLevel (const std::string &name)
	creates a new debuglevel More...

int	Predict (const Vector< double > &control)
	Does preditcion (with respect to the control parameters – u in [1]) for a single timestep! (use in alternation with Update(), Update first) More...

void	PrintDebugLevel (std::ostream &os=std::cout) const

void	RemoveDebugLevel (const long int lv)

void	RemoveDebugLevel (const std::string &name)

void	SetDebugLevel (const long int lv)

void	SetDebugLevel (const std::string &name)

void	SetDebugStream (const std::ostream &os)

void	SetInitial (const Vector< double > &initialState, const Matrix< double > &initialCov)
	Use this function to set the initial state and system-covariance (x(0) and P(0) in [1])! More...

void	SetMeasureCov (const Matrix< double > &measureNoiseCov)
	Set measurement noise covariance – R in [1] (may be updated for every timestep)! Measurement noise is assumed to be white and normaly distributed. More...

void	SetMeasureDerive (const Matrix< double > &derivedMeasurement, const Matrix< double > &derivedMeasurementError)
	Same as above for measurement function! The DerivedMeasurement matrix (H in [1]) contains the partial derivates of the measurement-function (h in [1]) with respect to each state komponent (dhi/dxj(i,j)). More...

void	SetProcessCov (const Matrix< double > &processNoiseCov)
	Set the process noise covariance – Q in [1] (may be updated for every timestep)! Process noise is assumed to be white and normaly distributed. More...

void	SetStateDerive (const Matrix< double > &derivedStateTrans, const Matrix< double > &derivedStateTransError)
	Set the differentiation-matrices for the EKF (see [1] for details). More...

void	ShowDebugLevel (std::ostream &os=std::cout) const
	prints all internally known debuglevels More...

virtual void	StateTransition (const Vector< double > &oldState, const Vector< double > &control, Vector< double > &newState)=0
	Implement the statetransition-function (f in [1]) here. More...

int	Update (const Vector< double > &measurement)
	Calculates the update step. More...

virtual	~ExtendedKalman ()

Static Public Member Functions
static long int	GetGlobalDebugLevel ()

static void	SetGlobalDebugLevel (long int lev)

Protected Member Functions
long	ConsumeNextFreeDebuglevel_ ()
	returns the next available debuglevel More...

Protected Attributes
long int	_liDebugLevel

long int	_liNextDebugLevel
	new concept, debuglevel are managed here in the debug class More...

std::map< std::string, long int >	_String2Debuglevel

Matrix< double >	A_
	derived state-trans. matrix and transposed More...

Matrix< double >	AT_

Matrix< double >	covariance_
	The covariance matrix corresponding to State_. More...

Matrix< double >	H_
	derived measurement matrix and transposed More...

Matrix< double >	HT_

Matrix< double >	Identity_
	identity matrix More...

Matrix< double >	K_
	and finally the Kalman gain More...

Vector< double >	lastKalmanUpdate_
	maybe needed for iterated update More...

int	measurementDim_

Vector< double >	posterioState_
	Contains the corrected (a posteriori) system estimate. More...

Vector< double >	priorState_
	Contains the prediction (a priori/initial) system estimate. More...

Matrix< double >	Q_
	system noise cov. More...

Matrix< double >	R_
	measurement noise cov. More...

SVD	svd_
	needed for matrix inversion More...

int	systemStateDim_
	dimensions of system space and measurement space More...

Matrix< double >	V_
	derived measurement error matrix and transposed More...

Matrix< double >	VT_

Matrix< double >	W_
	derived state-trans. error matrix and transposed More...

Matrix< double >	WT_

Static Protected Attributes
static std::ostream	_zDebugStream

static long int	GlobalDebugLevel = 0

Detailed Description

Classical ExtendedKalman-filter (EKF).

Implemented according to: www.cs.unc.edu/~tracker/media/pdf/SIGGRAPH2001_CoursePack_08.pdf [1] Implements an Extended Kalman Filter (non-linear),estimating the state of a system governed by a given non-linear statetransition-funcition. Derive from this class an implement the virtual statetrans.- and measurement-function. After setting the differentiation matrices you will get an EKF for your specific problem.

Author: apetersen 03.04.07

Definition at line 49 of file ExtendedKalman.hh.

Constructor & Destructor Documentation

ExtendedKalman::ExtendedKalman ( )

Definition at line 6 of file ExtendedKalman.cpp.

virtual BIAS::ExtendedKalman::~ExtendedKalman ( )

inlinevirtual

Definition at line 54 of file ExtendedKalman.hh.

Member Function Documentation

void BIAS::Debug::AddDebugLevel ( const long int lv )

inlineinherited

Examples:: EvaluateAlignment.cpp, ExampleAlignment.cpp, ExampleMinMax.cpp, ExampleRANSAC_double.cpp, ExampleRANSACPlane.cpp, and ExampleTriangulateOptimal.cpp.

Definition at line 355 of file Debug.hh.

Referenced by BIAS::GLProjectionParametersBase::AddDebugLevel(), BIAS::Triangulation::CorrectCorrespondences(), BIAS::FilterBase< InputStorageType, OutputStorageType >::FilterBase(), BIAS::ShowCamWxVideoSourceFactory::OpenDevice(), and BIAS::SceneOpenSceneGraph::SceneOpenSceneGraph().

void BIAS::Debug::AddDebugLevel ( const std::string & name )

inlineinherited

Definition at line 362 of file Debug.hh.

long BIAS::Debug::ConsumeNextFreeDebuglevel_ ( )

inlineprotectedinherited

returns the next available debuglevel

Author: woelk 09/2006

Definition at line 521 of file Debug.hh.

bool BIAS::Debug::DebugLevelIsSet ( const long int lv ) const

inlineinherited

Examples:: ExampleTriangulateOptimal.cpp.

Definition at line 341 of file Debug.hh.

Referenced by BIAS::GenSynthMatches::_AddUniformDistributedOutliers(), BIAS::GenSynthMatches::_CreateCamMovement(), BIAS::GenSynthMatches::_CreateStatic2DPoints(), BIAS::UnscentedTransform::ComputeSigmaPoints_(), BIAS::Triangulation::CorrectCorrespondences(), BIAS::GenSynthMatches::GetGTNormalizedF(), BIAS::MonteCarloTransform::GetSamples_(), and BIAS::MonteCarloTransform::Transform().

bool BIAS::Debug::DebugLevelIsSet ( const std::string & name ) const

inlineinherited

Definition at line 350 of file Debug.hh.

void BIAS::ExtendedKalman::GetCovariance ( Matrix< double > & cov ) const

inline

Returns the system state covariance.

It could be the covariance of the prediction, if called after the predict step or the covariance of the posterior, if GetState is called after the update step.

Definition at line 72 of file ExtendedKalman.hh.

int BIAS::Debug::GetDebugLevel ( ) const

inlineinherited

Examples:: ExampleCamera.cpp, ExampleCamPoseCalib.cpp, ExampleCopy.cpp, and ExampleParabola.cpp.

Definition at line 332 of file Debug.hh.

Referenced by BIAS::listenthread(), BIAS::Image< StorageType >::operator=(), and BIAS::ImageBase::operator=().

std::ostream& BIAS::Debug::GetDebugStream ( ) const

inlineinherited

Definition at line 405 of file Debug.hh.

void BIAS::Debug::GetDebugStream ( std::ostream & os ) const

inlineinherited

Definition at line 414 of file Debug.hh.

static long int BIAS::Debug::GetGlobalDebugLevel ( )

inlinestaticinherited

Definition at line 431 of file Debug.hh.

void BIAS::ExtendedKalman::GetState ( Vector< double > & state ) const

inline

Returns the predicted system state.

This is the prediction, it is to be called after the predict step!

Definition at line 59 of file ExtendedKalman.hh.

void BIAS::ExtendedKalman::GetStatePosteriori ( Vector< double > & state ) const

inline

Returns the corrected system state, which is the update-corrected state.

This is to be called after the update step!

Definition at line 65 of file ExtendedKalman.hh.

virtual void BIAS::ExtendedKalman::MeasurementFunction	(	const Vector< double > &	state,
		Vector< double > &	measurePred
	)

pure virtual

Implement this to correlate the actual state with the 'predicted' measurement (h in [1])! It should 'extract' the measurement from the state!

long int BIAS::Debug::Name2DebugLevel ( const std::string & name ) const

inlineinherited

looks up a debuglevel in the internal map, returns 0 if not found

Author: woelk 09/2006

Definition at line 454 of file Debug.hh.

long int BIAS::Debug::NewDebugLevel ( const std::string & name )

inlineinherited

creates a new debuglevel

Author: woelk 09/2006

Definition at line 474 of file Debug.hh.

Referenced by BIAS::COSAC< SolutionType >::COSAC(), BIAS::ProjectionParametersPerspectiveDepth::Init(), BIAS::MonteCarloTransform::MonteCarloTransform(), BIAS::PMatrixEstimation::PMatrixEstimation(), BIAS::PreemptiveRANSAC< SolutionType >::PreemptiveRANSAC(), BIAS::RANSACPreKnowledge< SolutionType >::RANSACPreKnowledge(), BIAS::ROI::ROI(), BIAS::RotationAveraging::RotationAveraging(), BIAS::SceneOpenSceneGraph::SceneOpenSceneGraph(), BIAS::ThreeDOutOpenSceneGraph::ThreeDOutOpenSceneGraph(), BIAS::Triangulation::Triangulation(), and BIAS::UnscentedTransform::UnscentedTransform().

int ExtendedKalman::Predict ( const Vector< double > & control )

Does preditcion (with respect to the control parameters – u in [1]) for a single timestep! (use in alternation with Update(), Update first)

Definition at line 13 of file ExtendedKalman.cpp.

void BIAS::Debug::PrintDebugLevel ( std::ostream & os = std::cout ) const

inlineinherited

Definition at line 383 of file Debug.hh.

void BIAS::Debug::RemoveDebugLevel ( const long int lv )

inlineinherited

Definition at line 369 of file Debug.hh.

void BIAS::Debug::RemoveDebugLevel ( const std::string & name )

inlineinherited

Definition at line 376 of file Debug.hh.

void BIAS::Debug::SetDebugLevel ( const long int lv )

inlineinherited

void BIAS::Debug::SetDebugLevel ( const std::string & name )

inlineinherited

Definition at line 325 of file Debug.hh.

void BIAS::Debug::SetDebugStream ( const std::ostream & os )

inlineinherited

Definition at line 398 of file Debug.hh.

static void BIAS::Debug::SetGlobalDebugLevel ( long int lev )

inlinestaticinherited

Definition at line 424 of file Debug.hh.

void BIAS::ExtendedKalman::SetInitial	(	const Vector< double > &	initialState,
		const Matrix< double > &	initialCov
	)

inline

Use this function to set the initial state and system-covariance (x(0) and P(0) in [1])!

Definition at line 90 of file ExtendedKalman.hh.

References TNT::Vector< T >::size().

void BIAS::ExtendedKalman::SetMeasureCov ( const Matrix< double > & measureNoiseCov )

inline

Set measurement noise covariance – R in [1] (may be updated for every timestep)! Measurement noise is assumed to be white and normaly distributed.

Definition at line 119 of file ExtendedKalman.hh.

void BIAS::ExtendedKalman::SetMeasureDerive	(	const Matrix< double > &	derivedMeasurement,
		const Matrix< double > &	derivedMeasurementError
	)

inline

Same as above for measurement function! The DerivedMeasurement matrix (H in [1]) contains the partial derivates of the measurement-function (h in [1]) with respect to each state komponent (dhi/dxj(i,j)).

The matrix DerivedMeasurementError (V in [1]) contains the partial derivatives of h with respect to each measurement-noise-komponent (dhi/dvj(i,j)).

These matrices may be changed for every timestep! (according to the 'real' derivatives h)

Definition at line 108 of file ExtendedKalman.hh.

References BIAS::Matrix< T >::Transpose().

void BIAS::ExtendedKalman::SetProcessCov ( const Matrix< double > & processNoiseCov )

inline

Set the process noise covariance – Q in [1] (may be updated for every timestep)! Process noise is assumed to be white and normaly distributed.

Definition at line 126 of file ExtendedKalman.hh.

void BIAS::ExtendedKalman::SetStateDerive	(	const Matrix< double > &	derivedStateTrans,
		const Matrix< double > &	derivedStateTransError
	)

inline

Set the differentiation-matrices for the EKF (see [1] for details).

The DerivedStateTrans matrix (A in [1]) contains the partial deriva- tives of the statetrans.-function (f in [1]) with respect to each state komponent (dfi/dxj(i,j)). The matrix DerivedStateTransError (W in [1]) contains the partial derivatives of f with respect to each process-noise-komponent (dfi/dwj(i,j)).

These matrices may be changed for every timestep! (according to the 'real' derivatives of f)

Definition at line 139 of file ExtendedKalman.hh.

References BIAS::Matrix< T >::Transpose().

void BIAS::Debug::ShowDebugLevel ( std::ostream & os = std::cout ) const

inlineinherited

prints all internally known debuglevels

Author: woelk 09/2006

Definition at line 496 of file Debug.hh.

virtual void BIAS::ExtendedKalman::StateTransition	(	const Vector< double > &	oldState,
		const Vector< double > &	control,
		Vector< double > &	newState
	)

pure virtual

Implement the statetransition-function (f in [1]) here.

It should compute the state x(k) for timestep k from the state x(k-1) for timestep k-1 and the control-params u(k)!

int ExtendedKalman::Update ( const Vector< double > & measurement )

Calculates the update step.

(use in alternation with Predict(), Update first)

Definition at line 31 of file ExtendedKalman.cpp.

References BIAS::Matrix< T >::SetIdentity(), and TNT::Vector< T >::size().

Member Data Documentation

long int BIAS::Debug::_liDebugLevel

protectedinherited

Definition at line 510 of file Debug.hh.

Referenced by BIAS::Debug::operator=(), and BIAS::ImageBase::operator=().

long int BIAS::Debug::_liNextDebugLevel

protectedinherited

new concept, debuglevel are managed here in the debug class

Definition at line 516 of file Debug.hh.

Referenced by BIAS::Debug::operator=().

std::map<std::string, long int> BIAS::Debug::_String2Debuglevel

protectedinherited

Definition at line 517 of file Debug.hh.

Referenced by BIAS::Debug::operator=().

std::ostream BIAS::Debug::_zDebugStream

staticprotectedinherited

Definition at line 511 of file Debug.hh.

Referenced by BIAS::Debug::operator=().

Matrix<double> BIAS::ExtendedKalman::A_

protected

derived state-trans. matrix and transposed

Definition at line 176 of file ExtendedKalman.hh.

Matrix<double> BIAS::ExtendedKalman::AT_

protected

Definition at line 176 of file ExtendedKalman.hh.

Matrix<double> BIAS::ExtendedKalman::covariance_

protected

The covariance matrix corresponding to State_.

Definition at line 167 of file ExtendedKalman.hh.

long int BIAS::Debug::GlobalDebugLevel = 0

staticprotectedinherited

Definition at line 513 of file Debug.hh.

Matrix<double> BIAS::ExtendedKalman::H_

protected

derived measurement matrix and transposed

Definition at line 170 of file ExtendedKalman.hh.

Matrix<double> BIAS::ExtendedKalman::HT_

protected

Definition at line 170 of file ExtendedKalman.hh.

Matrix<double> BIAS::ExtendedKalman::Identity_

protected

identity matrix

Definition at line 188 of file ExtendedKalman.hh.

Matrix<double> BIAS::ExtendedKalman::K_

protected

and finally the Kalman gain

Definition at line 191 of file ExtendedKalman.hh.

Vector<double> BIAS::ExtendedKalman::lastKalmanUpdate_

protected

maybe needed for iterated update

Definition at line 194 of file ExtendedKalman.hh.

int BIAS::ExtendedKalman::measurementDim_

protected

Definition at line 197 of file ExtendedKalman.hh.

Vector<double> BIAS::ExtendedKalman::posterioState_

protected

Contains the corrected (a posteriori) system estimate.

Definition at line 164 of file ExtendedKalman.hh.

Vector<double> BIAS::ExtendedKalman::priorState_

protected

Contains the prediction (a priori/initial) system estimate.

Definition at line 161 of file ExtendedKalman.hh.

Matrix<double> BIAS::ExtendedKalman::Q_

protected

system noise cov.

Definition at line 179 of file ExtendedKalman.hh.

Matrix<double> BIAS::ExtendedKalman::R_

protected

measurement noise cov.

Definition at line 173 of file ExtendedKalman.hh.

SVD BIAS::ExtendedKalman::svd_

protected

needed for matrix inversion

Definition at line 200 of file ExtendedKalman.hh.

int BIAS::ExtendedKalman::systemStateDim_

protected

dimensions of system space and measurement space

Definition at line 197 of file ExtendedKalman.hh.

Matrix<double> BIAS::ExtendedKalman::V_

protected

derived measurement error matrix and transposed

Definition at line 185 of file ExtendedKalman.hh.

Matrix<double> BIAS::ExtendedKalman::VT_

protected

Definition at line 185 of file ExtendedKalman.hh.

Matrix<double> BIAS::ExtendedKalman::W_

protected

derived state-trans. error matrix and transposed

Definition at line 182 of file ExtendedKalman.hh.

Matrix<double> BIAS::ExtendedKalman::WT_

protected

Definition at line 182 of file ExtendedKalman.hh.

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

StateEstimator/ExtendedKalman.hh
StateEstimator/ExtendedKalman.cpp

Public Member Functions

Static Public Member Functions

Protected Member Functions

Protected Attributes

Static Protected Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation