Chun Feng Huang / STATE_FEEDBACK

The generic full-state feedback control libery

STATE_FEEDBACK.h@4:958c25c1b151, 2017-01-04 (annotated)

Committer:
benson516
Date:
Wed Jan 04 10:09:44 2017 +0000
Revision:
4:958c25c1b151
Parent:
3:7ff53317e0a4 
Add "enable" state control

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benson516 0:8dbe2a4687b8 1 #ifndef STATE_FEEDBACK_H
benson516 0:8dbe2a4687b8 2 #define STATE_FEEDBACK_H
benson516 0:8dbe2a4687b8 3 //
benson516 0:8dbe2a4687b8 4 #include <vector>
benson516 0:8dbe2a4687b8 5
benson516 0:8dbe2a4687b8 6 using std::vector;
benson516 0:8dbe2a4687b8 7
benson516 0:8dbe2a4687b8 8 class STATE_FEEDBACK{
benson516 1:cdd434f6aa9a 9 public:
benson516 0:8dbe2a4687b8 10 // Dimensions
benson516 0:8dbe2a4687b8 11 size_t n; // Number of states
benson516 0:8dbe2a4687b8 12 size_t p; // Number of inputs of the plant
benson516 0:8dbe2a4687b8 13 size_t q; // Number of outputs of the plant, no use in full state feed back case
benson516 0:8dbe2a4687b8 14
benson516 0:8dbe2a4687b8 15 float Ts; // Sampling time
benson516 0:8dbe2a4687b8 16
benson516 0:8dbe2a4687b8 17 vector<vector<float> > K_full; // Full state feedback gain
benson516 2:0544e16ea933 18 // vector<vector<float> > N_xd; // Feed-forward gain for x_d, x_d = N_xd*r
benson516 2:0544e16ea933 19 // vector<vector<float> > N_ud; // Feed-forward gain for u_d, u_d = N_ud*r
benson516 2:0544e16ea933 20 vector<vector<float> > N_total; // Feed-forward gain for compensating u, u = -K*x + N_total*r
benson516 2:0544e16ea933 21
benson516 0:8dbe2a4687b8 22 //
benson516 0:8dbe2a4687b8 23 vector<float> states; // States
benson516 0:8dbe2a4687b8 24 vector<float> sys_inputs; // The inputs of the plant, "u", the "output" of the controller
benson516 0:8dbe2a4687b8 25 vector<float> sys_outputs; // The output of the plant, "y", the input of the controller
benson516 2:0544e16ea933 26 // Command (equalibrium state)
benson516 2:0544e16ea933 27 // vector<float> states_d; // x_d
benson516 2:0544e16ea933 28 // vector<float> inputs_d; // u_d
benson516 2:0544e16ea933 29 vector<float> sys_inputs_compensate; // N_total*r
benson516 2:0544e16ea933 30 vector<float> command; // r
benson516 0:8dbe2a4687b8 31
benson516 0:8dbe2a4687b8 32 STATE_FEEDBACK(size_t num_state, size_t num_in, size_t num_out, float samplingTime);
benson516 1:cdd434f6aa9a 33 // Assign Parameters
benson516 1:cdd434f6aa9a 34 void assign_K_full(float* K_full_in, size_t p_in, size_t n_in);
benson516 2:0544e16ea933 35 // void assign_N_xd(float* N_xd_in, size_t n_in, size_t q_in);
benson516 2:0544e16ea933 36 // void assign_N_ud(float* N_ud_in, size_t p_in, size_t q_in);
benson516 2:0544e16ea933 37 void assign_N_total(float* N_ud_in, size_t p_in, size_t q_in);
benson516 1:cdd434f6aa9a 38 //
benson516 4:958c25c1b151 39 void fullStateFeedBack_calc(bool enable);
benson516 0:8dbe2a4687b8 40
benson516 0:8dbe2a4687b8 41 private:
benson516 0:8dbe2a4687b8 42
benson516 0:8dbe2a4687b8 43 vector<float> zeros_n;
benson516 0:8dbe2a4687b8 44 vector<float> zeros_p;
benson516 0:8dbe2a4687b8 45 vector<float> zeros_q;
benson516 0:8dbe2a4687b8 46
benson516 2:0544e16ea933 47 // Command (equalibrium state) related calculation
benson516 3:7ff53317e0a4 48 void get_inputs_compensate(void); // Calculate the compensation variable, states_d and sys_inputs_compensate
benson516 2:0544e16ea933 49
benson516 1:cdd434f6aa9a 50 // Utilities
benson516 0:8dbe2a4687b8 51 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
benson516 1:cdd434f6aa9a 52 vector<float> Mat_multiply_Vec(const vector<vector<float> > &m_left, const vector<float> &v_right); // v_out = m_left*v_right
benson516 1:cdd434f6aa9a 53 vector<float> Get_VectorPlus(const vector<float> &v_a, const vector<float> &v_b, bool is_minus); // v_a + (or -) v_b
benson516 1:cdd434f6aa9a 54 vector<float> Get_VectorScalarMultiply(const vector<float> &v_a, float scale); // scale*v_a
benson516 1:cdd434f6aa9a 55
benson516 1:cdd434f6aa9a 56
benson516 0:8dbe2a4687b8 57 };
benson516 0:8dbe2a4687b8 58
benson516 0:8dbe2a4687b8 59 #endif