A controller that is immune to measurement errors and keep the true states at the desired value, also known as "Zero-Torque Control"

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers MEASUREMENT_ERROR_ADAPTATION_CONTROL.h Source File

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