An observer for estimating the friction-force ratio

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers Observer_ftRatio.cpp Source File

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 }