Diff: STATES_OBSERVER_DELAY.cpp

Revision:

1:085d41355949

Parent:

0:0699f8e638ca

--- a/STATES_OBSERVER_DELAY.cpp	Wed Jan 11 09:30:18 2017 +0000
+++ b/STATES_OBSERVER_DELAY.cpp	Fri Feb 10 18:27:14 2017 +0000
@@ -18,6 +18,7 @@
         enable_Cd = false; // Default: not using Cd
         q = n;
         zeros_q.resize(q, 0.0);
+        Cd.assign(q,zeros_n);
     }else{
         enable_Cd = true; // using Cd
         Cd.assign(q,zeros_n);
@@ -101,6 +102,29 @@
         }
     }
 }
+void STATES_OBSERVER_DELAY::assign_Lt(float* Lt_in, size_t n_in, size_t d_in, size_t q_in){ // Continuous-time version
+    // Lt_in is the pointer of a mutidimentional array with size n_in*(d_in+1) by q_in
+    if (n != n_in || d != d_in || q != q_in){
+        n = n_in;
+        q = q_in;
+        zeros_n.resize(n, 0.0);
+        zeros_q.resize(q, 0.0);
+        //
+        if (d != d_in){
+            d = d_in;
+            states_est_buffer.Init((d+1), zeros_n);
+        }
+        Ld.assign(n*(d+1), zeros_q);
+    }
+    //
+    for (size_t i = 0; i < n*(d+1); ++i){
+        for (size_t j = 0; j < q; ++j){
+            // Ld[i][j] = Lt_in[i][j];
+            Ld[i][j] = (*Lt_in)*Ts;
+            Lt_in++;
+        }
+    }
+}
 // ** Assign the continuous-time version of system matrices by matrices
 void STATES_OBSERVER_DELAY::assign_At(const vector<vector<float> > &At_in){
     size_t n_in = At_in.size();
@@ -139,6 +163,25 @@
         }
     }
 }
+void STATES_OBSERVER_DELAY::assign_Lt(const vector<vector<float> > &Lt_in){
+    size_t ndp1_in = Lt_in.size();
+    size_t q_in = Lt_in[0].size();
+    //
+    if ( n*(d+1) != ndp1_in || q != q_in){
+        n = ndp1_in/(d+1);
+        q = q_in;
+        zeros_n.resize(n, 0.0);
+        zeros_q.resize(q, 0.0);
+        Ld.assign(n*(d+1), zeros_q);
+    }
+    // Ld
+    for (size_t i = 0; i < n*(d+1); ++i){
+        for (size_t j = 0; j < q; ++j){
+            //
+            Ld[i][j] = Ts*Lt_in[i][j];
+        }
+    }
+}
 // Assign discrete-time version of system matrices directly
 void STATES_OBSERVER_DELAY::assign_Ad(float* Ad_in, size_t n_in){ // Discrete-time version
     // Ad_in is the pointer of a mutidimentional array with size n_in by n_in
@@ -177,6 +220,23 @@
 //
 void STATES_OBSERVER_DELAY::assign_Cd(float* Cd_in, size_t q_in, size_t n_in){
     // Cd_in is the pointer of a mutidimentional array with size q_in by n_in
+    // q = n if not using Cd
+    if (q_in == 0){
+        enable_Cd = false; // Default: not using Cd
+        q_in = n_in;
+        //
+        q = q_in;
+        n = n_in;
+        zeros_n.resize(n,0.0);
+        zeros_q.resize(q, 0.0);
+        Cd.assign(q,zeros_n);
+        //
+        return;
+    }else{
+        enable_Cd = true; // using Cd
+        // Cd.assign(q,zeros_n);
+    }
+
     if (n != n_in || q != q_in){
         n = n_in;
         q = q_in;
@@ -192,15 +252,7 @@
             Cd_in++;
         }
     }
-    // q = n if not using Cd
-    if (q == 0){
-        enable_Cd = false; // Default: not using Cd
-        q = n;
-        zeros_q.resize(q, 0.0);
-    }else{
-        enable_Cd = true; // using Cd
-        Cd.assign(q,zeros_n);
-    }
+
 }
 // Assignment for observer Gain
 void STATES_OBSERVER_DELAY::assign_Ld(float* Ld_in, size_t n_in, size_t d_in, size_t q_in){
@@ -237,6 +289,7 @@
         return;
     }
 
+
     // Inputs of the observer:  sys_inputs, sys_outputs
     //
     // Get the delayed estimation of states
@@ -252,6 +305,7 @@
         est_error = Get_VectorPlus(states_est_buffer.Get(d), sys_outputs,true); // minus
     }
 
+
     // Get estimation
     state_est = states_est_buffer.Get(0);
 
@@ -276,23 +330,46 @@
 
     // Rotate the buffer
     states_est_buffer.Insert(states_new);
+    
+
+    /*
+    // No-delay version
+    //----------------------------------------//
+    if (enable_Cd){
+        // y_est = Cd*states_est_delay_d
+        y_est = Mat_multiply_Vec(Cd, state_est);
+        // est_error = y_est - sys_outputs
+        est_error = Get_VectorPlus(y_est,sys_outputs, true); // minus
+    }else{
+        // est_error = y_est - sys_outputs
+        est_error = Get_VectorPlus(state_est, sys_outputs, true); // minus
+    }
+    //
+    state_est = Get_VectorPlus(Mat_multiply_Vec(Ad,state_est), Mat_multiply_Vec(Bd,sys_inputs), false); // plus
+    // Get_VectorIncrement(state_est, sys_extraDisturbance, false); // +=, extra disturbances
+    Get_VectorIncrement(state_est, Mat_multiply_Vec(Ld, est_error), true); // -=
+    */
 }
 
 // Utilities
 void STATES_OBSERVER_DELAY::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;
+
     // Size check
     if (v_out.size() != m_left.size()){
         v_out.resize(m_left.size());
     }
+
+    /*
+    //
+    static vector<float>::iterator it_out;
+    static vector<float>::iterator it_m_row;
+    static vector<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();
+        it_m_row = vector<vector<float> >(m_left)[i].begin();
+        it_v = vector<float>(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){
@@ -307,6 +384,19 @@
         }
         it_out++;
     }
+    */
+
+    // Indexing
+    for (size_t i = 0; i < m_left.size(); ++i){
+        //
+        v_out[i] = 0.0;
+        //
+        for (size_t j = 0; j < v_right.size(); ++j){
+            if (m_left[i][j] != 0.0 && v_right[j] != 0.0)
+                v_out[i] += m_left[i][j]*v_right[j];
+        }
+    }
+
 }
 vector<float> STATES_OBSERVER_DELAY::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;
@@ -314,16 +404,17 @@
     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;
+    static vector<float>::iterator it_m_row;
+    static vector<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();
+        it_m_row = vector<vector<float> >(m_left)[i].begin();
+        it_v = vector<float>(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){
@@ -338,6 +429,19 @@
         }
         it_out++;
     }
+    */
+
+    // Indexing
+    for (size_t i = 0; i < m_left.size(); ++i){
+        //
+        v_out[i] = 0.0;
+        //
+        for (size_t j = 0; j < v_right.size(); ++j){
+            if (m_left[i][j] != 0.0 && v_right[j] != 0.0)
+                v_out[i] += m_left[i][j]*v_right[j];
+        }
+    }
+
     return v_out;
 }
 vector<float> STATES_OBSERVER_DELAY::Get_VectorPlus(const vector<float> &v_a, const vector<float> &v_b, bool is_minus) // v_a + (or -) v_b
@@ -403,16 +507,17 @@
     if (v_out.size() != numRow_m_left){
         v_out.resize(numRow_m_left);
     }
+    /*
     // Iterators
     static vector<float>::iterator it_out;
     static vector<float>::iterator it_m_row;
-    static vector<const float>::iterator it_v;
+    static vector<float>::iterator it_v;
     //
     it_out = v_out.begin();
     for (size_t i = 0; i < numRow_m_left; ++i){
         *it_out = 0.0;
         it_m_row = (*it_m_left).begin();
-        it_v = v_right.begin();
+        it_v = vector<float>(v_right).begin();
         for (size_t j = 0; j < (*it_m_left).size(); ++j){
             // *it_out += m_left[i][j] * v_right[j];
             if (*it_m_row != 0.0 && *it_v != 0.0){
@@ -428,5 +533,22 @@
         it_out++;
         it_m_left++;
     }
+    */
+
+    //
+    // Indexing
+    for (size_t i = 0; i < numRow_m_left; ++i){
+        //
+        v_out[i] = 0.0;
+        //
+        for (size_t j = 0; j < v_right.size(); ++j){
+            if ( (*it_m_left)[j] != 0.0 && v_right[j] != 0.0)
+                v_out[i] += (*it_m_left)[j]*v_right[j];
+        }
+        //
+        it_m_left++;
+    }
+
+
     return v_out;
 }