CKalmanFilterCapable.h

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */
#ifndef CKalmanFilterCapable_H
#define CKalmanFilterCapable_H

#include <mrpt/math/CMatrixFixedNumeric.h>
#include <mrpt/math/CMatrixTemplateNumeric.h>
#include <mrpt/math/CArrayNumeric.h>
#include <mrpt/math/num_jacobian.h>
#include <mrpt/math/utils.h>
#include <mrpt/math/num_jacobian.h>
#include <mrpt/utils/CConfigFileBase.h>
#include <mrpt/utils/CTimeLogger.h>
#include <mrpt/utils/aligned_containers.h>
#include <mrpt/utils/CLoadableOptions.h>
#include <mrpt/utils/stl_containers_utils.h>
#include <mrpt/utils/COutputLogger.h>
#include <mrpt/utils/stl_containers_utils.h>  // find_in_vector
#include <mrpt/utils/CTicTac.h>
#include <mrpt/utils/CFileOutputStream.h>
#include <mrpt/utils/TEnumType.h>
#include <mrpt/system/vector_loadsave.h>

namespace mrpt
{
namespace bayes
{
/** The Kalman Filter algorithm to employ in bayes::CKalmanFilterCapable
  *  For further details on each algorithm see the tutorial:
 * http://www.mrpt.org/Kalman_Filters
  * \sa bayes::CKalmanFilterCapable::KF_options
  * \ingroup mrpt_bayes_grp
  */
enum TKFMethod
{
        kfEKFNaive = 0,
        kfEKFAlaDavison,
        kfIKFFull,
        kfIKF
};

// Forward declaration:
template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE,
                  typename KFTYPE>
class CKalmanFilterCapable;

/** Generic options for the Kalman Filter algorithm in itself.
  * \ingroup mrpt_bayes_grp
  */
struct TKF_options : public mrpt::utils::CLoadableOptions
{
        TKF_options(mrpt::utils::VerbosityLevel& verb_level_ref)
                : method(kfEKFNaive),
                  verbosity_level(verb_level_ref),
                  IKF_iterations(5),
                  enable_profiler(false),
                  use_analytic_transition_jacobian(true),
                  use_analytic_observation_jacobian(true),
                  debug_verify_analytic_jacobians(false),
                  debug_verify_analytic_jacobians_threshold(1e-2)
        {
        }

        void loadFromConfigFile(
                const mrpt::utils::CConfigFileBase& iniFile,
                const std::string& section) override
        {
                method = iniFile.read_enum<TKFMethod>(section, "method", method);
                verbosity_level = iniFile.read_enum<mrpt::utils::VerbosityLevel>(
                        section, "verbosity_level", verbosity_level);
                MRPT_LOAD_CONFIG_VAR(IKF_iterations, int, iniFile, section);
                MRPT_LOAD_CONFIG_VAR(enable_profiler, bool, iniFile, section);
                MRPT_LOAD_CONFIG_VAR(
                        use_analytic_transition_jacobian, bool, iniFile, section);
                MRPT_LOAD_CONFIG_VAR(
                        use_analytic_observation_jacobian, bool, iniFile, section);
                MRPT_LOAD_CONFIG_VAR(
                        debug_verify_analytic_jacobians, bool, iniFile, section);
                MRPT_LOAD_CONFIG_VAR(
                        debug_verify_analytic_jacobians_threshold, double, iniFile,
                        section);
        }

        /** This method must display clearly all the contents of the structure in
         * textual form, sending it to a CStream. */
        void dumpToTextStream(mrpt::utils::CStream& out) const override
        {
                out.printf("\n----------- [TKF_options] ------------ \n\n");
                out.printf(
                        "method                                  = %s\n",
                        mrpt::utils::TEnumType<TKFMethod>::value2name(method).c_str());
                out.printf(
                        "verbosity_level                         = %s\n",
                        mrpt::utils::TEnumType<mrpt::utils::VerbosityLevel>::value2name(
                                verbosity_level)
                                .c_str());
                out.printf(
                        "IKF_iterations                          = %i\n", IKF_iterations);
                out.printf(
                        "enable_profiler                         = %c\n",
                        enable_profiler ? 'Y' : 'N');
                out.printf("\n");
        }

        /** The method to employ (default: kfEKFNaive) */
        TKFMethod method;
        mrpt::utils::VerbosityLevel& verbosity_level;
        /** Number of refinement iterations, only for the IKF method. */
        int IKF_iterations;
        /** If enabled (default=false), detailed timing information will be dumped
         * to the console thru a CTimerLog at the end of the execution. */
        bool enable_profiler;
        /** (default=true) If true, OnTransitionJacobian will be called; otherwise,
         * the Jacobian will be estimated from a numeric approximation by calling
         * several times to OnTransitionModel. */
        bool use_analytic_transition_jacobian;
        /** (default=true) If true, OnObservationJacobians will be called;
         * otherwise, the Jacobian will be estimated from a numeric approximation by
         * calling several times to OnObservationModel. */
        bool use_analytic_observation_jacobian;
        /** (default=false) If true, will compute all the Jacobians numerically and
         * compare them to the analytical ones, throwing an exception on mismatch.
         */
        bool debug_verify_analytic_jacobians;
        /** (default-1e-2) Sets the threshold for the difference between the
         * analytic and the numerical jacobians */
        double debug_verify_analytic_jacobians_threshold;
};

/** Auxiliary functions, for internal usage of MRPT classes */
namespace detail
{
// Auxiliary functions.
template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE,
                  typename KFTYPE>
inline size_t getNumberOfLandmarksInMap(
        const CKalmanFilterCapable<VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE>&
                obj);
// Specialization:
template <size_t VEH_SIZE, size_t OBS_SIZE, size_t ACT_SIZE, typename KFTYPE>
inline size_t getNumberOfLandmarksInMap(
        const CKalmanFilterCapable<VEH_SIZE, OBS_SIZE, 0 /*FEAT_SIZE*/, ACT_SIZE,
                                                           KFTYPE>& obj);

template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE,
                  typename KFTYPE>
inline bool isMapEmpty(
        const CKalmanFilterCapable<VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE>&
                obj);
// Specialization:
template <size_t VEH_SIZE, size_t OBS_SIZE, size_t ACT_SIZE, typename KFTYPE>
inline bool isMapEmpty(
        const CKalmanFilterCapable<VEH_SIZE, OBS_SIZE, 0 /*FEAT_SIZE*/, ACT_SIZE,
                                                           KFTYPE>& obj);

template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE,
                  typename KFTYPE>
void addNewLandmarks(
        CKalmanFilterCapable<VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE>& obj,
        const typename CKalmanFilterCapable<VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE,
                                                                                KFTYPE>::vector_KFArray_OBS& Z,
        const vector_int& data_association,
        const typename CKalmanFilterCapable<VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE,
                                                                                KFTYPE>::KFMatrix_OxO& R);
template <size_t VEH_SIZE, size_t OBS_SIZE, size_t ACT_SIZE, typename KFTYPE>
void addNewLandmarks(
        CKalmanFilterCapable<VEH_SIZE, OBS_SIZE, 0 /* FEAT_SIZE=0 */, ACT_SIZE,
                                                 KFTYPE>& obj,
        const typename CKalmanFilterCapable<VEH_SIZE, OBS_SIZE, 0 /* FEAT_SIZE=0 */,
                                                                                ACT_SIZE, KFTYPE>::vector_KFArray_OBS&
                Z,
        const vector_int& data_association,
        const typename CKalmanFilterCapable<VEH_SIZE, OBS_SIZE, 0 /* FEAT_SIZE=0 */,
                                                                                ACT_SIZE, KFTYPE>::KFMatrix_OxO& R);
}

/** 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).
 *
 *  \sa mrpt::slam::CRangeBearingKFSLAM, mrpt::slam::CRangeBearingKFSLAM2D
 * \ingroup mrpt_bayes_grp
 */
template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE,
                  typename KFTYPE = double>
class CKalmanFilterCapable : public mrpt::utils::COutputLogger
{
   public:
        static inline size_t get_vehicle_size() { return VEH_SIZE; }
        static inline size_t get_observation_size() { return OBS_SIZE; }
        static inline size_t get_feature_size() { return FEAT_SIZE; }
        static inline size_t get_action_size() { return ACT_SIZE; }
        inline size_t getNumberOfLandmarksInTheMap() const
        {
                return detail::getNumberOfLandmarksInMap(*this);
        }
        inline bool isMapEmpty() const { return detail::isMapEmpty(*this); }
        /** The numeric type used in the Kalman Filter (default=double) */
        typedef KFTYPE kftype;
        /** My class, in a shorter name! */
        typedef CKalmanFilterCapable<VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE,
                                                                 KFTYPE>
                KFCLASS;

        // ---------- Many useful typedefs to short the notation a bit... --------
        typedef Eigen::Matrix<KFTYPE, Eigen::Dynamic, 1> KFVector;
        typedef mrpt::math::CMatrixTemplateNumeric<KFTYPE> KFMatrix;

        typedef mrpt::math::CMatrixFixedNumeric<KFTYPE, VEH_SIZE, VEH_SIZE>
                KFMatrix_VxV;
        typedef mrpt::math::CMatrixFixedNumeric<KFTYPE, OBS_SIZE, OBS_SIZE>
                KFMatrix_OxO;
        typedef mrpt::math::CMatrixFixedNumeric<KFTYPE, FEAT_SIZE, FEAT_SIZE>
                KFMatrix_FxF;
        typedef mrpt::math::CMatrixFixedNumeric<KFTYPE, ACT_SIZE, ACT_SIZE>
                KFMatrix_AxA;

        typedef mrpt::math::CMatrixFixedNumeric<KFTYPE, VEH_SIZE, OBS_SIZE>
                KFMatrix_VxO;
        typedef mrpt::math::CMatrixFixedNumeric<KFTYPE, VEH_SIZE, FEAT_SIZE>
                KFMatrix_VxF;

        typedef mrpt::math::CMatrixFixedNumeric<KFTYPE, FEAT_SIZE, VEH_SIZE>
                KFMatrix_FxV;
        typedef mrpt::math::CMatrixFixedNumeric<KFTYPE, FEAT_SIZE, OBS_SIZE>
                KFMatrix_FxO;

        typedef mrpt::math::CMatrixFixedNumeric<KFTYPE, OBS_SIZE, FEAT_SIZE>
                KFMatrix_OxF;
        typedef mrpt::math::CMatrixFixedNumeric<KFTYPE, OBS_SIZE, VEH_SIZE>
                KFMatrix_OxV;

        typedef mrpt::math::CArrayNumeric<KFTYPE, VEH_SIZE> KFArray_VEH;
        typedef mrpt::math::CArrayNumeric<KFTYPE, ACT_SIZE> KFArray_ACT;
        typedef mrpt::math::CArrayNumeric<KFTYPE, OBS_SIZE> KFArray_OBS;
        typedef typename mrpt::aligned_containers<KFArray_OBS>::vector_t
                vector_KFArray_OBS;
        typedef mrpt::math::CArrayNumeric<KFTYPE, FEAT_SIZE> KFArray_FEAT;

        inline size_t getStateVectorLength() const { return m_xkk.size(); }
        inline KFVector& internal_getXkk() { return m_xkk; }
        inline KFMatrix& internal_getPkk() { return m_pkk; }
        /** Returns the mean of the estimated value of the idx'th landmark (not
         * applicable to non-SLAM problems).
          * \exception std::exception On idx>= getNumberOfLandmarksInTheMap()
          */
        inline void getLandmarkMean(size_t idx, KFArray_FEAT& feat) const
        {
                ASSERT_(idx < getNumberOfLandmarksInTheMap())
                ::memcpy(
                        &feat[0], &m_xkk[VEH_SIZE + idx * FEAT_SIZE],
                        FEAT_SIZE * sizeof(m_xkk[0]));
        }
        /** Returns the covariance of the idx'th landmark (not applicable to
         * non-SLAM problems).
          * \exception std::exception On idx>= getNumberOfLandmarksInTheMap()
          */
        inline void getLandmarkCov(size_t idx, KFMatrix_FxF& feat_cov) const
        {
                m_pkk.extractMatrix(
                        VEH_SIZE + idx * FEAT_SIZE, VEH_SIZE + idx * FEAT_SIZE, feat_cov);
        }

   protected:
        /** @name Kalman filter state
                @{ */

        /** The system state vector. */
        KFVector m_xkk;
        /** The system full covariance matrix. */
        KFMatrix m_pkk;

        /** @} */

        mrpt::utils::CTimeLogger m_timLogger;

        /** @name Virtual methods for Kalman Filter implementation
                @{
         */

        /** Must return the action vector u.
          * \param out_u The action vector which will be passed to OnTransitionModel
          */
        virtual void OnGetAction(KFArray_ACT& out_u) const = 0;

        /** Implements the transition model \f$ \hat{x}_{k|k-1} = f(
         * \hat{x}_{k-1|k-1}, u_k ) \f$
          * \param in_u The vector returned by OnGetAction.
          * \param inout_x At input has \f[ \hat{x}_{k-1|k-1} \f] , at output must
         * have \f$ \hat{x}_{k|k-1} \f$ .
          * \param 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
          * \note Even if you return "out_skip=true", the value of "inout_x" MUST be
         * updated as usual (this is to allow numeric approximation of Jacobians).
          */
        virtual void OnTransitionModel(
                const KFArray_ACT& in_u, KFArray_VEH& inout_x,
                bool& out_skipPrediction) const = 0;

        /** Implements the transition Jacobian \f$ \frac{\partial f}{\partial x} \f$
          * \param out_F Must return the Jacobian.
          *  The returned matrix must be \f$V \times V\f$ with V being either the
         * size of the whole state vector (for non-SLAM problems) or VEH_SIZE (for
         * SLAM problems).
          */
        virtual void OnTransitionJacobian(KFMatrix_VxV& out_F) const
        {
                MRPT_UNUSED_PARAM(out_F);
                m_user_didnt_implement_jacobian = true;
        }

        /** Only called if using a numeric approximation of the transition Jacobian,
         * this method must return the increments in each dimension of the vehicle
         * state vector while estimating the Jacobian.
          */
        virtual void OnTransitionJacobianNumericGetIncrements(
                KFArray_VEH& out_increments) const
        {
                for (size_t i = 0; i < VEH_SIZE; i++) out_increments[i] = 1e-6;
        }

        /** Implements the transition noise covariance \f$ Q_k \f$
          * \param out_Q Must return the covariance matrix.
          *  The returned matrix must be of the same size than the jacobian from
         * OnTransitionJacobian
          */
        virtual void OnTransitionNoise(KFMatrix_VxV& out_Q) const = 0;

        /** This will be called before OnGetObservationsAndDataAssociation to allow
         * the application to reduce the number of covariance landmark predictions
         * to be made.
          *  For example, features which are known to be "out of sight" shouldn't be
         * added to the output list to speed up the calculations.
          * \param in_all_prediction_means The mean of each landmark predictions;
         * the computation or not of the corresponding covariances is what we're
         * trying to determined with this method.
          * \param out_LM_indices_to_predict The list of landmark indices in the map
         * [0,getNumberOfLandmarksInTheMap()-1] that should be predicted.
          * \note This is not a pure virtual method, so it should be implemented
         * only if desired. The default implementation returns a vector with all the
         * landmarks in the map.
          * \sa OnGetObservations, OnDataAssociation
          */
        virtual void OnPreComputingPredictions(
                const vector_KFArray_OBS& in_all_prediction_means,
                mrpt::vector_size_t& out_LM_indices_to_predict) const
        {
                MRPT_UNUSED_PARAM(in_all_prediction_means);
                // Default: all of them:
                const size_t N = this->getNumberOfLandmarksInTheMap();
                out_LM_indices_to_predict.resize(N);
                for (size_t i = 0; i < N; i++) out_LM_indices_to_predict[i] = i;
        }

        /** Return the observation NOISE covariance matrix, that is, the model of
         * the Gaussian additive noise of the sensor.
          * \param out_R The noise covariance matrix. It might be non diagonal, but
         * it'll usually be.
          * \note Upon call, it can be assumed that the previous contents of out_R
         * are all zeros.
          */
        virtual void OnGetObservationNoise(KFMatrix_OxO& out_R) const = 0;

        /** This is called between the KF prediction step and the update step, and
         * the application must return the observations and, when applicable, the
         * data association between these observations and the current map.
          *
          * \param out_z N vectors, each for one "observation" of length OBS_SIZE, N
         * being the number of "observations": how many observed landmarks for a
         * map, or just one if not applicable.
          * \param 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 (in
         * the range [0,getNumberOfLandmarksInTheMap()-1]), or -1 if it is a new map
         * element and we want to insert it at the end of this KF iteration.
          * \param in_all_predictions A vector with the prediction of ALL the
         * landmarks in the map. Note that, in contrast, in_S only comprises a
         * subset of all the landmarks.
          * \param in_S The full covariance matrix of the observation predictions
         * (i.e. the "innovation covariance matrix"). This is a M*O x M*O matrix
         * with M=length of "in_lm_indices_in_S".
          * \param in_lm_indices_in_S The indices of the map landmarks (range
         * [0,getNumberOfLandmarksInTheMap()-1]) that can be found in the matrix
         * in_S.
          *
          *  This method will be called just once for each complete KF iteration.
          * \note It is assumed that the observations are independent, i.e. there
         * are NO cross-covariances between them.
          */
        virtual void OnGetObservationsAndDataAssociation(
                vector_KFArray_OBS& out_z, mrpt::vector_int& out_data_association,
                const vector_KFArray_OBS& in_all_predictions, const KFMatrix& in_S,
                const vector_size_t& in_lm_indices_in_S, const KFMatrix_OxO& in_R) = 0;

        /** Implements the observation prediction \f$ h_i(x) \f$.
          * \param idx_landmark_to_predict The indices of the landmarks in the map
         * whose predictions are expected as output. For non SLAM-like problems,
         * this input value is undefined and the application should just generate
         * one observation for the given problem.
          * \param out_predictions The predicted observations.
          */
        virtual void OnObservationModel(
                const mrpt::vector_size_t& idx_landmarks_to_predict,
                vector_KFArray_OBS& out_predictions) const = 0;

        /** Implements the observation Jacobians \f$ \frac{\partial h_i}{\partial x}
         * \f$ and (when applicable) \f$ \frac{\partial h_i}{\partial y_i} \f$.
          * \param idx_landmark_to_predict The index of the landmark in the map
         * whose prediction is expected as output. For non SLAM-like problems, this
         * will be zero and the expected output is for the whole state vector.
          * \param Hx  The output Jacobian \f$ \frac{\partial h_i}{\partial x} \f$.
          * \param Hy  The output Jacobian \f$ \frac{\partial h_i}{\partial y_i}
         * \f$.
          */
        virtual void OnObservationJacobians(
                const size_t& idx_landmark_to_predict, KFMatrix_OxV& Hx,
                KFMatrix_OxF& Hy) const
        {
                MRPT_UNUSED_PARAM(idx_landmark_to_predict);
                MRPT_UNUSED_PARAM(Hx);
                MRPT_UNUSED_PARAM(Hy);
                m_user_didnt_implement_jacobian = true;
        }

        /** Only called if using a numeric approximation of the observation
         * Jacobians, this method must return the increments in each dimension of
         * the vehicle state vector while estimating the Jacobian.
          */
        virtual void OnObservationJacobiansNumericGetIncrements(
                KFArray_VEH& out_veh_increments,
                KFArray_FEAT& out_feat_increments) const
        {
                for (size_t i = 0; i < VEH_SIZE; i++) out_veh_increments[i] = 1e-6;
                for (size_t i = 0; i < FEAT_SIZE; i++) out_feat_increments[i] = 1e-6;
        }

        /** Computes A=A-B, which may need to be re-implemented depending on the
         * topology of the individual scalar components (eg, angles).
          */
        virtual void OnSubstractObservationVectors(
                KFArray_OBS& A, const KFArray_OBS& B) const
        {
                A -= B;
        }

   public:
        /** If applicable to the given problem, this method implements the inverse
         * observation model needed to extend the "map" with a new "element".
          * \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 \f$
         * y_n=y(x,z_n) \f$.
          * \param out_dyn_dxv The \f$F \times V\f$ Jacobian of the inv. sensor
         * model wrt the robot pose \f$ \frac{\partial y_n}{\partial x_v} \f$.
          * \param out_dyn_dhn The \f$F \times O\f$ Jacobian of the inv. sensor
         * model wrt the observation vector \f$ \frac{\partial y_n}{\partial h_n}
         * \f$.
          *
          *  - O: OBS_SIZE
          *  - V: VEH_SIZE
          *  - F: FEAT_SIZE
          *
          * \note 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.
          */
        virtual void OnInverseObservationModel(
                const KFArray_OBS& in_z, KFArray_FEAT& out_yn,
                KFMatrix_FxV& out_dyn_dxv, KFMatrix_FxO& out_dyn_dhn) const
        {
                MRPT_UNUSED_PARAM(in_z);
                MRPT_UNUSED_PARAM(out_yn);
                MRPT_UNUSED_PARAM(out_dyn_dxv);
                MRPT_UNUSED_PARAM(out_dyn_dhn);
                MRPT_START
                THROW_EXCEPTION(
                        "Inverse sensor model required but not implemented in derived "
                        "class.")
                MRPT_END
        }

        /** 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 \a
         * 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 \f$.
          *
          *  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 \f$
         * y_n=y(x,z_n) \f$.
          * \param out_dyn_dxv The \f$F \times V\f$ Jacobian of the inv. sensor
         * model wrt the robot pose \f$ \frac{\partial y_n}{\partial x_v} \f$.
          * \param out_dyn_dhn The \f$F \times O\f$ Jacobian of the inv. sensor
         * model wrt the observation vector \f$ \frac{\partial y_n}{\partial h_n}
         * \f$.
          * \param out_dyn_dhn_R_dyn_dhnT See the discussion above.
          *
          *  - O: OBS_SIZE
          *  - V: VEH_SIZE
          *  - F: FEAT_SIZE
          *
          * \note OnNewLandmarkAddedToMap will be also called after calling this
         * method if a landmark is actually being added to the map.
          */
        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
        {
                MRPT_UNUSED_PARAM(out_dyn_dhn_R_dyn_dhnT);
                MRPT_START
                OnInverseObservationModel(in_z, out_yn, out_dyn_dxv, out_dyn_dhn);
                out_use_dyn_dhn_jacobian = true;
                MRPT_END
        }

        /** If applicable to the given problem, do here any special handling of
         * adding a new landmark to the map.
          * \param 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.
          * \param 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.
          * \sa OnInverseObservationModel
          */
        virtual void OnNewLandmarkAddedToMap(
                const size_t in_obsIdx, const size_t in_idxNewFeat)
        {
                MRPT_UNUSED_PARAM(in_obsIdx);
                MRPT_UNUSED_PARAM(in_idxNewFeat);
                // Do nothing in this base class.
        }

        /** 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 OnNormalizeStateVector()
        {
                // Do nothing in this base class.
        }

        /** This method is called after finishing one KF iteration and before
         * returning from runOneKalmanIteration().
          */
        virtual void OnPostIteration()
        {
                // Do nothing in this base class.
        }

        /** @}
         */

   public:
        CKalmanFilterCapable()
                : mrpt::utils::COutputLogger("CKalmanFilterCapable"),
                  KF_options(this->m_min_verbosity_level),
                  m_user_didnt_implement_jacobian(true)
        /** Default constructor */ {}
        /** Destructor */
        virtual ~CKalmanFilterCapable() {}
        mrpt::utils::CTimeLogger& getProfiler() { return m_timLogger; }
        /** Generic options for the Kalman Filter algorithm itself. */
        TKF_options KF_options;

   private:
        //  "Local" variables to runOneKalmanIteration, declared here to avoid
        //   allocating them over and over again with each call.
        //  (The variables that go into the stack remains in the function body)
        vector_KFArray_OBS all_predictions;
        vector_size_t predictLMidxs;
        /** The vector of all partial Jacobians dh[i]_dx for each prediction */
        typename mrpt::aligned_containers<KFMatrix_OxV>::vector_t Hxs;
        /** The vector of all partial Jacobians dh[i]_dy[i] for each prediction */
        typename mrpt::aligned_containers<KFMatrix_OxF>::vector_t Hys;
        KFMatrix S;
        KFMatrix Pkk_subset;
        vector_KFArray_OBS Z;  // Each entry is one observation:
        KFMatrix K;  // Kalman gain
        KFMatrix S_1;  // Inverse of S
        KFMatrix dh_dx_full_obs;
        KFMatrix aux_K_dh_dx;

   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://www.mrpt.org/Kalman_Filters
          */
        void runOneKalmanIteration();

   private:
        mutable bool m_user_didnt_implement_jacobian;

        /** Auxiliary functions for Jacobian numeric estimation */
        static void KF_aux_estimate_trans_jacobian(
                const KFArray_VEH& x, const std::pair<KFCLASS*, KFArray_ACT>& dat,
                KFArray_VEH& out_x);
        static void KF_aux_estimate_obs_Hx_jacobian(
                const KFArray_VEH& x, const std::pair<KFCLASS*, size_t>& dat,
                KFArray_OBS& out_x);
        static void KF_aux_estimate_obs_Hy_jacobian(
                const KFArray_FEAT& x, const std::pair<KFCLASS*, size_t>& dat,
                KFArray_OBS& out_x);

        template <size_t VEH_SIZEb, size_t OBS_SIZEb, size_t FEAT_SIZEb,
                          size_t ACT_SIZEb, typename KFTYPEb>
        friend void detail::addNewLandmarks(
                CKalmanFilterCapable<VEH_SIZEb, OBS_SIZEb, FEAT_SIZEb, ACT_SIZEb,
                                                         KFTYPEb>& obj,
                const typename CKalmanFilterCapable<VEH_SIZEb, OBS_SIZEb, FEAT_SIZEb,
                                                                                        ACT_SIZEb,
                                                                                        KFTYPEb>::vector_KFArray_OBS& Z,
                const vector_int& data_association,
                const typename CKalmanFilterCapable<VEH_SIZEb, OBS_SIZEb, FEAT_SIZEb,
                                                                                        ACT_SIZEb, KFTYPEb>::KFMatrix_OxO&
                        R);
};  // end class

}  // end namespace

// Specializations MUST occur at the same namespace:
namespace utils
{
template <>
struct TEnumTypeFiller<bayes::TKFMethod>
{
        typedef bayes::TKFMethod enum_t;
        static void fill(bimap<enum_t, std::string>& m_map)
        {
                m_map.insert(bayes::kfEKFNaive, "kfEKFNaive");
                m_map.insert(bayes::kfEKFAlaDavison, "kfEKFAlaDavison");
                m_map.insert(bayes::kfIKFFull, "kfIKFFull");
                m_map.insert(bayes::kfIKF, "kfIKF");
        }
};
}  // End of namespace
}  // end namespace

// Template implementation:
#include "CKalmanFilterCapable_impl.h"

#endif