dynamics.h

#ifndef NAVLIB_PY_PLUS_PLUS
#ifndef INCLUDE_NAVLIB_DYNAMICS
#define INCLUDE_NAVLIB_DYNAMICS

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/function.hpp>
#include "navmath.h"

namespace ublas = boost::numeric::ublas;

namespace navlib
{
        class DynamicsModel
        {
        public:
                typedef ublas::vector<double> vector;
                typedef ublas::matrix<double> matrix;
                typedef boost::function< vector (double, vector, vector, vector) > PredictFun;

        public:
                DynamicsModel(PredictFun f, vector x0, vector params);
                virtual ~DynamicsModel() {}
                vector getParameters() const;
                vector getState() const;
                vector getStateDerivative(vector u) const;
                vector predict(vector x, vector u, double dt) const;
                void setState(vector x);
                void setParameters(vector p);
                vector step(vector u, double dt);
                void setTime(const double& t);

        protected:
                PredictFun _dxdt; 
                vector _x; 
                vector _p; 
                double _t; 
        };
        
        class UnicycleModel : public DynamicsModel
        {
        public:
                typedef ublas::vector<double> vector;

        public:
                UnicycleModel(double x, double y, double theta);
                virtual ~UnicycleModel() {}

        private:
                static vector predictFun(double t, vector x, vector u, vector p);
        };
}

#endif
#endif

---