Chun Feng Huang / Observer_ftRatio

An observer for estimating the friction-force ratio

Observer_ftRatio.h@1:fbae330f0e71, 2017-02-26 (annotated)

Committer:
benson516
Date:
Sun Feb 26 05:36:01 2017 +0000
Revision:
1:fbae330f0e71
Parent:
0:a936477fcd4a 
Ver. 1.00

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benson516 0:a936477fcd4a 1 #ifndef Observer_ftRatio_H
benson516 0:a936477fcd4a 2 #define Observer_ftRatio_H
benson516 0:a936477fcd4a 3 //
benson516 0:a936477fcd4a 4 #include "FILTER_LIB.h"
benson516 0:a936477fcd4a 5 // #include "STATES_TRANSFORM.h"
benson516 0:a936477fcd4a 6 #include "STATES_OBSERVER_DELAY.h"
benson516 0:a936477fcd4a 7 #include <vector>
benson516 0:a936477fcd4a 8 //
benson516 0:a936477fcd4a 9 using std::vector;
benson516 0:a936477fcd4a 10
benson516 0:a936477fcd4a 11 // #define NUM_THRESHOLD 0.001 // 10^3
benson516 0:a936477fcd4a 12
benson516 0:a936477fcd4a 13
benson516 0:a936477fcd4a 14
benson516 0:a936477fcd4a 15 class Observer_ftRatio{
benson516 0:a936477fcd4a 16 public:
benson516 0:a936477fcd4a 17 // Dimensions of the original system
benson516 0:a936477fcd4a 18 size_t n; // Number of states
benson516 0:a936477fcd4a 19 size_t p; // Number of inputs of the plant, the dimension of y
benson516 0:a936477fcd4a 20 size_t q; // Number of outputs of the plant, no use in full-state-measureable case
benson516 0:a936477fcd4a 21 // Dimensions of the observer
benson516 0:a936477fcd4a 22 size_t n_OB; // Number of states
benson516 0:a936477fcd4a 23 size_t p_OB; // Number of inputs of the plant, the dimension of y
benson516 0:a936477fcd4a 24 size_t q_OB; // Number of outputs of the plant, no use in full-state-measureable case
benson516 0:a936477fcd4a 25 size_t n_ME_OB; // Number of the measurement errors in the PI observer
benson516 0:a936477fcd4a 26
benson516 0:a936477fcd4a 27 float Ts; // Sampling time
benson516 0:a936477fcd4a 28 //
benson516 0:a936477fcd4a 29 bool enable;
benson516 0:a936477fcd4a 30
benson516 0:a936477fcd4a 31 //
benson516 0:a936477fcd4a 32 STATES_OBSERVER_DELAY OB;
benson516 0:a936477fcd4a 33
benson516 0:a936477fcd4a 34 // Parameters
benson516 0:a936477fcd4a 35 // vector<vector<float> > Bv1_ftOb;
benson516 0:a936477fcd4a 36 // Parameters for calculating ft_ideal
benson516 0:a936477fcd4a 37 vector<vector<float> > F1_ftOb;
benson516 0:a936477fcd4a 38 vector<vector<float> > F2_ftOb;
benson516 0:a936477fcd4a 39
benson516 0:a936477fcd4a 40 // Indexes
benson516 0:a936477fcd4a 41 // size_t idx_x_real; // Index for x_real
benson516 0:a936477fcd4a 42 size_t idx_x_ns; // Index for x_ns
benson516 0:a936477fcd4a 43 size_t idx_ft_error_est; // Index for ft_error_est
benson516 0:a936477fcd4a 44
benson516 0:a936477fcd4a 45 // Input signals
benson516 0:a936477fcd4a 46 vector<float> yc; // THe outputs of the nominal controller
benson516 0:a936477fcd4a 47 // States
benson516 0:a936477fcd4a 48 vector<float> ft_est;
benson516 0:a936477fcd4a 49 vector<float> ft_error_est;
benson516 0:a936477fcd4a 50 vector<float> ft_ideal_est;
benson516 0:a936477fcd4a 51 //
benson516 0:a936477fcd4a 52 vector<float> x_real_est; // Estimation of the real states
benson516 0:a936477fcd4a 53 // vector<float> x_ns_est; // Estimation of the states of the no-slip system
benson516 0:a936477fcd4a 54
benson516 0:a936477fcd4a 55 //
benson516 0:a936477fcd4a 56 vector<float> Measurement_error_est; // The estimation of measurement errors in PI observer
benson516 0:a936477fcd4a 57
benson516 0:a936477fcd4a 58 // The degree of no-slip
benson516 0:a936477fcd4a 59 vector<float> ratio_ft;
benson516 0:a936477fcd4a 60 vector<float> ratio_ft_filtered;
benson516 0:a936477fcd4a 61
benson516 0:a936477fcd4a 62
benson516 0:a936477fcd4a 63 Observer_ftRatio(float samplingTime);
benson516 0:a936477fcd4a 64 //
benson516 0:a936477fcd4a 65 void start();
benson516 0:a936477fcd4a 66 void pause();
benson516 0:a936477fcd4a 67 void stop();
benson516 0:a936477fcd4a 68 void reset();
benson516 0:a936477fcd4a 69 //
benson516 0:a936477fcd4a 70 void init(void);
benson516 0:a936477fcd4a 71 // Process of observer
benson516 0:a936477fcd4a 72 void iterateOnce(const vector<float> &yc_in, const vector<float> &x_real_in); // Execute the simulator once
benson516 0:a936477fcd4a 73
benson516 0:a936477fcd4a 74 private:
benson516 0:a936477fcd4a 75 vector<float> zeros_n;
benson516 0:a936477fcd4a 76 vector<float> zeros_p;
benson516 0:a936477fcd4a 77 vector<float> zeros_q;
benson516 0:a936477fcd4a 78 vector<float> zeros_n_ME_OB; // Number of the measurement errors in the PI observer
benson516 0:a936477fcd4a 79 //
benson516 0:a936477fcd4a 80 vector<float> ones_p;
benson516 0:a936477fcd4a 81
benson516 0:a936477fcd4a 82 // Private methods
benson516 0:a936477fcd4a 83 //
benson516 0:a936477fcd4a 84 void insertSignals(const vector<float> &yc_in, const vector<float> &x_real_in);
benson516 0:a936477fcd4a 85
benson516 0:a936477fcd4a 86
benson516 0:a936477fcd4a 87
benson516 0:a936477fcd4a 88 // Kalman-filter-based parameter identifier for ratio-ft(s)
benson516 0:a936477fcd4a 89 // TRANSFORM_MOTOR_separate_2_avgDelta T_ft_est, T_ft_ideal_est;
benson516 0:a936477fcd4a 90 FirstOrder_KalmanFilter KFID_ratio_ft_right;
benson516 0:a936477fcd4a 91 FirstOrder_KalmanFilter KFID_ratio_ft_left;
benson516 0:a936477fcd4a 92 // Filter for ratio_ft
benson516 0:a936477fcd4a 93 LPF_vector_nthOrderCritical LPF_ratio_ft;
benson516 0:a936477fcd4a 94
benson516 0:a936477fcd4a 95 // Saturation
benson516 0:a936477fcd4a 96 Saturation_vector SAT_ratio_ft;
benson516 0:a936477fcd4a 97
benson516 0:a936477fcd4a 98 // Utilities
benson516 0:a936477fcd4a 99 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
benson516 0:a936477fcd4a 100 vector<float> Mat_multiply_Vec(const vector<vector<float> > &m_left, const vector<float> &v_right); // v_out = m_left*v_right
benson516 0:a936477fcd4a 101 vector<float> Get_VectorPlus(const vector<float> &v_a, const vector<float> &v_b, bool is_minus); // v_a + (or -) v_b
benson516 0:a936477fcd4a 102 vector<float> Get_VectorScalarMultiply(const vector<float> &v_a, float scale); // scale*v_a
benson516 0:a936477fcd4a 103 // Increment
benson516 0:a936477fcd4a 104 void Get_VectorIncrement(vector<float> &v_a, const vector<float> &v_b, bool is_minus); // v_a += (or -=) v_b
benson516 0:a936477fcd4a 105
benson516 0:a936477fcd4a 106
benson516 0:a936477fcd4a 107 // Assign a matrix by primitive c-array
benson516 0:a936477fcd4a 108 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
benson516 0:a936477fcd4a 109
benson516 0:a936477fcd4a 110 };
benson516 0:a936477fcd4a 111
benson516 0:a936477fcd4a 112 #endif