Basic Image AlgorithmS Library
2.8.0
|
Iterated Extended Kalman-filter (IEKF). More...
#include <StateEstimator/IteratedExtendedKalman.hh>
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 |
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.
Definition at line 52 of file IteratedExtendedKalman.hh.
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_.
|
inlinevirtual |
Virtual standard destructor.
Definition at line 71 of file IteratedExtendedKalman.hh.
|
inlineinherited |
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().
|
inlineinherited |
|
inlineprotectedinherited |
|
inlineinherited |
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().
|
inlineinherited |
|
inlineinherited |
Definition at line 332 of file Debug.hh.
Referenced by BIAS::listenthread(), BIAS::Image< StorageType >::operator=(), and BIAS::ImageBase::operator=().
|
inlineinherited |
|
inlineinherited |
|
inlinestaticinherited |
|
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().
|
inline |
Returns the updated (a posteriori) system state covariance.
Definition at line 90 of file IteratedExtendedKalman.hh.
|
inline |
Returns the last corrected (a posteriori) system state.
Definition at line 85 of file IteratedExtendedKalman.hh.
|
inline |
Returns the last predicted (a priori) system state covariance.
Definition at line 80 of file IteratedExtendedKalman.hh.
|
inline |
Returns the predicted (a priori) system state.
Definition at line 75 of file IteratedExtendedKalman.hh.
|
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_.
|
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().
|
inlineinherited |
|
inlineinherited |
creates a new debuglevel
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().
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_.
|
inlineinherited |
|
inlineinherited |
|
inlineinherited |
|
inlineinherited |
Definition at line 318 of file Debug.hh.
Referenced by BIAS::ShowCamWxVideoSourceFactory::CreateBumbleBee(), BIAS::ShowCamWxVideoSourceFactory::CreateDCAM(), BIAS::VideoSource_MDcam::ScanBus(), BIAS::GLProjectionParametersBase::SetDebugLevel(), BIAS::VideoSource_Disk::VideoSource_Disk(), BIAS::VideoSource_DV2::VideoSource_DV2(), BIAS::VideoSource_DV::VideoSource_DV_Internal_Init_(), and BIAS::VideoSource_V4L::VideoSource_V4L().
|
inlineinherited |
|
inlineinherited |
|
inlinestaticinherited |
|
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().
|
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().
|
inlineinherited |
|
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_.
|
protectedinherited |
Definition at line 510 of file Debug.hh.
Referenced by BIAS::Debug::operator=(), and BIAS::ImageBase::operator=().
|
protectedinherited |
new concept, debuglevel are managed here in the debug class
Definition at line 516 of file Debug.hh.
Referenced by BIAS::Debug::operator=().
|
protectedinherited |
Definition at line 517 of file Debug.hh.
Referenced by BIAS::Debug::operator=().
|
staticprotectedinherited |
Definition at line 511 of file Debug.hh.
Referenced by BIAS::Debug::operator=().
|
protected |
derived state-trans. matrix and transposed
Definition at line 255 of file IteratedExtendedKalman.hh.
|
protected |
Definition at line 255 of file IteratedExtendedKalman.hh.
Referenced by Predict().
|
protected |
flag for using numerically more stable computations for update (higher computation effort)
Definition at line 222 of file IteratedExtendedKalman.hh.
Referenced by Update().
|
protected |
the covariance matrix corresponding to state_
Definition at line 243 of file IteratedExtendedKalman.hh.
|
protected |
the covariance matrix corresponding to statePost_
Definition at line 249 of file IteratedExtendedKalman.hh.
|
protected |
the covariance matrix corresponding to statePrior_
Definition at line 246 of file IteratedExtendedKalman.hh.
|
staticprotectedinherited |
|
protected |
measurement jacobian for measurement model and its transpose
Definition at line 252 of file IteratedExtendedKalman.hh.
Referenced by Update().
|
protected |
Definition at line 252 of file IteratedExtendedKalman.hh.
Referenced by Update().
|
protected |
identity matrix frequently used in Update
Definition at line 240 of file IteratedExtendedKalman.hh.
|
protected |
break update iteration if update improvement is smaller than iterEps
Definition at line 228 of file IteratedExtendedKalman.hh.
Referenced by Update().
|
protected |
number of maximal iterations for iterated update
Definition at line 225 of file IteratedExtendedKalman.hh.
Referenced by Update().
|
protected |
dimension of process noise
Definition at line 270 of file IteratedExtendedKalman.hh.
Referenced by Init(), IteratedExtendedKalman(), and Predict().
|
protected |
system noise cov.
Definition at line 258 of file IteratedExtendedKalman.hh.
|
protected |
contains the actual system estimate
Definition at line 231 of file IteratedExtendedKalman.hh.
|
protected |
contains the corrected (a posteriori) system estimate
Definition at line 237 of file IteratedExtendedKalman.hh.
|
protected |
contains the last prediction (a priori/initial) system estimate
Definition at line 234 of file IteratedExtendedKalman.hh.
|
protected |
needed for matrix inversion
Definition at line 273 of file IteratedExtendedKalman.hh.
Referenced by Update().
|
protected |
dimension of state space
Definition at line 267 of file IteratedExtendedKalman.hh.
Referenced by Init(), IteratedExtendedKalman(), Predict(), and Update().
|
protected |
measurement error jacobian for measurement model and its transpose
Definition at line 264 of file IteratedExtendedKalman.hh.
Referenced by Update().
|
protected |
Definition at line 264 of file IteratedExtendedKalman.hh.
Referenced by Update().
|
protected |
state-trans. error jacobian and transposed
Definition at line 261 of file IteratedExtendedKalman.hh.
|
protected |
Definition at line 261 of file IteratedExtendedKalman.hh.
Referenced by Predict().