Revision 0:8dbe2a4687b8, committed 2016-12-26

Comitter:

benson516

Date:

Mon Dec 26 07:54:17 2016 +0000

Child:

1:cdd434f6aa9a

Commit message:

First build, rewrite the matrix-vector multiplication using iterator operation (reduce the cost at indexing elements in vectors)

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/STATE_FEEDBACK.cpp	Mon Dec 26 07:54:17 2016 +0000
@@ -0,0 +1,37 @@
+#include "STATE_FEEDBACK.h"
+
+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))
+{
+    zeros_n.assign(n, 0.0);
+    zeros_p.assign(p, 0.0);
+    zeros_q.assign(q, 0.0);
+    //
+    states = zeros_n;
+    sys_inputs = zeros_p;
+    sys_outputs = zeros_q;
+
+}
+void STATE_FEEDBACK::fullStateFeedBack_calc(){
+    Mat_multiply_Vec(sys_inputs, K_full, states);
+}
+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;
+    //
+    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++;
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/STATE_FEEDBACK.h	Mon Dec 26 07:54:17 2016 +0000
@@ -0,0 +1,35 @@
+#ifndef STATE_FEEDBACK_H
+#define STATE_FEEDBACK_H
+//
+#include <vector>
+
+using std::vector;
+
+class STATE_FEEDBACK{
+    // Dimensions
+    size_t n; // Number of states
+    size_t p; // Number of inputs of the plant
+    size_t q; // Number of outputs of the plant, no use in full state feed back case
+
+    float Ts; // Sampling time
+
+    vector<vector<float> > K_full; // Full state feedback gain
+    //
+    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
+
+
+    STATE_FEEDBACK(size_t num_state, size_t num_in, size_t num_out, float samplingTime);
+    void fullStateFeedBack_calc();
+
+private:
+
+    vector<float> zeros_n;
+    vector<float> zeros_p;
+    vector<float> zeros_q;
+
+    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
+};
+
+#endif