00001 #ifndef INCLUDE_NAVLIB_EKFIDENTIFIER 00002 #define INCLUDE_NAVLIB_EKFIDENTIFIER 00003 00004 #include "identifier.h" 00005 00006 namespace navlib 00007 { 00009 class EkfIdentifier : public Identifier 00010 { 00011 public: 00012 typedef ublas::matrix<double> matrix; 00013 typedef ublas::vector<double> vector; 00014 00015 public: 00018 EkfIdentifier(boost::shared_ptr<DynamicsModel> dynamics, const double& eps=1.0e-9); 00022 EkfIdentifier(boost::shared_ptr<DynamicsModel> dynamics, matrix Px, matrix Pp); 00023 boost::shared_ptr<DynamicsModel> getModel() const; 00029 void predict(vector u, const double& dt, matrix Qx, matrix Qp); 00034 void correct(ExtendedKalmanFilter<double>::ObserveFun h, vector z, matrix R); 00035 00036 private: 00037 boost::shared_ptr<DynamicsModel> _dynamics; 00038 ExtendedKalmanFilter<double> _ekf; 00039 unsigned int _nx; 00040 unsigned int _np; 00041 00042 private: 00043 void init(boost::shared_ptr<DynamicsModel> dynamics, matrix Px, matrix Pp); 00044 vector ppredict(vector x, vector u, const double& dt); 00045 }; 00046 } 00047 00048 #endif