template class mrpt::bayes::CKalmanFilterCapable

Overview

Virtual base template for EKF, IEKF, and IKF Kalman filter implementations using the CRTP-like callback pattern.

Stores the state mean vector \(\hat{x}_{k|k}\) (m_xkk) and the full covariance matrix \(P_{k|k}\) (m_pkk). Derived classes implement the pure-virtual “On*” callback methods that describe the specific filtering problem (motion model, observation model, noise matrices, data association). The main entry point runOneKalmanIteration() then calls those callbacks in the correct order to carry out one predict-update cycle.

Supported filter variants (selected via KF_options.method):

kfEKFNaive: Standard Extended Kalman Filter.
kfEKFAlaDavison: EKF with efficient feature-by-feature updates (SLAM).
kfIKFFull: Iterated EKF (full re-linearization per iteration).

The algorithms are generic but biased toward SLAM-like problems; they are also applicable to non-SLAM estimation tasks.

runOneKalmanIteration() is protected; derived classes must expose a problem-specific public method that calls it.

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

For further details and examples see: https://www.mrpt.org/Kalman_Filters

Parameters:

VEH_SIZE	Dimension of the vehicle (robot) state sub-vector.
OBS_SIZE	Dimension of a single observation vector.
FEAT_SIZE	Dimension of each map feature state, or 0 if no map.
ACT_SIZE	Dimension of the action (control input) vector, or 0.
KFTYPE	Scalar type for all matrix/vector arithmetic (default: double).

Inherited Members

public:
    // structs

    struct TMsg;

    // methods

    COutputLogger& operator = (const COutputLogger&);
    COutputLogger& operator = (COutputLogger&&);

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.