Alex Pirciu / Mbed 2 deprecated BFMC

a

Dependencies:   mbed mbed-rtos

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?

UserRevisionLine numberNew 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