Diff: STATE_FEEDBACK.cpp

Revision:

2:0544e16ea933

Parent:

1:cdd434f6aa9a

Child:

3:7ff53317e0a4

--- 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++;