Main MRPT website > C++ reference
MRPT logo

CKalmanFilterCapable.h

Go to the documentation of this file.
00001 /* +---------------------------------------------------------------------------+
00002    |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
00003    |                                                                           |
00004    |                   http://mrpt.sourceforge.net/                            |
00005    |                                                                           |
00006    |   Copyright (C) 2005-2011  University of Malaga                           |
00007    |                                                                           |
00008    |    This software was written by the Machine Perception and Intelligent    |
00009    |      Robotics Lab, University of Malaga (Spain).                          |
00010    |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
00011    |                                                                           |
00012    |  This file is part of the MRPT project.                                   |
00013    |                                                                           |
00014    |     MRPT is free software: you can redistribute it and/or modify          |
00015    |     it under the terms of the GNU General Public License as published by  |
00016    |     the Free Software Foundation, either version 3 of the License, or     |
00017    |     (at your option) any later version.                                   |
00018    |                                                                           |
00019    |   MRPT is distributed in the hope that it will be useful,                 |
00020    |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
00021    |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
00022    |     GNU General Public License for more details.                          |
00023    |                                                                           |
00024    |     You should have received a copy of the GNU General Public License     |
00025    |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
00026    |                                                                           |
00027    +---------------------------------------------------------------------------+ */
00028 #ifndef CKalmanFilterCapable_H
00029 #define CKalmanFilterCapable_H
00030 
00031 #include <mrpt/math/CMatrixFixedNumeric.h>
00032 #include <mrpt/math/CMatrixTemplateNumeric.h>
00033 #include <mrpt/math/CArray.h>
00034 #include <mrpt/math/utils.h>
00035 
00036 #include <mrpt/utils/CTimeLogger.h>
00037 #include <mrpt/utils/CLoadableOptions.h>
00038 #include <mrpt/utils/CDebugOutputCapable.h>
00039 #include <mrpt/utils/stl_extensions.h>
00040 #include <mrpt/system/os.h>
00041 #include <mrpt/utils/CTicTac.h>
00042 #include <mrpt/utils/CFileOutputStream.h>
00043 
00044 #include <mrpt/bayes/link_pragmas.h>
00045 
00046 
00047 namespace mrpt
00048 {
00049         namespace bayes
00050         {
00051                 using namespace mrpt::utils;
00052                 using namespace mrpt::math;
00053                 using namespace mrpt;
00054                 using namespace std;
00055 
00056                 /** The Kalman Filter algorithm to employ in bayes::CKalmanFilterCapable
00057                   *  For further details on each algorithm see the tutorial: http://www.mrpt.org/Kalman_Filters
00058                   * \sa bayes::CKalmanFilterCapable::KF_options
00059                   */
00060                 enum TKFMethod {
00061                         kfEKFNaive = 0,
00062                         kfEKFAlaDavison,
00063                         kfIKFFull,
00064                         kfIKF
00065                 };
00066 
00067                 // Forward declaration:
00068                 template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE> class CKalmanFilterCapable;
00069 
00070                 namespace detail {
00071                 struct CRunOneKalmanIteration_addNewLandmarks;
00072                 }
00073 
00074                 /** Generic options for the Kalman Filter algorithm in itself.
00075                   */
00076                 struct BAYES_IMPEXP TKF_options : public utils::CLoadableOptions
00077                 {
00078                         TKF_options();
00079 
00080                         void  loadFromConfigFile(
00081                                 const mrpt::utils::CConfigFileBase      &source,
00082                                 const std::string               &section);
00083 
00084                         /** This method must display clearly all the contents of the structure in textual form, sending it to a CStream. */
00085                         void  dumpToTextStream(CStream  &out) const;
00086 
00087                         TKFMethod       method;                         //!< The method to employ (default: kfEKFNaive)
00088                         bool            verbose;                //!< If set to true timing and other information will be dumped during the execution (default=false)
00089                         int             IKF_iterations; //!< Number of refinement iterations, only for the IKF method.
00090                         bool            enable_profiler;//!< If enabled (default=false), detailed timing information will be dumped to the console thru a CTimerLog at the end of the execution.
00091                         bool            use_analytic_transition_jacobian;       //!< (default=true) If true, OnTransitionJacobian will be called; otherwise, the Jacobian will be estimated from a numeric approximation by calling several times to OnTransitionModel.
00092                         bool            use_analytic_observation_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.
00093                         bool            debug_verify_analytic_jacobians; //!< (default=false) If true, will compute all the Jacobians numerically and compare them to the analytical ones, throwing an exception on mismatch.
00094                         double          debug_verify_analytic_jacobians_threshold; //!< (default-1e-2) Sets the threshold for the difference between the analytic and the numerical jacobians
00095                 };
00096 
00097                 /** Auxiliary functions, for internal usage of MRPT classes */
00098                 namespace detail
00099                 {
00100                         // Auxiliary functions.
00101                         //template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE>
00102                         //void runOneKalmanIteration_addNewLandmarks(
00103                         //      CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,FEAT_SIZE,ACT_SIZE,KFTYPE> &obj,
00104                         //      const typename CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,FEAT_SIZE,ACT_SIZE,KFTYPE>::vector_KFArray_OBS &Z,
00105                         //      const mrpt::vector_int          &data_association,
00106                         //      const typename CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,FEAT_SIZE,ACT_SIZE,KFTYPE>::KFMatrix_OxO          &R
00107                         //      );
00108                         // Specialization:
00109                         //template <size_t VEH_SIZE, size_t OBS_SIZE, size_t ACT_SIZE, typename KFTYPE>
00110                         //void runOneKalmanIteration_addNewLandmarks(
00111                         //      CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,0 /* FEAT_SIZE=0 */,ACT_SIZE,KFTYPE> &obj,
00112                         //      const typename CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,0 /* FEAT_SIZE=0 */,ACT_SIZE,KFTYPE>::vector_KFArray_OBS& Z,
00113                         //      const mrpt::vector_int       &data_association,
00114                         //      const typename CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,0 /* FEAT_SIZE=0 */,ACT_SIZE,KFTYPE>::KFMatrix_OxO                &R
00115                         //      );
00116 
00117                         template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE>
00118                         inline size_t getNumberOfLandmarksInMap(const CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,FEAT_SIZE,ACT_SIZE,KFTYPE> &obj);
00119                         // Specialization:
00120                         template <size_t VEH_SIZE, size_t OBS_SIZE, size_t ACT_SIZE, typename KFTYPE>
00121                         inline size_t getNumberOfLandmarksInMap(const CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,0 /*FEAT_SIZE*/,ACT_SIZE,KFTYPE> &obj);
00122 
00123                         template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE>
00124                         inline bool isMapEmpty(const CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,FEAT_SIZE,ACT_SIZE,KFTYPE> &obj);
00125                         // Specialization:
00126                         template <size_t VEH_SIZE, size_t OBS_SIZE, size_t ACT_SIZE, typename KFTYPE>
00127                         inline bool isMapEmpty(const CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,0 /*FEAT_SIZE*/,ACT_SIZE,KFTYPE> &obj);
00128                 }
00129 
00130 
00131                 /** Virtual base for Kalman Filter (EKF,IEKF,UKF) implementations.
00132                  *   This base class stores the state vector and covariance matrix of the system. It has virtual methods that must be completed
00133                  *    by derived classes to address a given filtering problem. The main entry point of the algorithm is CKalmanFilterCapable::runOneKalmanIteration, which
00134                  *    should be called AFTER setting the desired filter options in KF_options, as well as any options in the derived class.
00135                  *   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.
00136                  *
00137                  *  For further details and examples, check out the tutorial: http://www.mrpt.org/Kalman_Filters
00138                  *
00139                  *  The Kalman filter algorithms are generic, but this implementation is biased to ease the implementation
00140                  *  of SLAM-like problems. However, it can be also applied to many generic problems not related to robotics or SLAM.
00141                  *
00142                  *  The meaning of the template parameters is:
00143                  *      - VEH_SIZE: The dimension of the "vehicle state": either the full state vector or the "vehicle" part if applicable.
00144                  *      - OBS_SIZE: The dimension of each observation (eg, 2 for pixel coordinates, 3 for 3D coordinates,etc).
00145                  *      - FEAT_SIZE: The dimension of the features in the system state (the "map"), or 0 if not applicable (the default if not implemented).
00146                  *      - ACT_SIZE: The dimension of each "action" u_k (or 0 if not applicable).
00147                  *      - KFTYPE: The numeric type of the matrices (default: double)
00148                  *
00149                  * Revisions:
00150                  *      - 2007: Antonio J. Ortiz de Galisteo (AJOGD)
00151                  *      - 2008/FEB: All KF classes corrected, reorganized, and rewritten (JLBC).
00152                  *      - 2008/MAR: Implemented IKF (JLBC).
00153                  *      - 2009/DEC: Totally rewritten as a generic template using fixed-size matrices where possible (JLBC).
00154                  *
00155                  *  \sa mrpt::slam::CRangeBearingKFSLAM, mrpt::slam::CRangeBearingKFSLAM2D
00156                  */
00157                 template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>
00158                 class CKalmanFilterCapable : public mrpt::utils::CDebugOutputCapable
00159                 {
00160                 public:
00161                         static inline size_t get_vehicle_size() { return VEH_SIZE; }
00162                         static inline size_t get_observation_size() { return OBS_SIZE; }
00163                         static inline size_t get_feature_size() { return FEAT_SIZE; }
00164                         static inline size_t get_action_size() { return ACT_SIZE; }
00165                         inline size_t getNumberOfLandmarksInTheMap() const { return detail::getNumberOfLandmarksInMap(*this); }
00166                         inline bool   isMapEmpty() const { return detail::isMapEmpty(*this); }
00167 
00168 
00169                         typedef KFTYPE kftype; //!< The numeric type used in the Kalman Filter (default=double)
00170                         typedef CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,FEAT_SIZE,ACT_SIZE,KFTYPE>  KFCLASS;  //!< My class, in a shorter name!
00171 
00172                         // ---------- Many useful typedefs to short the notation a bit... --------
00173                         typedef mrpt::dynamicsize_vector<KFTYPE> KFVector;
00174                         typedef CMatrixTemplateNumeric<KFTYPE>   KFMatrix;
00175 
00176                         typedef CMatrixFixedNumeric<KFTYPE,VEH_SIZE,VEH_SIZE>   KFMatrix_VxV;
00177                         typedef CMatrixFixedNumeric<KFTYPE,OBS_SIZE,OBS_SIZE>   KFMatrix_OxO;
00178                         typedef CMatrixFixedNumeric<KFTYPE,FEAT_SIZE,FEAT_SIZE> KFMatrix_FxF;
00179                         typedef CMatrixFixedNumeric<KFTYPE,ACT_SIZE,ACT_SIZE>   KFMatrix_AxA;
00180 
00181                         typedef CMatrixFixedNumeric<KFTYPE,VEH_SIZE,OBS_SIZE>   KFMatrix_VxO;
00182                         typedef CMatrixFixedNumeric<KFTYPE,VEH_SIZE,FEAT_SIZE>  KFMatrix_VxF;
00183 
00184                         typedef CMatrixFixedNumeric<KFTYPE,FEAT_SIZE,VEH_SIZE>  KFMatrix_FxV;
00185                         typedef CMatrixFixedNumeric<KFTYPE,FEAT_SIZE,OBS_SIZE>  KFMatrix_FxO;
00186 
00187                         typedef CMatrixFixedNumeric<KFTYPE,OBS_SIZE,FEAT_SIZE>  KFMatrix_OxF;
00188                         typedef CMatrixFixedNumeric<KFTYPE,OBS_SIZE,VEH_SIZE>   KFMatrix_OxV;
00189 
00190                         typedef CArrayNumeric<KFTYPE,VEH_SIZE>  KFArray_VEH;
00191                         typedef CArrayNumeric<KFTYPE,ACT_SIZE>  KFArray_ACT;
00192                         typedef CArrayNumeric<KFTYPE,OBS_SIZE>  KFArray_OBS;
00193                         typedef typename mrpt::aligned_containers<KFArray_OBS>::vector_t  vector_KFArray_OBS;
00194                         typedef CArrayNumeric<KFTYPE,FEAT_SIZE> KFArray_FEAT;
00195 
00196                         inline size_t getStateVectorLength() const { return m_xkk.size(); }
00197 
00198                         /** Returns the mean of the estimated value of the idx'th landmark (not applicable to non-SLAM problems).
00199                           * \exception std::exception On idx>= getNumberOfLandmarksInTheMap()
00200                           */
00201                         inline void getLandmarkMean(size_t idx, KFArray_FEAT &feat ) const {
00202                                 ASSERT_(idx<getNumberOfLandmarksInTheMap())
00203                                 ::memcpy(&feat[0], &m_xkk[VEH_SIZE+idx*FEAT_SIZE], FEAT_SIZE*sizeof(m_xkk[0]));
00204                         }
00205                         /** Returns the covariance of the idx'th landmark (not applicable to non-SLAM problems).
00206                           * \exception std::exception On idx>= getNumberOfLandmarksInTheMap()
00207                           */
00208                         inline void getLandmarkCov(size_t idx, KFMatrix_FxF &feat_cov ) const {
00209                                 m_pkk.extractMatrix(VEH_SIZE+idx*FEAT_SIZE,VEH_SIZE+idx*FEAT_SIZE,feat_cov);
00210                         }
00211 
00212                 protected:
00213                         /** @name Kalman filter state
00214                                 @{ */
00215 
00216                         KFVector  m_xkk;  //!< The system state vector.
00217                         KFMatrix  m_pkk;  //!< The system full covariance matrix.
00218 
00219                         /** @} */
00220 
00221                         mrpt::utils::CTimeLogger  m_timLogger;
00222 
00223                         /** @name Virtual methods for Kalman Filter implementation
00224                                 @{
00225                          */
00226 
00227                         /** Must return the action vector u.
00228                           * \param out_u The action vector which will be passed to OnTransitionModel
00229                           */
00230                         virtual void OnGetAction( KFArray_ACT &out_u ) const = 0;
00231 
00232                         /** Implements the transition model \f$ \hat{x}_{k|k-1} = f( \hat{x}_{k-1|k-1}, u_k ) \f$
00233                           * \param in_u The vector returned by OnGetAction.
00234                           * \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$ .
00235                           * \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
00236                           * \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).
00237                           */
00238                         virtual void OnTransitionModel(
00239                                 const KFArray_ACT &in_u,
00240                                 KFArray_VEH       &inout_x,
00241                                 bool &out_skipPrediction
00242                                 )  const = 0;
00243 
00244                         /** Implements the transition Jacobian \f$ \frac{\partial f}{\partial x} \f$
00245                           * \param out_F Must return the Jacobian.
00246                           *  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).
00247                           */
00248                         virtual void OnTransitionJacobian( KFMatrix_VxV  &out_F ) const
00249                         {
00250                                 m_user_didnt_implement_jacobian=true;
00251                         }
00252 
00253                         /** 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.
00254                           */
00255                         virtual void OnTransitionJacobianNumericGetIncrements(KFArray_VEH &out_increments) const
00256                         {
00257                                 std::fill_n(&out_increments[0], VEH_SIZE, 1e-6);
00258                         }
00259 
00260                         /** Implements the transition noise covariance \f$ Q_k \f$
00261                           * \param out_Q Must return the covariance matrix.
00262                           *  The returned matrix must be of the same size than the jacobian from OnTransitionJacobian
00263                           */
00264                         virtual void OnTransitionNoise( KFMatrix_VxV &out_Q )  const = 0;
00265 
00266                         /** This will be called before OnGetObservationsAndDataAssociation to allow the application to reduce the number of covariance landmark predictions to be made.
00267                           *  For example, features which are known to be "out of sight" shouldn't be added to the output list to speed up the calculations.
00268                           * \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.
00269                           * \param out_LM_indices_to_predict The list of landmark indices in the map [0,getNumberOfLandmarksInTheMap()-1] that should be predicted.
00270                           * \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.
00271                           * \sa OnGetObservations, OnDataAssociation
00272                           */
00273                         virtual void OnPreComputingPredictions(
00274                                 const vector_KFArray_OBS &in_all_prediction_means,
00275                                 mrpt::vector_size_t                             &out_LM_indices_to_predict ) const
00276                         {
00277                                 // Default: all of them:
00278                                 const size_t N = this->getNumberOfLandmarksInTheMap();
00279                                 out_LM_indices_to_predict.resize(N);
00280                                 for (size_t i=0;i<N;i++) out_LM_indices_to_predict[i]=i;
00281                         }
00282 
00283                         /** Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of the sensor.
00284                           * \param out_R The noise covariance matrix. It might be non diagonal, but it'll usually be.
00285                           * \note Upon call, it can be assumed that the previous contents of out_R are all zeros.
00286                           */
00287                         virtual void OnGetObservationNoise(KFMatrix_OxO &out_R)  const = 0;
00288 
00289                         /** 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.
00290                           *
00291                           * \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.
00292                           * \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.
00293                           * \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.
00294                           * \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".
00295                           * \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.
00296                           *
00297                           *  This method will be called just once for each complete KF iteration.
00298                           * \note It is assumed that the observations are independent, i.e. there are NO cross-covariances between them.
00299                           */
00300                         virtual void OnGetObservationsAndDataAssociation(
00301                                 vector_KFArray_OBS                      &out_z,
00302                                 mrpt::vector_int            &out_data_association,
00303                                 const vector_KFArray_OBS        &in_all_predictions,
00304                                 const KFMatrix              &in_S,
00305                                 const vector_size_t         &in_lm_indices_in_S,
00306                                 const KFMatrix_OxO          &in_R
00307                                 ) = 0;
00308 
00309                         /** Implements the observation prediction \f$ h_i(x) \f$.
00310                           * \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.
00311                           * \param out_predictions The predicted observations.
00312                           */
00313                         virtual void OnObservationModel(
00314                                 const mrpt::vector_size_t       &idx_landmarks_to_predict,
00315                                 vector_KFArray_OBS  &out_predictions
00316                                 ) const = 0;
00317 
00318                         /** Implements the observation Jacobians \f$ \frac{\partial h_i}{\partial x} \f$ and (when applicable) \f$ \frac{\partial h_i}{\partial y_i} \f$.
00319                           * \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.
00320                           * \param Hx  The output Jacobian \f$ \frac{\partial h_i}{\partial x} \f$.
00321                           * \param Hy  The output Jacobian \f$ \frac{\partial h_i}{\partial y_i} \f$.
00322                           */
00323                         virtual void OnObservationJacobians(
00324                                 const size_t &idx_landmark_to_predict,
00325                                 KFMatrix_OxV &Hx,
00326                                 KFMatrix_OxF &Hy
00327                                 ) const
00328                         {
00329                                 m_user_didnt_implement_jacobian=true;
00330                         }
00331 
00332                         /** 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.
00333                           */
00334                         virtual void OnObservationJacobiansNumericGetIncrements(
00335                                         KFArray_VEH  &out_veh_increments,
00336                                         KFArray_FEAT &out_feat_increments ) const
00337                         {
00338                                 std::fill_n(&out_veh_increments[0], VEH_SIZE, 1e-6);
00339                                 std::fill_n(&out_feat_increments[0], FEAT_SIZE, 1e-6);
00340                         }
00341 
00342                         /** Computes A=A-B, which may need to be re-implemented depending on the topology of the individual scalar components (eg, angles).
00343                           */
00344                         virtual void OnSubstractObservationVectors(KFArray_OBS &A, const KFArray_OBS &B) const
00345                         {
00346                                 A -= B;
00347                         }
00348 
00349                         /** If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element".
00350                           * \param in_z The observation vector whose inverse sensor model is to be computed. This is actually one of the vector<> returned by OnGetObservationsAndDataAssociation().
00351                           * \param out_yn The F-length vector with the inverse observation model \f$ y_n=y(x,z_n) \f$.
00352                           * \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$.
00353                           * \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$.
00354                           *
00355                           *  - O: OBS_SIZE
00356                           *  - V: VEH_SIZE
00357                           *  - F: FEAT_SIZE
00358                           *
00359                           * \note OnNewLandmarkAddedToMap will be also called after calling this method if a landmark is actually being added to the map.
00360                           * \deprecated This version of the method is deprecated. The alternative method is preferred to allow a greater flexibility.
00361                           */
00362                         virtual void  OnInverseObservationModel(
00363                                 const KFArray_OBS & in_z,
00364                                 KFArray_FEAT  & out_yn,
00365                                 KFMatrix_FxV  & out_dyn_dxv,
00366                                 KFMatrix_FxO  & out_dyn_dhn ) const
00367                         {
00368                                 MRPT_START
00369                                 THROW_EXCEPTION("Inverse sensor model required but not implemented in derived class.")
00370                                 MRPT_END
00371                         }
00372 
00373                         /** If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element".
00374                           *  The uncertainty in the new map feature comes from two parts: one from the vehicle uncertainty (through the out_dyn_dxv Jacobian),
00375                           *   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",
00376                           *   the base KalmanFilter class will compute the uncertainty of the landmark relative position from out_dyn_dhn.
00377                           *  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:
00378                           *
00379                           *         \f$ \frac{\partial y_n}{\partial h_n} R \frac{\partial y_n}{\partial h_n}^\top \f$.
00380                           *
00381                           *  but may be computed from additional terms, or whatever needed by the user.
00382                           *
00383                           * \param in_z The observation vector whose inverse sensor model is to be computed. This is actually one of the vector<> returned by OnGetObservationsAndDataAssociation().
00384                           * \param out_yn The F-length vector with the inverse observation model \f$ y_n=y(x,z_n) \f$.
00385                           * \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$.
00386                           * \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$.
00387                           * \param out_dyn_dhn_R_dyn_dhnT See the discussion above.
00388                           *
00389                           *  - O: OBS_SIZE
00390                           *  - V: VEH_SIZE
00391                           *  - F: FEAT_SIZE
00392                           *
00393                           * \note OnNewLandmarkAddedToMap will be also called after calling this method if a landmark is actually being added to the map.
00394                           */
00395                         virtual void  OnInverseObservationModel(
00396                                 const KFArray_OBS & in_z,
00397                                 KFArray_FEAT  & out_yn,
00398                                 KFMatrix_FxV  & out_dyn_dxv,
00399                                 KFMatrix_FxO  & out_dyn_dhn,
00400                                 KFMatrix_FxF  & out_dyn_dhn_R_dyn_dhnT,
00401                                 bool          & out_use_dyn_dhn_jacobian
00402                                  ) const
00403                         {
00404                                 MRPT_START
00405                                 OnInverseObservationModel(in_z,out_yn,out_dyn_dxv,out_dyn_dhn);
00406                                 out_use_dyn_dhn_jacobian=true;
00407                                 MRPT_END
00408                         }
00409 
00410                         /** If applicable to the given problem, do here any special handling of adding a new landmark to the map.
00411                           * \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.
00412                           * \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.
00413                           * \sa OnInverseObservationModel
00414                           */
00415                         virtual void OnNewLandmarkAddedToMap(
00416                                 const size_t    in_obsIdx,
00417                                 const size_t    in_idxNewFeat )
00418                         {
00419                                 // Do nothing in this base class.
00420                         }
00421 
00422                         /** 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.
00423                           */
00424                         virtual void OnNormalizeStateVector()
00425                         {
00426                                 // Do nothing in this base class.
00427                         }
00428 
00429                         /** This method is called after finishing one KF iteration and before returning from runOneKalmanIteration().
00430                           */
00431                         virtual void OnPostIteration()
00432                         {
00433                                 // Do nothing in this base class.
00434                         }
00435 
00436                         /** @}
00437                          */
00438 
00439                 public:
00440                         CKalmanFilterCapable() {} //!< Default constructor
00441                         virtual ~CKalmanFilterCapable() {}  //!< Destructor
00442 
00443                         mrpt::utils::CTimeLogger &getProfiler() { return m_timLogger; }
00444 
00445                         TKF_options KF_options; //!< Generic options for the Kalman Filter algorithm itself.
00446 
00447 
00448                 private:
00449                         //  "Local" variables to runOneKalmanIteration, declared here to avoid
00450                         //   allocating them over and over again with each call.
00451                         //  (The variables that go into the stack remains in the function body)
00452                         vector_KFArray_OBS              all_predictions;
00453                         vector_size_t                   predictLMidxs;
00454                         KFMatrix                                dh_dx;
00455                         KFMatrix                                dh_dx_full;
00456                         vector_size_t                   idxs;
00457                         KFMatrix                                S;
00458                         KFMatrix                                Pkk_subset;
00459                         vector_KFArray_OBS              Z;              // Each entry is one observation:
00460                         KFMatrix                                K;              // Kalman gain
00461                         KFMatrix                                S_1;    // Inverse of S
00462                         KFMatrix                                dh_dx_full_obs;
00463                         KFMatrix                                aux_K_dh_dx;
00464 
00465                 protected:
00466 
00467                         /** The main entry point, executes one complete step: prediction + update.
00468                           *  It is protected since derived classes must provide a problem-specific entry point for users.
00469                           *  The exact order in which this method calls the virtual method is explained in http://www.mrpt.org/Kalman_Filters.
00470                           */
00471                         void runOneKalmanIteration()
00472                         {
00473                                 MRPT_START
00474 
00475                                 m_timLogger.enable(KF_options.enable_profiler || KF_options.verbose);
00476                                 m_timLogger.enter("KF:complete_step");
00477 
00478                                 ASSERT_(size_t(m_xkk.size())==m_pkk.getColCount())
00479                                 ASSERT_(size_t(m_xkk.size())>=VEH_SIZE)
00480 
00481                                 // =============================================================
00482                                 //  1. CREATE ACTION MATRIX u FROM ODOMETRY
00483                                 // =============================================================
00484                                 KFArray_ACT  u;
00485 
00486                                 m_timLogger.enter("KF:1.OnGetAction");
00487                                 OnGetAction(u);
00488                                 m_timLogger.leave("KF:1.OnGetAction");
00489 
00490                                 // Sanity check:
00491                                 if (FEAT_SIZE) { ASSERTDEB_( (((m_xkk.size()-VEH_SIZE)/FEAT_SIZE)*FEAT_SIZE)== (m_xkk.size()-VEH_SIZE) ) }
00492 
00493                                 // =============================================================
00494                                 //  2. PREDICTION OF NEW POSE xv_{k+1|k}
00495                                 // =============================================================
00496                                 m_timLogger.enter("KF:2.prediction stage");
00497 
00498                                 const size_t N_map = getNumberOfLandmarksInTheMap();
00499 
00500                                 KFArray_VEH xv( &m_xkk[0] );  // Vehicle pose
00501 
00502                                 bool skipPrediction=false; // Wether to skip the prediction step (in SLAM this is desired for the first iteration...)
00503 
00504                                 // Update mean: xv will have the updated pose until we update it in the filterState later.
00505                                 //  This is to maintain a copy of the last robot pose in the state vector, required for the Jacobian computation.
00506                                 OnTransitionModel(u, xv, skipPrediction);
00507 
00508                                 if ( !skipPrediction )
00509                                 {
00510                                         // =============================================================
00511                                         //  3. PREDICTION OF COVARIANCE P_{k+1|k}
00512                                         // =============================================================
00513                                         // First, we compute de Jacobian fv_by_xv  (derivative of f_vehicle wrt x_vehicle):
00514                                         KFMatrix_VxV  dfv_dxv;
00515 
00516                                         // Try closed-form Jacobian first:
00517                                         m_user_didnt_implement_jacobian=false; // Set to true by the default method if not reimplemented in base class.
00518                                         if (KF_options.use_analytic_transition_jacobian)
00519                                                 OnTransitionJacobian(dfv_dxv);
00520 
00521                                         if (m_user_didnt_implement_jacobian || !KF_options.use_analytic_transition_jacobian || KF_options.debug_verify_analytic_jacobians)
00522                                         {       // Numeric approximation:
00523                                                 KFArray_VEH xkk_vehicle( &m_xkk[0] );  // A copy of the vehicle part of the state vector.
00524                                                 KFArray_VEH xkk_veh_increments;
00525                                                 OnTransitionJacobianNumericGetIncrements(xkk_veh_increments);
00526 
00527                                                 mrpt::math::estimateJacobian(
00528                                                         xkk_vehicle,
00529                                                         &KF_aux_estimate_trans_jacobian, //(const VECTORLIKE &x,const USERPARAM &y, VECTORLIKE3  &out),
00530                                                         xkk_veh_increments,
00531                                                         std::make_pair<KFCLASS*,KFArray_ACT>(this,u),
00532                                                         dfv_dxv);
00533 
00534                                                 if (KF_options.debug_verify_analytic_jacobians)
00535                                                 {
00536                                                         KFMatrix_VxV dfv_dxv_gt(UNINITIALIZED_MATRIX);
00537                                                         OnTransitionJacobian(dfv_dxv_gt);
00538                                                         if ((dfv_dxv-dfv_dxv_gt).Abs().sumAll()>KF_options.debug_verify_analytic_jacobians_threshold)
00539                                                         {
00540                                                                 std::cerr << "[KalmanFilter] ERROR: User analytical transition Jacobians are wrong: \n"
00541                                                                         << " Real dfv_dxv: \n" << dfv_dxv << "\n Analytical dfv_dxv:\n" << dfv_dxv_gt << "Diff:\n" << (dfv_dxv-dfv_dxv_gt) << "\n";
00542                                                                 THROW_EXCEPTION("ERROR: User analytical transition Jacobians are wrong (More details dumped to cerr)")
00543                                                         }
00544                                                 }
00545 
00546                                         }
00547 
00548                                         // Q is the process noise covariance matrix, is associated to the robot movement and is necesary to calculate the prediction P(k+1|k)
00549                                         KFMatrix_VxV  Q;
00550                                         OnTransitionNoise(Q);
00551 
00552                                         // ====================================
00553                                         //  3.1:  Pxx submatrix
00554                                         // ====================================
00555                                         // Replace old covariance:
00556                                         m_pkk.block(0,0,VEH_SIZE,VEH_SIZE) =
00557                                                 Q +
00558                                                 dfv_dxv * m_pkk.block(0,0,VEH_SIZE,VEH_SIZE) * dfv_dxv.transpose();
00559 
00560                                         // ====================================
00561                                         //  3.2:  All Pxy_i
00562                                         // ====================================
00563                                         // Now, update the cov. of landmarks, if any:
00564                                         KFMatrix_VxF aux;
00565                                         for (size_t i=0 ; i<N_map ; i++)
00566                                         {
00567                                                 // aux = dfv_dxv(...) * m_pkk(...)
00568                                                 dfv_dxv.multiply_subMatrix(
00569                                                         m_pkk,
00570                                                         aux,  // Output
00571                                                         VEH_SIZE+i*FEAT_SIZE,   // Offset col
00572                                                         0,                                               // Offset row
00573                                                         FEAT_SIZE                        // Number of columns desired in output
00574                                                 );
00575 
00576                                                 m_pkk.insertMatrix         (0,                VEH_SIZE+i*FEAT_SIZE,  aux );
00577                                                 m_pkk.insertMatrixTranspose(VEH_SIZE+i*FEAT_SIZE, 0               ,  aux );
00578                                         }
00579 
00580                                         // =============================================================
00581                                         //  4. NOW WE CAN OVERWRITE THE NEW STATE VECTOR
00582                                         // =============================================================
00583                                         for (size_t i=0;i<VEH_SIZE;i++)
00584                                                 m_xkk[i]=xv[i];
00585 
00586                                         // Normalize, if neccesary.
00587                                         OnNormalizeStateVector();
00588 
00589                                 } // end if (!skipPrediction)
00590 
00591 
00592                                 const double tim_pred = m_timLogger.leave("KF:2.prediction stage");
00593 
00594 
00595                                 // =============================================================
00596                                 //  5. PREDICTION OF OBSERVATIONS AND COMPUTE JACOBIANS
00597                                 // =============================================================
00598                                 m_timLogger.enter("KF:3.predict all obs");
00599 
00600                                 KFMatrix_OxO  R;        // Sensor uncertainty (covariance matrix): R
00601                                 OnGetObservationNoise(R);
00602 
00603                                 // Predict the observations for all the map LMs, so the user
00604                                 //  can decide if their covariances (more costly) must be computed as well:
00605                                 all_predictions.resize(N_map);
00606                                 OnObservationModel(
00607                                         mrpt::math::sequenceStdVec<size_t,1>(0,N_map),
00608                                         all_predictions);
00609 
00610                                 const double tim_pred_obs = m_timLogger.leave("KF:3.predict all obs");
00611 
00612                                 m_timLogger.enter("KF:4.decide pred obs");
00613 
00614                                 // Decide if some of the covariances shouldn't be predicted.
00615                                 predictLMidxs.clear();
00616                                 if (FEAT_SIZE==0)
00617                                         // In non-SLAM problems, just do one prediction, for the entire system state:
00618                                         predictLMidxs.assign(1,0);
00619                                 else
00620                                         // On normal SLAM problems:
00621                                         OnPreComputingPredictions(all_predictions, predictLMidxs);
00622 
00623 
00624                                 m_timLogger.leave("KF:4.decide pred obs");
00625 
00626                                 // =============================================================
00627                                 //  6. COMPUTE INNOVATION MATRIX "S"
00628                                 // =============================================================
00629                                 // Do the prediction of the observation covariances:
00630                                 // Compute S:  S = H P ~H + R
00631                                 //
00632                                 // Build a big dh_dx Jacobian composed of the small block Jacobians.
00633                                 // , but: it's actually a subset of the full Jacobian, since the non-predicted
00634                                 //  features do not appear.
00635                                 //
00636                                 //  dh_dx: O·M x V+M·F
00637                                 //      S: O·M x O·M
00638                                 //  M = |predictLMidxs|
00639                                 //  O=size of each observation.
00640                                 //  F=size of features in the map
00641                                 //
00642 
00643                                 // This is the beginning of a loop if we are doing sequential
00644                                 //  data association, where after each incorporation of an observation
00645                                 //  into the filter we recompute the vehicle state, then reconsider the
00646                                 //  data associations:
00647                                 // TKFFusionMethod fusion_strategy
00648 
00649 
00650                                 m_timLogger.enter("KF:5.build Jacobians");
00651 
00652                                 const size_t N_pred = FEAT_SIZE==0 ?
00653                                         1 /* In non-SLAM problems, there'll be only 1 fixed observation */ :
00654                                         predictLMidxs.size();
00655 
00656                                 dh_dx.zeros(N_pred*OBS_SIZE, VEH_SIZE + FEAT_SIZE * N_pred ); // Init to zeros.
00657                                 dh_dx_full.zeros(N_pred*OBS_SIZE, VEH_SIZE + FEAT_SIZE * N_map ); // Init to zeros.
00658 
00659                                 // Indices in xkk (& Pkk) that are involved in this observation (used below).
00660                                 idxs.clear();
00661                                 idxs.reserve(VEH_SIZE+N_pred*FEAT_SIZE);
00662 
00663                                 for (size_t i=0;i<VEH_SIZE;i++) idxs.push_back(i);
00664 
00665                                 for (size_t i=0;i<N_pred;++i)
00666                                 {
00667                                         const size_t lm_idx = FEAT_SIZE==0 ? 0 : predictLMidxs[i];
00668                                         KFMatrix_OxV Hx(UNINITIALIZED_MATRIX);
00669                                         KFMatrix_OxF Hy(UNINITIALIZED_MATRIX);
00670 
00671                                         // Try the analitic Jacobian first:
00672                                         m_user_didnt_implement_jacobian=false; // Set to true by the default method if not reimplemented in base class.
00673                                         if (KF_options.use_analytic_transition_jacobian)
00674                                                 OnObservationJacobians(lm_idx,Hx,Hy);
00675 
00676                                         if (m_user_didnt_implement_jacobian || !KF_options.use_analytic_observation_jacobian || KF_options.debug_verify_analytic_jacobians)
00677                                         {       // Numeric approximation:
00678                                                 const size_t lm_idx_in_statevector = VEH_SIZE+lm_idx*FEAT_SIZE;
00679 
00680                                                 const KFArray_VEH  x_vehicle( &m_xkk[0] );
00681                                                 const KFArray_FEAT x_feat( &m_xkk[lm_idx_in_statevector] );
00682 
00683                                                 KFArray_VEH  xkk_veh_increments;
00684                                                 KFArray_FEAT feat_increments;
00685                                                 OnObservationJacobiansNumericGetIncrements(xkk_veh_increments, feat_increments);
00686 
00687                                                 mrpt::math::estimateJacobian(
00688                                                         x_vehicle,
00689                                                         &KF_aux_estimate_obs_Hx_jacobian,
00690                                                         xkk_veh_increments,
00691                                                         std::make_pair<KFCLASS*,size_t>(this,lm_idx),
00692                                                         Hx);
00693                                                 // The state vector was temporarily modified by KF_aux_estimate_*, restore it:
00694 						::memcpy(&m_xkk[0],&x_vehicle[0],sizeof(m_xkk[0])*VEH_SIZE);
00695 
00696                                                 mrpt::math::estimateJacobian(
00697                                                         x_feat,
00698                                                         &KF_aux_estimate_obs_Hy_jacobian,
00699                                                         feat_increments,
00700                                                         std::make_pair<KFCLASS*,size_t>(this,lm_idx),
00701                                                         Hy);
00702                                                 // The state vector was temporarily modified by KF_aux_estimate_*, restore it:
00703 						::memcpy(&m_xkk[lm_idx_in_statevector],&x_feat[0],sizeof(m_xkk[0])*FEAT_SIZE);
00704 
00705                                                 if (KF_options.debug_verify_analytic_jacobians)
00706                                                 {
00707                                                         KFMatrix_OxV Hx_gt(UNINITIALIZED_MATRIX);
00708                                                         KFMatrix_OxF Hy_gt(UNINITIALIZED_MATRIX);
00709                                                         OnObservationJacobians(lm_idx,Hx_gt,Hy_gt);
00710                                                         if ((Hx-Hx_gt).Abs().sumAll()>KF_options.debug_verify_analytic_jacobians_threshold) {
00711                                                                 std::cerr << "[KalmanFilter] ERROR: User analytical observation Hx Jacobians are wrong: \n"
00712                                                                         << " Real Hx: \n" << Hx << "\n Analytical Hx:\n" << Hx_gt << "Diff:\n" << Hx-Hx_gt << "\n";
00713                                                                 THROW_EXCEPTION("ERROR: User analytical observation Hx Jacobians are wrong (More details dumped to cerr)")
00714                                                         }
00715                                                         if ((Hy-Hy_gt).Abs().sumAll()>KF_options.debug_verify_analytic_jacobians_threshold) {
00716                                                                 std::cerr << "[KalmanFilter] ERROR: User analytical observation Hy Jacobians are wrong: \n"
00717                                                                         << " Real Hy: \n" << Hy << "\n Analytical Hx:\n" << Hy_gt << "Diff:\n" << Hy-Hy_gt << "\n";
00718                                                                 THROW_EXCEPTION("ERROR: User analytical observation Hy Jacobians are wrong (More details dumped to cerr)")
00719                                                         }
00720                                                 }
00721                                         }
00722 
00723                                         dh_dx.insertMatrix(i*OBS_SIZE,0, Hx);
00724                                         if (FEAT_SIZE!=0)
00725                                                 dh_dx.insertMatrix(i*OBS_SIZE,VEH_SIZE+i*FEAT_SIZE, Hy);
00726 
00727                                         dh_dx_full.insertMatrix(i*OBS_SIZE,0, Hx);
00728                                         if (FEAT_SIZE!=0)
00729                                         {
00730                                                 dh_dx_full.insertMatrix(i*OBS_SIZE,VEH_SIZE+lm_idx*FEAT_SIZE, Hy);
00731 
00732                                                 for (size_t k=0;k<FEAT_SIZE;k++)
00733                                                         idxs.push_back(k+VEH_SIZE+FEAT_SIZE*lm_idx);
00734                                         }
00735                                 }
00736                                 m_timLogger.leave("KF:5.build Jacobians");
00737 
00738                                 // Compute S:  S = H P ~H + R
00739                                 // *TODO*: This can be accelerated by exploiting the sparsity of dh_dx!!!
00740                                 // ------------------------------------
00741                                 m_timLogger.enter("KF:6.build S");
00742 
00743                                 S.setSize(N_pred*OBS_SIZE,N_pred*OBS_SIZE);
00744 
00745                                 // (TODO: Implement multiply_HCHt for a subset of COV directly.)
00746                                 // Extract the subset of m_Pkk that is involved in this observation:
00747                                 m_pkk.extractSubmatrixSymmetrical(idxs,Pkk_subset);
00748 
00749                                 // S = dh_dx * m_pkk(subset) * (~dh_dx);
00750                                 dh_dx.multiply_HCHt(Pkk_subset,S);
00751 
00752                                 // Sum the "R" term:
00753                                 if ( FEAT_SIZE>0 )
00754                                 {
00755                                         for (size_t i=0;i<N_pred;++i)
00756                                         {
00757                                                 const size_t obs_idx_off = i*OBS_SIZE;
00758                                                 for (size_t j=0;j<OBS_SIZE;j++)
00759                                                         for (size_t k=0;k<OBS_SIZE;k++)
00760                                                                 S.get_unsafe(obs_idx_off+j,obs_idx_off+k) += R.get_unsafe(j,k);
00761                                         }
00762                                 }
00763                                 else
00764                                 {       // Not a SLAM-like EKF problem:
00765                                         ASSERTDEB_(S.getColCount() == OBS_SIZE );
00766                                         S+=R;
00767                                 }
00768 
00769                                 m_timLogger.leave("KF:6.build S");
00770 
00771 
00772                                 Z.clear();      // Each entry is one observation:
00773                                 vector_int  data_association;  // -1: New map feature.>=0: Indexes in the state vector
00774 
00775                                 m_timLogger.enter("KF:7.get obs & DA");
00776 
00777                                 // Get observations and do data-association:
00778                                 OnGetObservationsAndDataAssociation(
00779                                         Z, data_association, // Out
00780                                         all_predictions, S, predictLMidxs, R  // In
00781                                         );
00782 
00783                                 ASSERTDEB_(data_association.size()==Z.size() || (data_association.empty() && FEAT_SIZE==0));
00784 
00785                                 const double tim_obs_DA = m_timLogger.leave("KF:7.get obs & DA");
00786 
00787                                 // =============================================================
00788                                 //  7. UPDATE USING THE KALMAN GAIN
00789                                 // =============================================================
00790                                 // Update, only if there are observations!
00791                                 if ( !Z.empty() )
00792                                 {
00793                                         m_timLogger.enter("KF:8.update stage");
00794 
00795                                         switch (KF_options.method)
00796                                         {
00797                                                 // -----------------------
00798                                                 //  FULL KF- METHOD
00799                                                 // -----------------------
00800                                         case kfEKFNaive:
00801                                         case kfIKFFull:
00802                                         {
00803                                                 // Build the whole Jacobian dh_dx matrix
00804                                                 // ---------------------------------------------
00805                                                 // Keep only those whose DA is not -1
00806                                                 vector_int  mapIndicesForKFUpdate(data_association.size());
00807                                                 mapIndicesForKFUpdate.resize( std::distance(mapIndicesForKFUpdate.begin(),
00808                                                         std::remove_copy_if(
00809                                                                 data_association.begin(),
00810                                                                 data_association.end(),
00811                                                                 mapIndicesForKFUpdate.begin(),
00812                                                                 binder1st<equal_to<int> >(equal_to<int>(),-1) ) ) );
00813 
00814                                                 const size_t N_upd = (FEAT_SIZE==0) ?
00815                                                         1 :     // Non-SLAM problems: Just one observation for the entire system.
00816                                                         mapIndicesForKFUpdate.size(); // SLAM: # of observed known landmarks
00817 
00818                                                 // Just one, or several update iterations??
00819                                                 const size_t nKF_iterations = (KF_options.method==kfEKFNaive) ?  1 : KF_options.IKF_iterations;
00820 
00821                                                 const KFVector xkk_0 = m_xkk;
00822 
00823                                                 // For each IKF iteration (or 1 for EKF)
00824                                                 if (N_upd>0) // Do not update if we have no observations!
00825                                                 {
00826                                                         for (size_t IKF_iteration=0;IKF_iteration<nKF_iterations;IKF_iteration++)
00827                                                         {
00828                                                                 // Compute ytilde = OBS - PREDICTION
00829                                                                 KFVector  ytilde(OBS_SIZE*N_upd);
00830                                                                 size_t    ytilde_idx = 0;
00831 
00832                                                                 // TODO: Use a Matrix view of "dh_dx_full" instead of creating a copy into "dh_dx_full_obs"
00833                                                                 dh_dx_full_obs.zeros(N_upd*OBS_SIZE, VEH_SIZE + FEAT_SIZE * N_map ); // Init to zeros.
00834                                                                 KFMatrix S_observed;    // The KF "S" matrix: A re-ordered, subset, version of the prediction S:
00835 
00836                                                                 if (FEAT_SIZE!=0)
00837                                                                 {       // SLAM problems:
00838                                                                         vector_size_t S_idxs;
00839                                                                         S_idxs.reserve(OBS_SIZE*N_upd);
00840 
00841                                                                         for (size_t i=0;i<data_association.size();++i)
00842                                                                         {
00843                                                                                 if (data_association[i]<0) continue;
00844 
00845                                                                                 const size_t assoc_idx_in_map = static_cast<size_t>(data_association[i]);
00846                                                                                 const size_t assoc_idx_in_pred = mrpt::utils::find_in_vector(assoc_idx_in_map, predictLMidxs);
00847                                                                                 ASSERTMSG_(assoc_idx_in_pred!=string::npos, "OnPreComputingPredictions() didn't recommend the prediction of a landmark which has been actually observed!")
00848                                                                                 // TODO: In these cases, extend the prediction right now instead of launching an exception... or is this a bad idea??
00849 
00850                                                                                 // Build the subset dh_dx_full_obs:
00851                                                                                 for (size_t k=0;k<OBS_SIZE;k++)
00852                                                                                 {  // S_idxs.size() is used as counter for "dh_dx_full_obs".
00853 											::memcpy(
00854                                                                                                 dh_dx_full_obs.get_unsafe_row(S_idxs.size()),
00855                                                                                                 dh_dx_full.get_unsafe_row(assoc_idx_in_pred*OBS_SIZE + k ),
00856                                                                                                 sizeof(dh_dx_full(0,0))*(VEH_SIZE + FEAT_SIZE * N_map) );
00857                                                                                         S_idxs.push_back(assoc_idx_in_pred*OBS_SIZE+k);
00858                                                                                 }
00859 
00860                                                                                 // ytilde_i = Z[i] - all_predictions[i]
00861                                                                                 KFArray_OBS ytilde_i = Z[i];
00862                                                                                 OnSubstractObservationVectors(ytilde_i,all_predictions[predictLMidxs[assoc_idx_in_pred]]);
00863                                                                                 for (size_t k=0;k<OBS_SIZE;k++)
00864                                                                                         ytilde[ytilde_idx++] = ytilde_i[k];
00865                                                                         }
00866                                                                         // Extract the subset that is involved in this observation:
00867                                                                         S.extractSubmatrixSymmetrical(S_idxs,S_observed);
00868                                                                 }
00869                                                                 else
00870                                                                 {       // Non-SLAM problems:
00871                                                                         ASSERT_(Z.size()==1 && all_predictions.size()==1)
00872 
00873                                                                         dh_dx_full_obs = dh_dx_full;
00874                                                                         KFArray_OBS ytilde_i = Z[0];
00875                                                                         OnSubstractObservationVectors(ytilde_i,all_predictions[0]);
00876                                                                         for (size_t k=0;k<OBS_SIZE;k++)
00877                                                                                 ytilde[ytilde_idx++] = ytilde_i[k];
00878                                                                         // Extract the subset that is involved in this observation:
00879                                                                         S_observed = S;
00880                                                                 }
00881 
00882                                                                 // Compute the full K matrix:
00883                                                                 // ------------------------------
00884                                                                 m_timLogger.enter("KF:8.update stage:1.FULLKF:build K");
00885 
00886                                                                 K.setSize(m_pkk.getRowCount(), S_observed.getColCount() );
00887 
00888                                                                 // K = m_pkk * (~dh_dx) * S.inv() );
00889                                                                 K.multiply_ABt(m_pkk, dh_dx_full_obs);
00890 
00891                                                                 //KFMatrix S_1( S_observed.getRowCount(), S_observed.getColCount() );
00892                                                                 S_observed.inv(S_1); // Do NOT call inv_fast since it destroys S
00893                                                                 K*=S_1;
00894 
00895                                                                 m_timLogger.leave("KF:8.update stage:1.FULLKF:build K");
00896 
00897                                                                 // Use the full K matrix to update the mean:
00898                                                                 if (nKF_iterations==1)
00899                                                                 {
00900                                                                         m_timLogger.enter("KF:8.update stage:2.FULLKF:update xkk");
00901                                                                         m_xkk += K * ytilde;
00902                                                                         m_timLogger.leave("KF:8.update stage:2.FULLKF:update xkk");
00903                                                                 }
00904                                                                 else
00905                                                                 {
00906                                                                         m_timLogger.enter("KF:8.update stage:2.FULLKF:iter.update xkk");
00907 
00908                                                                         KFVector  HAx_column;
00909                                                                         dh_dx_full_obs.multiply_Ab( m_xkk - xkk_0, HAx_column);
00910 
00911                                                                         m_xkk = xkk_0;
00912                                                                         K.multiply_Ab(
00913                                                                                 (ytilde-HAx_column),
00914                                                                                 m_xkk,
00915                                                                                 true /* Accumulate in output */
00916                                                                                 );
00917 
00918                                                                         m_timLogger.leave("KF:8.update stage:2.FULLKF:iter.update xkk");
00919                                                                 }
00920 
00921                                                                 // Update the covariance just at the end
00922                                                                 //  of iterations if we are in IKF, always in normal EKF.
00923                                                                 if (IKF_iteration == (nKF_iterations-1) )
00924                                                                 {
00925                                                                         m_timLogger.enter("KF:8.update stage:3.FULLKF:update Pkk");
00926 
00927                                                                         // Use the full K matrix to update the covariance:
00928                                                                         //m_pkk = (I - K*dh_dx ) * m_pkk;
00929                                                                         // TODO: "Optimize this: sparsity!"
00930 
00931                                                                         // K * dh_dx_full
00932                                                                         aux_K_dh_dx.multiply(K,dh_dx_full_obs);
00933 
00934                                                                         // aux_K_dh_dx  <-- I-aux_K_dh_dx
00935                                                                         const size_t stat_len = aux_K_dh_dx.getColCount();
00936                                                                         for (size_t r=0;r<stat_len;r++)
00937                                                                                 for (size_t c=0;c<stat_len;c++)
00938                                                                                         if (r==c)
00939                                                                                              aux_K_dh_dx.get_unsafe(r,c)=-aux_K_dh_dx.get_unsafe(r,c) + kftype(1);
00940                                                                                         else aux_K_dh_dx.get_unsafe(r,c)=-aux_K_dh_dx.get_unsafe(r,c);
00941 
00942                                                                         m_pkk.multiply_result_is_symmetric(aux_K_dh_dx, m_pkk );
00943 
00944                                                                         m_timLogger.leave("KF:8.update stage:3.FULLKF:update Pkk");
00945                                                                 }
00946                                                         } // end for each IKF iteration
00947                                                 }
00948                                         }
00949                                         break;
00950 
00951                                         // --------------------------------------------------------------------
00952                                         // - EKF 'a la' Davison: One observation element at once
00953                                         // --------------------------------------------------------------------
00954                                         case kfEKFAlaDavison:
00955                                         {
00956                                                 // For each observed landmark/whole system state:
00957                                                 for (size_t obsIdx=0;obsIdx<Z.size();obsIdx++)
00958                                                 {
00959                                                         // Known & mapped landmark?
00960                                                         bool   doit;
00961                                                         size_t idxInTheFilter=0;
00962 
00963                                                         if (data_association.empty())
00964                                                         {
00965                                                                 doit = true;
00966                                                         }
00967                                                         else
00968                                                         {
00969                                                                 doit = data_association[obsIdx] >= 0;
00970                                                                 if (doit)
00971                                                                         idxInTheFilter = data_association[obsIdx];
00972                                                         }
00973 
00974                                                         if ( doit )
00975                                                         {
00976                                                                 m_timLogger.enter("KF:8.update stage:1.ScalarAtOnce.prepare");
00977 
00978                                                                 // Already mapped: OK
00979                                                                 const size_t idx_off = VEH_SIZE + idxInTheFilter*FEAT_SIZE; // The offset in m_xkk & Pkk.
00980 
00981                                                                 // Compute just the part of the Jacobian that we need using the current updated m_xkk:
00982                                                                 vector_KFArray_OBS  pred_obs;
00983                                                                 OnObservationModel( vector_size_t(1,idxInTheFilter),pred_obs);
00984                                                                 ASSERTDEB_(pred_obs.size()==1);
00985 
00986                                                                 // ytilde = observation - prediction
00987                                                                 KFArray_OBS ytilde = Z[obsIdx];
00988                                                                 OnSubstractObservationVectors(ytilde, pred_obs[0]);
00989 
00990                                                                 // Jacobians:
00991                                                                 // dh_dx: already is (N_pred*OBS_SIZE) x (VEH_SIZE + FEAT_SIZE * N_pred )
00992                                                                 //         with N_pred = |predictLMidxs|
00993 
00994                                                                 KFMatrix_OxV Hx(UNINITIALIZED_MATRIX);
00995                                                                 KFMatrix_OxF Hy(UNINITIALIZED_MATRIX);
00996                                                                 const size_t i_idx_in_preds = mrpt::utils::find_in_vector(idxInTheFilter,predictLMidxs);
00997                                                                 ASSERTMSG_(i_idx_in_preds!=string::npos, "OnPreComputingPredictions() didn't recommend the prediction of a landmark which has been actually observed!")
00998                                                                 dh_dx.extractMatrix(i_idx_in_preds*OBS_SIZE,0, Hx);
00999                                                                 dh_dx.extractMatrix(i_idx_in_preds*OBS_SIZE,VEH_SIZE+i_idx_in_preds*OBS_SIZE, Hy);
01000 
01001 
01002                                                                 m_timLogger.leave("KF:8.update stage:1.ScalarAtOnce.prepare");
01003 
01004                                                                 // For each component of the observation:
01005                                                                 for (size_t j=0;j<OBS_SIZE;j++)
01006                                                                 {
01007                                                                         m_timLogger.enter("KF:8.update stage:2.ScalarAtOnce.update");
01008 
01009                                                                         // Compute the scalar S_i for each component j of the observation:
01010                                                                         // Sij = dhij_dxv Pxx dhij_dxv^t + 2 * dhij_dyi Pyix dhij_dxv + dhij_dyi Pyiyi dhij_dyi^t + R
01011                                                                         //          ^^
01012                                                                         //         Hx:(O*LxSv)
01013                                                                         //       \----------------------/ \--------------------------/  \------------------------/ \-/
01014                                                                         //               TERM 1                   TERM 2                        TERM 3              R
01015                                                                         //
01016                                                                         // O: Observation size (3)
01017                                                                         // L: # landmarks
01018                                                                         // Sv: Vehicle state size (6)
01019                                                                         //
01020 
01021                 #if defined(_DEBUG)
01022                                                                         {
01023                                                                                 // This algorithm is applicable only if the scalar component of the sensor noise are INDEPENDENT:
01024                                                                                 for (size_t a=0;a<OBS_SIZE;a++)
01025                                                                                         for (size_t b=0;b<OBS_SIZE;b++)
01026                                                                                                 if ( a!=b )
01027                                                                                                         if (R(a,b)!=0)
01028                                                                                                                 THROW_EXCEPTION("This KF algorithm assumes independent noise components in the observation (matrix R). Select another KF algorithm.")
01029                                                                         }
01030                 #endif
01031                                                                         // R:
01032                                                                         KFTYPE Sij = R.get_unsafe(j,j);
01033 
01034                                                                         // TERM 1:
01035                                                                         for (size_t k=0;k<VEH_SIZE;k++)
01036                                                                         {
01037                                                                                 KFTYPE accum = 0;
01038                                                                                 for (size_t q=0;q<VEH_SIZE;q++)
01039                                                                                         accum += Hx.get_unsafe(j,q) * m_pkk.get_unsafe(q,k);
01040                                                                                 Sij+= Hx.get_unsafe(j,k) * accum;
01041                                                                         }
01042 
01043                                                                         // TERM 2:
01044                                                                         KFTYPE term2=0;
01045                                                                         for (size_t k=0;k<VEH_SIZE;k++)
01046                                                                         {
01047                                                                                 KFTYPE accum = 0;
01048                                                                                 for (size_t q=0;q<FEAT_SIZE;q++)
01049                                                                                         accum += Hy.get_unsafe(j,q) * m_pkk.get_unsafe(idx_off+q,k);
01050                                                                                 term2+= Hx.get_unsafe(j,k) * accum;
01051                                                                         }
01052                                                                         Sij += 2 * term2;
01053 
01054                                                                         // TERM 3:
01055                                                                         for (size_t k=0;k<FEAT_SIZE;k++)
01056                                                                         {
01057                                                                                 KFTYPE accum = 0;
01058                                                                                 for (size_t q=0;q<FEAT_SIZE;q++)
01059                                                                                         accum += Hy.get_unsafe(j,q) * m_pkk.get_unsafe(idx_off+q,idx_off+k);
01060                                                                                 Sij+= Hy.get_unsafe(j,k) * accum;
01061                                                                         }
01062 
01063                                                                         // Compute the Kalman gain "Kij" for this observation element:
01064                                                                         // -->  K = m_pkk * (~dh_dx) * S.inv() );
01065                                                                         size_t N = m_pkk.getColCount();
01066                                                                         vector<KFTYPE> Kij( N );
01067 
01068                                                                         for (size_t k=0;k<N;k++)
01069                                                                         {
01070                                                                                 KFTYPE K_tmp = 0;
01071 
01072                                                                                 // dhi_dxv term
01073                                                                                 size_t q;
01074                                                                                 for (q=0;q<VEH_SIZE;q++)
01075                                                                                         K_tmp+= m_pkk.get_unsafe(k,q) * Hx.get_unsafe(j,q);
01076 
01077                                                                                 // dhi_dyi term
01078                                                                                 for (q=0;q<FEAT_SIZE;q++)
01079                                                                                         K_tmp+= m_pkk.get_unsafe(k,idx_off+q) * Hy.get_unsafe(j,q);
01080 
01081                                                                                 Kij[k] = K_tmp / Sij;
01082                                                                         } // end for k
01083 
01084 
01085                                                                         // Update the state vector m_xkk:
01086                                                                         //  x' = x + Kij * ytilde(ij)
01087                                                                         for (size_t k=0;k<N;k++)
01088                                                                                 m_xkk[k] += Kij[k] * ytilde[j];
01089 
01090                                                                         // Update the covariance Pkk:
01091                                                                         // P' =  P - Kij * Sij * Kij^t
01092                                                                         {
01093                                                                                 for (size_t k=0;k<N;k++)
01094                                                                                 {
01095                                                                                         for (size_t q=k;q<N;q++)  // Half matrix
01096                                                                                         {
01097                                                                                                 m_pkk(k,q) -= Sij * Kij[k] * Kij[q];
01098                                                                                                 // It is symmetric
01099                                                                                                 m_pkk(q,k) = m_pkk(k,q);
01100                                                                                         }
01101 
01102                 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
01103                                                                                         if (m_pkk(k,k)<0)
01104                                                                                         {
01105                                                                                                 m_pkk.saveToTextFile("Pkk_err.txt");
01106                                                                                                 mrpt::system::vectorToTextFile(Kij,"Kij.txt");
01107                                                                                                 ASSERT_(m_pkk(k,k)>0)
01108                                                                                         }
01109                 #endif
01110                                                                                 }
01111                                                                         }
01112 
01113 
01114                                                                         m_timLogger.leave("KF:8.update stage:2.ScalarAtOnce.update");
01115                                                                 } // end for j
01116                                                         } // end if mapped
01117                                                 } // end for each observed LM.
01118                                         }
01119                                         break;
01120 
01121                                         // --------------------------------------------------------------------
01122                                         // - IKF method, processing each observation scalar secuentially:
01123                                         // --------------------------------------------------------------------
01124                                         case kfIKF:  // TODO !!
01125                                         {
01126                                                 THROW_EXCEPTION("IKF scalar by scalar not implemented yet.");
01127 #if 0
01128                                                 KFMatrix h,Hx,Hy;
01129 
01130                                                 // Just one, or several update iterations??
01131                                                 size_t nKF_iterations = KF_options.IKF_iterations;
01132 
01133                                                 // To avoid wasting time, if we are doing 1 iteration only, do not reserve memory for this matrix:
01134                                                 KFMatrix                *saved_Pkk=NULL;
01135                                                 if (nKF_iterations>1)
01136                                                 {
01137                                                         // Create a copy of Pkk for later restoring it at the beginning of each iteration:
01138                                                         saved_Pkk = new KFMatrix( m_pkk );
01139                                                 }
01140 
01141                                                 KFVector        xkk_0 = m_xkk; // First state vector at the beginning of IKF
01142                                                 KFVector        xkk_next_iter = m_xkk;  // for IKF only
01143 
01144                                                 // For each IKF iteration (or 1 for EKF)
01145                                                 for (size_t IKF_iteration=0;IKF_iteration<nKF_iterations;IKF_iteration++)
01146                                                 {
01147                                                         // Restore initial covariance matrix.
01148                                                         // The updated mean is stored in m_xkk and is improved with each estimation,
01149                                                         //  and so do the Jacobians which use that improving mean.
01150                                                         if (IKF_iteration>0)
01151                                                         {
01152                                                                 m_pkk = *saved_Pkk;
01153                                                                 xkk_next_iter = xkk_0;
01154                                                         }
01155 
01156                                                         // For each observed landmark/whole system state:
01157                                                         for (size_t obsIdx=0;obsIdx<Z.getRowCount();obsIdx++)
01158                                                         {
01159                                                                 // Known & mapped landmark?
01160                                                                 bool   doit;
01161                                                                 size_t idxInTheFilter=0;
01162 
01163                                                                 if (data_association.empty())
01164                                                                 {
01165                                                                         doit = true;
01166                                                                 }
01167                                                                 else
01168                                                                 {
01169                                                                         doit = data_association[obsIdx] >= 0;
01170                                                                         if (doit)
01171                                                                                 idxInTheFilter = data_association[obsIdx];
01172                                                                 }
01173 
01174                                                                 if ( doit )
01175                                                                 {
01176                                                                         // Already mapped: OK
01177                                                                         const size_t idx_off      = VEH_SIZE + idxInTheFilter *FEAT_SIZE; // The offset in m_xkk & Pkk.
01178                                                                         const size_t R_row_offset = obsIdx*OBS_SIZE;  // Offset row in R
01179 
01180                                                                         // Compute just the part of the Jacobian that we need using the current updated m_xkk:
01181                                                                         KFVector   ytilde; // Diff. observation - prediction
01182                                                                         OnObservationModelAndJacobians(
01183                                                                                 Z,
01184                                                                                 data_association,
01185                                                                                 false,                  // Only a partial Jacobian
01186                                                                                 (int)obsIdx,    // Which row from Z
01187                                                                                 ytilde,
01188                                                                                 Hx,
01189                                                                                 Hy );
01190 
01191                                                                         ASSERTDEB_(ytilde.size() == OBS_SIZE )
01192                                                                         ASSERTDEB_(Hx.getRowCount() == OBS_SIZE )
01193                                                                         ASSERTDEB_(Hx.getColCount() == VEH_SIZE )
01194 
01195                                                                         if (FEAT_SIZE>0)
01196                                                                         {
01197                                                                                 ASSERTDEB_(Hy.getRowCount() == OBS_SIZE )
01198                                                                                 ASSERTDEB_(Hy.getColCount() == FEAT_SIZE )
01199                                                                         }
01200 
01201                                                                         // Compute the OxO matrix S_i for each observation:
01202                                                                         // Si  = TERM1 + TERM2 + TERM2^t + TERM3 + R
01203                                                                         //
01204                                                                         // TERM1: dhi_dxv Pxx dhi_dxv^t
01205                                                                         //          ^^
01206                                                                         //         Hx:(OxV)
01207                                                                         //
01208                                                                         // TERM2: dhi_dyi Pyix dhi_dxv
01209                                                                         //          ^^
01210                                                                         //         Hy:(OxF)
01211                                                                         //
01212                                                                         // TERM3: dhi_dyi Pyiyi dhi_dyi^t
01213                                                                         //
01214                                                                         // i: obsIdx
01215                                                                         // O: Observation size
01216                                                                         // F: Feature size
01217                                                                         // V: Vehicle state size
01218                                                                         //
01219 
01220                                                                         // R:
01221                                                                         KFMatrix Si(OBS_SIZE,OBS_SIZE);
01222                                                                         R.extractMatrix(R_row_offset,0, Si);
01223 
01224                                                                         size_t k;
01225                                                                         KFMatrix        term(OBS_SIZE,OBS_SIZE);
01226 
01227                                                                         // TERM1: dhi_dxv Pxx dhi_dxv^t
01228                                                                         Hx.multiply_HCHt(
01229                                                                                 m_pkk,          // The cov. matrix
01230                                                                                 Si,                     // The output
01231                                                                                 true,           // Yes, submatrix of m_pkk only
01232                                                                                 0,                      // Offset in m_pkk
01233                                                                                 true            // Yes, accum results in output.
01234                                                                         );
01235 
01236                                                                         // TERM2: dhi_dyi Pyix dhi_dxv
01237                                                                         //  + its transpose:
01238                                                                         KFMatrix        Pyix( FEAT_SIZE, VEH_SIZE );
01239                                                                         m_pkk.extractMatrix(idx_off,0, Pyix);  // Extract cross cov. to Pyix
01240 
01241                                                                         term.multiply_ABCt( Hy, Pyix, Hx ); //term = Hy * Pyix * ~Hx;
01242                                                                         Si.add_AAt( term );  // Si += A + ~A
01243 
01244                                                                         // TERM3: dhi_dyi Pyiyi dhi_dyi^t
01245                                                                         Hy.multiply_HCHt(
01246                                                                                 m_pkk,          // The cov. matrix
01247                                                                                 Si,                     // The output
01248                                                                                 true,           // Yes, submatrix of m_pkk only
01249                                                                                 idx_off,    // Offset in m_pkk
01250                                                                                 true            // Yes, accum results in output.
01251                                                                         );
01252 
01253                                                                         // Compute the inverse of Si:
01254                                                                         KFMatrix Si_1(OBS_SIZE,OBS_SIZE);
01255 
01256                                                                         // Compute the Kalman gain "Ki" for this i'th observation:
01257                                                                         // -->  Ki = m_pkk * (~dh_dx) * S.inv();
01258                                                                         size_t N = m_pkk.getColCount();
01259 
01260                                                                         KFMatrix Ki( N, OBS_SIZE );
01261 
01262                                                                         for (k=0;k<N;k++)  // for each row of K
01263                                                                         {
01264                                                                                 size_t q;
01265 
01266                                                                                 for (size_t c=0;c<OBS_SIZE;c++)  // for each column of K
01267                                                                                 {
01268                                                                                         KFTYPE  K_tmp = 0;
01269 
01270                                                                                         // dhi_dxv term
01271                                                                                         for (q=0;q<VEH_SIZE;q++)
01272                                                                                                 K_tmp+= m_pkk.get_unsafe(k,q) * Hx.get_unsafe(c,q);
01273 
01274                                                                                         // dhi_dyi term
01275                                                                                         for (q=0;q<FEAT_SIZE;q++)
01276                                                                                                 K_tmp+= m_pkk.get_unsafe(k,idx_off+q) * Hy.get_unsafe(c,q);
01277 
01278                                                                                         Ki.set_unsafe(k,c, K_tmp);
01279                                                                                 } // end for c
01280                                                                         } // end for k
01281 
01282                                                                         Ki.multiply(Ki, Si.inv() );  // K = (...) * S^-1
01283 
01284 
01285                                                                         // Update the state vector m_xkk:
01286                                                                         if (nKF_iterations==1)
01287                                                                         {
01288                                                                                 // EKF:
01289                                                                                 //  x' = x + Kij * ytilde(ij)
01290                                                                                 for (k=0;k<N;k++)
01291                                                                                         for (size_t q=0;q<OBS_SIZE;q++)
01292                                                                                                 m_xkk[k] += Ki.get_unsafe(k,q) * ytilde[q];
01293                                                                         }
01294                                                                         else
01295                                                                         {
01296                                                                                 // IKF:
01297                                                                                 mrpt::dynamicsize_vector<KFTYPE> HAx(OBS_SIZE);
01298                                                                                 size_t o,q;
01299                                                                                 // HAx = H*(x0-xi)
01300                                                                                 for (o=0;o<OBS_SIZE;o++)
01301                                                                                 {
01302                                                                                         KFTYPE  tmp = 0;
01303                                                                                         for (q=0;q<VEH_SIZE;q++)
01304                                                                                                 tmp += Hx.get_unsafe(o,q) * (xkk_0[q] - m_xkk[q]);
01305 
01306                                                                                         for (q=0;q<FEAT_SIZE;q++)
01307                                                                                                 tmp += Hy.get_unsafe(o,q) * (xkk_0[idx_off+q] - m_xkk[idx_off+q]);
01308 
01309                                                                                         HAx[o] = tmp;
01310                                                                                 }
01311 
01312                                                                                 // Compute only once (ytilde[j] - HAx)
01313                                                                                 for (o=0;o<OBS_SIZE;o++)
01314                                                                                         HAx[o] = ytilde[o] - HAx[o];
01315 
01316                                                                                 //  x' = x_0 + Kij * ( ytilde(ij) - H*(x0-xi))   -> xi: i=this iteration
01317                                                                                 //  xkk_next_iter is initialized to xkk_0 at each IKF iteration.
01318                                                                                 for (k=0;k<N;k++)
01319                                                                                 {
01320                                                                                         KFTYPE   tmp = xkk_next_iter[k];
01321                                                                                         //m_xkk[k] = xkk_0[k] + Kij[k] * (ytilde[j] - HAx );
01322                                                                                         for (o=0;o<OBS_SIZE;o++)
01323                                                                                                 tmp += Ki.get_unsafe(k,o) * HAx[o];
01324 
01325                                                                                         xkk_next_iter[k] = tmp;
01326                                                                                 }
01327                                                                         }
01328 
01329                                                                         // Update the covariance Pkk:
01330                                                                         // P' =  P - Kij * Sij * Kij^t
01331                                                                         //if (IKF_iteration==(nKF_iterations-1))   // Just for the last IKF iteration
01332                                                                         {
01333                                                                                 // m_pkk -= Ki * Si * ~Ki;
01334                                                                                 Ki.multiplyByMatrixAndByTransposeNonSymmetric(
01335                                                                                         Si,
01336                                                                                         m_pkk,   // Output
01337                                                                                         true,    // Accumulate
01338                                                                                         true);   // Substract instead of sum.
01339 
01340                                                                                 m_pkk.force_symmetry();
01341 
01342                         /*                                                      for (k=0;k<N;k++)
01343                                                                                 {
01344                                                                                         for (size_t q=k;q<N;q++)  // Half matrix
01345                                                                                         {
01346                                                                                                 m_pkk(k,q) -= Sij * Kij[k] * Kij[q];
01347                                                                                                 // It is symmetric
01348                                                                                                 m_pkk(q,k) = m_pkk(k,q);
01349                                                                                         }
01350 
01351                         #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
01352                                                                                         if (m_pkk(k,k)<0)
01353                                                                                         {
01354                                                                                                 m_pkk.saveToTextFile("Pkk_err.txt");
01355                                                                                                 mrpt::system::vectorToTextFile(Kij,"Kij.txt");
01356                                                                                                 ASSERT_(m_pkk(k,k)>0)
01357                                                                                         }
01358                         #endif
01359                                                                                 }
01360                                                                                 */
01361                                                                         }
01362 
01363                                                                 } // end if doit
01364 
01365                                                         } // end for each observed LM.
01366 
01367                                                         // Now, save the new iterated mean:
01368                                                         if (nKF_iterations>1)
01369                                                         {
01370                         #if 0
01371                                                                 cout << "IKF iter: " << IKF_iteration << " -> " << xkk_next_iter << endl;
01372                         #endif
01373                                                                 m_xkk = xkk_next_iter;
01374                                                         }
01375 
01376                                                 } // end for each IKF_iteration
01377 
01378                                                 // Copy of pkk not required anymore:
01379                                                 if (saved_Pkk) delete saved_Pkk;
01380 
01381 #endif
01382                                                 }
01383                                                 break;
01384 
01385                                                 default:
01386                                                         THROW_EXCEPTION("Invalid value of options.KF_method");
01387                                                 } // end switch method
01388 
01389                                         }
01390 
01391                                         const double tim_update = m_timLogger.leave("KF:8.update stage");
01392 
01393                                         OnNormalizeStateVector();
01394 
01395                                         // =============================================================
01396                                         //  8. INTRODUCE NEW LANDMARKS
01397                                         // =============================================================
01398                                         if (!data_association.empty())
01399                                         {
01400                                                 detail::CRunOneKalmanIteration_addNewLandmarks()(*this, Z, data_association,R);
01401                                         } // end if data_association!=empty
01402 
01403                                         if (KF_options.verbose)
01404                                         {
01405                                                 printf_debug("[KF] %u LMs | Pr: %.2fms | Pr.Obs: %.2fms | Obs.DA: %.2fms | Upd: %.2fms\n",
01406                                                         static_cast<unsigned int>(getNumberOfLandmarksInTheMap()),
01407                                                         1e3*tim_pred,
01408                                                         1e3*tim_pred_obs,
01409                                                         1e3*tim_obs_DA,
01410                                                         1e3*tim_update
01411                                                         );
01412                                         }
01413 
01414                                         // Post iteration user code:
01415                                         OnPostIteration();
01416 
01417                                         m_timLogger.leave("KF:complete_step");
01418 
01419                                         MRPT_END
01420 
01421                                 } // End of "runOneKalmanIteration"
01422 
01423 
01424 
01425                 private:
01426                         mutable bool m_user_didnt_implement_jacobian;
01427 
01428                         /** Auxiliary functions for Jacobian numeric estimation */
01429                         static void KF_aux_estimate_trans_jacobian(
01430                                 const KFArray_VEH &x,
01431                                 const std::pair<KFCLASS*,KFArray_ACT> &dat,
01432                                 KFArray_VEH &out_x)
01433                         {
01434                                 bool dumm;
01435                                 out_x=x;
01436                                 dat.first->OnTransitionModel(dat.second,out_x, dumm);
01437                         }
01438                         static void KF_aux_estimate_obs_Hx_jacobian(
01439                                 const KFArray_VEH &x,
01440                                 const std::pair<KFCLASS*,size_t> &dat,
01441                                 KFArray_OBS &out_x)
01442                         {
01443                                 vector_size_t  idxs_to_predict(1,dat.second);
01444                                 vector_KFArray_OBS prediction;
01445                                 // Overwrite (temporarily!) the affected part of the state vector:
01446 				::memcpy(&dat.first->m_xkk[0],&x[0],sizeof(x[0])*VEH_SIZE);
01447                                 dat.first->OnObservationModel(idxs_to_predict,prediction);
01448                                 ASSERTDEB_(prediction.size()==1);
01449                                 out_x=prediction[0];
01450                         }
01451                         static void KF_aux_estimate_obs_Hy_jacobian(
01452                                 const KFArray_FEAT &x,
01453                                 const std::pair<KFCLASS*,size_t> &dat,
01454                                 KFArray_OBS &out_x)
01455                         {
01456                                 vector_size_t  idxs_to_predict(1,dat.second);
01457                                 vector_KFArray_OBS  prediction;
01458                                 const size_t lm_idx_in_statevector = VEH_SIZE+FEAT_SIZE*dat.second;
01459                                 // Overwrite (temporarily!) the affected part of the state vector:
01460 				::memcpy(&dat.first->m_xkk[lm_idx_in_statevector],&x[0],sizeof(x[0])*FEAT_SIZE);
01461                                 dat.first->OnObservationModel(idxs_to_predict,prediction);
01462                                 ASSERTDEB_(prediction.size()==1);
01463                                 out_x=prediction[0];
01464                         }
01465 
01466 
01467                         friend struct detail::CRunOneKalmanIteration_addNewLandmarks;
01468 
01469                 }; // end class
01470 
01471                 namespace detail
01472                 {
01473                         // generic version for SLAM. There is a speciation below for NON-SLAM problems.                 
01474                         struct CRunOneKalmanIteration_addNewLandmarks {
01475                                 template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE>
01476                                 void operator()(
01477                                         CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,FEAT_SIZE,ACT_SIZE,KFTYPE> &obj,
01478                                         const typename CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,FEAT_SIZE,ACT_SIZE,KFTYPE>::vector_KFArray_OBS & Z,
01479                                         const vector_int       &data_association,
01480                                         const typename CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,FEAT_SIZE,ACT_SIZE,KFTYPE>::KFMatrix_OxO          &R
01481                                         )
01482                                 {
01483                                         typedef CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,FEAT_SIZE,ACT_SIZE,KFTYPE> KF;
01484 
01485                                         for (size_t idxObs=0;idxObs<Z.size();idxObs++)
01486                                         {
01487                                                 // Is already in the map?
01488                                                 if ( data_association[idxObs] < 0 )  // Not in the map yet!
01489                                                 {
01490                                                         obj.m_timLogger.enter("KF:9.create new LMs");
01491                                                         // Add it:
01492 
01493                                                         // Append to map of IDs <-> position in the state vector:
01494                                                         ASSERTDEB_(FEAT_SIZE>0)
01495                                                         ASSERTDEB_( 0 == ((obj.m_xkk.size() - VEH_SIZE) % FEAT_SIZE) ) // Sanity test
01496 
01497                                                         const size_t  newIndexInMap = (obj.m_xkk.size() - VEH_SIZE) / FEAT_SIZE;
01498 
01499                                                         // Inverse sensor model:
01500                                                         typename KF::KFArray_FEAT yn;
01501                                                         typename KF::KFMatrix_FxV dyn_dxv;
01502                                                         typename KF::KFMatrix_FxO dyn_dhn;
01503                                                         typename KF::KFMatrix_FxF dyn_dhn_R_dyn_dhnT;
01504                                                         bool use_dyn_dhn_jacobian=true;
01505 
01506                                                         // Compute the inv. sensor model and its Jacobians:
01507                                                         obj.OnInverseObservationModel(
01508                                                                 Z[idxObs],
01509                                                                 yn,
01510                                                                 dyn_dxv,
01511                                                                 dyn_dhn,
01512                                                                 dyn_dhn_R_dyn_dhnT,
01513                                                                 use_dyn_dhn_jacobian );
01514 
01515                                                         // And let the application do any special handling of adding a new feature to the map:
01516                                                         obj.OnNewLandmarkAddedToMap(
01517                                                                 idxObs,
01518                                                                 newIndexInMap
01519                                                                 );
01520 
01521                                                         ASSERTDEB_( yn.size() == FEAT_SIZE )
01522 
01523                                                         // Append to m_xkk:
01524                                                         size_t q;
01525                                                         size_t idx = obj.m_xkk.size();
01526                                                         obj.m_xkk.resize( obj.m_xkk.size() + FEAT_SIZE );
01527 
01528                                                         for (q=0;q<FEAT_SIZE;q++)
01529                                                                 obj.m_xkk[idx+q] = yn[q];
01530 
01531                                                         // --------------------
01532                                                         // Append to Pkk:
01533                                                         // --------------------
01534                                                         ASSERTDEB_( obj.m_pkk.getColCount()==idx && obj.m_pkk.getRowCount()==idx );
01535 
01536                                                         obj.m_pkk.setSize( idx+FEAT_SIZE,idx+FEAT_SIZE );
01537 
01538                                                         // Fill the Pxyn term:
01539                                                         // --------------------
01540                                                         typename KF::KFMatrix_VxV Pxx;
01541                                                         obj.m_pkk.extractMatrix(0,0,Pxx);
01542                                                         typename KF::KFMatrix_FxV Pxyn; // Pxyn = dyn_dxv * Pxx
01543                                                         Pxyn.multiply( dyn_dxv, Pxx );
01544 
01545                                                         obj.m_pkk.insertMatrix( idx,0, Pxyn );
01546                                                         obj.m_pkk.insertMatrixTranspose( 0,idx, Pxyn );
01547 
01548                                                         // Fill the Pyiyn terms:
01549                                                         // --------------------
01550                                                         const size_t nLMs = (idx-VEH_SIZE)/FEAT_SIZE; // Number of previous landmarks:
01551                                                         for (q=0;q<nLMs;q++)
01552                                                         {
01553                                                                 typename KF::KFMatrix_VxF  P_x_yq(UNINITIALIZED_MATRIX);
01554                                                                 obj.m_pkk.extractMatrix(0,VEH_SIZE+q*FEAT_SIZE,P_x_yq) ;
01555 
01556                                                                 typename KF::KFMatrix_FxF P_cross(UNINITIALIZED_MATRIX);
01557                                                                 P_cross.multiply(dyn_dxv, P_x_yq );
01558 
01559                                                                 obj.m_pkk.insertMatrix(idx,VEH_SIZE+q*FEAT_SIZE, P_cross );
01560                                                                 obj.m_pkk.insertMatrixTranspose(VEH_SIZE+q*FEAT_SIZE,idx, P_cross );
01561                                                         } // end each previous LM(q)
01562 
01563                                                         // Fill the Pynyn term:
01564                                                         //  P_yn_yn =  (dyn_dxv * Pxx * ~dyn_dxv) + (dyn_dhn * R * ~dyn_dhn);
01565                                                         // --------------------
01566                                                         typename KF::KFMatrix_FxF P_yn_yn(UNINITIALIZED_MATRIX);
01567                                                         dyn_dxv.multiply_HCHt(Pxx,  P_yn_yn);
01568                                                         if (use_dyn_dhn_jacobian)
01569                                                                         dyn_dhn.multiply_HCHt(R, P_yn_yn, true); // Accumulate in P_yn_yn
01570                                                         else    P_yn_yn+=dyn_dhn_R_dyn_dhnT;
01571 
01572                                                         obj.m_pkk.insertMatrix(idx,idx, P_yn_yn );
01573 
01574                                                         obj.m_timLogger.leave("KF:9.create new LMs");
01575                                                 }
01576                                         }
01577                                 }
01578 
01579                                 template <size_t VEH_SIZE, size_t OBS_SIZE, size_t ACT_SIZE, typename KFTYPE>
01580                                 void operator()(
01581                                         CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,0 /* FEAT_SIZE=0 */,ACT_SIZE,KFTYPE> &obj,
01582                                         const typename CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,0 /* FEAT_SIZE=0 */,ACT_SIZE,KFTYPE>::vector_KFArray_OBS & Z,
01583                                         const vector_int       &data_association,
01584                                         const typename CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,0 /* FEAT_SIZE=0 */,ACT_SIZE,KFTYPE>::KFMatrix_OxO                &R
01585                                 )
01586                                 {
01587                                         // Do nothing: this is NOT a SLAM problem.
01588                                 }
01589 
01590                         }; // end runOneKalmanIteration_addNewLandmarks
01591 
01592                         template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE>
01593                         inline size_t getNumberOfLandmarksInMap(const CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,FEAT_SIZE,ACT_SIZE,KFTYPE> &obj)
01594                         {
01595                                 return (obj.getStateVectorLength()-VEH_SIZE)/FEAT_SIZE;
01596                         }
01597                         // Specialization for FEAT_SIZE=0
01598                         template <size_t VEH_SIZE, size_t OBS_SIZE, size_t ACT_SIZE, typename KFTYPE>
01599                         inline size_t getNumberOfLandmarksInMap(const CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,0 /*FEAT_SIZE*/,ACT_SIZE,KFTYPE> &obj)
01600                         {
01601                                 return 0;
01602                         }
01603 
01604                         template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE>
01605                         inline bool isMapEmpty(const CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,FEAT_SIZE,ACT_SIZE,KFTYPE> &obj)
01606                         {
01607                                 return (obj.getStateVectorLength()==VEH_SIZE);
01608                         }
01609                         // Specialization for FEAT_SIZE=0
01610                         template <size_t VEH_SIZE, size_t OBS_SIZE, size_t ACT_SIZE, typename KFTYPE>
01611                         inline bool isMapEmpty (const CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,0 /*FEAT_SIZE*/,ACT_SIZE,KFTYPE> &obj)
01612                         {
01613                                 return true;
01614                         }
01615 
01616                 } // end namespace "detail"
01617 
01618         } // end namespace
01619 } // end namespace
01620 
01621 #endif



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:16:28 UTC 2011