00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
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
00046 enum TKFMethod {
00047 kfEKFNaive = 0,
00048 kfEKFAlaDavison,
00049 kfIKFFull,
00050 kfIKF
00051 };
00052
00055 typedef double KFTYPE;
00056
00074 class MRPTDLLIMPEXP CKalmanFilterCapable
00075 {
00076 protected:
00079 math::CVectorTemplate<KFTYPE> m_xkk;
00080
00083 math::CMatrixTemplateNumeric<KFTYPE> m_pkk;
00084
00089 void runOneKalmanIteration();
00090
00098 virtual void OnGetAction( CVectorTemplate<KFTYPE> &out_u ) = 0;
00099
00105 virtual void OnTransitionModel(
00106 const CVectorTemplate<KFTYPE>& in_u,
00107 CVectorTemplate<KFTYPE>& inout_x,
00108 bool &out_skipPrediction
00109 ) = 0;
00110
00115 virtual void OnTransitionJacobian(
00116 CMatrixTemplateNumeric<KFTYPE> & out_F
00117 ) = 0;
00118
00123 virtual void OnTransitionNoise(
00124 CMatrixTemplateNumeric<KFTYPE> & out_Q
00125 ) = 0;
00126
00134 virtual void OnGetObservations(
00135 CMatrixTemplateNumeric<KFTYPE> &out_z,
00136 CVectorTemplate<KFTYPE> &out_R2,
00137 vector_int &out_data_association
00138 )
00139 {
00140 MRPT_TRY_START
00141 THROW_EXCEPTION("One version of the OnGetObservations method must be implemented in the derived class.")
00142 MRPT_TRY_END
00143 }
00144
00152 virtual void OnGetObservations(
00153 CMatrixTemplateNumeric<KFTYPE> &out_z,
00154 CMatrixTemplateNumeric<KFTYPE> &out_R,
00155 vector_int &out_data_association
00156 );
00157
00173 virtual void OnObservationModelAndJacobians(
00174 const CMatrixTemplateNumeric<KFTYPE> &in_z,
00175 const vector_int &in_data_association,
00176 const bool &in_full,
00177 const int &in_obsIdx,
00178 CVectorTemplate<KFTYPE> &out_innov,
00179 CMatrixTemplateNumeric<KFTYPE> &out_Hx,
00180 CMatrixTemplateNumeric<KFTYPE> &out_Hy
00181 ) = 0;
00182
00195 virtual void OnInverseObservationModel(
00196 const CMatrixTemplateNumeric<KFTYPE> &in_z,
00197 const size_t &in_obsIdx,
00198 const size_t &in_idxNewFeat,
00199 CVectorTemplate<KFTYPE> &out_yn,
00200 CMatrixTemplateNumeric<KFTYPE> &out_dyn_dxv,
00201 CMatrixTemplateNumeric<KFTYPE> &out_dyn_dhn );
00202
00205 virtual void OnNormalizeStateVector();
00206
00209 virtual void OnPostIteration();
00210
00211
00214 virtual size_t get_vehicle_size() const = 0;
00215
00218 virtual size_t get_feature_size() const
00219 {
00220 return 0;
00221 }
00222
00225 virtual size_t get_observation_size() const= 0;
00226
00229 virtual size_t get_vehicle_state_offset() const
00230 {
00231 return 0;
00232 }
00233
00236 virtual size_t get_feature_state_offset() const
00237 {
00238 return get_vehicle_size();
00239 }
00240
00245 public:
00246
00249 CKalmanFilterCapable();
00250
00253 virtual ~CKalmanFilterCapable();
00254
00257 struct MRPTDLLIMPEXP TKF_options : public utils::CLoadableOptions
00258 {
00259 TKF_options();
00260
00261 void loadFromConfigFile(
00262 const mrpt::utils::CConfigFileBase &source,
00263 const std::string §ion);
00264
00267 void dumpToTextStream(CStream &out);
00268
00269
00272 TKFMethod method;
00273
00276 bool verbose;
00277
00280 double veryLargeR2;
00281
00284 int IKF_iterations;
00285 };
00286
00289 TKF_options KF_options;
00290
00291 };
00292
00293 }
00294 }
00295 #endif