template class mrpt::bayes::CKalmanFilterCapable

Overview

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://www.mrpt.org/Kalman_Filters

The Kalman filter algorithms are generic, but this implementation is biased to ease the implementation of SLAM-like problems. However, it can be also applied to many generic problems not related to robotics or SLAM.

The meaning of the template parameters is:

VEH_SIZE: The dimension of the “vehicle state”: either the full state vector or the “vehicle” part if applicable.
OBS_SIZE: The dimension of each observation (eg, 2 for pixel coordinates, 3 for 3D coordinates,etc).
FEAT_SIZE: The dimension of the features in the system state (the “map”), or 0 if not applicable (the default if not implemented).
ACT_SIZE: The dimension of each “action” u_k (or 0 if not applicable).
KFTYPE: The numeric type of the matrices (default: double)

Revisions:

2007: Antonio J. Ortiz de Galisteo (AJOGD)
2008/FEB: All KF classes corrected, reorganized, and rewritten (JLBC).
2008/MAR: Implemented IKF (JLBC).
2009/DEC: Totally rewritten as a generic template using fixed-size matrices where possible (JLBC).

Inherited Members

public:
    // structs

    struct TMsg;

Typedefs

typedef KFTYPE kftype

The numeric type used in the Kalman Filter (default=double)

typedef CKalmanFilterCapable<VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE> KFCLASS

My class, in a shorter name!

Fields

TKF_options KF_options

Generic options for the Kalman Filter algorithm itself.

Methods

virtual void OnInverseObservationModel(
    ] const KFArray_OBS& in_z,
    ] KFArray_FEAT& out_yn,
    ] KFMatrix_FxV& out_dyn_dxv,
    ] KFMatrix_FxO& out_dyn_dhn
    ) const

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

O: OBS_SIZE
V: VEH_SIZE
F: FEAT_SIZE

OnNewLandmarkAddedToMap will be also called after calling this method if a landmark is actually being added to the map.

Deprecated This version of the method is deprecated. The alternative method is preferred to allow a greater flexibility.

Parameters:

in_z	The observation vector whose inverse sensor model is to be computed. This is actually one of the vector<> returned by OnGetObservationsAndDataAssociation().
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}\).

virtual void OnInverseObservationModel(
    const KFArray_OBS& in_z,
    KFArray_FEAT& out_yn,
    KFMatrix_FxV& out_dyn_dxv,
    KFMatrix_FxO& out_dyn_dhn,
    ] KFMatrix_FxF& out_dyn_dhn_R_dyn_dhnT,
    bool& out_use_dyn_dhn_jacobian
    ) const

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

The uncertainty in the new map feature comes from two parts: one from the vehicle uncertainty (through the out_dyn_dxv Jacobian), and another from the uncertainty in the observation itself. By default, out_use_dyn_dhn_jacobian=true on call, and if it’s left at “true”, the base KalmanFilter class will compute the uncertainty of the landmark relative position from out_dyn_dhn. Only in some problems (e.g. MonoSLAM), it’ll be needed for the application to directly return the covariance matrix out_dyn_dhn_R_dyn_dhnT, which is the equivalent to:

\f$ \frac{\partial y_n}{\partial h_n} R \frac{\partial

y_n}{partial h_n}^top \(. but may be computed from additional terms, or whatever needed by the user. \param in_z The observation vector whose inverse sensor model is to be computed. This is actually one of the vector<> returned by OnGetObservationsAndDataAssociation(). \param out_yn The F-length vector with the inverse observation model\) y_n=y(x,z_n) \(. \param out_dyn_dxv The\) @_fakenlF times V \(Jacobian of the inv. sensor model wrt the robot pose\) frac{partial y_n}{partial x_v} \(. \param out_dyn_dhn The\) @_fakenlF times O \(Jacobian of the inv. sensor model wrt the observation vector\) frac{partial y_n}{partial h_n}

virtual void OnNewLandmarkAddedToMap(] size_t in_obsIdx, ] size_t in_idxNewFeat)

If applicable to the given problem, do here any special handling of adding a new landmark to the map.

Parameters:

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.