Diff: MEASUREMENT_ERROR_ADAPTATION_CONTROL.h

Revision:

0:533d5685b66c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MEASUREMENT_ERROR_ADAPTATION_CONTROL.h	Wed Jan 11 09:32:26 2017 +0000
@@ -0,0 +1,77 @@
+#ifndef MEASUREMENT_ERROR_ADAPTATION_CONTROL_H
+#define MEASUREMENT_ERROR_ADAPTATION_CONTROL_H
+//
+#include <vector>
+
+using std::vector;
+
+class MEASUREMENT_ERROR_ADAPTATION_CONTROL{
+public:
+    // Dimensions
+    size_t n; // Number of states
+    size_t p; // Number of inputs of the plant
+    size_t q; // Number of channels that have measurement errors
+
+    float Ts; // Sampling time
+
+    // System parameters
+    vector<vector<float> > C_error; // Measurement error input matrix
+    // Controller parameters
+    vector<vector<float> > K_full; // Full state feedback gain
+    vector<vector<float> > K_phi; // Gain for integral action
+    vector<vector<float> > N_xd; // Feed-forward gain for x_d, x_d = N_xd*r
+    vector<vector<float> > N_ud; // Feed-forward gain for u_d, u_d = N_ud*r
+
+
+    // States
+    vector<float> states_est; // States, "x_est", related to states_d (treats states_d as the origin)
+    vector<float> sys_inputs; // The inputs of the plant, "u", the "output" of the controller
+    vector<float> sys_output; // The output of the plant, "y", the "input" of the controller
+    vector<float> MeasurementErrors; // The measurement error of the sensors, "phi"
+    // Command (equalibrium state)
+    vector<float> states_d; // x_d
+    vector<float> inputs_d; // u_d
+    vector<float> command; // r
+
+
+    MEASUREMENT_ERROR_ADAPTATION_CONTROL(size_t num_state, size_t num_in, size_t num_out, float samplingTime);
+    // Assign Parameters
+    void assign_C_error(float* C_error_in, size_t n_in, size_t q_in);
+    void assign_K_full(float* K_full_in, size_t p_in, size_t n_in);
+    void assign_K_phi(float* K_phi_in, size_t q_in, size_t n_in);
+    //
+    void assign_N_xd(float* N_xd_in, size_t n_in, size_t p_in);
+    void assign_N_ud(float* N_ud_in, size_t p_in); // p by p square matrix
+
+    //
+    void iterateOnce(bool enable);
+
+private:
+
+    vector<float> zeros_n;
+    vector<float> zeros_p;
+    vector<float> zeros_q;
+
+    // Command (equalibrium state) related calculation
+    void get_inputs_compensate(void); // Calculate the compensation variable, states_d and sys_inputs_compensate
+
+    // Calculate the states_est
+    void get_states_est(void); // Calculate the states_est from MeasurementErrors and states_d
+
+    // Calculate the estimation of MeasurementErrors
+    void get_MeasurementErrors_est(bool enable); // Calculate the MeasurementErrors
+
+
+    // Utilities
+    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
+    vector<float> Mat_multiply_Vec(const vector<vector<float> > &m_left, const vector<float> &v_right); // v_out = m_left*v_right
+    vector<float> Get_VectorPlus(const vector<float> &v_a, const vector<float> &v_b, bool is_minus); // v_a + (or -) v_b
+    vector<float> Get_VectorScalarMultiply(const vector<float> &v_a, float scale); // scale*v_a
+    // Increment
+    void Get_VectorIncrement(vector<float> &v_a, const vector<float> &v_b, bool is_minus); // v_a += (or -=) v_b
+
+
+};
+
+
+#endif