The library for full-state feedback control with integral action
Fork of STATE_FEEDBACK_INTEGRAL by
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 }
Generated on Tue Jul 26 2022 05:38:02 by
1.7.2
