The generic full-state feedback control libery
Fork of STATE_FEEDBACK by
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 }
Generated on Wed Jul 20 2022 10:49:47 by
1.7.2
