The generic full-state feedback control libery
Fork of STATE_FEEDBACK by
STATE_FEEDBACK.h
00001 #ifndef STATE_FEEDBACK_H 00002 #define STATE_FEEDBACK_H 00003 // 00004 #include <vector> 00005 00006 using std::vector; 00007 00008 class STATE_FEEDBACK{ 00009 public: 00010 // Dimensions 00011 size_t n; // Number of states 00012 size_t p; // Number of inputs of the plant 00013 size_t q; // Number of outputs of the plant, no use in full state feed back case 00014 00015 float Ts; // Sampling time 00016 00017 vector<vector<float> > K_full; // Full state feedback gain 00018 // vector<vector<float> > N_xd; // Feed-forward gain for x_d, x_d = N_xd*r 00019 // vector<vector<float> > N_ud; // Feed-forward gain for u_d, u_d = N_ud*r 00020 vector<vector<float> > N_total; // Feed-forward gain for compensating u, u = -K*x + N_total*r 00021 00022 // 00023 vector<float> states; // States 00024 vector<float> sys_inputs; // The inputs of the plant, "u", the "output" of the controller 00025 vector<float> sys_outputs; // The output of the plant, "y", the input of the controller 00026 // Command (equalibrium state) 00027 // vector<float> states_d; // x_d 00028 // vector<float> inputs_d; // u_d 00029 vector<float> sys_inputs_compensate; // N_total*r 00030 vector<float> command; // r 00031 00032 STATE_FEEDBACK(size_t num_state, size_t num_in, size_t num_out, float samplingTime); 00033 // Assign Parameters 00034 void assign_K_full(float* K_full_in, size_t p_in, size_t n_in); 00035 // void assign_N_xd(float* N_xd_in, size_t n_in, size_t q_in); 00036 // void assign_N_ud(float* N_ud_in, size_t p_in, size_t q_in); 00037 void assign_N_total(float* N_ud_in, size_t p_in, size_t q_in); 00038 // 00039 void fullStateFeedBack_calc(bool enable); 00040 00041 private: 00042 00043 vector<float> zeros_n; 00044 vector<float> zeros_p; 00045 vector<float> zeros_q; 00046 00047 // Command (equalibrium state) related calculation 00048 void get_inputs_compensate(void); // Calculate the compensation variable, states_d and sys_inputs_compensate 00049 00050 // Utilities 00051 void Mat_multiply_Vec(vector<float> &v_out, const vector<vector<float> > &m_left, const vector<float> &v_right); // v_out = m_left*v_right 00052 vector<float> Mat_multiply_Vec(const vector<vector<float> > &m_left, const vector<float> &v_right); // v_out = m_left*v_right 00053 vector<float> Get_VectorPlus(const vector<float> &v_a, const vector<float> &v_b, bool is_minus); // v_a + (or -) v_b 00054 vector<float> Get_VectorScalarMultiply(const vector<float> &v_a, float scale); // scale*v_a 00055 00056 00057 }; 00058 00059 #endif
Generated on Wed Jul 20 2022 10:49:47 by
1.7.2
