An observer for estimating the friction-force ratio

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers Observer_ftRatio.h Source File

Observer_ftRatio.h

00001 #ifndef Observer_ftRatio_H
00002 #define Observer_ftRatio_H
00003 //
00004 #include "FILTER_LIB.h"
00005 // #include "STATES_TRANSFORM.h"
00006 #include "STATES_OBSERVER_DELAY.h"
00007 #include <vector>
00008 //
00009 using std::vector;
00010 
00011 // #define NUM_THRESHOLD 0.001 // 10^3
00012 
00013 
00014 
00015 class Observer_ftRatio{
00016 public:
00017     // Dimensions of the original system
00018     size_t n; // Number of states
00019     size_t p; // Number of inputs of the plant, the dimension of y
00020     size_t q; // Number of outputs of the plant, no use in full-state-measureable case
00021     // Dimensions of the observer
00022     size_t n_OB; // Number of states
00023     size_t p_OB; // Number of inputs of the plant, the dimension of y
00024     size_t q_OB; // Number of outputs of the plant, no use in full-state-measureable case
00025     size_t n_ME_OB; // Number of the measurement errors in the PI observer
00026 
00027     float Ts; // Sampling time
00028     //
00029     bool enable;
00030 
00031     //
00032     STATES_OBSERVER_DELAY OB;
00033 
00034     // Parameters
00035     // vector<vector<float> > Bv1_ftOb;
00036     // Parameters for calculating ft_ideal
00037     vector<vector<float> > F1_ftOb;
00038     vector<vector<float> > F2_ftOb;
00039 
00040     // Indexes
00041     // size_t idx_x_real; // Index for x_real
00042     size_t idx_x_ns; // Index for x_ns
00043     size_t idx_ft_error_est; // Index for ft_error_est
00044 
00045     // Input signals
00046     vector<float> yc; // THe outputs of the nominal controller
00047     // States
00048     vector<float> ft_est;
00049     vector<float> ft_error_est;
00050     vector<float> ft_ideal_est;
00051     //
00052     vector<float> x_real_est; // Estimation of the real states
00053     // vector<float> x_ns_est; // Estimation of the states of the no-slip system
00054 
00055     //
00056     vector<float> Measurement_error_est; // The estimation of measurement errors in PI observer
00057 
00058     // The degree of no-slip
00059     vector<float> ratio_ft;
00060     vector<float> ratio_ft_filtered;
00061 
00062 
00063     Observer_ftRatio(float samplingTime);
00064     //
00065     void start();
00066     void pause();
00067     void stop();
00068     void reset();
00069     //
00070     void init(void);
00071     // Process of observer
00072     void iterateOnce(const vector<float> &yc_in, const vector<float> &x_real_in); // Execute the simulator once
00073 
00074 private:
00075     vector<float> zeros_n;
00076     vector<float> zeros_p;
00077     vector<float> zeros_q;
00078     vector<float> zeros_n_ME_OB; // Number of the measurement errors in the PI observer
00079     //
00080     vector<float> ones_p;
00081 
00082     // Private methods
00083     //
00084     void insertSignals(const vector<float> &yc_in, const vector<float> &x_real_in);
00085 
00086 
00087 
00088     // Kalman-filter-based parameter identifier for ratio-ft(s)
00089     // TRANSFORM_MOTOR_separate_2_avgDelta T_ft_est, T_ft_ideal_est;
00090     FirstOrder_KalmanFilter KFID_ratio_ft_right;
00091     FirstOrder_KalmanFilter KFID_ratio_ft_left;
00092     // Filter for ratio_ft
00093     LPF_vector_nthOrderCritical LPF_ratio_ft;
00094 
00095     // Saturation
00096     Saturation_vector SAT_ratio_ft;
00097 
00098     // Utilities
00099     void Mat_multiply_Vec(vector<float> &v_out, const vector<vector<float> > &m_left, const vector<float> &v_right); // v_out = m_left*v_right
00100     vector<float> Mat_multiply_Vec(const vector<vector<float> > &m_left, const vector<float> &v_right); // v_out = m_left*v_right
00101     vector<float> Get_VectorPlus(const vector<float> &v_a, const vector<float> &v_b, bool is_minus); // v_a + (or -) v_b
00102     vector<float> Get_VectorScalarMultiply(const vector<float> &v_a, float scale); // scale*v_a
00103     // Increment
00104     void Get_VectorIncrement(vector<float> &v_a, const vector<float> &v_b, bool is_minus); // v_a += (or -=) v_b
00105 
00106 
00107     // Assign a matrix by primitive c-array
00108     void 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
00109 
00110 };
00111 
00112 #endif