A controller that is immune to measurement errors and keep the true states at the desired value, also known as "Zero-Torque Control"
MEASUREMENT_ERROR_ADAPTATION_CONTROL.h
00001 #ifndef MEASUREMENT_ERROR_ADAPTATION_CONTROL_H 00002 #define MEASUREMENT_ERROR_ADAPTATION_CONTROL_H 00003 // 00004 #include <vector> 00005 00006 using std::vector; 00007 00008 class MEASUREMENT_ERROR_ADAPTATION_CONTROL{ 00009 public: 00010 // Dimensions 00011 size_t n; // Number of states 00012 size_t p; // Number of inputs of the plant 00013 size_t q; // Number of channels that have measurement errors 00014 00015 float Ts; // Sampling time 00016 00017 // System parameters 00018 vector<vector<float> > C_error; // Measurement error input matrix 00019 // Controller parameters 00020 vector<vector<float> > K_full; // Full state feedback gain 00021 vector<vector<float> > K_phi; // Gain for integral action 00022 vector<vector<float> > N_xd; // Feed-forward gain for x_d, x_d = N_xd*r 00023 vector<vector<float> > N_ud; // Feed-forward gain for u_d, u_d = N_ud*r 00024 00025 00026 // States 00027 vector<float> states_est; // States, "x_est", related to states_d (treats states_d as the origin) 00028 vector<float> sys_inputs; // The inputs of the plant, "u", the "output" of the controller 00029 vector<float> sys_output; // The output of the plant, "y", the "input" of the controller 00030 vector<float> MeasurementErrors; // The measurement error of the sensors, "phi" 00031 // Command (equalibrium state) 00032 vector<float> states_d; // x_d 00033 vector<float> inputs_d; // u_d 00034 vector<float> command; // r 00035 00036 00037 MEASUREMENT_ERROR_ADAPTATION_CONTROL(size_t num_state, size_t num_in, size_t num_out, float samplingTime); 00038 // Assign Parameters 00039 void assign_C_error(float* C_error_in, size_t n_in, size_t q_in); 00040 void assign_K_full(float* K_full_in, size_t p_in, size_t n_in); 00041 void assign_K_phi(float* K_phi_in, size_t q_in, size_t n_in); 00042 // 00043 void assign_N_xd(float* N_xd_in, size_t n_in, size_t p_in); 00044 void assign_N_ud(float* N_ud_in, size_t p_in); // p by p square matrix 00045 00046 // 00047 void iterateOnce(bool enable); 00048 00049 private: 00050 00051 vector<float> zeros_n; 00052 vector<float> zeros_p; 00053 vector<float> zeros_q; 00054 00055 // Command (equalibrium state) related calculation 00056 void get_inputs_compensate(void); // Calculate the compensation variable, states_d and sys_inputs_compensate 00057 00058 // Calculate the states_est 00059 void get_states_est(void); // Calculate the states_est from MeasurementErrors and states_d 00060 00061 // Calculate the estimation of MeasurementErrors 00062 void get_MeasurementErrors_est(bool enable); // Calculate the MeasurementErrors 00063 00064 00065 // Utilities 00066 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 00067 vector<float> Mat_multiply_Vec(const vector<vector<float> > &m_left, const vector<float> &v_right); // v_out = m_left*v_right 00068 vector<float> Get_VectorPlus(const vector<float> &v_a, const vector<float> &v_b, bool is_minus); // v_a + (or -) v_b 00069 vector<float> Get_VectorScalarMultiply(const vector<float> &v_a, float scale); // scale*v_a 00070 // Increment 00071 void Get_VectorIncrement(vector<float> &v_a, const vector<float> &v_b, bool is_minus); // v_a += (or -=) v_b 00072 00073 00074 }; 00075 00076 00077 #endif
Generated on Tue Aug 2 2022 05:40:37 by
1.7.2