00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://mrpt.sourceforge.net/ | 00005 | | 00006 | Copyright (C) 2005-2009 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/CMatrixTemplateNumeric.h> 00032 #include <mrpt/math/CVectorTemplate.h> 00033 #include <mrpt/utils/CLoadableOptions.h> 00034 00035 namespace mrpt 00036 { 00037 namespace bayes 00038 { 00039 using namespace mrpt::utils; 00040 using namespace mrpt::math; 00041 00042 /** The Kalman Filter algorithm to employ in bayes::CKalmanFilterCapable 00043 * For further details on each algorithm see the tutorial: http://babel.isa.uma.es/mrpt/index.php/Kalman_Filters 00044 * \sa bayes::CKalmanFilterCapable::KF_options 00045 */ 00046 enum TKFMethod { 00047 kfEKFNaive = 0, 00048 kfEKFAlaDavison, 00049 kfIKFFull, 00050 kfIKF 00051 }; 00052 00053 /** The type of all vectors and matrices in CKalmanFilterCapable 00054 */ 00055 typedef double KFTYPE; 00056 00057 /** Virtual base for Kalman Filter (EKF,IEKF,UKF) implementations. 00058 * This base class stores the state vector and covariance matrix of the system. It has virtual methods that must be completed 00059 * by derived classes to address a given filtering problem. The main entry point of the algorithm is CKalmanFilterCapable::runOneKalmanIteration, which 00060 * should be called AFTER setting the desired filter options in KF_options, as well as any options in the derived class. 00061 * 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. 00062 * 00063 * For further details and examples, check out the tutorial: http://babel.isa.uma.es/mrpt/index.php/Kalman_Filters 00064 * 00065 * \note Since MRPT 0.5.5 there are two methods "OnGetObservations": one returning the sensor noise covariance matrix as a whole matrix, and the other as the diagonal vector. The derived class must implement just one of them. 00066 * 00067 * Revisions: 00068 * - 2007: Antonio J. Ortiz de Galisteo (AJOGD) 00069 * - 2008/FEB: All KF classes corrected, reorganized, and rewritten (JLBC). 00070 * - 2008/MAR: Implemented IKF (JLBC). 00071 * 00072 * \sa mrpt::slam::CRangeBearingKFSLAM 00073 */ 00074 class MRPTDLLIMPEXP CKalmanFilterCapable 00075 { 00076 protected: 00077 /** The system state vector. 00078 */ 00079 math::CVectorTemplate<KFTYPE> m_xkk; 00080 00081 /** The system full covariance matrix. 00082 */ 00083 math::CMatrixTemplateNumeric<KFTYPE> m_pkk; 00084 00085 /** The kalman innovation matrix using in some applications (i.e. MonoSLAM). 00086 */ 00087 math::CMatrixTemplateNumeric<KFTYPE> m_S; 00088 00089 00090 /** The main entry point, executes one complete step: prediction + update. 00091 * It is protected since derived classes must provide a problem-specific entry point for users. 00092 * The exact order in which this method calls the virtual method is explained in http://babel.isa.uma.es/mrpt/index.php/Kalman_Filters. 00093 */ 00094 void runOneKalmanIteration(); 00095 00096 /** @name Virtual methods for Kalman Filter implementation 00097 @{ 00098 */ 00099 00100 /** Must return the action vector u. 00101 * \param out_u The action vector which will be passed to OnTransitionModel 00102 */ 00103 virtual void OnGetAction( CVectorTemplate<KFTYPE> &out_u ) = 0; 00104 00105 /** Implements the transition model \f$ \hat{x}_{k|k-1} = f( \hat{x}_{k-1|k-1}, u_k ) \f$ 00106 * \param in_u The vector returned by OnGetAction. 00107 * \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$ . 00108 * \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 00109 */ 00110 virtual void OnTransitionModel( 00111 const CVectorTemplate<KFTYPE>& in_u, 00112 CVectorTemplate<KFTYPE>& inout_x, 00113 bool &out_skipPrediction 00114 ) = 0; 00115 00116 /** Implements the transition Jacobian \f$ \frac{\partial f}{\partial x} \f$ 00117 * \param out_F Must return the Jacobian. 00118 * The returned matrix must be \f$N \times N\f$ with N being either the size of the whole state vector or get_vehicle_size(). 00119 */ 00120 virtual void OnTransitionJacobian( 00121 CMatrixTemplateNumeric<KFTYPE> & out_F 00122 ) = 0; 00123 00124 /** Implements the transition noise covariance \f$ Q_k \f$ 00125 * \param out_Q Must return the covariance matrix. 00126 * The returned matrix must be of the same size than the jacobian from OnTransitionJacobian 00127 */ 00128 virtual void OnTransitionNoise( 00129 CMatrixTemplateNumeric<KFTYPE> & out_Q 00130 ) = 0; 00131 00132 /** This is called between the KF prediction step and the update step to allow the application to employ the prior distribution of the system state in the detection of observations (data association), where applicable. 00133 * \param out_z A \f$N \times O\f$ matrix, with O being get_observation_size() and N the number of "observations": how many observed landmarks for a map, or just one if not applicable. 00134 * \param out_R2 A vector of size O (O being get_observation_size()) with the *variances* of the sensor noise for each of the observation components. This is constant for all the observations (where N>1), and in the naive Kalman method the value TKF_options::veryLargeR2 is used for unobserved map elements. 00135 * \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, or -1 if it is a new map element and we want to insert it at the end of this KF iteration. 00136 * This method will be called just once for each complete KF iteration. 00137 * \note Since MRPT 0.5.5, the method "OnGetObservations" returning R as a matrix is called instead from the Kalman Filter engine. However, for compatibility, a default virtual method calls this method and build the R matrix from the diagonal R2. 00138 */ 00139 virtual void OnGetObservations( 00140 CMatrixTemplateNumeric<KFTYPE> &out_z, 00141 CVectorTemplate<KFTYPE> &out_R2, 00142 vector_int &out_data_association 00143 ) 00144 { 00145 MRPT_TRY_START 00146 THROW_EXCEPTION("One version of the OnGetObservations method must be implemented in the derived class.") 00147 MRPT_TRY_END 00148 } 00149 00150 /** This is called between the KF prediction step and the update step to allow the application to employ the prior distribution of the system state in the detection of observations (data association), where applicable. 00151 * \param out_z A \f$N \times O\f$ matrix, with O being get_observation_size() and N the number of "observations": how many observed landmarks for a map, or just one if not applicable. 00152 * \param out_R A matrix of size N·OxO (O being get_observation_size() and N the number of observations). It is the covariance matrix of the sensor noise for each of the returned observations. The order must correspond to that in out_z. 00153 * \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, or -1 if it is a new map element and we want to insert it at the end of this KF iteration. 00154 * This method will be called just once for each complete KF iteration. 00155 * \note Since MRPT 0.5.5, the method "OnGetObservations" returning R as a matrix is called instead from the Kalman Filter engine. However, for compatibility, a default virtual method calls this method and build the R matrix from the diagonal R2. 00156 */ 00157 virtual void OnGetObservations( 00158 CMatrixTemplateNumeric<KFTYPE> &out_z, 00159 CMatrixTemplateNumeric<KFTYPE> &out_R, 00160 vector_int &out_data_association 00161 ); 00162 00163 /** Implements the observation prediction \f$ h_i(x) \f$ and the Jacobians \f$ \frac{\partial h_i}{\partial x} \f$ and (when applicable) \f$ \frac{\partial h_i}{\partial y_i} \f$. 00164 * \param in_z This is the same matrix returned by OnGetObservations(), passed here for reference. 00165 * \param in_data_association The vector returned by OnGetObservations(), passed here for reference. 00166 * \param in_full If set to true, all the Jacobians and predictions must be computed at once. Otherwise, just those for the observation in_obsIdx. 00167 * \param in_obsIdx If in_full=false, the row of the observation (in in_z and in_data_association) whose innovation & Jacobians are to be returned now. 00168 * \param out_innov The difference between the expected observation and the real one: \f$ \tilde{y} = z - h(x) \f$. It must be a vector of length O*N (in_full=true) or O (in_full=false), with O=get_observation_size() and N=number of rows in in_z. 00169 * \param out_Hx One or a vertical stack of \f$ \frac{\partial h_i}{\partial x} \f$. 00170 * \param out_Hy An empty matrix, or one or a vertical stack of \f$ \frac{\partial h_i}{\partial y_i} \f$. 00171 * 00172 * out_Hx must consist of 1 (in_full=false) or N (in_full=true) vertically stacked \f$O \times V\f$ matrices, and out_Hy must consist of 1 or N \f$O \times F\f$ matrices, with: 00173 * - N: number of rows in in_z (number of observed features, or 1 in general). 00174 * - O: get_observation_size() 00175 * - V: get_vehicle_size() 00176 * - F: get_feature_size() 00177 */ 00178 virtual void OnObservationModelAndJacobians( 00179 const CMatrixTemplateNumeric<KFTYPE> &in_z, 00180 const vector_int &in_data_association, 00181 const bool &in_full, 00182 const int &in_obsIdx, 00183 CVectorTemplate<KFTYPE> &out_innov, 00184 CMatrixTemplateNumeric<KFTYPE> &out_Hx, 00185 CMatrixTemplateNumeric<KFTYPE> &out_Hy 00186 ) = 0; 00187 00188 /** If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element". 00189 * \param in_z This is the same matrix returned by OnGetObservations(). 00190 * \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. 00191 * \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. 00192 * \param out_yn The F-length vector with the inverse observation model \f$ y_n=y(x,z_n) \f$. 00193 * \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$. 00194 * \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$. 00195 * 00196 * - O: get_observation_size() 00197 * - V: get_vehicle_size() 00198 * - F: get_feature_size() 00199 */ 00200 virtual void OnInverseObservationModel( 00201 const CMatrixTemplateNumeric<KFTYPE> &in_z, 00202 const size_t &in_obsIdx, 00203 const size_t &in_idxNewFeat, 00204 CVectorTemplate<KFTYPE> &out_yn, 00205 CMatrixTemplateNumeric<KFTYPE> &out_dyn_dxv, 00206 CMatrixTemplateNumeric<KFTYPE> &out_dyn_dhn ); 00207 00208 /** 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. 00209 */ 00210 virtual void OnNormalizeStateVector(); 00211 00212 /** This method is called after finishing one KF iteration and before returning from runOneKalmanIteration(). 00213 */ 00214 virtual void OnPostIteration(); 00215 00216 00217 /** Must return the dimension of the "vehicle state": either the full state vector or the "vehicle" part if applicable. 00218 */ 00219 virtual size_t get_vehicle_size() const = 0; 00220 00221 /** Must return the dimension of the features in the system state (the "map"), or 0 if not applicable (the default if not implemented). 00222 */ 00223 virtual size_t get_feature_size() const 00224 { 00225 return 0; // The default for not SLAM-like problems. 00226 } 00227 00228 /** Must return the dimension of each observation (eg, 2 for pixel coordinates, 3 for 3D coordinates,etc). 00229 */ 00230 virtual size_t get_observation_size() const= 0; 00231 00232 /** The index of the first component of the vehicle state within the whole state vector/matrix (default=0) 00233 */ 00234 virtual size_t get_vehicle_state_offset() const 00235 { 00236 return 0; 00237 } 00238 00239 /** The index of the first component of the features part in the state vector/matrix (default=after the vehicle state, assuming it is at the begining of the vector) 00240 */ 00241 virtual size_t get_feature_state_offset() const 00242 { 00243 return get_vehicle_size(); 00244 } 00245 00246 /** @} 00247 */ 00248 00249 00250 public: 00251 00252 /** Default constructor 00253 */ 00254 CKalmanFilterCapable(); 00255 00256 /** Destructor 00257 */ 00258 virtual ~CKalmanFilterCapable(); 00259 00260 /** Generic options for the Kalman Filter algorithm in itself. 00261 */ 00262 struct MRPTDLLIMPEXP TKF_options : public utils::CLoadableOptions 00263 { 00264 TKF_options(); 00265 00266 void loadFromConfigFile( 00267 const mrpt::utils::CConfigFileBase &source, 00268 const std::string §ion); 00269 00270 /** This method must display clearly all the contents of the structure in textual form, sending it to a CStream. 00271 */ 00272 void dumpToTextStream(CStream &out) const; 00273 00274 00275 /** The method to employ (default: kfEKFAlaDavison) 00276 */ 00277 TKFMethod method; 00278 00279 /** If set to true timing information will be dumped to the console (default=false) 00280 */ 00281 bool verbose; 00282 00283 /** Value used as the diagonal of R for unobserved map elements, only in the kfEKFNaive algorithm. 00284 */ 00285 double veryLargeR2; 00286 00287 /** Number of refinement iterations, only for the IKF method. 00288 */ 00289 int IKF_iterations; 00290 }; 00291 00292 /** Generic options for the Kalman Filter algorithm in itself. 00293 */ 00294 TKF_options KF_options; 00295 00296 }; // end class 00297 00298 } // end namespace 00299 } // end namespace 00300 #endif
Page generated by Doxygen 1.5.9 for MRPT 0.7.1 SVN: at Mon Aug 17 22:32:05 EDT 2009 |