An observer that can estimate the states from delayed signals

Fork of STATES_OBSERVER_DELAY by Project_WIPV_antiSlip

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers STATES_OBSERVER_DELAY.cpp Source File

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 }