template class mrpt::bayes::CKalmanFilterCapable

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

See also:

mrpt::slam::CRangeBearingKFSLAM, mrpt::slam::CRangeBearingKFSLAM2D

#include <mrpt/bayes/CKalmanFilterCapable.h>

template <
    size_t VEH_SIZE,
    size_t OBS_SIZE,
    size_t FEAT_SIZE,
    size_t ACT_SIZE,
    typename KFTYPE = double
    >
class CKalmanFilterCapable: public mrpt::system::COutputLogger
{
public:
    // typedefs

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

    //
fields

    TKF_options KF_options;

    //
methods

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

    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;

    virtual void OnNewLandmarkAddedToMap(] const size_t in_obsIdx, ] const size_t in_idxNewFeat);
    virtual void OnNormalizeStateVector();
    virtual void OnPostIteration();
    void getLandmarkMean(size_t idx, KFArray_FEAT& feat) const;
    void getLandmarkCov(size_t idx, KFMatrix_FxF& feat_cov) const;
};

// direct descendants

class CRangeBearingKFSLAM;
class CRangeBearingKFSLAM2D;

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:

\(\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.

  • 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.

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}\).

out_dyn_dhn_R_dyn_dhnT

See the discussion above.

virtual void OnNewLandmarkAddedToMap(
    ] const size_t in_obsIdx,
    ] const 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.

See also:

OnInverseObservationModel

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

void getLandmarkMean(size_t idx, KFArray_FEAT& feat) const

Returns the mean of the estimated value of the idx’th landmark (not applicable to non-SLAM problems).

Parameters:

std::exception

On idx>= getNumberOfLandmarksInTheMap()

void getLandmarkCov(size_t idx, KFMatrix_FxF& feat_cov) const

Returns the covariance of the idx’th landmark (not applicable to non-SLAM problems).

Parameters:

std::exception

On idx>= getNumberOfLandmarksInTheMap()