The library for full-state feedback control with integral action

Fork of STATE_FEEDBACK_INTEGRAL by Project_WIPV_antiSlip

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers STATE_FEEDBACK_INTEGRAL.h Source File

STATE_FEEDBACK_INTEGRAL.h

00001 #ifndef STATE_FEEDBACK_INTEGRAL_H
00002 #define STATE_FEEDBACK_INTEGRAL_H
00003 //
00004 #include <vector>
00005 
00006 using std::vector;
00007 
00008 class STATE_FEEDBACK_INTEGRAL{
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     // System parameters
00018     vector<vector<float> > E_out; // System output matrix
00019     // Controller parameters
00020     vector<vector<float> > K_full; // Full state feedback gain
00021     vector<vector<float> > K_int; // Gain for integral action
00022 
00023 
00024     // States
00025     vector<float> states; // States
00026     vector<float> sys_inputs; // The inputs of the plant, "u", the "output" of the controller
00027     vector<float> sys_outputs; // The output of the plant, "y", the input of the controller
00028     // Command (equalibrium state)
00029     vector<float> command; // r
00030     // Integral state
00031     vector<float> state_int; // x_i
00032 
00033     STATE_FEEDBACK_INTEGRAL(size_t num_state, size_t num_in, size_t num_out, float samplingTime);
00034     // Assign Parameters
00035     void assign_E_out(float* E_out_in, size_t q_in, size_t n_in);
00036     void assign_K_full(float* K_full_in, size_t p_in, size_t n_in);
00037     void assign_K_int(float* K_int_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     // Calculate the sys_outputs
00048     void get_sys_outputs(void); // Calculate the sys_outputs from states, by mutiplying E_out
00049 
00050     // Calculate the Integral
00051     void get_integral(bool enable); // Calculate the state_int
00052 
00053 
00054     // Utilities
00055     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
00056     vector<float> Mat_multiply_Vec(const vector<vector<float> > &m_left, const vector<float> &v_right); // v_out = m_left*v_right
00057     vector<float> Get_VectorPlus(const vector<float> &v_a, const vector<float> &v_b, bool is_minus); // v_a + (or -) v_b
00058     vector<float> Get_VectorScalarMultiply(const vector<float> &v_a, float scale); // scale*v_a
00059     // Increment
00060     void Get_VectorIncrement(vector<float> &v_a, const vector<float> &v_b, bool is_minus); // v_a += (or -=) v_b
00061 
00062 
00063 };
00064 
00065 #endif