Diff: MEASUREMENT_ERROR_ADAPTATION_CONTROL.cpp

Revision:

0:533d5685b66c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MEASUREMENT_ERROR_ADAPTATION_CONTROL.cpp	Wed Jan 11 09:32:26 2017 +0000
@@ -0,0 +1,287 @@
+#include "MEASUREMENT_ERROR_ADAPTATION_CONTROL.h"
+// The controller is for the plant with (p, n, q) system
+// Dimensions:
+//
+// Inputs, u    |   States, x
+//     p     ---->      n
+//           q -- ^
+//  Measurement errors, phi
+
+MEASUREMENT_ERROR_ADAPTATION_CONTROL::MEASUREMENT_ERROR_ADAPTATION_CONTROL(size_t num_state, size_t num_in, size_t num_MeasurementError, float samplingTime):
+    n(num_state), p(num_in), q(num_MeasurementError), Ts(samplingTime),
+    C_error(num_state, vector<float>(num_MeasurementError,0.0)),
+    K_full(num_in, vector<float>(num_state,0.0)),
+    K_phi(num_MeasurementError, vector<float>(num_state,0.0)),
+    N_xd(num_state, vector<float>(num_in, 0.0)),
+    N_ud(num_in, vector<float>(num_in, 0.0))
+{
+    //
+    zeros_n.assign(n, 0.0);
+    zeros_p.assign(p, 0.0);
+    zeros_q.assign(q, 0.0);
+
+    // States
+    states_est = zeros_n; // states_est, "x_est"
+    sys_inputs = zeros_p; // The inputs of the plant, "u"
+    sys_output = zeros_n; // The output of the plant, "y"
+    MeasurementErrors = zeros_q; // The measurement error of the sensors, "phi"
+
+    // Command (equalibrium state)
+    states_d = zeros_n; // x_d
+    inputs_d = zeros_p; // u_d
+    command = zeros_p; // r
+
+
+}
+// Assign Parameters
+void MEASUREMENT_ERROR_ADAPTATION_CONTROL::assign_C_error(float* C_error_in, size_t n_in, size_t q_in){
+    // C_error_in is the pointer of a mutidimentional array with size n_in by q_in
+    if (n != n_in || q != q_in){
+        n = n_in;
+        q = q_in;
+        zeros_n.resize(n, 0.0);
+        zeros_q.resize(q, 0.0);
+        C_error.assign(n, zeros_q);
+    }
+    //
+    for (size_t i = 0; i < n; ++i){
+        for (size_t j = 0; j < q; ++j){
+            // C_error[i][j] = C_error_in[i][j];
+            C_error[i][j] = *C_error_in;
+            C_error_in++;
+        }
+    }
+}
+void MEASUREMENT_ERROR_ADAPTATION_CONTROL::assign_K_full(float* K_full_in, size_t p_in, size_t n_in){
+    // K_full_in is the pointer of a mutidimentional array with size p_in by n_in
+    if (n != n_in || p != p_in){
+        p = p_in;
+        n = n_in;
+        zeros_n.resize(n, 0.0);
+        zeros_p.resize(p, 0.0);
+        K_full.assign(p, zeros_n);
+    }
+    //
+    for (size_t i = 0; i < p; ++i){
+        for (size_t j = 0; j < n; ++j){
+            // K_full[i][j] = K_full_in[i][j];
+            K_full[i][j] = *K_full_in;
+            K_full_in++;
+        }
+    }
+}
+void MEASUREMENT_ERROR_ADAPTATION_CONTROL::assign_K_phi(float* K_phi_in, size_t q_in, size_t n_in){
+    // K_phi_in is the pointer of a mutidimentional array with size q_in by n_in
+    if (q != q_in || n != n_in){
+        q = q_in;
+        n = n_in;
+        zeros_q.resize(q, 0.0);
+        zeros_n.resize(n, 0.0);
+        K_phi.assign(q, zeros_n);
+    }
+    //
+    for (size_t i = 0; i < q; ++i){
+        for (size_t j = 0; j < n; ++j){
+            // K_phi[i][j] = K_phi_in[i][j];
+            K_phi[i][j] = *K_phi_in;
+            K_phi_in++;
+        }
+    }
+}
+//
+void MEASUREMENT_ERROR_ADAPTATION_CONTROL::assign_N_xd(float* N_xd_in, size_t n_in, size_t p_in){
+    // N_xd_in is the pointer of a mutidimentional array with size n_in by p_in
+    if (n != n_in || p != p_in){
+        n = n_in;
+        p = p_in;
+        zeros_n.resize(n, 0.0);
+        zeros_p.resize(p, 0.0);
+        N_xd.assign(n, zeros_p);
+    }
+    //
+    for (size_t i = 0; i < n; ++i){
+        for (size_t j = 0; j < p; ++j){
+            // N_xd[i][j] = N_xd_in[i][j];
+            N_xd[i][j] = *N_xd_in;
+            N_xd_in++;
+        }
+    }
+}
+void MEASUREMENT_ERROR_ADAPTATION_CONTROL::assign_N_ud(float* N_ud_in, size_t p_in){
+    // N_ud_in is the pointer of a mutidimentional array with size p_in by p_in
+    if (p != p_in){
+        p = p_in;
+        zeros_p.resize(p, 0.0);
+        N_ud.assign(p, zeros_p);
+    }
+    //
+    for (size_t i = 0; i < p; ++i){
+        for (size_t j = 0; j < p; ++j){
+            // N_ud[i][j] = N_ud_in[i][j];
+            N_ud[i][j] = *N_ud_in;
+            N_ud_in++;
+        }
+    }
+}
+//
+void MEASUREMENT_ERROR_ADAPTATION_CONTROL::iterateOnce(bool enable){
+    //----------------------------//
+    // Input: sys_output ("y"), command ("r")
+    // Get: sys_inputs ("u")
+    //----------------------------//
+
+    // Control law
+    if (enable){
+        //
+        get_inputs_compensate(); // Get states_d, inputs_d from command
+
+        // states_est = (sys_output - C_error*MeasurementErrors) - states_d
+        get_states_est();
+
+        // sys_inputs = inputs_d - K_full*states_est
+        sys_inputs = Get_VectorPlus(inputs_d, Mat_multiply_Vec(K_full, states_est),true); // minus
+    }else{
+        sys_inputs = zeros_p;
+    }
+
+    // Integral action
+    get_MeasurementErrors_est(enable);
+}
+
+// Private functions
+// Command (equalibrium state) related calculation
+void MEASUREMENT_ERROR_ADAPTATION_CONTROL::get_inputs_compensate(void){ // Calculate the compensation variable, states_d and sys_inputs_compensate
+    //
+    Mat_multiply_Vec(states_d, N_xd, command);
+    Mat_multiply_Vec(inputs_d, N_ud, command);
+}
+// Calculate the states_est
+void MEASUREMENT_ERROR_ADAPTATION_CONTROL::get_states_est(void){ // Calculate the sys_outputs from states_est, by mutiplying C_error
+    // states_est = (sys_output - C_error*MeasurementErrors) - states_d
+    states_est = Get_VectorPlus(Get_VectorPlus(sys_output, Mat_multiply_Vec(C_error, MeasurementErrors), true), states_d, true); // minus
+}
+// Calculate the estimation of MeasurementErrors
+void MEASUREMENT_ERROR_ADAPTATION_CONTROL::get_MeasurementErrors_est(bool enable){ // Calculate the MeasurementErrors
+    //
+    // Integral action
+    // MeasurementErrors -= Ts*(K_phi*states_est)
+    if (enable){
+        Get_VectorIncrement(MeasurementErrors, Get_VectorScalarMultiply(Mat_multiply_Vec(K_phi,states_est), Ts) , true); // -=
+    }else{
+        MeasurementErrors = zeros_q;
+    }
+}
+// Utilities
+void MEASUREMENT_ERROR_ADAPTATION_CONTROL::Mat_multiply_Vec(vector<float> &v_out, const vector<vector<float> > &m_left, const vector<float> &v_right){ // v_out = m_left*v_right
+    static vector<float>::iterator it_out;
+    static vector<const float>::iterator it_m_row;
+    static vector<const float>::iterator it_v;
+    //
+    it_out = v_out.begin();
+    for (size_t i = 0; i < m_left.size(); ++i){
+        *it_out = 0.0;
+        it_m_row = m_left[i].begin();
+        it_v = v_right.begin();
+        for (size_t j = 0; j < m_left[i].size(); ++j){
+            // *it_out += m_left[i][j] * v_right[j];
+            if (*it_m_row != 0.0 && *it_v != 0.0){
+                (*it_out) += (*it_m_row) * (*it_v);
+            }else{
+                // (*it_out) += 0.0
+            }
+            // (*it_out) += (*it_m_row) * (*it_v);
+            //
+            it_m_row++;
+            it_v++;
+        }
+        it_out++;
+    }
+}
+vector<float> MEASUREMENT_ERROR_ADAPTATION_CONTROL::Mat_multiply_Vec(const vector<vector<float> > &m_left, const vector<float> &v_right){ // v_out = m_left*v_right
+    static vector<float> v_out;
+    // Size check
+    if (v_out.size() != m_left.size()){
+        v_out.resize(m_left.size());
+    }
+    // Iterators
+    static vector<float>::iterator it_out;
+    static vector<const float>::iterator it_m_row;
+    static vector<const float>::iterator it_v;
+    //
+    it_out = v_out.begin();
+    for (size_t i = 0; i < m_left.size(); ++i){
+        *it_out = 0.0;
+        it_m_row = m_left[i].begin();
+        it_v = v_right.begin();
+        for (size_t j = 0; j < m_left[i].size(); ++j){
+            // *it_out += m_left[i][j] * v_right[j];
+            if (*it_m_row != 0.0 && *it_v != 0.0){
+                (*it_out) += (*it_m_row) * (*it_v);
+            }else{
+                // (*it_out) += 0.0
+            }
+            // (*it_out) += (*it_m_row) * (*it_v);
+            //
+            it_m_row++;
+            it_v++;
+        }
+        it_out++;
+    }
+    return v_out;
+}
+vector<float> MEASUREMENT_ERROR_ADAPTATION_CONTROL::Get_VectorPlus(const vector<float> &v_a, const vector<float> &v_b, bool is_minus) // v_a + (or -) v_b
+{
+    static vector<float> v_c;
+    // Size check
+    if (v_c.size() != v_a.size()){
+        v_c.resize(v_a.size());
+    }
+    //
+    for (size_t i = 0; i < v_a.size(); ++i){
+        if (is_minus){
+            v_c[i] = v_a[i] - v_b[i];
+        }else{
+            v_c[i] = v_a[i] + v_b[i];
+        }
+    }
+    return v_c;
+}
+vector<float> MEASUREMENT_ERROR_ADAPTATION_CONTROL::Get_VectorScalarMultiply(const vector<float> &v_a, float scale) // scale*v_a
+{
+    static vector<float> v_c;
+    // Size check
+    if (v_c.size() != v_a.size()){
+        v_c.resize(v_a.size());
+    }
+    // for pure negative
+    if (scale == -1.0){
+        for (size_t i = 0; i < v_a.size(); ++i){
+            v_c[i] = -v_a[i];
+        }
+        return v_c;
+    }
+    // else
+    for (size_t i = 0; i < v_a.size(); ++i){
+        v_c[i] = scale*v_a[i];
+
+    }
+    return v_c;
+}
+// Increment
+void MEASUREMENT_ERROR_ADAPTATION_CONTROL::Get_VectorIncrement(vector<float> &v_a, const vector<float> &v_b, bool is_minus){ // v_a += (or -=) v_b
+    // Size check
+    if (v_a.size() != v_b.size()){
+        v_a.resize(v_b.size());
+    }
+    //
+    if (is_minus){ // -=
+        for (size_t i = 0; i < v_b.size(); ++i){
+            v_a[i] -= v_b[i];
+        }
+    }else{ // +=
+        for (size_t i = 0; i < v_b.size(); ++i){
+            v_a[i] += v_b[i];
+        }
+    }
+
+}