An observer that can estimate the states from delayed signals
Fork of STATES_OBSERVER_DELAY by
STATES_OBSERVER_DELAY.cpp
00001 #include "STATES_OBSERVER_DELAY.h" 00002 00003 STATES_OBSERVER_DELAY::STATES_OBSERVER_DELAY(size_t num_state, size_t num_in, size_t num_out, size_t delay_sample, float samplingTime): 00004 n(num_state), p(num_in), q(num_out), d(delay_sample), Ts(samplingTime), 00005 states_est_buffer((delay_sample+1),num_state) 00006 { 00007 // To enble, run *.start() function 00008 enable = false; 00009 flag_reset = false; 00010 // 00011 zeros_n.assign(n, 0.0); 00012 zeros_p.assign(p, 0.0); 00013 zeros_q.assign(q, 0.0); 00014 // Parameters for observer 00015 Ad.assign(n,zeros_n); 00016 Bd.assign(n,zeros_p); 00017 if (num_out == 0){ 00018 enable_Cd = false; // Default: not using Cd 00019 q = n; 00020 zeros_q.resize(q, 0.0); 00021 Cd.assign(q,zeros_n); 00022 }else{ 00023 enable_Cd = true; // using Cd 00024 Cd.assign(q,zeros_n); 00025 } 00026 // Gain matrix 00027 Ld.assign(n*(d+1),zeros_q); // Ld is an n(d+1) by q matrix (or n(d+1) by n if Cd is not used) 00028 00029 // Input-signals of the observer 00030 sys_inputs = zeros_p; 00031 sys_outputs = zeros_q; 00032 sys_extraDisturbance = zeros_n; 00033 // Variables of the observer 00034 states_est_buffer.Reset(zeros_n); 00035 state_est = zeros_n; 00036 // states_est_delay_d = zeros_n; 00037 y_est = zeros_q; 00038 est_error = zeros_q; 00039 } 00040 void STATES_OBSERVER_DELAY::start(){ 00041 enable = true; 00042 } 00043 void STATES_OBSERVER_DELAY::stop(){ 00044 if (!enable){ 00045 return; 00046 } 00047 enable = false; 00048 flag_reset = true; 00049 } 00050 void STATES_OBSERVER_DELAY::reset(){ 00051 // 00052 // Input-signals of the observer 00053 sys_inputs = zeros_p; 00054 sys_outputs = zeros_q; 00055 // Variables of the observer 00056 states_est_buffer.Reset(zeros_n); 00057 state_est = zeros_n; 00058 // states_est_delay_d = zeros_n; 00059 y_est = zeros_q; 00060 est_error = zeros_q; 00061 } 00062 00063 00064 // Assignments for observer 00065 // Assign continuous-time version of system matrices 00066 void STATES_OBSERVER_DELAY::assign_At(float* At_in, size_t n_in){ // Continuous-time version 00067 // At_in is the pointer of a mutidimentional array with size n_in by n_in 00068 if (n != n_in){ 00069 n = n_in; 00070 zeros_n.resize(n, 0.0); 00071 Ad.assign(n, zeros_n); 00072 } 00073 // 00074 for (size_t i = 0; i < n; ++i){ 00075 for (size_t j = 0; j < n; ++j){ 00076 // Ad[i][j] = At_in[i][j]; 00077 Ad[i][j] = (*At_in)*Ts; 00078 // 00079 if (i == j){ 00080 Ad[i][j] += 1.0; 00081 } 00082 // 00083 At_in++; 00084 } 00085 } 00086 } 00087 void STATES_OBSERVER_DELAY::assign_Bt(float* Bt_in, size_t n_in, size_t p_in){ // Continuous-time version 00088 // Bt_in is the pointer of a mutidimentional array with size n_in by p_in 00089 if (n != n_in || p != p_in){ 00090 n = n_in; 00091 p = p_in; 00092 zeros_n.resize(n, 0.0); 00093 zeros_p.resize(p, 0.0); 00094 Bd.assign(n, zeros_p); 00095 } 00096 // 00097 for (size_t i = 0; i < n; ++i){ 00098 for (size_t j = 0; j < p; ++j){ 00099 // Bd[i][j] = Bt_in[i][j]; 00100 Bd[i][j] = (*Bt_in)*Ts; 00101 Bt_in++; 00102 } 00103 } 00104 } 00105 void STATES_OBSERVER_DELAY::assign_Lt(float* Lt_in, size_t n_in, size_t d_in, size_t q_in){ // Continuous-time version 00106 // Lt_in is the pointer of a mutidimentional array with size n_in*(d_in+1) by q_in 00107 if (n != n_in || d != d_in || q != q_in){ 00108 n = n_in; 00109 q = q_in; 00110 zeros_n.resize(n, 0.0); 00111 zeros_q.resize(q, 0.0); 00112 // 00113 if (d != d_in){ 00114 d = d_in; 00115 states_est_buffer.Init((d+1), zeros_n); 00116 } 00117 Ld.assign(n*(d+1), zeros_q); 00118 } 00119 // 00120 for (size_t i = 0; i < n*(d+1); ++i){ 00121 for (size_t j = 0; j < q; ++j){ 00122 // Ld[i][j] = Lt_in[i][j]; 00123 Ld[i][j] = (*Lt_in)*Ts; 00124 Lt_in++; 00125 } 00126 } 00127 } 00128 // ** Assign the continuous-time version of system matrices by matrices 00129 void STATES_OBSERVER_DELAY::assign_At(const vector<vector<float> > &At_in){ 00130 size_t n_in = At_in.size(); 00131 if (n != n_in){ 00132 n = n_in; 00133 zeros_n.resize(n, 0.0); 00134 Ad.assign(n, zeros_n); 00135 } 00136 // Ad 00137 for (size_t i = 0; i < n; ++i){ 00138 for (size_t j = 0; j < n; ++j){ 00139 // 00140 Ad[i][j] = Ts*At_in[i][j]; 00141 // 00142 if (i == j){ 00143 Ad[i][j] += 1.0; 00144 } 00145 } 00146 } 00147 } 00148 void STATES_OBSERVER_DELAY::assign_Bt(const vector<vector<float> > &Bt_in){ 00149 size_t n_in = Bt_in.size(); 00150 size_t p_in = Bt_in[0].size(); 00151 if (n != n_in || p != p_in){ 00152 n = n_in; 00153 p = p_in; 00154 zeros_n.resize(n, 0.0); 00155 zeros_p.resize(p, 0.0); 00156 Bd.assign(n, zeros_p); 00157 } 00158 // Bd 00159 for (size_t i = 0; i < n; ++i){ 00160 for (size_t j = 0; j < p; ++j){ 00161 // Bd[i][j] = Bt_in[i][j]; 00162 Bd[i][j] = Ts*Bt_in[i][j]; 00163 } 00164 } 00165 } 00166 void STATES_OBSERVER_DELAY::assign_Lt(const vector<vector<float> > &Lt_in){ 00167 size_t ndp1_in = Lt_in.size(); 00168 size_t q_in = Lt_in[0].size(); 00169 // 00170 if ( n*(d+1) != ndp1_in || q != q_in){ 00171 n = ndp1_in/(d+1); 00172 q = q_in; 00173 zeros_n.resize(n, 0.0); 00174 zeros_q.resize(q, 0.0); 00175 Ld.assign(n*(d+1), zeros_q); 00176 } 00177 // Ld 00178 for (size_t i = 0; i < n*(d+1); ++i){ 00179 for (size_t j = 0; j < q; ++j){ 00180 // 00181 Ld[i][j] = Ts*Lt_in[i][j]; 00182 } 00183 } 00184 } 00185 // Assign discrete-time version of system matrices directly 00186 void STATES_OBSERVER_DELAY::assign_Ad(float* Ad_in, size_t n_in){ // Discrete-time version 00187 // Ad_in is the pointer of a mutidimentional array with size n_in by n_in 00188 if (n != n_in){ 00189 n = n_in; 00190 zeros_n.resize(n, 0.0); 00191 Ad.assign(n, zeros_n); 00192 } 00193 // 00194 for (size_t i = 0; i < n; ++i){ 00195 for (size_t j = 0; j < n; ++j){ 00196 // Ad[i][j] = Ad_in[i][j]; 00197 Ad[i][j] = *Ad_in; 00198 Ad_in++; 00199 } 00200 } 00201 } 00202 void STATES_OBSERVER_DELAY::assign_Bd(float* Bd_in, size_t n_in, size_t p_in){ // Discrete-time version 00203 // Bd_in is the pointer of a mutidimentional array with size n_in by p_in 00204 if (n != n_in || p != p_in){ 00205 n = n_in; 00206 p = p_in; 00207 zeros_n.resize(n, 0.0); 00208 zeros_p.resize(p, 0.0); 00209 Bd.assign(n, zeros_p); 00210 } 00211 // 00212 for (size_t i = 0; i < n; ++i){ 00213 for (size_t j = 0; j < p; ++j){ 00214 // Bd[i][j] = Bd_in[i][j]; 00215 Bd[i][j] = *Bd_in; 00216 Bd_in++; 00217 } 00218 } 00219 } 00220 // 00221 void STATES_OBSERVER_DELAY::assign_Cd(float* Cd_in, size_t q_in, size_t n_in){ 00222 // Cd_in is the pointer of a mutidimentional array with size q_in by n_in 00223 // q = n if not using Cd 00224 if (q_in == 0){ 00225 enable_Cd = false; // Default: not using Cd 00226 q_in = n_in; 00227 // 00228 q = q_in; 00229 n = n_in; 00230 zeros_n.resize(n,0.0); 00231 zeros_q.resize(q, 0.0); 00232 Cd.assign(q,zeros_n); 00233 // 00234 return; 00235 }else{ 00236 enable_Cd = true; // using Cd 00237 // Cd.assign(q,zeros_n); 00238 } 00239 00240 if (n != n_in || q != q_in){ 00241 n = n_in; 00242 q = q_in; 00243 zeros_n.resize(n, 0.0); 00244 zeros_q.resize(q, 0.0); 00245 Cd.assign(q, zeros_n); 00246 } 00247 // 00248 for (size_t i = 0; i < q; ++i){ 00249 for (size_t j = 0; j < n; ++j){ 00250 // Cd[i][j] = Cd_in[i][j]; 00251 Cd[i][j] = *Cd_in; 00252 Cd_in++; 00253 } 00254 } 00255 00256 } 00257 // Assignment for observer Gain 00258 void STATES_OBSERVER_DELAY::assign_Ld(float* Ld_in, size_t n_in, size_t d_in, size_t q_in){ 00259 // Ld_in is the pointer of a mutidimentional array with size n_in*(d_in+1) by q_in 00260 if (n != n_in || d != d_in || q != q_in){ 00261 n = n_in; 00262 q = q_in; 00263 zeros_n.resize(n, 0.0); 00264 zeros_q.resize(q, 0.0); 00265 // 00266 if (d != d_in){ 00267 d = d_in; 00268 states_est_buffer.Init((d+1), zeros_n); 00269 } 00270 Ld.assign(n*(d+1), zeros_q); 00271 } 00272 // 00273 for (size_t i = 0; i < n*(d+1); ++i){ 00274 for (size_t j = 0; j < q; ++j){ 00275 // Ld[i][j] = Ld_in[i][j]; 00276 Ld[i][j] = *Ld_in; 00277 Ld_in++; 00278 } 00279 } 00280 } 00281 00282 00283 void STATES_OBSERVER_DELAY::iterateOnce(void){ 00284 if(!enable){ 00285 if (flag_reset){ 00286 reset(); 00287 flag_reset = false; 00288 } 00289 return; 00290 } 00291 00292 00293 // Inputs of the observer: sys_inputs, sys_outputs 00294 // 00295 // Get the delayed estimation of states 00296 // states_est_delay_d = states_est_buffer.Get(d); // Get the element that is d samples ago 00297 // 00298 if (enable_Cd){ 00299 // y_est = Cd*states_est_delay_d 00300 y_est = Mat_multiply_Vec(Cd, states_est_buffer.Get(d)); 00301 // est_error = y_est - sys_outputs 00302 est_error = Get_VectorPlus(y_est,sys_outputs,true); // minus 00303 }else{ 00304 // est_error = y_est - sys_outputs 00305 est_error = Get_VectorPlus(states_est_buffer.Get(d), sys_outputs,true); // minus 00306 } 00307 00308 00309 // Get estimation 00310 state_est = states_est_buffer.Get(0); 00311 00312 //-------------------------------Update-----------------------------// 00313 static vector<vector<float> >::iterator it_Ld; 00314 it_Ld = Ld.begin(); // it_Ld points to L0 00315 // 00316 static vector<float> states_new; 00317 // x_(k+1) = (Ad*x_k + Bd*sys_inputs + sys_extraDisturbance) - L*est_error) 00318 states_new = Get_VectorPlus(Mat_multiply_Vec(Ad,state_est), Mat_multiply_Vec(Bd,sys_inputs), false); // plus 00319 Get_VectorIncrement(states_new, sys_extraDisturbance, false); // +=, extra disturbances 00320 Get_VectorIncrement(states_new, Partial_Mat_multiply_Vec(it_Ld, n, est_error), true); // -= 00321 // states_new is equal to x_(k+1) and will be used to .Insert() the buffer 00322 00323 // Update all the other states 00324 // from k to (k-d)+1, totally d samples 00325 // Currently it_Ld is pointing to "L1" 00326 for (size_t i = 0; i < d; ++i){ 00327 // it_Ld will be automatically updated during iteration 00328 states_est_buffer.Increase(i, Partial_Mat_multiply_Vec(it_Ld, n, est_error), true); // -= 00329 } 00330 00331 // Rotate the buffer 00332 states_est_buffer.Insert(states_new); 00333 00334 00335 /* 00336 // No-delay version 00337 //----------------------------------------// 00338 if (enable_Cd){ 00339 // y_est = Cd*states_est_delay_d 00340 y_est = Mat_multiply_Vec(Cd, state_est); 00341 // est_error = y_est - sys_outputs 00342 est_error = Get_VectorPlus(y_est,sys_outputs, true); // minus 00343 }else{ 00344 // est_error = y_est - sys_outputs 00345 est_error = Get_VectorPlus(state_est, sys_outputs, true); // minus 00346 } 00347 // 00348 state_est = Get_VectorPlus(Mat_multiply_Vec(Ad,state_est), Mat_multiply_Vec(Bd,sys_inputs), false); // plus 00349 // Get_VectorIncrement(state_est, sys_extraDisturbance, false); // +=, extra disturbances 00350 Get_VectorIncrement(state_est, Mat_multiply_Vec(Ld, est_error), true); // -= 00351 */ 00352 } 00353 00354 // Utilities 00355 void STATES_OBSERVER_DELAY::Mat_multiply_Vec(vector<float> &v_out, const vector<vector<float> > &m_left, const vector<float> &v_right){ // v_out = m_left*v_right 00356 00357 // Size check 00358 if (v_out.size() != m_left.size()){ 00359 v_out.resize(m_left.size()); 00360 } 00361 00362 /* 00363 // 00364 static vector<float>::iterator it_out; 00365 static vector<float>::iterator it_m_row; 00366 static vector<float>::iterator it_v; 00367 // 00368 it_out = v_out.begin(); 00369 for (size_t i = 0; i < m_left.size(); ++i){ 00370 *it_out = 0.0; 00371 it_m_row = vector<vector<float> >(m_left)[i].begin(); 00372 it_v = vector<float>(v_right).begin(); 00373 for (size_t j = 0; j < m_left[i].size(); ++j){ 00374 // *it_out += m_left[i][j] * v_right[j]; 00375 if (*it_m_row != 0.0 && *it_v != 0.0){ 00376 (*it_out) += (*it_m_row) * (*it_v); 00377 }else{ 00378 // (*it_out) += 0.0 00379 } 00380 // (*it_out) += (*it_m_row) * (*it_v); 00381 // 00382 it_m_row++; 00383 it_v++; 00384 } 00385 it_out++; 00386 } 00387 */ 00388 00389 // Indexing 00390 for (size_t i = 0; i < m_left.size(); ++i){ 00391 // 00392 v_out[i] = 0.0; 00393 // 00394 for (size_t j = 0; j < v_right.size(); ++j){ 00395 if (m_left[i][j] != 0.0 && v_right[j] != 0.0) 00396 v_out[i] += m_left[i][j]*v_right[j]; 00397 } 00398 } 00399 00400 } 00401 vector<float> STATES_OBSERVER_DELAY::Mat_multiply_Vec(const vector<vector<float> > &m_left, const vector<float> &v_right){ // v_out = m_left*v_right 00402 static vector<float> v_out; 00403 // Size check 00404 if (v_out.size() != m_left.size()){ 00405 v_out.resize(m_left.size()); 00406 } 00407 /* 00408 // Iterators 00409 static vector<float>::iterator it_out; 00410 static vector<float>::iterator it_m_row; 00411 static vector<float>::iterator it_v; 00412 // 00413 it_out = v_out.begin(); 00414 for (size_t i = 0; i < m_left.size(); ++i){ 00415 *it_out = 0.0; 00416 it_m_row = vector<vector<float> >(m_left)[i].begin(); 00417 it_v = vector<float>(v_right).begin(); 00418 for (size_t j = 0; j < m_left[i].size(); ++j){ 00419 // *it_out += m_left[i][j] * v_right[j]; 00420 if (*it_m_row != 0.0 && *it_v != 0.0){ 00421 (*it_out) += (*it_m_row) * (*it_v); 00422 }else{ 00423 // (*it_out) += 0.0 00424 } 00425 // (*it_out) += (*it_m_row) * (*it_v); 00426 // 00427 it_m_row++; 00428 it_v++; 00429 } 00430 it_out++; 00431 } 00432 */ 00433 00434 // Indexing 00435 for (size_t i = 0; i < m_left.size(); ++i){ 00436 // 00437 v_out[i] = 0.0; 00438 // 00439 for (size_t j = 0; j < v_right.size(); ++j){ 00440 if (m_left[i][j] != 0.0 && v_right[j] != 0.0) 00441 v_out[i] += m_left[i][j]*v_right[j]; 00442 } 00443 } 00444 00445 return v_out; 00446 } 00447 vector<float> STATES_OBSERVER_DELAY::Get_VectorPlus(const vector<float> &v_a, const vector<float> &v_b, bool is_minus) // v_a + (or -) v_b 00448 { 00449 static vector<float> v_c; 00450 // Size check 00451 if (v_c.size() != v_a.size()){ 00452 v_c.resize(v_a.size()); 00453 } 00454 // 00455 for (size_t i = 0; i < v_a.size(); ++i){ 00456 if (is_minus){ 00457 v_c[i] = v_a[i] - v_b[i]; 00458 }else{ 00459 v_c[i] = v_a[i] + v_b[i]; 00460 } 00461 } 00462 return v_c; 00463 } 00464 vector<float> STATES_OBSERVER_DELAY::Get_VectorScalarMultiply(const vector<float> &v_a, float scale) // scale*v_a 00465 { 00466 static vector<float> v_c; 00467 // Size check 00468 if (v_c.size() != v_a.size()){ 00469 v_c.resize(v_a.size()); 00470 } 00471 // for pure negative 00472 if (scale == -1.0){ 00473 for (size_t i = 0; i < v_a.size(); ++i){ 00474 v_c[i] = -v_a[i]; 00475 } 00476 return v_c; 00477 } 00478 // else 00479 for (size_t i = 0; i < v_a.size(); ++i){ 00480 v_c[i] = scale*v_a[i]; 00481 00482 } 00483 return v_c; 00484 } 00485 // Increment 00486 void STATES_OBSERVER_DELAY::Get_VectorIncrement(vector<float> &v_a, const vector<float> &v_b, bool is_minus){ // v_a += (or -=) v_b 00487 // Size check 00488 if (v_a.size() != v_b.size()){ 00489 v_a.resize(v_b.size()); 00490 } 00491 // 00492 if (is_minus){ // -= 00493 for (size_t i = 0; i < v_b.size(); ++i){ 00494 v_a[i] -= v_b[i]; 00495 } 00496 }else{ // += 00497 for (size_t i = 0; i < v_b.size(); ++i){ 00498 v_a[i] += v_b[i]; 00499 } 00500 } 00501 00502 } 00503 // Partial matrix multiplication 00504 vector<float> STATES_OBSERVER_DELAY::Partial_Mat_multiply_Vec(vector<vector<float> >::iterator &it_m_left, size_t numRow_m_left, const vector<float> &v_right){ // v_out = m_left*v_right 00505 static vector<float> v_out; 00506 // Size check 00507 if (v_out.size() != numRow_m_left){ 00508 v_out.resize(numRow_m_left); 00509 } 00510 /* 00511 // Iterators 00512 static vector<float>::iterator it_out; 00513 static vector<float>::iterator it_m_row; 00514 static vector<float>::iterator it_v; 00515 // 00516 it_out = v_out.begin(); 00517 for (size_t i = 0; i < numRow_m_left; ++i){ 00518 *it_out = 0.0; 00519 it_m_row = (*it_m_left).begin(); 00520 it_v = vector<float>(v_right).begin(); 00521 for (size_t j = 0; j < (*it_m_left).size(); ++j){ 00522 // *it_out += m_left[i][j] * v_right[j]; 00523 if (*it_m_row != 0.0 && *it_v != 0.0){ 00524 (*it_out) += (*it_m_row) * (*it_v); 00525 }else{ 00526 // (*it_out) += 0.0 00527 } 00528 // (*it_out) += (*it_m_row) * (*it_v); 00529 // 00530 it_m_row++; 00531 it_v++; 00532 } 00533 it_out++; 00534 it_m_left++; 00535 } 00536 */ 00537 00538 // 00539 // Indexing 00540 for (size_t i = 0; i < numRow_m_left; ++i){ 00541 // 00542 v_out[i] = 0.0; 00543 // 00544 for (size_t j = 0; j < v_right.size(); ++j){ 00545 if ( (*it_m_left)[j] != 0.0 && v_right[j] != 0.0) 00546 v_out[i] += (*it_m_left)[j]*v_right[j]; 00547 } 00548 // 00549 it_m_left++; 00550 } 00551 00552 00553 return v_out; 00554 }
Generated on Tue Aug 2 2022 05:42:54 by
1.7.2
