Basic Image AlgorithmS Library
2.8.0
|
Classical ExtendedKalman-filter (EKF). More...
#include <StateEstimator/ExtendedKalman.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 |
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 |
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.
Definition at line 49 of file ExtendedKalman.hh.
ExtendedKalman::ExtendedKalman | ( | ) |
Definition at line 6 of file ExtendedKalman.cpp.
|
inlinevirtual |
Definition at line 54 of file ExtendedKalman.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 |
|
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.
|
inlineinherited |
Definition at line 332 of file Debug.hh.
Referenced by BIAS::listenthread(), BIAS::Image< StorageType >::operator=(), and BIAS::ImageBase::operator=().
|
inlineinherited |
|
inlineinherited |
|
inlinestaticinherited |
|
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.
|
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.
|
pure virtual |
Implement this to correlate the actual state with the 'predicted' measurement (h in [1])! It should 'extract' the measurement from the state!
|
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().
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.
|
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 |
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().
|
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.
|
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().
|
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.
|
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().
|
inlineinherited |
|
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().
|
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 176 of file ExtendedKalman.hh.
|
protected |
Definition at line 176 of file ExtendedKalman.hh.
|
protected |
The covariance matrix corresponding to State_.
Definition at line 167 of file ExtendedKalman.hh.
|
staticprotectedinherited |
|
protected |
derived measurement matrix and transposed
Definition at line 170 of file ExtendedKalman.hh.
|
protected |
Definition at line 170 of file ExtendedKalman.hh.
|
protected |
identity matrix
Definition at line 188 of file ExtendedKalman.hh.
|
protected |
and finally the Kalman gain
Definition at line 191 of file ExtendedKalman.hh.
|
protected |
maybe needed for iterated update
Definition at line 194 of file ExtendedKalman.hh.
|
protected |
Definition at line 197 of file ExtendedKalman.hh.
|
protected |
Contains the corrected (a posteriori) system estimate.
Definition at line 164 of file ExtendedKalman.hh.
|
protected |
Contains the prediction (a priori/initial) system estimate.
Definition at line 161 of file ExtendedKalman.hh.
|
protected |
system noise cov.
Definition at line 179 of file ExtendedKalman.hh.
|
protected |
measurement noise cov.
Definition at line 173 of file ExtendedKalman.hh.
|
protected |
needed for matrix inversion
Definition at line 200 of file ExtendedKalman.hh.
|
protected |
dimensions of system space and measurement space
Definition at line 197 of file ExtendedKalman.hh.
|
protected |
derived measurement error matrix and transposed
Definition at line 185 of file ExtendedKalman.hh.
|
protected |
Definition at line 185 of file ExtendedKalman.hh.
|
protected |
derived state-trans. error matrix and transposed
Definition at line 182 of file ExtendedKalman.hh.
|
protected |
Definition at line 182 of file ExtendedKalman.hh.