Diff: main.cpp

Revision:

6:7982fe5f1806

Parent:

5:c37def827168

--- a/main.cpp	Mon Nov 25 23:44:37 2019 +0000
+++ b/main.cpp	Tue Nov 26 14:28:36 2019 +0000
@@ -31,8 +31,7 @@
 BiQuad mybq(0.3403575989782886,0.6807151979565772,0.3403575989782886, -1.511491371967327e-16,0.36143039591315457);
 BiQuadChain bqc;
 
-
-#define SLEEP_TIME                  10 // (msec)
+#define SLEEP_TIME                  80 // (msec)
 
 
 // main() runs in its own thread in the OS
@@ -54,6 +53,7 @@
     
     MatrixXd acc_raw(3,0);
     Vector3d acc_new;
+    Vector3d acc_previous;
     MatrixXd C;
     MatrixXd vec, val;
     int dim = 1;    //dimension of PCA
@@ -64,7 +64,7 @@
     bqc.add(&mybq);
     
     //vector<float> res_list;
-    
+    acc_new << 0,0,0;
     while (true) {
         
         //Blink LED and wait 1 seconds
@@ -81,7 +81,11 @@
         
         //append new data to matrix acc_raw
         //adding the columns
+        acc_previous = acc_new;
         acc_new << AccRead[0],AccRead[1],AccRead[2];
+        //cout << "acc_previous:"<<acc_previous<<"\n\r";
+        //printf("\n\r");
+        
         acc_raw.conservativeResize(acc_raw.rows(), acc_raw.cols()+1);
         acc_raw.col(acc_raw.cols()-1) = acc_new;
         
@@ -90,7 +94,9 @@
         {
             if(number > 2)
             {
-                //cout << acc_raw << endl;
+                //cout << "acc_raw"<< acc_raw << "\n\r";
+                //printf("\n\r");
+                
                 //run PCA
                 MatrixXd X1=pca.featurnormail(acc_raw);
                 pca.ComComputeCov(X1, C);
@@ -108,7 +114,7 @@
                 {
                     res_smooth = bqc.step(res(i));
                     res_list.push_back(res_smooth);
-                    //printf("result after filter in for loop %d: %f\n\r",i,res_smooth);
+                    //printf("result%d: %f\n\r",i,res_smooth);
                     //std::cout << "\t" << bqc.step(  ) << std::endl;
                 }
                 int len = res_list.size();
@@ -120,6 +126,13 @@
                 
                 //clear the matrix to contain new data
                 acc_raw.conservativeResize(3, 0);
+                
+                //overlap windows
+                acc_raw.conservativeResize(acc_raw.rows(), acc_raw.cols()+1);
+                acc_raw.col(acc_raw.cols()-1) = acc_previous;
+                
+                acc_raw.conservativeResize(acc_raw.rows(), acc_raw.cols()+1);
+                acc_raw.col(acc_raw.cols()-1) = acc_new;
             }
         }
         number = number +1;