The generic full-state feedback control libery

Fork of STATE_FEEDBACK by Project_WIPV_antiSlip

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers STATE_FEEDBACK.h Source File

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