Chun Feng Huang / MEASUREMENT_ERROR_ADAPTATION_CONTROL

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@0:533d5685b66c, 2017-01-11 (annotated)

Committer:
benson516
Date:
Wed Jan 11 09:32:26 2017 +0000
Revision:
0:533d5685b66c
First compilation of this library

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benson516 0:533d5685b66c 1 #ifndef MEASUREMENT_ERROR_ADAPTATION_CONTROL_H
benson516 0:533d5685b66c 2 #define MEASUREMENT_ERROR_ADAPTATION_CONTROL_H
benson516 0:533d5685b66c 3 //
benson516 0:533d5685b66c 4 #include <vector>
benson516 0:533d5685b66c 5
benson516 0:533d5685b66c 6 using std::vector;
benson516 0:533d5685b66c 7
benson516 0:533d5685b66c 8 class MEASUREMENT_ERROR_ADAPTATION_CONTROL{
benson516 0:533d5685b66c 9 public:
benson516 0:533d5685b66c 10 // Dimensions
benson516 0:533d5685b66c 11 size_t n; // Number of states
benson516 0:533d5685b66c 12 size_t p; // Number of inputs of the plant
benson516 0:533d5685b66c 13 size_t q; // Number of channels that have measurement errors
benson516 0:533d5685b66c 14
benson516 0:533d5685b66c 15 float Ts; // Sampling time
benson516 0:533d5685b66c 16
benson516 0:533d5685b66c 17 // System parameters
benson516 0:533d5685b66c 18 vector<vector<float> > C_error; // Measurement error input matrix
benson516 0:533d5685b66c 19 // Controller parameters
benson516 0:533d5685b66c 20 vector<vector<float> > K_full; // Full state feedback gain
benson516 0:533d5685b66c 21 vector<vector<float> > K_phi; // Gain for integral action
benson516 0:533d5685b66c 22 vector<vector<float> > N_xd; // Feed-forward gain for x_d, x_d = N_xd*r
benson516 0:533d5685b66c 23 vector<vector<float> > N_ud; // Feed-forward gain for u_d, u_d = N_ud*r
benson516 0:533d5685b66c 24
benson516 0:533d5685b66c 25
benson516 0:533d5685b66c 26 // States
benson516 0:533d5685b66c 27 vector<float> states_est; // States, "x_est", related to states_d (treats states_d as the origin)
benson516 0:533d5685b66c 28 vector<float> sys_inputs; // The inputs of the plant, "u", the "output" of the controller
benson516 0:533d5685b66c 29 vector<float> sys_output; // The output of the plant, "y", the "input" of the controller
benson516 0:533d5685b66c 30 vector<float> MeasurementErrors; // The measurement error of the sensors, "phi"
benson516 0:533d5685b66c 31 // Command (equalibrium state)
benson516 0:533d5685b66c 32 vector<float> states_d; // x_d
benson516 0:533d5685b66c 33 vector<float> inputs_d; // u_d
benson516 0:533d5685b66c 34 vector<float> command; // r
benson516 0:533d5685b66c 35
benson516 0:533d5685b66c 36
benson516 0:533d5685b66c 37 MEASUREMENT_ERROR_ADAPTATION_CONTROL(size_t num_state, size_t num_in, size_t num_out, float samplingTime);
benson516 0:533d5685b66c 38 // Assign Parameters
benson516 0:533d5685b66c 39 void assign_C_error(float* C_error_in, size_t n_in, size_t q_in);
benson516 0:533d5685b66c 40 void assign_K_full(float* K_full_in, size_t p_in, size_t n_in);
benson516 0:533d5685b66c 41 void assign_K_phi(float* K_phi_in, size_t q_in, size_t n_in);
benson516 0:533d5685b66c 42 //
benson516 0:533d5685b66c 43 void assign_N_xd(float* N_xd_in, size_t n_in, size_t p_in);
benson516 0:533d5685b66c 44 void assign_N_ud(float* N_ud_in, size_t p_in); // p by p square matrix
benson516 0:533d5685b66c 45
benson516 0:533d5685b66c 46 //
benson516 0:533d5685b66c 47 void iterateOnce(bool enable);
benson516 0:533d5685b66c 48
benson516 0:533d5685b66c 49 private:
benson516 0:533d5685b66c 50
benson516 0:533d5685b66c 51 vector<float> zeros_n;
benson516 0:533d5685b66c 52 vector<float> zeros_p;
benson516 0:533d5685b66c 53 vector<float> zeros_q;
benson516 0:533d5685b66c 54
benson516 0:533d5685b66c 55 // Command (equalibrium state) related calculation
benson516 0:533d5685b66c 56 void get_inputs_compensate(void); // Calculate the compensation variable, states_d and sys_inputs_compensate
benson516 0:533d5685b66c 57
benson516 0:533d5685b66c 58 // Calculate the states_est
benson516 0:533d5685b66c 59 void get_states_est(void); // Calculate the states_est from MeasurementErrors and states_d
benson516 0:533d5685b66c 60
benson516 0:533d5685b66c 61 // Calculate the estimation of MeasurementErrors
benson516 0:533d5685b66c 62 void get_MeasurementErrors_est(bool enable); // Calculate the MeasurementErrors
benson516 0:533d5685b66c 63
benson516 0:533d5685b66c 64
benson516 0:533d5685b66c 65 // Utilities
benson516 0:533d5685b66c 66 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:533d5685b66c 67 vector<float> Mat_multiply_Vec(const vector<vector<float> > &m_left, const vector<float> &v_right); // v_out = m_left*v_right
benson516 0:533d5685b66c 68 vector<float> Get_VectorPlus(const vector<float> &v_a, const vector<float> &v_b, bool is_minus); // v_a + (or -) v_b
benson516 0:533d5685b66c 69 vector<float> Get_VectorScalarMultiply(const vector<float> &v_a, float scale); // scale*v_a
benson516 0:533d5685b66c 70 // Increment
benson516 0:533d5685b66c 71 void Get_VectorIncrement(vector<float> &v_a, const vector<float> &v_b, bool is_minus); // v_a += (or -=) v_b
benson516 0:533d5685b66c 72
benson516 0:533d5685b66c 73
benson516 0:533d5685b66c 74 };
benson516 0:533d5685b66c 75
benson516 0:533d5685b66c 76
benson516 0:533d5685b66c 77 #endif