An observer for estimating the friction-force ratio
Embed:
(wiki syntax)
Show/hide line numbers
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
Generated on Thu Jul 14 2022 22:46:42 by
1.7.2