Basic Image AlgorithmS Library  2.8.0
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
Public Member Functions | Static Public Member Functions | Protected Member Functions | Protected Attributes | Static Protected Attributes | List of all members
BIAS::IteratedExtendedKalman Class Reference

Iterated Extended Kalman-filter (IEKF). More...

#include <StateEstimator/IteratedExtendedKalman.hh>

+ Inheritance diagram for BIAS::IteratedExtendedKalman:
+ Collaboration diagram for BIAS::IteratedExtendedKalman:

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
 
int GetDebugLevel () const
 
std::ostream & GetDebugStream () const
 
void GetDebugStream (std::ostream &os) const
 
virtual void GetMeasureJacobian (const BIAS::Vector< double > &state, BIAS::Matrix< double > &measJacobian, BIAS::Matrix< double > &measErrorJacobian, const unsigned int index=1)
 Returns the jacobian matrix of the measurement-function with respect to the state (H in [1]) as well as the jacobian with respect to the noise model (V in [1]). More...
 
void GetPosterioriCovariance (BIAS::Matrix< double > &cov) const
 Returns the updated (a posteriori) system state covariance. More...
 
void GetPosterioriState (BIAS::Vector< double > &state) const
 Returns the last corrected (a posteriori) system state. More...
 
void GetPriorCovariance (BIAS::Matrix< double > &cov) const
 Returns the last predicted (a priori) system state covariance. More...
 
void GetPriorState (BIAS::Vector< double > &state) const
 Returns the predicted (a priori) system state. More...
 
virtual void GetTransitionJacobian (const BIAS::Vector< double > &state, const BIAS::Vector< double > &control, BIAS::Matrix< double > &transJacobian, BIAS::Matrix< double > &transErrorJacobian)
 Returns the jacobian matrices (by state and process noise) for state transition function. More...
 
void Init (const BIAS::Vector< double > &initialState, const BIAS::Matrix< double > &initialCov, const BIAS::Matrix< double > &processNoiseCov)
 Initialize the kalman filter before using update/predict. More...
 
 IteratedExtendedKalman ()
 Standard constructor. More...
 
 IteratedExtendedKalman (unsigned int maxIters, double iterEps, bool bUseStable=false)
 Constructor using given parameters. More...
 
virtual void MeasurementPrediction (const BIAS::Vector< double > &state, BIAS::Vector< double > &measurePred, const unsigned int index=1)
 Implements the measurement prediction function (h in [1])! Since more than one measurement is allowed, an index for determining the respective function can be given. 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...
 
void Predict (const BIAS::Vector< double > &control)
 Implements a single preditcion step (with respect to the control parameters – u[k] in [1]). 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 SetProcessCov (const BIAS::Matrix< double > &processNoiseCov)
 Set the process noise covariance – Q in [1] — May be updated for every timestep! —. More...
 
void SetState (BIAS::Vector< double > &state, BIAS::Matrix< double > &cov)
 Set a modified a state. More...
 
void ShowDebugLevel (std::ostream &os=std::cout) const
 prints all internally known debuglevels More...
 
virtual void StateTransition (const BIAS::Vector< double > &state, const BIAS::Vector< double > &control, BIAS::Vector< double > &prediction)
 Implements the statetransition-function (f in [1]). More...
 
void Update (const BIAS::Vector< double > &measurement, const BIAS::Matrix< double > &measureNoise, const unsigned int index)
 Implememts the iterative update step using measurement and its covariance. More...
 
virtual ~IteratedExtendedKalman ()
 Virtual standard destructor. More...
 

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
 
BIAS::Matrix< double > A_
 derived state-trans. matrix and transposed More...
 
BIAS::Matrix< double > AT_
 
const bool bUseStable_
 flag for using numerically more stable computations for update (higher computation effort) More...
 
BIAS::Matrix< double > covariance_
 the covariance matrix corresponding to state_ More...
 
BIAS::Matrix< double > covariancePost_
 the covariance matrix corresponding to statePost_ More...
 
BIAS::Matrix< double > covariancePrior_
 the covariance matrix corresponding to statePrior_ More...
 
BIAS::Matrix< double > H_
 measurement jacobian for measurement model and its transpose More...
 
BIAS::Matrix< double > HT_
 
BIAS::Matrix< double > Identity_
 identity matrix frequently used in Update More...
 
double iterEps_
 break update iteration if update improvement is smaller than iterEps More...
 
unsigned int maxIters_
 number of maximal iterations for iterated update More...
 
unsigned int processNoiseDim_
 dimension of process noise More...
 
BIAS::Matrix< double > Q_
 system noise cov. More...
 
BIAS::Vector< double > state_
 contains the actual system estimate More...
 
BIAS::Vector< double > statePost_
 contains the corrected (a posteriori) system estimate More...
 
BIAS::Vector< double > statePrior_
 contains the last prediction (a priori/initial) system estimate More...
 
BIAS::SVD svd_
 needed for matrix inversion More...
 
unsigned int systemStateDim_
 dimension of state space More...
 
BIAS::Matrix< double > V_
 measurement error jacobian for measurement model and its transpose More...
 
BIAS::Matrix< double > VT_
 
BIAS::Matrix< double > W_
 state-trans. error jacobian and transposed More...
 
BIAS::Matrix< double > WT_
 

Static Protected Attributes

static std::ostream _zDebugStream
 
static long int GlobalDebugLevel = 0
 

Detailed Description

Iterated Extended Kalman-filter (IEKF).

Implemented according to: www.cs.unc.edu/~tracker/media/pdf/SIGGRAPH2001_CoursePack_08.pdf [1] with additional iteration option. Implements an Iterated Extended Kalman Filter, estimating the state of a system driven by a given possibly non- linear statetransition-funcition. Derive from this class an implement the virtual prediction functions (state transition, measurement prediction). Jacobians for model functions and according noise models have to be given.

Author
apetersen 05.08.10
Examples:
ExampleIEKF.cpp.

Definition at line 52 of file IteratedExtendedKalman.hh.

Constructor & Destructor Documentation

IteratedExtendedKalman::IteratedExtendedKalman ( )

Standard constructor.

Initiates object using maxIters=20, iterEps=1e-4 and bUseStable=false.

Definition at line 6 of file IteratedExtendedKalman.cpp.

References processNoiseDim_, and systemStateDim_.

IteratedExtendedKalman::IteratedExtendedKalman ( unsigned int  maxIters,
double  iterEps,
bool  bUseStable = false 
)

Constructor using given parameters.

maxIters maximum iterations for update iterEps break update iteration if update < iterEps bUseStable use stable (slower) update equation

Definition at line 14 of file IteratedExtendedKalman.cpp.

References processNoiseDim_, and systemStateDim_.

virtual BIAS::IteratedExtendedKalman::~IteratedExtendedKalman ( )
inlinevirtual

Virtual standard destructor.

Definition at line 71 of file IteratedExtendedKalman.hh.

Member Function Documentation

void BIAS::Debug::AddDebugLevel ( const long int  lv)
inlineinherited
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
bool BIAS::Debug::DebugLevelIsSet ( const std::string &  name) const
inlineinherited

Definition at line 350 of file Debug.hh.

int BIAS::Debug::GetDebugLevel ( ) const
inlineinherited
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.

virtual void BIAS::IteratedExtendedKalman::GetMeasureJacobian ( const BIAS::Vector< double > &  state,
BIAS::Matrix< double > &  measJacobian,
BIAS::Matrix< double > &  measErrorJacobian,
const unsigned int  index = 1 
)
inlinevirtual

Returns the jacobian matrix of the measurement-function with respect to the state (H in [1]) as well as the jacobian with respect to the noise model (V in [1]).

The matrices will be atomatically initialized according to the measurement/measurement noise bevore passed to function call. Has to be implemented in child class. state the state used for measurement prediction measJacobian jacobian matrix of measurement funciton (by state) measErrorJacobian jacobian matrix of MF (by process noise)

Definition at line 198 of file IteratedExtendedKalman.hh.

Referenced by Update().

void BIAS::IteratedExtendedKalman::GetPosterioriCovariance ( BIAS::Matrix< double > &  cov) const
inline

Returns the updated (a posteriori) system state covariance.

Definition at line 90 of file IteratedExtendedKalman.hh.

void BIAS::IteratedExtendedKalman::GetPosterioriState ( BIAS::Vector< double > &  state) const
inline

Returns the last corrected (a posteriori) system state.

Definition at line 85 of file IteratedExtendedKalman.hh.

void BIAS::IteratedExtendedKalman::GetPriorCovariance ( BIAS::Matrix< double > &  cov) const
inline

Returns the last predicted (a priori) system state covariance.

Definition at line 80 of file IteratedExtendedKalman.hh.

void BIAS::IteratedExtendedKalman::GetPriorState ( BIAS::Vector< double > &  state) const
inline

Returns the predicted (a priori) system state.

Definition at line 75 of file IteratedExtendedKalman.hh.

virtual void BIAS::IteratedExtendedKalman::GetTransitionJacobian ( const BIAS::Vector< double > &  state,
const BIAS::Vector< double > &  control,
BIAS::Matrix< double > &  transJacobian,
BIAS::Matrix< double > &  transErrorJacobian 
)
inlinevirtual

Returns the jacobian matrices (by state and process noise) for state transition function.

The matrices will be atomatically initialized according to the system state/process noise bevore passed to function call. Has to be implemented in child class! state state used for prediction control control vector used for prediction transJacobian jacobian matrix of transition funciton (by state) transErrorJacobian jacobian matrix of TF (by process noise)

Definition at line 156 of file IteratedExtendedKalman.hh.

Referenced by Predict().

void IteratedExtendedKalman::Init ( const BIAS::Vector< double > &  initialState,
const BIAS::Matrix< double > &  initialCov,
const BIAS::Matrix< double > &  processNoiseCov 
)

Initialize the kalman filter before using update/predict.

initialState initial state initialCov covariance for initial state processNoiseCov process (prediction) noise covariance

Definition at line 24 of file IteratedExtendedKalman.cpp.

References A_, covariance_, covariancePost_, covariancePrior_, Identity_, TNT::Matrix< T >::newsize(), TNT::Matrix< T >::num_rows(), processNoiseDim_, Q_, BIAS::Matrix< T >::SetIdentity(), TNT::Vector< T >::size(), state_, statePost_, statePrior_, systemStateDim_, and W_.

virtual void BIAS::IteratedExtendedKalman::MeasurementPrediction ( const BIAS::Vector< double > &  state,
BIAS::Vector< double > &  measurePred,
const unsigned int  index = 1 
)
inlinevirtual

Implements the measurement prediction function (h in [1])! Since more than one measurement is allowed, an index for determining the respective function can be given.

Has to be implemented in child class! state the state to predict the measurement from measurePred the predicted measurement index the index for the measurement funtion to be applied

Definition at line 180 of file IteratedExtendedKalman.hh.

Referenced by Update().

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
void IteratedExtendedKalman::Predict ( const BIAS::Vector< double > &  control)

Implements a single preditcion step (with respect to the control parameters – u[k] in [1]).

It can be called more than once between two updates (iterativ prediction). control control vector (u[k]) to be used for prediction

Definition at line 48 of file IteratedExtendedKalman.cpp.

References A_, AT_, covariance_, covariancePrior_, GetTransitionJacobian(), TNT::Matrix< T >::newsize(), TNT::Matrix< T >::num_cols(), TNT::Matrix< T >::num_rows(), processNoiseDim_, Q_, state_, statePrior_, StateTransition(), systemStateDim_, BIAS::Matrix< T >::Transpose(), W_, and WT_.

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::IteratedExtendedKalman::SetProcessCov ( const BIAS::Matrix< double > &  processNoiseCov)
inline

Set the process noise covariance – Q in [1] — May be updated for every timestep! —.

Definition at line 112 of file IteratedExtendedKalman.hh.

References TNT::Matrix< T >::num_cols(), and TNT::Matrix< T >::num_rows().

void BIAS::IteratedExtendedKalman::SetState ( BIAS::Vector< double > &  state,
BIAS::Matrix< double > &  cov 
)
inline

Set a modified a state.

Use this if an external transformation has been applied to the state (e.g. coordinate transformations) or the state dimension has changed. Modify state covariance accordingly.

Definition at line 97 of file IteratedExtendedKalman.hh.

References TNT::Matrix< T >::num_cols(), TNT::Matrix< T >::num_rows(), TNT::Matrix< T >::size(), and TNT::Vector< T >::size().

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::IteratedExtendedKalman::StateTransition ( const BIAS::Vector< double > &  state,
const BIAS::Vector< double > &  control,
BIAS::Vector< double > &  prediction 
)
inlinevirtual

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

Predict the 'state' using the transition function and 'control' vector to get 'prediction'! Has to be implemented in child class! state a posterior state control vector containing parameters for prediction prediction predicte a priori state

Definition at line 138 of file IteratedExtendedKalman.hh.

Referenced by Predict().

void IteratedExtendedKalman::Update ( const BIAS::Vector< double > &  measurement,
const BIAS::Matrix< double > &  measureNoise,
const unsigned int  index 
)

Implememts the iterative update step using measurement and its covariance.

Can be call more than once between two predicts. Index indicates which measurement funciton shall be used. measurement measurement vector measureNoise covariance for measurement index indicator for used meansurement function

Definition at line 85 of file IteratedExtendedKalman.cpp.

References bUseStable_, covariance_, covariancePost_, GetMeasureJacobian(), H_, HT_, Identity_, BIAS::SVD::Invert(), iterEps_, maxIters_, MeasurementPrediction(), TNT::Matrix< T >::newsize(), TNT::Matrix< T >::num_cols(), TNT::Matrix< T >::num_rows(), BIAS::Matrix< T >::SetIdentity(), TNT::Vector< T >::size(), state_, statePost_, svd_, systemStateDim_, BIAS::Matrix< T >::Transpose(), V_, and VT_.

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

BIAS::Matrix<double> BIAS::IteratedExtendedKalman::A_
protected

derived state-trans. matrix and transposed

Definition at line 255 of file IteratedExtendedKalman.hh.

Referenced by Init(), and Predict().

BIAS::Matrix<double> BIAS::IteratedExtendedKalman::AT_
protected

Definition at line 255 of file IteratedExtendedKalman.hh.

Referenced by Predict().

const bool BIAS::IteratedExtendedKalman::bUseStable_
protected

flag for using numerically more stable computations for update (higher computation effort)

Definition at line 222 of file IteratedExtendedKalman.hh.

Referenced by Update().

BIAS::Matrix<double> BIAS::IteratedExtendedKalman::covariance_
protected

the covariance matrix corresponding to state_

Definition at line 243 of file IteratedExtendedKalman.hh.

Referenced by Init(), Predict(), and Update().

BIAS::Matrix<double> BIAS::IteratedExtendedKalman::covariancePost_
protected

the covariance matrix corresponding to statePost_

Definition at line 249 of file IteratedExtendedKalman.hh.

Referenced by Init(), and Update().

BIAS::Matrix<double> BIAS::IteratedExtendedKalman::covariancePrior_
protected

the covariance matrix corresponding to statePrior_

Definition at line 246 of file IteratedExtendedKalman.hh.

Referenced by Init(), and Predict().

long int BIAS::Debug::GlobalDebugLevel = 0
staticprotectedinherited

Definition at line 513 of file Debug.hh.

BIAS::Matrix<double> BIAS::IteratedExtendedKalman::H_
protected

measurement jacobian for measurement model and its transpose

Definition at line 252 of file IteratedExtendedKalman.hh.

Referenced by Update().

BIAS::Matrix<double> BIAS::IteratedExtendedKalman::HT_
protected

Definition at line 252 of file IteratedExtendedKalman.hh.

Referenced by Update().

BIAS::Matrix<double> BIAS::IteratedExtendedKalman::Identity_
protected

identity matrix frequently used in Update

Definition at line 240 of file IteratedExtendedKalman.hh.

Referenced by Init(), and Update().

double BIAS::IteratedExtendedKalman::iterEps_
protected

break update iteration if update improvement is smaller than iterEps

Definition at line 228 of file IteratedExtendedKalman.hh.

Referenced by Update().

unsigned int BIAS::IteratedExtendedKalman::maxIters_
protected

number of maximal iterations for iterated update

Definition at line 225 of file IteratedExtendedKalman.hh.

Referenced by Update().

unsigned int BIAS::IteratedExtendedKalman::processNoiseDim_
protected

dimension of process noise

Definition at line 270 of file IteratedExtendedKalman.hh.

Referenced by Init(), IteratedExtendedKalman(), and Predict().

BIAS::Matrix<double> BIAS::IteratedExtendedKalman::Q_
protected

system noise cov.

Definition at line 258 of file IteratedExtendedKalman.hh.

Referenced by Init(), and Predict().

BIAS::Vector<double> BIAS::IteratedExtendedKalman::state_
protected

contains the actual system estimate

Definition at line 231 of file IteratedExtendedKalman.hh.

Referenced by Init(), Predict(), and Update().

BIAS::Vector<double> BIAS::IteratedExtendedKalman::statePost_
protected

contains the corrected (a posteriori) system estimate

Definition at line 237 of file IteratedExtendedKalman.hh.

Referenced by Init(), and Update().

BIAS::Vector<double> BIAS::IteratedExtendedKalman::statePrior_
protected

contains the last prediction (a priori/initial) system estimate

Definition at line 234 of file IteratedExtendedKalman.hh.

Referenced by Init(), and Predict().

BIAS::SVD BIAS::IteratedExtendedKalman::svd_
protected

needed for matrix inversion

Definition at line 273 of file IteratedExtendedKalman.hh.

Referenced by Update().

unsigned int BIAS::IteratedExtendedKalman::systemStateDim_
protected

dimension of state space

Definition at line 267 of file IteratedExtendedKalman.hh.

Referenced by Init(), IteratedExtendedKalman(), Predict(), and Update().

BIAS::Matrix<double> BIAS::IteratedExtendedKalman::V_
protected

measurement error jacobian for measurement model and its transpose

Definition at line 264 of file IteratedExtendedKalman.hh.

Referenced by Update().

BIAS::Matrix<double> BIAS::IteratedExtendedKalman::VT_
protected

Definition at line 264 of file IteratedExtendedKalman.hh.

Referenced by Update().

BIAS::Matrix<double> BIAS::IteratedExtendedKalman::W_
protected

state-trans. error jacobian and transposed

Definition at line 261 of file IteratedExtendedKalman.hh.

Referenced by Init(), and Predict().

BIAS::Matrix<double> BIAS::IteratedExtendedKalman::WT_
protected

Definition at line 261 of file IteratedExtendedKalman.hh.

Referenced by Predict().


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