Project_WIPV_antiSlip / STATE_FEEDBACK_INTEGRAL

The library for full-state feedback control with integral action

Fork of STATE_FEEDBACK_INTEGRAL by Project_WIPV_antiSlip

STATE_FEEDBACK_INTEGRAL.h@1:f19909200517, 2017-02-07 (annotated)

Committer:
benson516
Date:
Tue Feb 07 09:53:58 2017 +0000
Revision:
1:f19909200517
Parent:
0:37e27f2930a3 
test succeed

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benson516 0:37e27f2930a3 1 #ifndef STATE_FEEDBACK_INTEGRAL_H
benson516 0:37e27f2930a3 2 #define STATE_FEEDBACK_INTEGRAL_H
benson516 0:37e27f2930a3 3 //
benson516 0:37e27f2930a3 4 #include <vector>
benson516 0:37e27f2930a3 5
benson516 0:37e27f2930a3 6 using std::vector;
benson516 0:37e27f2930a3 7
benson516 0:37e27f2930a3 8 class STATE_FEEDBACK_INTEGRAL{
benson516 0:37e27f2930a3 9 public:
benson516 0:37e27f2930a3 10 // Dimensions
benson516 0:37e27f2930a3 11 size_t n; // Number of states
benson516 0:37e27f2930a3 12 size_t p; // Number of inputs of the plant
benson516 0:37e27f2930a3 13 size_t q; // Number of outputs of the plant, no use in full state feed back case
benson516 0:37e27f2930a3 14
benson516 0:37e27f2930a3 15 float Ts; // Sampling time
benson516 0:37e27f2930a3 16
benson516 0:37e27f2930a3 17 // System parameters
benson516 0:37e27f2930a3 18 vector<vector<float> > E_out; // System output matrix
benson516 0:37e27f2930a3 19 // Controller parameters
benson516 0:37e27f2930a3 20 vector<vector<float> > K_full; // Full state feedback gain
benson516 0:37e27f2930a3 21 vector<vector<float> > K_int; // Gain for integral action
benson516 0:37e27f2930a3 22
benson516 0:37e27f2930a3 23
benson516 0:37e27f2930a3 24 // States
benson516 0:37e27f2930a3 25 vector<float> states; // States
benson516 0:37e27f2930a3 26 vector<float> sys_inputs; // The inputs of the plant, "u", the "output" of the controller
benson516 0:37e27f2930a3 27 vector<float> sys_outputs; // The output of the plant, "y", the input of the controller
benson516 0:37e27f2930a3 28 // Command (equalibrium state)
benson516 0:37e27f2930a3 29 vector<float> command; // r
benson516 0:37e27f2930a3 30 // Integral state
benson516 0:37e27f2930a3 31 vector<float> state_int; // x_i
benson516 0:37e27f2930a3 32
benson516 0:37e27f2930a3 33 STATE_FEEDBACK_INTEGRAL(size_t num_state, size_t num_in, size_t num_out, float samplingTime);
benson516 0:37e27f2930a3 34 // Assign Parameters
benson516 0:37e27f2930a3 35 void assign_E_out(float* E_out_in, size_t q_in, size_t n_in);
benson516 0:37e27f2930a3 36 void assign_K_full(float* K_full_in, size_t p_in, size_t n_in);
benson516 0:37e27f2930a3 37 void assign_K_int(float* K_int_in, size_t p_in, size_t q_in);
benson516 0:37e27f2930a3 38 //
benson516 0:37e27f2930a3 39 void fullStateFeedBack_calc(bool enable);
benson516 0:37e27f2930a3 40
benson516 0:37e27f2930a3 41 private:
benson516 0:37e27f2930a3 42
benson516 0:37e27f2930a3 43 vector<float> zeros_n;
benson516 0:37e27f2930a3 44 vector<float> zeros_p;
benson516 0:37e27f2930a3 45 vector<float> zeros_q;
benson516 0:37e27f2930a3 46
benson516 0:37e27f2930a3 47 // Calculate the sys_outputs
benson516 0:37e27f2930a3 48 void get_sys_outputs(void); // Calculate the sys_outputs from states, by mutiplying E_out
benson516 0:37e27f2930a3 49
benson516 0:37e27f2930a3 50 // Calculate the Integral
benson516 0:37e27f2930a3 51 void get_integral(bool enable); // Calculate the state_int
benson516 0:37e27f2930a3 52
benson516 0:37e27f2930a3 53
benson516 0:37e27f2930a3 54 // Utilities
benson516 0:37e27f2930a3 55 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 0:37e27f2930a3 56 vector<float> Mat_multiply_Vec(const vector<vector<float> > &m_left, const vector<float> &v_right); // v_out = m_left*v_right
benson516 0:37e27f2930a3 57 vector<float> Get_VectorPlus(const vector<float> &v_a, const vector<float> &v_b, bool is_minus); // v_a + (or -) v_b
benson516 0:37e27f2930a3 58 vector<float> Get_VectorScalarMultiply(const vector<float> &v_a, float scale); // scale*v_a
benson516 0:37e27f2930a3 59 // Increment
benson516 0:37e27f2930a3 60 void Get_VectorIncrement(vector<float> &v_a, const vector<float> &v_b, bool is_minus); // v_a += (or -=) v_b
benson516 0:37e27f2930a3 61
benson516 0:37e27f2930a3 62
benson516 0:37e27f2930a3 63 };
benson516 0:37e27f2930a3 64
benson516 0:37e27f2930a3 65 #endif