00001 #ifndef NAVLIB_PY_PLUS_PLUS 00002 #ifndef INCLUDE_NAVLIB_DYNAMICS 00003 #define INCLUDE_NAVLIB_DYNAMICS 00004 00005 #include <boost/numeric/ublas/vector.hpp> 00006 #include <boost/numeric/ublas/matrix.hpp> 00007 #include <boost/function.hpp> 00008 #include "navmath.h" 00009 00010 namespace ublas = boost::numeric::ublas; 00011 00012 namespace navlib 00013 { 00016 class DynamicsModel 00017 { 00018 public: 00020 typedef ublas::vector<double> vector; 00021 typedef ublas::matrix<double> matrix; 00022 typedef boost::function< vector (double, vector, vector, vector) > PredictFun; 00023 00024 public: 00028 DynamicsModel(PredictFun f, vector x0, vector params); 00029 virtual ~DynamicsModel() {} 00031 vector getParameters() const; 00033 vector getState() const; 00036 vector getStateDerivative(vector u) const; 00041 vector predict(vector x, vector u, double dt) const; 00044 void setState(vector x); 00047 void setParameters(vector p); 00051 vector step(vector u, double dt); 00054 void setTime(const double& t); 00055 00056 protected: 00057 PredictFun _dxdt; 00058 vector _x; 00059 vector _p; 00060 double _t; 00061 }; 00062 00064 class UnicycleModel : public DynamicsModel 00065 { 00066 public: 00067 typedef ublas::vector<double> vector; 00068 00069 public: 00073 UnicycleModel(double x, double y, double theta); 00074 virtual ~UnicycleModel() {} 00075 00076 private: 00082 static vector predictFun(double t, vector x, vector u, vector p); 00083 }; 00084 } 00085 00086 #endif 00087 #endif