An observer for estimating the friction-force ratio
Embed:
(wiki syntax)
Show/hide line numbers
Observer_ftRatio.cpp
00001 #include "Observer_ftRatio.h" 00002 00003 // 00004 float Dimension_ftOb[] = { 00005 #include "Dimensions_ftOb.txt" // Load the parameter Dimensions_ftOb 00006 }; 00007 // 00008 00009 Observer_ftRatio::Observer_ftRatio(float samplingTime): 00010 Ts(samplingTime), 00011 OB(Dimension_ftOb[3], Dimension_ftOb[4], Dimension_ftOb[5], 0, samplingTime), 00012 KFID_ratio_ft_right(samplingTime, 1.0, 0.0, 1.0, 1.0, 1.0, false), // For identifing the ratio_ft of the right wheel 00013 KFID_ratio_ft_left(samplingTime, 1.0, 0.0, 1.0, 1.0, 1.0, false), // For identifing the ratio_ft of the left wheel 00014 SAT_ratio_ft(Dimension_ftOb[1], 1.0, 0.0), 00015 LPF_ratio_ft(Dimension_ftOb[1], samplingTime, 10.0, 1) // fc = 10 Hz 00016 { 00017 // To enble, run *.start() function 00018 enable = false; 00019 // Dimensions of the original system 00020 n = Dimension_ftOb[0]; 00021 p = Dimension_ftOb[1]; 00022 q = Dimension_ftOb[2]; 00023 // Dimensions of the observer 00024 n_OB = Dimension_ftOb[3]; 00025 p_OB = Dimension_ftOb[4]; 00026 q_OB = Dimension_ftOb[5]; 00027 n_ME_OB = Dimension_ftOb[6]; // Number of the measurement errors in the PI observer 00028 // 00029 zeros_n.assign(n, 0.0); 00030 zeros_p.assign(p, 0.0); 00031 zeros_q.assign(q, 0.0); 00032 zeros_n_ME_OB.assign(n_ME_OB, 0.0); 00033 // 00034 ones_p.assign(p, 1.0); 00035 00036 // Assign parameters 00037 // init(); 00038 00039 // Indexes 00040 // idx_x_real = 0; // Index for x_real_est 00041 idx_x_ns = n; 00042 idx_ft_error_est = 2*n; // Index for ft_error_est 00043 00044 // Input signals 00045 yc = zeros_p; 00046 // States 00047 ft_est = zeros_p; 00048 ft_error_est = zeros_p; 00049 ft_ideal_est = zeros_p; 00050 // 00051 x_real_est = zeros_n; // Estimation of the real states 00052 // x_ns_est = zeros_n; // Estimation of the states of the no-slip system 00053 00054 // The estimation of measurement errors in PI observer 00055 Measurement_error_est = zeros_n_ME_OB; 00056 00057 // The degree of no-slip 00058 // ratio_ft = zeros_p; 00059 ratio_ft = ones_p; // No slip 00060 ratio_ft_filtered = ones_p; // No slip 00061 00062 } 00063 void Observer_ftRatio::start(){ 00064 enable = true; 00065 // 00066 OB.start(); 00067 } 00068 void Observer_ftRatio::pause(){ 00069 enable = false; 00070 // 00071 OB.enable = false; 00072 } 00073 void Observer_ftRatio::stop(){ 00074 if (!enable){ 00075 return; 00076 } 00077 enable = false; 00078 // Reset 00079 reset(); 00080 00081 // 00082 OB.stop(); 00083 } 00084 void Observer_ftRatio::reset(){ 00085 // 00086 // Input signals 00087 yc = zeros_p; 00088 // States 00089 ft_est = zeros_p; 00090 ft_error_est = zeros_p; 00091 ft_ideal_est = zeros_p; 00092 // 00093 x_real_est = zeros_n; // Estimation of the real states 00094 // x_ns_est = zeros_n; // Estimation of the states of the no-slip system 00095 00096 // The estimation of measurement errors in PI observer 00097 Measurement_error_est.assign(2, 0.0); 00098 00099 // Reset the identifier for ratio_ft(s) 00100 KFID_ratio_ft_right.reset(1.0); // 1.0 for no-slip (full friction-force) 00101 KFID_ratio_ft_left.reset(1.0); // 1.0 for no-slip (full friction-force) 00102 00103 // The degree of no-slip 00104 // ratio_ft = zeros_p; 00105 ratio_ft = ones_p; // No slip 00106 ratio_ft_filtered = ones_p;// No slip 00107 00108 // Reset the low-pass filter for ratio_ft 00109 LPF_ratio_ft.reset(ratio_ft); 00110 00111 // 00112 OB.reset(); 00113 } 00114 // 00115 void Observer_ftRatio::init(void){ 00116 // 00117 float _A_ftOb[] = { 00118 #include "A_ftOb.txt" // Load the parameter A_ftOb 00119 }; 00120 float _By_ftOb[] = { 00121 #include "By_ftOb.txt" // Load the parameter By_ftOb 00122 }; 00123 /* 00124 float _Bv1_ftOb[] = { 00125 #include "Bv1_ftOb.txt" // Load the parameter Bv1_ftOb 00126 }; 00127 */ 00128 float _C_ftOb[] = { 00129 #include "C_ftOb.txt" // Load the parameter C_ftOb 00130 }; 00131 // 00132 float _F1_ftOb[] = { 00133 #include "F1_ftOb.txt" // Load the parameter F1_ftOb 00134 }; 00135 float _F2_ftOb[] = { 00136 #include "F2_ftOb.txt" // Load the parameter F2_ftOb 00137 }; 00138 // 00139 float _L_ftOb[] = { 00140 #include "L_ftOb.txt" // Load the parameter L_ftOb 00141 }; 00142 // 00143 OB.assign_At(_A_ftOb, n_OB); 00144 OB.assign_Bt(_By_ftOb, n_OB, p_OB); 00145 OB.assign_Cd(_C_ftOb, q_OB, n_OB); 00146 // 00147 OB.assign_Lt(_L_ftOb, n_OB, 0, q_OB); 00148 00149 //-------------------------// 00150 // assign_Matrix(Bv1_ftOb, _Bv1_ftOb, n_OB, p_OB); 00151 // 00152 assign_Matrix(F1_ftOb, _F1_ftOb, p, n); 00153 assign_Matrix(F2_ftOb, _F2_ftOb, p, p); 00154 00155 } 00156 00157 // 00158 void Observer_ftRatio::iterateOnce(const vector<float> &yc_in, const vector<float> &x_real_in){ 00159 if(!enable){ 00160 return; 00161 } 00162 // Insert values 00163 insertSignals(yc_in, x_real_in); 00164 00165 // Progress one time slot 00166 OB.iterateOnce(); 00167 00168 00169 // Clean up the estimation of w_ideal_avg and w_ideal_delta 00170 00171 00172 /* 00173 // for test 00174 return; 00175 // 00176 */ 00177 00178 00179 // Get the results 00180 //--------------------------// 00181 size_t idx = 0; 00182 // x_real_est 00183 for (size_t i = 0; i < n; ++i){ 00184 x_real_est[i] = OB.state_est[idx]; 00185 // 00186 idx++; 00187 } 00188 00189 // ft_est 00190 for (size_t i = 0; i < p; ++i){ 00191 ft_est[i] = OB.state_est[idx]; 00192 // 00193 idx++; 00194 } 00195 00196 // Measurement_error_est 00197 for (size_t i = 0; i < n_ME_OB; ++i){ 00198 Measurement_error_est[i] = OB.state_est[idx]; 00199 // 00200 idx++; 00201 } 00202 //--------------------------// 00203 00204 00205 00206 // ft_ideal_est 00207 // ft_ideal_est = F1_ftOb*x_real_est + F2_ftOb*yc 00208 ft_ideal_est = Mat_multiply_Vec(F1_ftOb, x_real_est); 00209 Get_VectorIncrement(ft_ideal_est, Mat_multiply_Vec(F2_ftOb, yc), false); // += 00210 00211 // ft_error_est = ft_est - ft_ideal_est; 00212 ft_error_est = Get_VectorPlus(ft_est, ft_ideal_est, true); // minus 00213 00214 /* 00215 // The ratio_ft 00216 for (size_t i = 0; i < p; ++i){ 00217 // 00218 if (ft_ideal_est[i] < NUM_THRESHOLD && (-ft_ideal_est[i]) < NUM_THRESHOLD){ 00219 ratio_ft[i] = 1.0; // no-slip 00220 }else{ 00221 ratio_ft[i] = ft_est[i]/ft_ideal_est[i]; 00222 } 00223 // Saturation 00224 if (ratio_ft[i] > 1.0){ 00225 ratio_ft[i] = 1.0; 00226 }else if (ratio_ft[i] < 0.0){ 00227 ratio_ft[i] = 0.0; 00228 } 00229 // 00230 } 00231 */ 00232 00233 // Identify the ratio_ft 00234 //----------------------------------------------------// 00235 /* 00236 // Transform from (rifht, left) to (average, delta) 00237 T_ft_est.assign_R_L(ft_est); 00238 T_ft_ideal_est.assign_R_L(ft_ideal_est); 00239 00240 // ratio_ft_avg 00241 KFID_ratio_ft_avg.C = T_ft_ideal_est.get_avg(); 00242 ratio_ft[0] = KFID_ratio_ft_avg.filter(0.0, T_ft_est.get_avg()); 00243 // ratio_ft_delta 00244 KFID_ratio_ft_delta.C = T_ft_ideal_est.get_delta(); 00245 ratio_ft[1] = KFID_ratio_ft_delta.filter(0.0, T_ft_est.get_delta()); 00246 */ 00247 00248 // Right-side ratio 00249 KFID_ratio_ft_right.C = ft_ideal_est[0]; 00250 ratio_ft[0] = KFID_ratio_ft_right.filter(0.0, ft_est[0] ); 00251 // Left-side ratio 00252 KFID_ratio_ft_left.C = ft_ideal_est[1]; 00253 ratio_ft[1] = KFID_ratio_ft_left.filter(0.0, ft_est[1] ); 00254 00255 // Saturation 00256 ratio_ft = SAT_ratio_ft.filter(ratio_ft); 00257 //----------------------------------------------------// 00258 00259 // Filtering 00260 ratio_ft_filtered = LPF_ratio_ft.filter(ratio_ft); 00261 00262 } 00263 00264 // Private methods 00265 // 00266 void Observer_ftRatio::insertSignals(const vector<float> &yc_in, const vector<float> &x_real_in){ 00267 // 00268 yc = yc_in; 00269 // Insert values 00270 OB.sys_inputs = yc_in; 00271 OB.sys_outputs = x_real_in; 00272 // 00273 // OB.sys_extraDisturbance = Mat_multiply_Vec(Bv1_ftOb, v1_in); 00274 } 00275 00276 00277 00278 // Utilities 00279 void Observer_ftRatio::Mat_multiply_Vec(vector<float> &v_out, const vector<vector<float> > &m_left, const vector<float> &v_right){ // v_out = m_left*v_right 00280 00281 // Size check 00282 if (v_out.size() != m_left.size()){ 00283 v_out.resize(m_left.size()); 00284 } 00285 00286 /* 00287 // Iterators 00288 static vector<float>::iterator it_out; 00289 static vector<float>::iterator it_m_row; 00290 static vector<float>::iterator it_v; 00291 00292 // 00293 it_out = v_out.begin(); 00294 for (size_t i = 0; i < m_left.size(); ++i){ 00295 *it_out = 0.0; 00296 it_m_row = vector<vector<float> >(m_left)[i].begin(); 00297 it_v = vector<float>(v_right).begin(); 00298 for (size_t j = 0; j < m_left[i].size(); ++j){ 00299 // *it_out += m_left[i][j] * v_right[j]; 00300 if (*it_m_row != 0.0 && *it_v != 0.0){ 00301 (*it_out) += (*it_m_row) * (*it_v); 00302 }else{ 00303 // (*it_out) += 0.0 00304 } 00305 // (*it_out) += (*it_m_row) * (*it_v); 00306 // 00307 it_m_row++; 00308 it_v++; 00309 } 00310 it_out++; 00311 } 00312 */ 00313 00314 // Indexing 00315 for (size_t i = 0; i < m_left.size(); ++i){ 00316 // 00317 v_out[i] = 0.0; 00318 // 00319 for (size_t j = 0; j < v_right.size(); ++j){ 00320 if (m_left[i][j] != 0.0 && v_right[j] != 0.0) 00321 v_out[i] += m_left[i][j]*v_right[j]; 00322 } 00323 } 00324 00325 } 00326 vector<float> Observer_ftRatio::Mat_multiply_Vec(const vector<vector<float> > &m_left, const vector<float> &v_right){ // v_out = m_left*v_right 00327 static vector<float> v_out; 00328 // Size check 00329 if (v_out.size() != m_left.size()){ 00330 v_out.resize(m_left.size()); 00331 } 00332 00333 /* 00334 // Iterators 00335 static vector<float>::iterator it_out; 00336 static vector<float>::iterator it_m_row; 00337 static vector<float>::iterator it_v; 00338 // 00339 it_out = v_out.begin(); 00340 for (size_t i = 0; i < m_left.size(); ++i){ 00341 *it_out = 0.0; 00342 it_m_row = vector<vector<float> >(m_left)[i].begin(); 00343 it_v = vector<float>(v_right).begin(); 00344 for (size_t j = 0; j < m_left[i].size(); ++j){ 00345 // *it_out += m_left[i][j] * v_right[j]; 00346 if (*it_m_row != 0.0 && *it_v != 0.0){ 00347 (*it_out) += (*it_m_row) * (*it_v); 00348 }else{ 00349 // (*it_out) += 0.0 00350 } 00351 // (*it_out) += (*it_m_row) * (*it_v); 00352 // 00353 it_m_row++; 00354 it_v++; 00355 } 00356 it_out++; 00357 } 00358 */ 00359 00360 // Indexing 00361 for (size_t i = 0; i < m_left.size(); ++i){ 00362 // 00363 v_out[i] = 0.0; 00364 // 00365 for (size_t j = 0; j < v_right.size(); ++j){ 00366 if (m_left[i][j] != 0.0 && v_right[j] != 0.0) 00367 v_out[i] += m_left[i][j]*v_right[j]; 00368 } 00369 } 00370 00371 return v_out; 00372 } 00373 vector<float> Observer_ftRatio::Get_VectorPlus(const vector<float> &v_a, const vector<float> &v_b, bool is_minus) // v_a + (or -) v_b 00374 { 00375 static vector<float> v_c; 00376 // Size check 00377 if (v_c.size() != v_a.size()){ 00378 v_c.resize(v_a.size()); 00379 } 00380 // 00381 for (size_t i = 0; i < v_a.size(); ++i){ 00382 if (is_minus){ 00383 v_c[i] = v_a[i] - v_b[i]; 00384 }else{ 00385 v_c[i] = v_a[i] + v_b[i]; 00386 } 00387 } 00388 return v_c; 00389 } 00390 vector<float> Observer_ftRatio::Get_VectorScalarMultiply(const vector<float> &v_a, float scale) // scale*v_a 00391 { 00392 static vector<float> v_c; 00393 // Size check 00394 if (v_c.size() != v_a.size()){ 00395 v_c.resize(v_a.size()); 00396 } 00397 // for pure negative 00398 if (scale == -1.0){ 00399 for (size_t i = 0; i < v_a.size(); ++i){ 00400 v_c[i] = -v_a[i]; 00401 } 00402 return v_c; 00403 } 00404 // else 00405 for (size_t i = 0; i < v_a.size(); ++i){ 00406 v_c[i] = scale*v_a[i]; 00407 00408 } 00409 return v_c; 00410 } 00411 // Increment 00412 void Observer_ftRatio::Get_VectorIncrement(vector<float> &v_a, const vector<float> &v_b, bool is_minus){ // v_a += (or -=) v_b 00413 // Size check 00414 if (v_a.size() != v_b.size()){ 00415 v_a.resize(v_b.size()); 00416 } 00417 // 00418 if (is_minus){ // -= 00419 for (size_t i = 0; i < v_b.size(); ++i){ 00420 v_a[i] -= v_b[i]; 00421 } 00422 }else{ // += 00423 for (size_t i = 0; i < v_b.size(); ++i){ 00424 v_a[i] += v_b[i]; 00425 } 00426 } 00427 00428 } 00429 00430 // 00431 void Observer_ftRatio::assign_Matrix(vector<vector<float> > &M, float* M_in, size_t m_in, size_t n_in){ // M_in is a m_in by n_in array 00432 // M_in is the pointer of a mutidimentional array with size m_in by n_in 00433 M.resize(m_in, vector<float>(n_in, 0.0)); 00434 // 00435 for (size_t i = 0; i < m_in; ++i){ 00436 for (size_t j = 0; j < n_in; ++j){ 00437 // M[i][j] = M_in[i][j]; 00438 M[i][j] = *M_in; 00439 M_in++; 00440 } 00441 } 00442 }
Generated on Thu Jul 14 2022 22:46:42 by
1.7.2