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

STATE_FEEDBACK.cpp

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