Diff: STATE_FEEDBACK.cpp

Revision:

1:cdd434f6aa9a

Parent:

0:8dbe2a4687b8

Child:

2:0544e16ea933

--- a/STATE_FEEDBACK.cpp	Mon Dec 26 07:54:17 2016 +0000
+++ b/STATE_FEEDBACK.cpp	Tue Dec 27 07:44:12 2016 +0000
@@ -12,13 +12,34 @@
     sys_outputs = zeros_q;
 
 }
+// Assign Parameters
+void STATE_FEEDBACK::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){
+        n = n_in;
+        p = p_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 STATE_FEEDBACK::fullStateFeedBack_calc(){
-    Mat_multiply_Vec(sys_inputs, K_full, states);
+    sys_inputs = Get_VectorScalarMultiply(Mat_multiply_Vec(K_full, states),-1.0);
 }
+
+// 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
-    vector<float>::iterator it_out;
-    vector<const float>::iterator it_m_row;
-    vector<const float>::iterator it_v;
+    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){
@@ -35,3 +56,68 @@
         it_out++;
     }
 }
+vector<float> STATE_FEEDBACK::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];
+            (*it_out) += (*it_m_row) * (*it_v);
+            //
+            it_m_row++;
+            it_v++;
+        }
+        it_out++;
+    }
+    return v_out;
+}
+vector<float> STATE_FEEDBACK::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 < n; ++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> STATE_FEEDBACK::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 < n; ++i){
+            v_c[i] = -v_a[i];
+        }
+        return v_c;
+    }
+    // else
+    for (size_t i = 0; i < n; ++i){
+        v_c[i] = scale*v_a[i];
+
+    }
+    return v_c;
+}