File content as of revision 1:92f42e198925:

/* 
 * 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
 * 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                  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()
{   
    //new mpu(data,clk,address),in constructor addr7bit<<1
    mpu9250 *mpu = new mpu9250(p26,p27,addr7bit);
    //scale of acc and gyro
    mpu->initMPU9250(0x00,0x00);

    float AccRead[3];
    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
        led1 = !led1;
        thread_sleep_for(SLEEP_TIME);
        //read and convert date
        mpu->ReadConvertAll(AccRead,GyroRead,TempRead);
        printf("acc value is (%f,%f,%f).\n\r",AccRead[0],AccRead[1],AccRead[2]);
        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;
    }
}