Alex Pirciu
/
BFMC
a
include/Examples/SystemModels/ackermannmodel.hpp@1:ceee5a608e7c, 2019-03-28 (annotated)
- Committer:
- alexpirciu
- Date:
- Thu Mar 28 07:44:42 2019 +0000
- Revision:
- 1:ceee5a608e7c
assa
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
alexpirciu | 1:ceee5a608e7c | 1 | |
alexpirciu | 1:ceee5a608e7c | 2 | /** |
alexpirciu | 1:ceee5a608e7c | 3 | ****************************************************************************** |
alexpirciu | 1:ceee5a608e7c | 4 | * @file AckermannModel.hpp |
alexpirciu | 1:ceee5a608e7c | 5 | * @author RBRO/PJ-IU |
alexpirciu | 1:ceee5a608e7c | 6 | * @version V1.0.0 |
alexpirciu | 1:ceee5a608e7c | 7 | * @date day-month-year |
alexpirciu | 1:ceee5a608e7c | 8 | * @brief This file contains the class declaration for the Ackermann model. |
alexpirciu | 1:ceee5a608e7c | 9 | ****************************************************************************** |
alexpirciu | 1:ceee5a608e7c | 10 | */ |
alexpirciu | 1:ceee5a608e7c | 11 | |
alexpirciu | 1:ceee5a608e7c | 12 | /* Include guard */ |
alexpirciu | 1:ceee5a608e7c | 13 | #ifndef ACKERMANN_MODEL_HPP |
alexpirciu | 1:ceee5a608e7c | 14 | #define ACKERMANN_MODEL_HPP |
alexpirciu | 1:ceee5a608e7c | 15 | |
alexpirciu | 1:ceee5a608e7c | 16 | #include <math.h> |
alexpirciu | 1:ceee5a608e7c | 17 | #include <SystemModels/systemmodels.hpp> |
alexpirciu | 1:ceee5a608e7c | 18 | #include <Examples/SystemModels/ackermanntypes.hpp> |
alexpirciu | 1:ceee5a608e7c | 19 | |
alexpirciu | 1:ceee5a608e7c | 20 | /* Definition of PI value */ |
alexpirciu | 1:ceee5a608e7c | 21 | #ifndef M_PI |
alexpirciu | 1:ceee5a608e7c | 22 | #define M_PI 3.14159265358979323846 |
alexpirciu | 1:ceee5a608e7c | 23 | #endif |
alexpirciu | 1:ceee5a608e7c | 24 | |
alexpirciu | 1:ceee5a608e7c | 25 | /* Definition of degrees to radians transformation */ |
alexpirciu | 1:ceee5a608e7c | 26 | #ifndef DEG2RAD |
alexpirciu | 1:ceee5a608e7c | 27 | #define DEG2RAD M_PI/180.0 |
alexpirciu | 1:ceee5a608e7c | 28 | #endif |
alexpirciu | 1:ceee5a608e7c | 29 | |
alexpirciu | 1:ceee5a608e7c | 30 | namespace examples |
alexpirciu | 1:ceee5a608e7c | 31 | { |
alexpirciu | 1:ceee5a608e7c | 32 | namespace systemmodels |
alexpirciu | 1:ceee5a608e7c | 33 | { |
alexpirciu | 1:ceee5a608e7c | 34 | namespace ackermannmodel |
alexpirciu | 1:ceee5a608e7c | 35 | { |
alexpirciu | 1:ceee5a608e7c | 36 | /* System model type */ |
alexpirciu | 1:ceee5a608e7c | 37 | template<class T,uint32_t NA,uint32_t NB,uint32_t NC> |
alexpirciu | 1:ceee5a608e7c | 38 | using CSystemModelType = ::systemmodels::nlti::mimo::CDiscreteTimeSystemModel<T,NA,NB,NC>; |
alexpirciu | 1:ceee5a608e7c | 39 | /* Jacobian matrix*/ |
alexpirciu | 1:ceee5a608e7c | 40 | template<class T,uint32_t NA,uint32_t NB,uint32_t NC> |
alexpirciu | 1:ceee5a608e7c | 41 | using CJacobianMatricesType = ::systemmodels::nlti::mimo::CJacobianMatrices<T,NA,NB,NC>; |
alexpirciu | 1:ceee5a608e7c | 42 | //! CAckermannModel class. |
alexpirciu | 1:ceee5a608e7c | 43 | /*! |
alexpirciu | 1:ceee5a608e7c | 44 | * It inherits class CSystemModelType. |
alexpirciu | 1:ceee5a608e7c | 45 | */ |
alexpirciu | 1:ceee5a608e7c | 46 | class CAckermannModel:public CSystemModelType<double,2,10,5> |
alexpirciu | 1:ceee5a608e7c | 47 | { |
alexpirciu | 1:ceee5a608e7c | 48 | public: |
alexpirciu | 1:ceee5a608e7c | 49 | /* Constructor */ |
alexpirciu | 1:ceee5a608e7c | 50 | CAckermannModel(const double f_dt |
alexpirciu | 1:ceee5a608e7c | 51 | ,const double f_gamma |
alexpirciu | 1:ceee5a608e7c | 52 | ,const double f_wb |
alexpirciu | 1:ceee5a608e7c | 53 | ,const double f_b |
alexpirciu | 1:ceee5a608e7c | 54 | ,const double f_J |
alexpirciu | 1:ceee5a608e7c | 55 | ,const double f_K |
alexpirciu | 1:ceee5a608e7c | 56 | ,const double f_R |
alexpirciu | 1:ceee5a608e7c | 57 | ,const double f_L); |
alexpirciu | 1:ceee5a608e7c | 58 | /* Constructor */ |
alexpirciu | 1:ceee5a608e7c | 59 | CAckermannModel(const CStatesType& f_states |
alexpirciu | 1:ceee5a608e7c | 60 | ,const double f_dt |
alexpirciu | 1:ceee5a608e7c | 61 | ,const double f_gamma |
alexpirciu | 1:ceee5a608e7c | 62 | ,const double f_wb |
alexpirciu | 1:ceee5a608e7c | 63 | ,const double f_b |
alexpirciu | 1:ceee5a608e7c | 64 | ,const double f_J |
alexpirciu | 1:ceee5a608e7c | 65 | ,const double f_K |
alexpirciu | 1:ceee5a608e7c | 66 | ,const double f_R |
alexpirciu | 1:ceee5a608e7c | 67 | ,const double f_L); |
alexpirciu | 1:ceee5a608e7c | 68 | /* Update method */ |
alexpirciu | 1:ceee5a608e7c | 69 | CStatesType update(const CInputType& f_input); |
alexpirciu | 1:ceee5a608e7c | 70 | |
alexpirciu | 1:ceee5a608e7c | 71 | /* Calculate output method */ |
alexpirciu | 1:ceee5a608e7c | 72 | COutputType calculateOutput(const CInputType& f_input); |
alexpirciu | 1:ceee5a608e7c | 73 | private: |
alexpirciu | 1:ceee5a608e7c | 74 | /* gamma=Meter/Rotation */ |
alexpirciu | 1:ceee5a608e7c | 75 | const double m_gamma; |
alexpirciu | 1:ceee5a608e7c | 76 | /* Wheel base distance in meter */ |
alexpirciu | 1:ceee5a608e7c | 77 | const double m_wb; |
alexpirciu | 1:ceee5a608e7c | 78 | /* Motor Constants */ |
alexpirciu | 1:ceee5a608e7c | 79 | const double m_bJ,m_KJ,m_KL,m_RL,m_L; |
alexpirciu | 1:ceee5a608e7c | 80 | }; |
alexpirciu | 1:ceee5a608e7c | 81 | |
alexpirciu | 1:ceee5a608e7c | 82 | //! CJMAckermannModel class. |
alexpirciu | 1:ceee5a608e7c | 83 | /*! |
alexpirciu | 1:ceee5a608e7c | 84 | * It inherits class CJacobianMatricesType. |
alexpirciu | 1:ceee5a608e7c | 85 | */ |
alexpirciu | 1:ceee5a608e7c | 86 | class CJMAckermannModel:public CJacobianMatricesType<double,2,10,5> |
alexpirciu | 1:ceee5a608e7c | 87 | { |
alexpirciu | 1:ceee5a608e7c | 88 | public: |
alexpirciu | 1:ceee5a608e7c | 89 | CJMAckermannModel (double f_dt |
alexpirciu | 1:ceee5a608e7c | 90 | ,double f_gamma |
alexpirciu | 1:ceee5a608e7c | 91 | ,double f_wb |
alexpirciu | 1:ceee5a608e7c | 92 | ,double f_b |
alexpirciu | 1:ceee5a608e7c | 93 | ,double f_J |
alexpirciu | 1:ceee5a608e7c | 94 | ,double f_K |
alexpirciu | 1:ceee5a608e7c | 95 | ,double f_R |
alexpirciu | 1:ceee5a608e7c | 96 | ,double f_L) |
alexpirciu | 1:ceee5a608e7c | 97 | :m_dt(f_dt) |
alexpirciu | 1:ceee5a608e7c | 98 | ,m_gamma(f_gamma) |
alexpirciu | 1:ceee5a608e7c | 99 | ,m_wb(f_wb) |
alexpirciu | 1:ceee5a608e7c | 100 | ,m_bJ(f_b/f_J) |
alexpirciu | 1:ceee5a608e7c | 101 | ,m_KJ(f_K/f_J) |
alexpirciu | 1:ceee5a608e7c | 102 | ,m_KL(f_K/f_L) |
alexpirciu | 1:ceee5a608e7c | 103 | ,m_RL(f_R/f_L) |
alexpirciu | 1:ceee5a608e7c | 104 | { |
alexpirciu | 1:ceee5a608e7c | 105 | m_ObservationMatrix=initObservationMatrix(); |
alexpirciu | 1:ceee5a608e7c | 106 | } |
alexpirciu | 1:ceee5a608e7c | 107 | |
alexpirciu | 1:ceee5a608e7c | 108 | |
alexpirciu | 1:ceee5a608e7c | 109 | |
alexpirciu | 1:ceee5a608e7c | 110 | CJMObservationType getJMObservation( const CStatesType& f_states |
alexpirciu | 1:ceee5a608e7c | 111 | ,const CInputType& f_input){ |
alexpirciu | 1:ceee5a608e7c | 112 | return m_ObservationMatrix; |
alexpirciu | 1:ceee5a608e7c | 113 | } |
alexpirciu | 1:ceee5a608e7c | 114 | |
alexpirciu | 1:ceee5a608e7c | 115 | private: |
alexpirciu | 1:ceee5a608e7c | 116 | linalg::CMatrix<double,5,10> initObservationMatrix(){ |
alexpirciu | 1:ceee5a608e7c | 117 | linalg::CMatrix<double,5,10> l_data; |
alexpirciu | 1:ceee5a608e7c | 118 | l_data[0][2]=l_data[1][3]=1.f/m_dt; |
alexpirciu | 1:ceee5a608e7c | 119 | l_data[0][4]=l_data[1][5]=-1.f/m_dt; |
alexpirciu | 1:ceee5a608e7c | 120 | l_data[2][7]=1.f; |
alexpirciu | 1:ceee5a608e7c | 121 | l_data[3][8]=m_gamma; |
alexpirciu | 1:ceee5a608e7c | 122 | l_data[4][9]=1.f; |
alexpirciu | 1:ceee5a608e7c | 123 | return l_data; |
alexpirciu | 1:ceee5a608e7c | 124 | } |
alexpirciu | 1:ceee5a608e7c | 125 | |
alexpirciu | 1:ceee5a608e7c | 126 | inline void setJacobianStateMatrixWithOne(CJMTransitionType& f_matrix){ |
alexpirciu | 1:ceee5a608e7c | 127 | f_matrix[0][0]=f_matrix[1][1]=1; |
alexpirciu | 1:ceee5a608e7c | 128 | f_matrix[4][2]=f_matrix[5][3]=1; |
alexpirciu | 1:ceee5a608e7c | 129 | f_matrix[6][6]=1; |
alexpirciu | 1:ceee5a608e7c | 130 | return; |
alexpirciu | 1:ceee5a608e7c | 131 | } |
alexpirciu | 1:ceee5a608e7c | 132 | |
alexpirciu | 1:ceee5a608e7c | 133 | public: |
alexpirciu | 1:ceee5a608e7c | 134 | CJMTransitionType getJMTransition( const CStatesType& f_states |
alexpirciu | 1:ceee5a608e7c | 135 | ,const CInputType& f_input){ |
alexpirciu | 1:ceee5a608e7c | 136 | |
alexpirciu | 1:ceee5a608e7c | 137 | CJMTransitionType l_data(CJMTransitionType::zeros()); |
alexpirciu | 1:ceee5a608e7c | 138 | setJacobianStateMatrixWithOne(l_data); |
alexpirciu | 1:ceee5a608e7c | 139 | CState l_states(f_states); |
alexpirciu | 1:ceee5a608e7c | 140 | CInput l_input(f_input); |
alexpirciu | 1:ceee5a608e7c | 141 | |
alexpirciu | 1:ceee5a608e7c | 142 | //Setting values in the jacobian matrix |
alexpirciu | 1:ceee5a608e7c | 143 | l_data[0][2]=m_dt; |
alexpirciu | 1:ceee5a608e7c | 144 | l_data[1][3]=m_dt; |
alexpirciu | 1:ceee5a608e7c | 145 | |
alexpirciu | 1:ceee5a608e7c | 146 | l_data[2][6]=-m_gamma*l_states.omega()*sin(l_states.teta_rad()); |
alexpirciu | 1:ceee5a608e7c | 147 | l_data[2][8]=m_gamma*cos(l_states.teta_rad()); |
alexpirciu | 1:ceee5a608e7c | 148 | |
alexpirciu | 1:ceee5a608e7c | 149 | l_data[3][6]=m_gamma*l_states.omega()*cos(l_states.teta_rad()); |
alexpirciu | 1:ceee5a608e7c | 150 | l_data[3][8]=m_gamma*sin(l_states.teta_rad()); |
alexpirciu | 1:ceee5a608e7c | 151 | |
alexpirciu | 1:ceee5a608e7c | 152 | double l_alpha_rad=l_input.alpha()*DEG2RAD; |
alexpirciu | 1:ceee5a608e7c | 153 | l_data[7][8]=l_data[6][8]=m_dt*m_gamma*tan(l_alpha_rad)/m_wb; |
alexpirciu | 1:ceee5a608e7c | 154 | //l_data[7][11]=l_data[6][11]=m_dt*m_gamma*f_state.omega/(m_wb*pow(cos(l_alpha_rad),2)); |
alexpirciu | 1:ceee5a608e7c | 155 | |
alexpirciu | 1:ceee5a608e7c | 156 | l_data[8][8]=(1-m_dt*m_bJ); |
alexpirciu | 1:ceee5a608e7c | 157 | l_data[8][9]=m_dt*m_KJ; |
alexpirciu | 1:ceee5a608e7c | 158 | l_data[9][8]=m_dt*m_KL; |
alexpirciu | 1:ceee5a608e7c | 159 | l_data[9][9]=(1-m_dt*m_RL); |
alexpirciu | 1:ceee5a608e7c | 160 | //l_data[9][10]=m_dt; |
alexpirciu | 1:ceee5a608e7c | 161 | return l_data; |
alexpirciu | 1:ceee5a608e7c | 162 | } |
alexpirciu | 1:ceee5a608e7c | 163 | |
alexpirciu | 1:ceee5a608e7c | 164 | private: |
alexpirciu | 1:ceee5a608e7c | 165 | /* Constant values */ |
alexpirciu | 1:ceee5a608e7c | 166 | /* Time step */ |
alexpirciu | 1:ceee5a608e7c | 167 | const double m_dt; |
alexpirciu | 1:ceee5a608e7c | 168 | /* gamma=Meter/Rotation */ |
alexpirciu | 1:ceee5a608e7c | 169 | const double m_gamma; |
alexpirciu | 1:ceee5a608e7c | 170 | /* Wheel base distance in meter */ |
alexpirciu | 1:ceee5a608e7c | 171 | const double m_wb; |
alexpirciu | 1:ceee5a608e7c | 172 | /* Motor Constants */ |
alexpirciu | 1:ceee5a608e7c | 173 | const double m_bJ,m_KJ,m_KL,m_RL; |
alexpirciu | 1:ceee5a608e7c | 174 | /* Please leave this matrix to be the last member in this class, |
alexpirciu | 1:ceee5a608e7c | 175 | as it will be initialized */ |
alexpirciu | 1:ceee5a608e7c | 176 | linalg::CMatrix<double,5,10> m_ObservationMatrix; |
alexpirciu | 1:ceee5a608e7c | 177 | }; |
alexpirciu | 1:ceee5a608e7c | 178 | }; |
alexpirciu | 1:ceee5a608e7c | 179 | }; |
alexpirciu | 1:ceee5a608e7c | 180 | }; |
alexpirciu | 1:ceee5a608e7c | 181 | |
alexpirciu | 1:ceee5a608e7c | 182 | #endif |