A controller that is immune to measurement errors and keep the true states at the desired value, also known as "Zero-Torque Control"
MEASUREMENT_ERROR_ADAPTATION_CONTROL.cpp
00001 #include "MEASUREMENT_ERROR_ADAPTATION_CONTROL.h" 00002 // The controller is for the plant with (p, n, q) system 00003 // Dimensions: 00004 // 00005 // Inputs, u | States, x 00006 // p ----> n 00007 // q -- ^ 00008 // Measurement errors, phi 00009 00010 MEASUREMENT_ERROR_ADAPTATION_CONTROL::MEASUREMENT_ERROR_ADAPTATION_CONTROL(size_t num_state, size_t num_in, size_t num_MeasurementError, float samplingTime): 00011 n(num_state), p(num_in), q(num_MeasurementError), Ts(samplingTime), 00012 C_error(num_state, vector<float>(num_MeasurementError,0.0)), 00013 K_full(num_in, vector<float>(num_state,0.0)), 00014 K_phi(num_MeasurementError, vector<float>(num_state,0.0)), 00015 N_xd(num_state, vector<float>(num_in, 0.0)), 00016 N_ud(num_in, vector<float>(num_in, 0.0)) 00017 { 00018 // 00019 zeros_n.assign(n, 0.0); 00020 zeros_p.assign(p, 0.0); 00021 zeros_q.assign(q, 0.0); 00022 00023 // States 00024 states_est = zeros_n; // states_est, "x_est" 00025 sys_inputs = zeros_p; // The inputs of the plant, "u" 00026 sys_output = zeros_n; // The output of the plant, "y" 00027 MeasurementErrors = zeros_q; // The measurement error of the sensors, "phi" 00028 00029 // Command (equalibrium state) 00030 states_d = zeros_n; // x_d 00031 inputs_d = zeros_p; // u_d 00032 command = zeros_p; // r 00033 00034 00035 } 00036 // Assign Parameters 00037 void MEASUREMENT_ERROR_ADAPTATION_CONTROL::assign_C_error(float* C_error_in, size_t n_in, size_t q_in){ 00038 // C_error_in is the pointer of a mutidimentional array with size n_in by q_in 00039 if (n != n_in || q != q_in){ 00040 n = n_in; 00041 q = q_in; 00042 zeros_n.resize(n, 0.0); 00043 zeros_q.resize(q, 0.0); 00044 C_error.assign(n, zeros_q); 00045 } 00046 // 00047 for (size_t i = 0; i < n; ++i){ 00048 for (size_t j = 0; j < q; ++j){ 00049 // C_error[i][j] = C_error_in[i][j]; 00050 C_error[i][j] = *C_error_in; 00051 C_error_in++; 00052 } 00053 } 00054 } 00055 void MEASUREMENT_ERROR_ADAPTATION_CONTROL::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 p = p_in; 00059 n = n_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 MEASUREMENT_ERROR_ADAPTATION_CONTROL::assign_K_phi(float* K_phi_in, size_t q_in, size_t n_in){ 00074 // K_phi_in is the pointer of a mutidimentional array with size q_in by n_in 00075 if (q != q_in || n != n_in){ 00076 q = q_in; 00077 n = n_in; 00078 zeros_q.resize(q, 0.0); 00079 zeros_n.resize(n, 0.0); 00080 K_phi.assign(q, zeros_n); 00081 } 00082 // 00083 for (size_t i = 0; i < q; ++i){ 00084 for (size_t j = 0; j < n; ++j){ 00085 // K_phi[i][j] = K_phi_in[i][j]; 00086 K_phi[i][j] = *K_phi_in; 00087 K_phi_in++; 00088 } 00089 } 00090 } 00091 // 00092 void MEASUREMENT_ERROR_ADAPTATION_CONTROL::assign_N_xd(float* N_xd_in, size_t n_in, size_t p_in){ 00093 // N_xd_in is the pointer of a mutidimentional array with size n_in by p_in 00094 if (n != n_in || p != p_in){ 00095 n = n_in; 00096 p = p_in; 00097 zeros_n.resize(n, 0.0); 00098 zeros_p.resize(p, 0.0); 00099 N_xd.assign(n, zeros_p); 00100 } 00101 // 00102 for (size_t i = 0; i < n; ++i){ 00103 for (size_t j = 0; j < p; ++j){ 00104 // N_xd[i][j] = N_xd_in[i][j]; 00105 N_xd[i][j] = *N_xd_in; 00106 N_xd_in++; 00107 } 00108 } 00109 } 00110 void MEASUREMENT_ERROR_ADAPTATION_CONTROL::assign_N_ud(float* N_ud_in, size_t p_in){ 00111 // N_ud_in is the pointer of a mutidimentional array with size p_in by p_in 00112 if (p != p_in){ 00113 p = p_in; 00114 zeros_p.resize(p, 0.0); 00115 N_ud.assign(p, zeros_p); 00116 } 00117 // 00118 for (size_t i = 0; i < p; ++i){ 00119 for (size_t j = 0; j < p; ++j){ 00120 // N_ud[i][j] = N_ud_in[i][j]; 00121 N_ud[i][j] = *N_ud_in; 00122 N_ud_in++; 00123 } 00124 } 00125 } 00126 // 00127 void MEASUREMENT_ERROR_ADAPTATION_CONTROL::iterateOnce(bool enable){ 00128 //----------------------------// 00129 // Input: sys_output ("y"), command ("r") 00130 // Get: sys_inputs ("u") 00131 //----------------------------// 00132 00133 // Control law 00134 if (enable){ 00135 // 00136 get_inputs_compensate(); // Get states_d, inputs_d from command 00137 00138 // states_est = (sys_output - C_error*MeasurementErrors) - states_d 00139 get_states_est(); 00140 00141 // sys_inputs = inputs_d - K_full*states_est 00142 sys_inputs = Get_VectorPlus(inputs_d, Mat_multiply_Vec(K_full, states_est),true); // minus 00143 }else{ 00144 sys_inputs = zeros_p; 00145 } 00146 00147 // Integral action 00148 get_MeasurementErrors_est(enable); 00149 } 00150 00151 // Private functions 00152 // Command (equalibrium state) related calculation 00153 void MEASUREMENT_ERROR_ADAPTATION_CONTROL::get_inputs_compensate(void){ // Calculate the compensation variable, states_d and sys_inputs_compensate 00154 // 00155 Mat_multiply_Vec(states_d, N_xd, command); 00156 Mat_multiply_Vec(inputs_d, N_ud, command); 00157 } 00158 // Calculate the states_est 00159 void MEASUREMENT_ERROR_ADAPTATION_CONTROL::get_states_est(void){ // Calculate the sys_outputs from states_est, by mutiplying C_error 00160 // states_est = (sys_output - C_error*MeasurementErrors) - states_d 00161 states_est = Get_VectorPlus(Get_VectorPlus(sys_output, Mat_multiply_Vec(C_error, MeasurementErrors), true), states_d, true); // minus 00162 } 00163 // Calculate the estimation of MeasurementErrors 00164 void MEASUREMENT_ERROR_ADAPTATION_CONTROL::get_MeasurementErrors_est(bool enable){ // Calculate the MeasurementErrors 00165 // 00166 // Integral action 00167 // MeasurementErrors -= Ts*(K_phi*states_est) 00168 if (enable){ 00169 Get_VectorIncrement(MeasurementErrors, Get_VectorScalarMultiply(Mat_multiply_Vec(K_phi,states_est), Ts) , true); // -= 00170 }else{ 00171 MeasurementErrors = zeros_q; 00172 } 00173 } 00174 // Utilities 00175 void MEASUREMENT_ERROR_ADAPTATION_CONTROL::Mat_multiply_Vec(vector<float> &v_out, const vector<vector<float> > &m_left, const vector<float> &v_right){ // v_out = m_left*v_right 00176 static vector<float>::iterator it_out; 00177 static vector<const float>::iterator it_m_row; 00178 static vector<const float>::iterator it_v; 00179 // 00180 it_out = v_out.begin(); 00181 for (size_t i = 0; i < m_left.size(); ++i){ 00182 *it_out = 0.0; 00183 it_m_row = m_left[i].begin(); 00184 it_v = v_right.begin(); 00185 for (size_t j = 0; j < m_left[i].size(); ++j){ 00186 // *it_out += m_left[i][j] * v_right[j]; 00187 if (*it_m_row != 0.0 && *it_v != 0.0){ 00188 (*it_out) += (*it_m_row) * (*it_v); 00189 }else{ 00190 // (*it_out) += 0.0 00191 } 00192 // (*it_out) += (*it_m_row) * (*it_v); 00193 // 00194 it_m_row++; 00195 it_v++; 00196 } 00197 it_out++; 00198 } 00199 } 00200 vector<float> MEASUREMENT_ERROR_ADAPTATION_CONTROL::Mat_multiply_Vec(const vector<vector<float> > &m_left, const vector<float> &v_right){ // v_out = m_left*v_right 00201 static vector<float> v_out; 00202 // Size check 00203 if (v_out.size() != m_left.size()){ 00204 v_out.resize(m_left.size()); 00205 } 00206 // Iterators 00207 static vector<float>::iterator it_out; 00208 static vector<const float>::iterator it_m_row; 00209 static vector<const float>::iterator it_v; 00210 // 00211 it_out = v_out.begin(); 00212 for (size_t i = 0; i < m_left.size(); ++i){ 00213 *it_out = 0.0; 00214 it_m_row = m_left[i].begin(); 00215 it_v = v_right.begin(); 00216 for (size_t j = 0; j < m_left[i].size(); ++j){ 00217 // *it_out += m_left[i][j] * v_right[j]; 00218 if (*it_m_row != 0.0 && *it_v != 0.0){ 00219 (*it_out) += (*it_m_row) * (*it_v); 00220 }else{ 00221 // (*it_out) += 0.0 00222 } 00223 // (*it_out) += (*it_m_row) * (*it_v); 00224 // 00225 it_m_row++; 00226 it_v++; 00227 } 00228 it_out++; 00229 } 00230 return v_out; 00231 } 00232 vector<float> MEASUREMENT_ERROR_ADAPTATION_CONTROL::Get_VectorPlus(const vector<float> &v_a, const vector<float> &v_b, bool is_minus) // v_a + (or -) v_b 00233 { 00234 static vector<float> v_c; 00235 // Size check 00236 if (v_c.size() != v_a.size()){ 00237 v_c.resize(v_a.size()); 00238 } 00239 // 00240 for (size_t i = 0; i < v_a.size(); ++i){ 00241 if (is_minus){ 00242 v_c[i] = v_a[i] - v_b[i]; 00243 }else{ 00244 v_c[i] = v_a[i] + v_b[i]; 00245 } 00246 } 00247 return v_c; 00248 } 00249 vector<float> MEASUREMENT_ERROR_ADAPTATION_CONTROL::Get_VectorScalarMultiply(const vector<float> &v_a, float scale) // scale*v_a 00250 { 00251 static vector<float> v_c; 00252 // Size check 00253 if (v_c.size() != v_a.size()){ 00254 v_c.resize(v_a.size()); 00255 } 00256 // for pure negative 00257 if (scale == -1.0){ 00258 for (size_t i = 0; i < v_a.size(); ++i){ 00259 v_c[i] = -v_a[i]; 00260 } 00261 return v_c; 00262 } 00263 // else 00264 for (size_t i = 0; i < v_a.size(); ++i){ 00265 v_c[i] = scale*v_a[i]; 00266 00267 } 00268 return v_c; 00269 } 00270 // Increment 00271 void MEASUREMENT_ERROR_ADAPTATION_CONTROL::Get_VectorIncrement(vector<float> &v_a, const vector<float> &v_b, bool is_minus){ // v_a += (or -=) v_b 00272 // Size check 00273 if (v_a.size() != v_b.size()){ 00274 v_a.resize(v_b.size()); 00275 } 00276 // 00277 if (is_minus){ // -= 00278 for (size_t i = 0; i < v_b.size(); ++i){ 00279 v_a[i] -= v_b[i]; 00280 } 00281 }else{ // += 00282 for (size_t i = 0; i < v_b.size(); ++i){ 00283 v_a[i] += v_b[i]; 00284 } 00285 } 00286 00287 }
Generated on Tue Aug 2 2022 05:40:37 by
1.7.2