Alex Pirciu
/
BFMC
a
include/Examples/SystemModels/ackermannmodel.hpp
- Committer:
- alexpirciu
- Date:
- 2019-03-28
- Revision:
- 1:ceee5a608e7c
File content as of revision 1:ceee5a608e7c:
/** ****************************************************************************** * @file AckermannModel.hpp * @author RBRO/PJ-IU * @version V1.0.0 * @date day-month-year * @brief This file contains the class declaration for the Ackermann model. ****************************************************************************** */ /* Include guard */ #ifndef ACKERMANN_MODEL_HPP #define ACKERMANN_MODEL_HPP #include <math.h> #include <SystemModels/systemmodels.hpp> #include <Examples/SystemModels/ackermanntypes.hpp> /* Definition of PI value */ #ifndef M_PI #define M_PI 3.14159265358979323846 #endif /* Definition of degrees to radians transformation */ #ifndef DEG2RAD #define DEG2RAD M_PI/180.0 #endif namespace examples { namespace systemmodels { namespace ackermannmodel { /* System model type */ template<class T,uint32_t NA,uint32_t NB,uint32_t NC> using CSystemModelType = ::systemmodels::nlti::mimo::CDiscreteTimeSystemModel<T,NA,NB,NC>; /* Jacobian matrix*/ template<class T,uint32_t NA,uint32_t NB,uint32_t NC> using CJacobianMatricesType = ::systemmodels::nlti::mimo::CJacobianMatrices<T,NA,NB,NC>; //! CAckermannModel class. /*! * It inherits class CSystemModelType. */ class CAckermannModel:public CSystemModelType<double,2,10,5> { public: /* Constructor */ CAckermannModel(const double f_dt ,const double f_gamma ,const double f_wb ,const double f_b ,const double f_J ,const double f_K ,const double f_R ,const double f_L); /* Constructor */ CAckermannModel(const CStatesType& f_states ,const double f_dt ,const double f_gamma ,const double f_wb ,const double f_b ,const double f_J ,const double f_K ,const double f_R ,const double f_L); /* Update method */ CStatesType update(const CInputType& f_input); /* Calculate output method */ COutputType calculateOutput(const CInputType& f_input); private: /* gamma=Meter/Rotation */ const double m_gamma; /* Wheel base distance in meter */ const double m_wb; /* Motor Constants */ const double m_bJ,m_KJ,m_KL,m_RL,m_L; }; //! CJMAckermannModel class. /*! * It inherits class CJacobianMatricesType. */ class CJMAckermannModel:public CJacobianMatricesType<double,2,10,5> { public: CJMAckermannModel (double f_dt ,double f_gamma ,double f_wb ,double f_b ,double f_J ,double f_K ,double f_R ,double f_L) :m_dt(f_dt) ,m_gamma(f_gamma) ,m_wb(f_wb) ,m_bJ(f_b/f_J) ,m_KJ(f_K/f_J) ,m_KL(f_K/f_L) ,m_RL(f_R/f_L) { m_ObservationMatrix=initObservationMatrix(); } CJMObservationType getJMObservation( const CStatesType& f_states ,const CInputType& f_input){ return m_ObservationMatrix; } private: linalg::CMatrix<double,5,10> initObservationMatrix(){ linalg::CMatrix<double,5,10> l_data; l_data[0][2]=l_data[1][3]=1.f/m_dt; l_data[0][4]=l_data[1][5]=-1.f/m_dt; l_data[2][7]=1.f; l_data[3][8]=m_gamma; l_data[4][9]=1.f; return l_data; } inline void setJacobianStateMatrixWithOne(CJMTransitionType& f_matrix){ f_matrix[0][0]=f_matrix[1][1]=1; f_matrix[4][2]=f_matrix[5][3]=1; f_matrix[6][6]=1; return; } public: CJMTransitionType getJMTransition( const CStatesType& f_states ,const CInputType& f_input){ CJMTransitionType l_data(CJMTransitionType::zeros()); setJacobianStateMatrixWithOne(l_data); CState l_states(f_states); CInput l_input(f_input); //Setting values in the jacobian matrix l_data[0][2]=m_dt; l_data[1][3]=m_dt; l_data[2][6]=-m_gamma*l_states.omega()*sin(l_states.teta_rad()); l_data[2][8]=m_gamma*cos(l_states.teta_rad()); l_data[3][6]=m_gamma*l_states.omega()*cos(l_states.teta_rad()); l_data[3][8]=m_gamma*sin(l_states.teta_rad()); double l_alpha_rad=l_input.alpha()*DEG2RAD; l_data[7][8]=l_data[6][8]=m_dt*m_gamma*tan(l_alpha_rad)/m_wb; //l_data[7][11]=l_data[6][11]=m_dt*m_gamma*f_state.omega/(m_wb*pow(cos(l_alpha_rad),2)); l_data[8][8]=(1-m_dt*m_bJ); l_data[8][9]=m_dt*m_KJ; l_data[9][8]=m_dt*m_KL; l_data[9][9]=(1-m_dt*m_RL); //l_data[9][10]=m_dt; return l_data; } private: /* Constant values */ /* Time step */ const double m_dt; /* gamma=Meter/Rotation */ const double m_gamma; /* Wheel base distance in meter */ const double m_wb; /* Motor Constants */ const double m_bJ,m_KJ,m_KL,m_RL; /* Please leave this matrix to be the last member in this class, as it will be initialized */ linalg::CMatrix<double,5,10> m_ObservationMatrix; }; }; }; }; #endif