MRPT logo

mrpt::bayes::CKalmanFilterCapable Class Reference

Virtual base for Kalman Filter (EKF,IEKF,UKF) implementations. More...

#include <mrpt/bayes/CKalmanFilterCapable.h>

Inheritance diagram for mrpt::bayes::CKalmanFilterCapable:

mrpt::slam::CRangeBearingKFSLAM

List of all members.

Classes

struct  TKF_options
 Generic options for the Kalman Filter algorithm in itself. More...

Public Member Functions

 CKalmanFilterCapable ()
 Default constructor.
virtual ~CKalmanFilterCapable ()
 Destructor.

Public Attributes

TKF_options KF_options
 Generic options for the Kalman Filter algorithm in itself.

Protected Member Functions

void runOneKalmanIteration ()
 The main entry point, executes one complete step: prediction + update.
Virtual methods for Kalman Filter implementation
virtual void OnGetAction (CVectorTemplate< KFTYPE > &out_u)=0
 Must return the action vector u.
virtual void OnTransitionModel (const CVectorTemplate< KFTYPE > &in_u, CVectorTemplate< KFTYPE > &inout_x, bool &out_skipPrediction)=0
 Implements the transition model $ \hat{x}_{k|k-1} = f( \hat{x}_{k-1|k-1}, u_k ) $.
virtual void OnTransitionJacobian (CMatrixTemplateNumeric< KFTYPE > &out_F)=0
 Implements the transition Jacobian $ \frac{\partial f}{\partial x} $.
virtual void OnTransitionNoise (CMatrixTemplateNumeric< KFTYPE > &out_Q)=0
 Implements the transition noise covariance $ Q_k $.
virtual void OnGetObservations (CMatrixTemplateNumeric< KFTYPE > &out_z, CVectorTemplate< KFTYPE > &out_R2, vector_int &out_data_association)
 This is called between the KF prediction step and the update step to allow the application to employ the prior distribution of the system state in the detection of observations (data association), where applicable.
virtual void OnGetObservations (CMatrixTemplateNumeric< KFTYPE > &out_z, CMatrixTemplateNumeric< KFTYPE > &out_R, vector_int &out_data_association)
 This is called between the KF prediction step and the update step to allow the application to employ the prior distribution of the system state in the detection of observations (data association), where applicable.
virtual void OnObservationModelAndJacobians (const CMatrixTemplateNumeric< KFTYPE > &in_z, const vector_int &in_data_association, const bool &in_full, const int &in_obsIdx, CVectorTemplate< KFTYPE > &out_innov, CMatrixTemplateNumeric< KFTYPE > &out_Hx, CMatrixTemplateNumeric< KFTYPE > &out_Hy)=0
 Implements the observation prediction $ h_i(x) $ and the Jacobians $ \frac{\partial h_i}{\partial x} $ and (when applicable) $ \frac{\partial h_i}{\partial y_i} $.
virtual void OnInverseObservationModel (const CMatrixTemplateNumeric< KFTYPE > &in_z, const size_t &in_obsIdx, const size_t &in_idxNewFeat, CVectorTemplate< KFTYPE > &out_yn, CMatrixTemplateNumeric< KFTYPE > &out_dyn_dxv, CMatrixTemplateNumeric< KFTYPE > &out_dyn_dhn)
 If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element".
virtual void OnNormalizeStateVector ()
 This method is called after the prediction and after the update, to give the user an opportunity to normalize the state vector (eg, keep angles within -pi,pi range) if the application requires it.
virtual void OnPostIteration ()
 This method is called after finishing one KF iteration and before returning from runOneKalmanIteration().
virtual size_t get_vehicle_size () const =0
 Must return the dimension of the "vehicle state": either the full state vector or the "vehicle" part if applicable.
virtual size_t get_feature_size () const
 Must return the dimension of the features in the system state (the "map"), or 0 if not applicable (the default if not implemented).
virtual size_t get_observation_size () const =0
 Must return the dimension of each observation (eg, 2 for pixel coordinates, 3 for 3D coordinates,etc).
virtual size_t get_vehicle_state_offset () const
 The index of the first component of the vehicle state within the whole state vector/matrix (default=0).
virtual size_t get_feature_state_offset () const
 The index of the first component of the features part in the state vector/matrix (default=after the vehicle state, assuming it is at the begining of the vector).

Protected Attributes

math::CVectorTemplate< KFTYPEm_xkk
 The system state vector.
math::CMatrixTemplateNumeric
< KFTYPE
m_pkk
 The system full covariance matrix.


Detailed Description

Virtual base for Kalman Filter (EKF,IEKF,UKF) implementations.

This base class stores the state vector and covariance matrix of the system. It has virtual methods that must be completed by derived classes to address a given filtering problem. The main entry point of the algorithm is CKalmanFilterCapable::runOneKalmanIteration, which should be called AFTER setting the desired filter options in KF_options, as well as any options in the derived class. Note that the main entry point is protected, so derived classes must offer another method more specific to a given problem which, internally, calls runOneKalmanIteration.

For further details and examples, check out the tutorial: http://babel.isa.uma.es/mrpt/index.php/Kalman_Filters

Note:
Since MRPT 0.5.5 there are two methods "OnGetObservations": one returning the sensor noise covariance matrix as a whole matrix, and the other as the diagonal vector. The derived class must implement just one of them.
Revisions:

See also:
mrpt::slam::CRangeBearingKFSLAM

Definition at line 74 of file CKalmanFilterCapable.h.


Constructor & Destructor Documentation

mrpt::bayes::CKalmanFilterCapable::CKalmanFilterCapable (  ) 

Default constructor.

virtual mrpt::bayes::CKalmanFilterCapable::~CKalmanFilterCapable (  )  [virtual]

Destructor.


Member Function Documentation

virtual size_t mrpt::bayes::CKalmanFilterCapable::get_feature_size (  )  const [inline, protected, virtual]

Must return the dimension of the features in the system state (the "map"), or 0 if not applicable (the default if not implemented).

Reimplemented in mrpt::slam::CRangeBearingKFSLAM.

Definition at line 218 of file CKalmanFilterCapable.h.

virtual size_t mrpt::bayes::CKalmanFilterCapable::get_feature_state_offset (  )  const [inline, protected, virtual]

The index of the first component of the features part in the state vector/matrix (default=after the vehicle state, assuming it is at the begining of the vector).

Definition at line 236 of file CKalmanFilterCapable.h.

virtual size_t mrpt::bayes::CKalmanFilterCapable::get_observation_size (  )  const [protected, pure virtual]

Must return the dimension of each observation (eg, 2 for pixel coordinates, 3 for 3D coordinates,etc).

Implemented in mrpt::slam::CRangeBearingKFSLAM.

virtual size_t mrpt::bayes::CKalmanFilterCapable::get_vehicle_size (  )  const [protected, pure virtual]

Must return the dimension of the "vehicle state": either the full state vector or the "vehicle" part if applicable.

Implemented in mrpt::slam::CRangeBearingKFSLAM.

virtual size_t mrpt::bayes::CKalmanFilterCapable::get_vehicle_state_offset (  )  const [inline, protected, virtual]

The index of the first component of the vehicle state within the whole state vector/matrix (default=0).

Definition at line 229 of file CKalmanFilterCapable.h.

virtual void mrpt::bayes::CKalmanFilterCapable::OnGetAction ( CVectorTemplate< KFTYPE > &  out_u  )  [protected, pure virtual]

Must return the action vector u.

Parameters:
out_u The action vector which will be passed to OnTransitionModel

Implemented in mrpt::slam::CRangeBearingKFSLAM.

virtual void mrpt::bayes::CKalmanFilterCapable::OnGetObservations ( CMatrixTemplateNumeric< KFTYPE > &  out_z,
CMatrixTemplateNumeric< KFTYPE > &  out_R,
vector_int out_data_association 
) [protected, virtual]

This is called between the KF prediction step and the update step to allow the application to employ the prior distribution of the system state in the detection of observations (data association), where applicable.

Parameters:
out_z A $N \times O$ matrix, with O being get_observation_size() and N the number of "observations": how many observed landmarks for a map, or just one if not applicable.
out_R A matrix of size N·OxO (O being get_observation_size() and N the number of observations). It is the covariance matrix of the sensor noise for each of the returned observations. The order must correspond to that in out_z.
out_data_association An empty vector or, where applicable, a vector where the i'th element corresponds to the position of the observation in the i'th row of out_z within the system state vector, or -1 if it is a new map element and we want to insert it at the end of this KF iteration. This method will be called just once for each complete KF iteration.
Note:
Since MRPT 0.5.5, the method "OnGetObservations" returning R as a matrix is called instead from the Kalman Filter engine. However, for compatibility, a default virtual method calls this method and build the R matrix from the diagonal R2.

Reimplemented in mrpt::slam::CRangeBearingKFSLAM.

virtual void mrpt::bayes::CKalmanFilterCapable::OnGetObservations ( CMatrixTemplateNumeric< KFTYPE > &  out_z,
CVectorTemplate< KFTYPE > &  out_R2,
vector_int out_data_association 
) [inline, protected, virtual]

This is called between the KF prediction step and the update step to allow the application to employ the prior distribution of the system state in the detection of observations (data association), where applicable.

Parameters:
out_z A $N \times O$ matrix, with O being get_observation_size() and N the number of "observations": how many observed landmarks for a map, or just one if not applicable.
out_R2 A vector of size O (O being get_observation_size()) with the *variances* of the sensor noise for each of the observation components. This is constant for all the observations (where N>1), and in the naive Kalman method the value TKF_options::veryLargeR2 is used for unobserved map elements.
out_data_association An empty vector or, where applicable, a vector where the i'th element corresponds to the position of the observation in the i'th row of out_z within the system state vector, or -1 if it is a new map element and we want to insert it at the end of this KF iteration. This method will be called just once for each complete KF iteration.
Note:
Since MRPT 0.5.5, the method "OnGetObservations" returning R as a matrix is called instead from the Kalman Filter engine. However, for compatibility, a default virtual method calls this method and build the R matrix from the diagonal R2.

Reimplemented in mrpt::slam::CRangeBearingKFSLAM.

Definition at line 134 of file CKalmanFilterCapable.h.

References MRPT_TRY_END, MRPT_TRY_START, and THROW_EXCEPTION.

Referenced by mrpt::slam::CRangeBearingKFSLAM::OnGetObservations().

virtual void mrpt::bayes::CKalmanFilterCapable::OnInverseObservationModel ( const CMatrixTemplateNumeric< KFTYPE > &  in_z,
const size_t &  in_obsIdx,
const size_t &  in_idxNewFeat,
CVectorTemplate< KFTYPE > &  out_yn,
CMatrixTemplateNumeric< KFTYPE > &  out_dyn_dxv,
CMatrixTemplateNumeric< KFTYPE > &  out_dyn_dhn 
) [protected, virtual]

If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element".

Parameters:
in_z This is the same matrix returned by OnGetObservations().
in_obsIndex The index of the observation whose inverse sensor is to be computed. It corresponds to the row in in_z where the observation can be found.
in_idxNewFeat The index that this new feature will have in the state vector (0:just after the vehicle state, 1: after that,...). Save this number so data association can be done according to these indices.
out_yn The F-length vector with the inverse observation model $ y_n=y(x,z_n) $.
out_dyn_dxv The $F \times V$ Jacobian of the inv. sensor model wrt the robot pose $ \frac{\partial y_n}{\partial x_v} $.
out_dyn_dhn The $F \times O$ Jacobian of the inv. sensor model wrt the observation vector $ \frac{\partial y_n}{\partial h_n} $.

Reimplemented in mrpt::slam::CRangeBearingKFSLAM.

virtual void mrpt::bayes::CKalmanFilterCapable::OnNormalizeStateVector (  )  [protected, virtual]

This method is called after the prediction and after the update, to give the user an opportunity to normalize the state vector (eg, keep angles within -pi,pi range) if the application requires it.

Reimplemented in mrpt::slam::CRangeBearingKFSLAM.

virtual void mrpt::bayes::CKalmanFilterCapable::OnObservationModelAndJacobians ( const CMatrixTemplateNumeric< KFTYPE > &  in_z,
const vector_int in_data_association,
const bool &  in_full,
const int &  in_obsIdx,
CVectorTemplate< KFTYPE > &  out_innov,
CMatrixTemplateNumeric< KFTYPE > &  out_Hx,
CMatrixTemplateNumeric< KFTYPE > &  out_Hy 
) [protected, pure virtual]

Implements the observation prediction $ h_i(x) $ and the Jacobians $ \frac{\partial h_i}{\partial x} $ and (when applicable) $ \frac{\partial h_i}{\partial y_i} $.

Parameters:
in_z This is the same matrix returned by OnGetObservations(), passed here for reference.
in_data_association The vector returned by OnGetObservations(), passed here for reference.
in_full If set to true, all the Jacobians and predictions must be computed at once. Otherwise, just those for the observation in_obsIdx.
in_obsIdx If in_full=false, the row of the observation (in in_z and in_data_association) whose innovation & Jacobians are to be returned now.
out_innov The difference between the expected observation and the real one: $ \tilde{y} = z - h(x) $. It must be a vector of length O*N (in_full=true) or O (in_full=false), with O=get_observation_size() and N=number of rows in in_z.
out_Hx One or a vertical stack of $ \frac{\partial h_i}{\partial x} $.
out_Hy An empty matrix, or one or a vertical stack of $ \frac{\partial h_i}{\partial y_i} $.
out_Hx must consist of 1 (in_full=false) or N (in_full=true) vertically stacked $O \times V$ matrices, and out_Hy must consist of 1 or N $O \times F$ matrices, with:

Implemented in mrpt::slam::CRangeBearingKFSLAM.

virtual void mrpt::bayes::CKalmanFilterCapable::OnPostIteration (  )  [protected, virtual]

This method is called after finishing one KF iteration and before returning from runOneKalmanIteration().

virtual void mrpt::bayes::CKalmanFilterCapable::OnTransitionJacobian ( CMatrixTemplateNumeric< KFTYPE > &  out_F  )  [protected, pure virtual]

Implements the transition Jacobian $ \frac{\partial f}{\partial x} $.

Parameters:
out_F Must return the Jacobian. The returned matrix must be $N \times N$ with N being either the size of the whole state vector or get_vehicle_size().

Implemented in mrpt::slam::CRangeBearingKFSLAM.

virtual void mrpt::bayes::CKalmanFilterCapable::OnTransitionModel ( const CVectorTemplate< KFTYPE > &  in_u,
CVectorTemplate< KFTYPE > &  inout_x,
bool &  out_skipPrediction 
) [protected, pure virtual]

Implements the transition model $ \hat{x}_{k|k-1} = f( \hat{x}_{k-1|k-1}, u_k ) $.

Parameters:
in_u The vector returned by OnGetAction.
inout_x At input has

\[ \hat{x}_{k-1|k-1} \]

, at output must have $ \hat{x}_{k|k-1} $ .

out_skip Set this to true if for some reason you want to skip the prediction step (to do not modify either the vector or the covariance). Default:false

Implemented in mrpt::slam::CRangeBearingKFSLAM.

virtual void mrpt::bayes::CKalmanFilterCapable::OnTransitionNoise ( CMatrixTemplateNumeric< KFTYPE > &  out_Q  )  [protected, pure virtual]

Implements the transition noise covariance $ Q_k $.

Parameters:
out_Q Must return the covariance matrix. The returned matrix must be of the same size than the jacobian from OnTransitionJacobian

Implemented in mrpt::slam::CRangeBearingKFSLAM.

void mrpt::bayes::CKalmanFilterCapable::runOneKalmanIteration (  )  [protected]

The main entry point, executes one complete step: prediction + update.

It is protected since derived classes must provide a problem-specific entry point for users. The exact order in which this method calls the virtual method is explained in http://babel.isa.uma.es/mrpt/index.php/Kalman_Filters.


Member Data Documentation

Generic options for the Kalman Filter algorithm in itself.

Definition at line 289 of file CKalmanFilterCapable.h.

The system full covariance matrix.

Definition at line 83 of file CKalmanFilterCapable.h.

The system state vector.

Definition at line 79 of file CKalmanFilterCapable.h.




Page generated by Doxygen 1.5.8 for MRPT 0.6.5 SVN:exported at Mon Jan 12 13:00:16 UTC 2009