Revision 2:0544e16ea933, committed 2016-12-29

Comitter:

benson516

Date:

Thu Dec 29 16:40:52 2016 +0000

Parent:

1:cdd434f6aa9a

Child:

3:7ff53317e0a4

Commit message:

Add the capability of tracking control

Changed in this revision

--- a/STATE_FEEDBACK.cpp	Tue Dec 27 07:44:12 2016 +0000
+++ b/STATE_FEEDBACK.cpp	Thu Dec 29 16:40:52 2016 +0000
@@ -1,8 +1,23 @@
 #include "STATE_FEEDBACK.h"
+// The controller is for the plant with (p, n, q) system
+// Dimensions:
+//
+// Inputs, u   |   States, x    |   outputs, y
+//     p     -->      n        -->      q
+//
+//
 
 STATE_FEEDBACK::STATE_FEEDBACK(size_t num_state, size_t num_in, size_t num_out, float samplingTime):
-    n(num_state), p(num_in), q(num_out), Ts(samplingTime), K_full(num_in, vector<float>(num_state,0.0))
+    n(num_state), p(num_in), q(num_out), Ts(samplingTime),
+    K_full(num_in, vector<float>(num_state,0.0)),
+    // N_xd(num_state, vector<float>(num_out, 0.0)),
+    // N_ud(num_in, vector<float>(num_out, 0.0)),
+    N_total(num_in, vector<float>(num_out,0.0))
 {
+    // Normally, q = p
+    if (q > p)
+        q = p;
+    //
     zeros_n.assign(n, 0.0);
     zeros_p.assign(p, 0.0);
     zeros_q.assign(q, 0.0);
@@ -11,6 +26,13 @@
     sys_inputs = zeros_p;
     sys_outputs = zeros_q;
 
+    // Command (equalibrium state)
+    // states_d = zeros_n;
+    // inputs_d = zeros_p;
+    sys_inputs_compensate = zeros_p;
+    command = zeros_q; // q = p
+
+
 }
 // Assign Parameters
 void STATE_FEEDBACK::assign_K_full(float* K_full_in, size_t p_in, size_t n_in){
@@ -31,10 +53,80 @@
         }
     }
 }
+/*
+void STATE_FEEDBACK::assign_N_xd(float* N_xd_in, size_t n_in, size_t q_in){
+    // N_xd_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);
+        N_xd.assign(n, zeros_q);
+    }
+    //
+    for (size_t i = 0; i < n; ++i){
+        for (size_t j = 0; j < q; ++j){
+            // N_xd[i][j] = N_xd_in[i][j];
+            N_xd[i][j] = *N_xd_in;
+            N_xd_in++;
+        }
+    }
+}
+void STATE_FEEDBACK::assign_N_ud(float* N_ud_in, size_t p_in, size_t q_in){
+    // N_ud_in is the pointer of a mutidimentional array with size p_in by q_in
+    if (p != p_in || q != q_in){
+        p = p_in;
+        q = q_in;
+        zeros_p.resize(p, 0.0);
+        zeros_q.resize(q, 0.0);
+        N_ud.assign(p, zeros_q);
+    }
+    //
+    for (size_t i = 0; i < p; ++i){
+        for (size_t j = 0; j < q; ++j){
+            // N_ud[i][j] = N_ud_in[i][j];
+            N_ud[i][j] = *N_ud_in;
+            N_ud_in++;
+        }
+    }
+}
+*/
+void STATE_FEEDBACK::assign_N_total(float* N_total_in, size_t p_in, size_t q_in){
+    // N_total_in is the pointer of a mutidimentional array with size p_in by q_in
+    if (p != p_in || q != q_in){
+        p = p_in;
+        q = q_in;
+        zeros_p.resize(p, 0.0);
+        zeros_q.resize(q, 0.0);
+        N_total.assign(p, zeros_q);
+    }
+    //
+    for (size_t i = 0; i < p; ++i){
+        for (size_t j = 0; j < q; ++j){
+            // N_total[i][j] = N_total_in[i][j];
+            N_total[i][j] = *N_total_in;
+            N_total_in++;
+        }
+    }
+}
 void STATE_FEEDBACK::fullStateFeedBack_calc(){
-    sys_inputs = Get_VectorScalarMultiply(Mat_multiply_Vec(K_full, states),-1.0);
+    // sys_inputs = Get_VectorScalarMultiply(Mat_multiply_Vec(K_full, states),-1.0);
+    //
+    // With command input
+    get_Xd_ud();
+    // sys_inputs = Get_VectorPlus(inputs_d, Mat_multiply_Vec(K_full, Get_VectorPlus(states, states_d, true)), true); // minus
+    sys_inputs = Get_VectorPlus(sys_inputs_compensate, Mat_multiply_Vec(K_full, states), true); // minus
 }
 
+// Private functions
+// Command (equalibrium state) related calculation
+void STATE_FEEDBACK::get_Xd_ud(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);
+    //
+    Mat_multiply_Vec(sys_inputs_compensate, N_total, command);
+
+}
 // Utilities
 void STATE_FEEDBACK::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;
@@ -48,7 +140,12 @@
         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];
-            (*it_out) += (*it_m_row) * (*it_v);
+            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++;
@@ -74,7 +171,12 @@
         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];
-            (*it_out) += (*it_m_row) * (*it_v);
+            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++;

--- a/STATE_FEEDBACK.h	Tue Dec 27 07:44:12 2016 +0000
+++ b/STATE_FEEDBACK.h	Thu Dec 29 16:40:52 2016 +0000
@@ -15,17 +15,28 @@
     float Ts; // Sampling time
 
     vector<vector<float> > K_full; // Full state feedback gain
+    // 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
+    vector<vector<float> > N_total; // Feed-forward gain for compensating u, u = -K*x + N_total*r
+
     //
     vector<float> states; // States
     vector<float> sys_inputs; // The inputs of the plant, "u", the "output" of the controller
     vector<float> sys_outputs; // The output of the plant, "y", the input of the controller
-
+    // Command (equalibrium state)
+    // vector<float> states_d; // x_d
+    // vector<float> inputs_d; // u_d
+    vector<float> sys_inputs_compensate; // N_total*r
+    vector<float> command; // r
 
     STATE_FEEDBACK(size_t num_state, size_t num_in, size_t num_out, float samplingTime);
     // Assign Parameters
     void assign_K_full(float* K_full_in, size_t p_in, size_t n_in);
+    // void assign_N_xd(float* N_xd_in, size_t n_in, size_t q_in);
+    // void assign_N_ud(float* N_ud_in, size_t p_in, size_t q_in);
+    void assign_N_total(float* N_ud_in, size_t p_in, size_t q_in);
     //
-    void fullStateFeedBack_calc();
+    void fullStateFeedBack_calc(void);
 
 private:
 
@@ -33,6 +44,9 @@
     vector<float> zeros_p;
     vector<float> zeros_q;
 
+    // Command (equalibrium state) related calculation
+    void get_Xd_ud(void); // Calculate the compensation variable, states_d and sys_inputs_compensate
+
     // 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