Diff: main.cpp

Revision:

1:92f42e198925

Parent:

0:44701eab0261

Child:

2:d4c480d17944

--- a/main.cpp	Wed Nov 06 12:36:55 2019 +0000
+++ b/main.cpp	Thu Nov 14 01:28:23 2019 +0000
@@ -1,23 +1,98 @@
 /* 
- * reading and print acc and gyro date from MPU9250
+ * read and print acc, gyro,temperature date from MPU9250
+ * and transform accelerate data to one dimension.
  * in terminal:
  *  ls /dev/tty.*
  *  screen /dev/tty.usbmodem14102 9600
+ * to see the result
  *
  * mbed Microcontroller Library
- * Copyright (c) 2018 ARM Limited
- * SPDX-License-Identifier: Apache-2.0
+ * Eigen Library
  */
 
 #include "mbed.h"
 #include "platform/mbed_thread.h"
 #include "stats_report.h"
 #include "MPU9250.h"
+#include <Eigen/Dense.h>
+#include <iostream>
+
+using namespace std;
+using namespace Eigen;
 
 DigitalOut led1(LED1);
 const int addr7bit = 0x68; // 7bit I2C address,AD0 is 0
 
-#define SLEEP_TIME                  1000 // (msec)
+#define SLEEP_TIME                  5000 // (msec)
+
+
+/*
+  * Normalize the Matrix X
+  */
+ MatrixXd featurnormail(MatrixXd &X)
+{
+    //I don't know why to use the transpose
+    //compute the mean of every dimension
+    MatrixXd X1 = X.transpose();
+    MatrixXd meanval = X1.colwise().mean();
+    
+    //normalization
+    RowVectorXd meanvecRow = meanval;
+    X1.rowwise() -= meanvecRow;
+    
+    return X1.transpose();
+}
+
+ /*
+  * Compute the Covariane Matrix of X, put to C
+  * C = 1/m * X * X.transpose 
+  */
+void ComComputeCov(MatrixXd &X, MatrixXd &C)
+{
+   
+    C = X*X.adjoint();//same as XT*X a
+    //translate to array
+    C = C.array() / X.cols();
+}
+ 
+ 
+/*
+ * Compute the eigenvalue and eigenvector of C
+ * val = (first eigenvalue) --smallest --not important
+ *              .
+ *              .
+ *              .
+ *       (last eigenvalue)  --largest -- important
+ *
+ * vec = (first eigenvector, ... , last eigenvector)
+ *           not important          important
+ */
+void ComputEig(MatrixXd &C, MatrixXd &vec, MatrixXd &val)
+{
+    //SelfAdjointEigenSolver will sort the values automatically
+    SelfAdjointEigenSolver<MatrixXd> eig(C);
+    vec = eig.eigenvectors();
+    val = eig.eigenvalues();
+}
+ 
+/* Compute the dimension need to include enough information of raw data.
+ * form large index to small index, since the val is sorted from small to large.
+ * in some cases, just decide the number of dimension, instead of compute it.
+ */
+int ComputDim(MatrixXd &val)
+{
+    int dim;
+    double sum = 0;
+    for (int i = val.rows() - 1; i >= 0;--i)
+    {
+        sum += val(i, 0);
+        dim = i;
+        if (sum / val.sum()>=0.8)//80% of the information
+            break;
+    }
+    return val.rows() - dim;
+}
+
 
 // main() runs in its own thread in the OS
 int main()
@@ -31,6 +106,13 @@
     float GyroRead[3];
     float TempRead[1];
     
+    
+    MatrixXd acc_raw(3,0);
+    Vector3d acc_new;
+    MatrixXd C;
+    MatrixXd vec, val;
+    int dim = 1;    //dimension of PCA
+    
     while (true) {
         
         //Blink LED and wait 1 seconds
@@ -42,5 +124,23 @@
         printf("gyro value is (%f,%f,%f).\n\r",GyroRead[0],GyroRead[1],GyroRead[2]);
         printf("temp value is %f.\n\r",TempRead[0]);
         
+        //append new data to matrix acc_raw
+        //adding the columns
+        acc_new << AccRead[0],AccRead[1],AccRead[2];
+        acc_raw.conservativeResize(acc_raw.rows(), acc_raw.cols()+1);
+        acc_raw.col(acc_raw.cols()-1) = acc_new;
+        
+        cout << "acc_raw:" << acc_raw << endl;
+        
+        //run PCA
+        MatrixXd X1=featurnormail(acc_raw);
+        ComComputeCov(X1, C);
+        ComputEig(C, vec, val);
+        //select dim num of eigenvector from right to left. right is important
+        //compute the result array
+        MatrixXd res = vec.rightCols(dim).transpose()*X1;
+        
+        //show the result after PCA
+        cout << "result" << res << endl;
     }
 }