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.cpp Source File

STATE_FEEDBACK_INTEGRAL.cpp

00001 #include "STATE_FEEDBACK_INTEGRAL.h"
00002 // The controller is for the plant with (p, n, q) system
00003 // Dimensions:
00004 //
00005 // Inputs, u   |   States, x    |   outputs, y
00006 //     p     -->      n        -->      q
00007 //
00008 //
00009 
00010 STATE_FEEDBACK_INTEGRAL::STATE_FEEDBACK_INTEGRAL(size_t num_state, size_t num_in, size_t num_out, float samplingTime):
00011     n(num_state), p(num_in), q(num_out), Ts(samplingTime),
00012     E_out(num_out, vector<float>(num_state,0.0)),
00013     K_full(num_in, vector<float>(num_state,0.0)),
00014     K_int(num_in, vector<float>(num_out,0.0))
00015 {
00016     // Normally, q = p
00017     if (q > p)
00018         q = p;
00019     //
00020     zeros_n.assign(n, 0.0);
00021     zeros_p.assign(p, 0.0);
00022     zeros_q.assign(q, 0.0);
00023 
00024     // States
00025     states = zeros_n;
00026     sys_inputs = zeros_p;
00027     sys_outputs = zeros_q;
00028 
00029     // Command (equalibrium state)
00030     command = zeros_q; // q = p
00031 
00032     // Integral state
00033     state_int = zeros_q;
00034 
00035 }
00036 // Assign Parameters
00037 void STATE_FEEDBACK_INTEGRAL::assign_E_out(float* E_out_in, size_t q_in, size_t n_in){
00038     // E_out_in is the pointer of a mutidimentional array with size q_in by n_in
00039     if (q != q_in || n != n_in){
00040         q = q_in;
00041         n = n_in;
00042         zeros_q.resize(q, 0.0);
00043         zeros_n.resize(n, 0.0);
00044         E_out.assign(q, zeros_n);
00045     }
00046     //
00047     for (size_t i = 0; i < q; ++i){
00048         for (size_t j = 0; j < n; ++j){
00049             // E_out[i][j] = E_out_in[i][j];
00050             E_out[i][j] = *E_out_in;
00051             E_out_in++;
00052         }
00053     }
00054 }
00055 void STATE_FEEDBACK_INTEGRAL::assign_K_full(float* K_full_in, size_t p_in, size_t n_in){
00056     // K_full_in is the pointer of a mutidimentional array with size p_in by n_in
00057     if (n != n_in || p != p_in){
00058         n = n_in;
00059         p = p_in;
00060         zeros_n.resize(n, 0.0);
00061         zeros_p.resize(p, 0.0);
00062         K_full.assign(p, zeros_n);
00063     }
00064     //
00065     for (size_t i = 0; i < p; ++i){
00066         for (size_t j = 0; j < n; ++j){
00067             // K_full[i][j] = K_full_in[i][j];
00068             K_full[i][j] = *K_full_in;
00069             K_full_in++;
00070         }
00071     }
00072 }
00073 void STATE_FEEDBACK_INTEGRAL::assign_K_int(float* K_int_in, size_t p_in, size_t q_in){
00074     // K_int_in is the pointer of a mutidimentional array with size p_in by q_in
00075     if (p != p_in || q != q_in){
00076         p = p_in;
00077         q = q_in;
00078         zeros_p.resize(p, 0.0);
00079         zeros_q.resize(q, 0.0);
00080         K_int.assign(p, zeros_q);
00081     }
00082     //
00083     for (size_t i = 0; i < p; ++i){
00084         for (size_t j = 0; j < q; ++j){
00085             // K_int[i][j] = K_int_in[i][j];
00086             K_int[i][j] = *K_int_in;
00087             K_int_in++;
00088         }
00089     }
00090 }
00091 //
00092 void STATE_FEEDBACK_INTEGRAL::fullStateFeedBack_calc(bool enable){
00093 
00094     // Control law
00095     if (enable){
00096         // sys_inputs = (-K_full*states) - K_int*state_int
00097         sys_inputs = Get_VectorPlus(Get_VectorScalarMultiply(Mat_multiply_Vec(K_full, states),-1.0), Mat_multiply_Vec(K_int, state_int) ,true); // minus
00098     }else{
00099         sys_inputs = zeros_p;
00100     }
00101 
00102     // Integral action
00103     get_integral(enable);
00104 }
00105 
00106 // Private functions
00107 // Calculate the sys_outputs
00108 void STATE_FEEDBACK_INTEGRAL::get_sys_outputs(void){ // Calculate the sys_outputs from states, by mutiplying E_out
00109     // sys_outputs = E_out*states
00110     Mat_multiply_Vec(sys_outputs, E_out, states);
00111 
00112 }
00113 // Calculate the Integral
00114 void STATE_FEEDBACK_INTEGRAL::get_integral(bool enable){ // Calculate the state_int
00115     //
00116     // Calculate the sys_outputs
00117     get_sys_outputs();
00118 
00119     // Integral action
00120     // state_int += sys_outputs - command
00121     if (enable){
00122         Get_VectorIncrement(state_int, Get_VectorScalarMultiply(Get_VectorPlus(sys_outputs,command,true),Ts) ,false); // +=
00123     }else{
00124         state_int = zeros_q;
00125     }
00126 }
00127 // Utilities
00128 void STATE_FEEDBACK_INTEGRAL::Mat_multiply_Vec(vector<float> &v_out, const vector<vector<float> > &m_left, const vector<float> &v_right){ // v_out = m_left*v_right
00129     static vector<float>::iterator it_out;
00130     static vector<const float>::iterator it_m_row;
00131     static vector<const float>::iterator it_v;
00132     //
00133     it_out = v_out.begin();
00134     for (size_t i = 0; i < m_left.size(); ++i){
00135         *it_out = 0.0;
00136         it_m_row = m_left[i].begin();
00137         it_v = v_right.begin();
00138         for (size_t j = 0; j < m_left[i].size(); ++j){
00139             // *it_out += m_left[i][j] * v_right[j];
00140             if (*it_m_row != 0.0 && *it_v != 0.0){
00141                 (*it_out) += (*it_m_row) * (*it_v);
00142             }else{
00143                 // (*it_out) += 0.0
00144             }
00145             // (*it_out) += (*it_m_row) * (*it_v);
00146             //
00147             it_m_row++;
00148             it_v++;
00149         }
00150         it_out++;
00151     }
00152 }
00153 vector<float> STATE_FEEDBACK_INTEGRAL::Mat_multiply_Vec(const vector<vector<float> > &m_left, const vector<float> &v_right){ // v_out = m_left*v_right
00154     static vector<float> v_out;
00155     // Size check
00156     if (v_out.size() != m_left.size()){
00157         v_out.resize(m_left.size());
00158     }
00159     // Iterators
00160     static vector<float>::iterator it_out;
00161     static vector<const float>::iterator it_m_row;
00162     static vector<const float>::iterator it_v;
00163     //
00164     it_out = v_out.begin();
00165     for (size_t i = 0; i < m_left.size(); ++i){
00166         *it_out = 0.0;
00167         it_m_row = m_left[i].begin();
00168         it_v = v_right.begin();
00169         for (size_t j = 0; j < m_left[i].size(); ++j){
00170             // *it_out += m_left[i][j] * v_right[j];
00171             if (*it_m_row != 0.0 && *it_v != 0.0){
00172                 (*it_out) += (*it_m_row) * (*it_v);
00173             }else{
00174                 // (*it_out) += 0.0
00175             }
00176             // (*it_out) += (*it_m_row) * (*it_v);
00177             //
00178             it_m_row++;
00179             it_v++;
00180         }
00181         it_out++;
00182     }
00183     return v_out;
00184 }
00185 vector<float> STATE_FEEDBACK_INTEGRAL::Get_VectorPlus(const vector<float> &v_a, const vector<float> &v_b, bool is_minus) // v_a + (or -) v_b
00186 {
00187     static vector<float> v_c;
00188     // Size check
00189     if (v_c.size() != v_a.size()){
00190         v_c.resize(v_a.size());
00191     }
00192     //
00193     for (size_t i = 0; i < v_a.size(); ++i){
00194         if (is_minus){
00195             v_c[i] = v_a[i] - v_b[i];
00196         }else{
00197             v_c[i] = v_a[i] + v_b[i];
00198         }
00199     }
00200     return v_c;
00201 }
00202 vector<float> STATE_FEEDBACK_INTEGRAL::Get_VectorScalarMultiply(const vector<float> &v_a, float scale) // scale*v_a
00203 {
00204     static vector<float> v_c;
00205     // Size check
00206     if (v_c.size() != v_a.size()){
00207         v_c.resize(v_a.size());
00208     }
00209     // for pure negative
00210     if (scale == -1.0){
00211         for (size_t i = 0; i < v_a.size(); ++i){
00212             v_c[i] = -v_a[i];
00213         }
00214         return v_c;
00215     }
00216     // else
00217     for (size_t i = 0; i < v_a.size(); ++i){
00218         v_c[i] = scale*v_a[i];
00219 
00220     }
00221     return v_c;
00222 }
00223 // Increment
00224 void STATE_FEEDBACK_INTEGRAL::Get_VectorIncrement(vector<float> &v_a, const vector<float> &v_b, bool is_minus){ // v_a += (or -=) v_b
00225     // Size check
00226     if (v_a.size() != v_b.size()){
00227         v_a.resize(v_b.size());
00228     }
00229     //
00230     if (is_minus){ // -=
00231         for (size_t i = 0; i < v_b.size(); ++i){
00232             v_a[i] -= v_b[i];
00233         }
00234     }else{ // +=
00235         for (size_t i = 0; i < v_b.size(); ++i){
00236             v_a[i] += v_b[i];
00237         }
00238     }
00239 
00240 }