wenchang fei / Mbed OS the-lastest-code

the lastest code by Castle. 27 Nov

Dependencies:   mpu9250_i2c biquadFilter PCA peakdetection Eigen

main.cpp@1:92f42e198925, 2019-11-14 (annotated)

Committer:
castlefei
Date:
Thu Nov 14 01:28:23 2019 +0000
Revision:
1:92f42e198925
Parent:
0:44701eab0261
Child:
2:d4c480d17944
second test vistion; read data and transform acc to one dimension using PCA

Who changed what in which revision?

UserRevisionLine numberNew contents of line
castlefei 0:44701eab0261 1 /*
castlefei 1:92f42e198925 2 * read and print acc, gyro,temperature date from MPU9250
castlefei 1:92f42e198925 3 * and transform accelerate data to one dimension.
castlefei 0:44701eab0261 4 * in terminal:
castlefei 0:44701eab0261 5 * ls /dev/tty.*
castlefei 0:44701eab0261 6 * screen /dev/tty.usbmodem14102 9600
castlefei 1:92f42e198925 7 * to see the result
castlefei 0:44701eab0261 8 *
castlefei 0:44701eab0261 9 * mbed Microcontroller Library
castlefei 1:92f42e198925 10 * Eigen Library
castlefei 0:44701eab0261 11 */
castlefei 0:44701eab0261 12
castlefei 0:44701eab0261 13 #include "mbed.h"
castlefei 0:44701eab0261 14 #include "platform/mbed_thread.h"
castlefei 0:44701eab0261 15 #include "stats_report.h"
castlefei 0:44701eab0261 16 #include "MPU9250.h"
castlefei 1:92f42e198925 17 #include <Eigen/Dense.h>
castlefei 1:92f42e198925 18 #include <iostream>
castlefei 1:92f42e198925 19
castlefei 1:92f42e198925 20 using namespace std;
castlefei 1:92f42e198925 21 using namespace Eigen;
castlefei 0:44701eab0261 22
castlefei 0:44701eab0261 23 DigitalOut led1(LED1);
castlefei 0:44701eab0261 24 const int addr7bit = 0x68; // 7bit I2C address,AD0 is 0
castlefei 0:44701eab0261 25
castlefei 1:92f42e198925 26 #define SLEEP_TIME 5000 // (msec)
castlefei 1:92f42e198925 27
castlefei 1:92f42e198925 28
castlefei 1:92f42e198925 29 /*
castlefei 1:92f42e198925 30 * Normalize the Matrix X
castlefei 1:92f42e198925 31 */
castlefei 1:92f42e198925 32 MatrixXd featurnormail(MatrixXd &X)
castlefei 1:92f42e198925 33 {
castlefei 1:92f42e198925 34 //I don't know why to use the transpose
castlefei 1:92f42e198925 35 //compute the mean of every dimension
castlefei 1:92f42e198925 36 MatrixXd X1 = X.transpose();
castlefei 1:92f42e198925 37 MatrixXd meanval = X1.colwise().mean();
castlefei 1:92f42e198925 38
castlefei 1:92f42e198925 39 //normalization
castlefei 1:92f42e198925 40 RowVectorXd meanvecRow = meanval;
castlefei 1:92f42e198925 41 X1.rowwise() -= meanvecRow;
castlefei 1:92f42e198925 42
castlefei 1:92f42e198925 43 return X1.transpose();
castlefei 1:92f42e198925 44 }
castlefei 1:92f42e198925 45
castlefei 1:92f42e198925 46 /*
castlefei 1:92f42e198925 47 * Compute the Covariane Matrix of X, put to C
castlefei 1:92f42e198925 48 * C = 1/m * X * X.transpose
castlefei 1:92f42e198925 49 */
castlefei 1:92f42e198925 50 void ComComputeCov(MatrixXd &X, MatrixXd &C)
castlefei 1:92f42e198925 51 {
castlefei 1:92f42e198925 52
castlefei 1:92f42e198925 53 C = X*X.adjoint();//same as XT*X a
castlefei 1:92f42e198925 54 //translate to array
castlefei 1:92f42e198925 55 C = C.array() / X.cols();
castlefei 1:92f42e198925 56 }
castlefei 1:92f42e198925 57
castlefei 1:92f42e198925 58
castlefei 1:92f42e198925 59 /*
castlefei 1:92f42e198925 60 * Compute the eigenvalue and eigenvector of C
castlefei 1:92f42e198925 61 * val = (first eigenvalue) --smallest --not important
castlefei 1:92f42e198925 62 * .
castlefei 1:92f42e198925 63 * .
castlefei 1:92f42e198925 64 * .
castlefei 1:92f42e198925 65 * (last eigenvalue) --largest -- important
castlefei 1:92f42e198925 66 *
castlefei 1:92f42e198925 67 * vec = (first eigenvector, ... , last eigenvector)
castlefei 1:92f42e198925 68 * not important important
castlefei 1:92f42e198925 69 */
castlefei 1:92f42e198925 70 void ComputEig(MatrixXd &C, MatrixXd &vec, MatrixXd &val)
castlefei 1:92f42e198925 71 {
castlefei 1:92f42e198925 72 //SelfAdjointEigenSolver will sort the values automatically
castlefei 1:92f42e198925 73 SelfAdjointEigenSolver<MatrixXd> eig(C);
castlefei 1:92f42e198925 74 vec = eig.eigenvectors();
castlefei 1:92f42e198925 75 val = eig.eigenvalues();
castlefei 1:92f42e198925 76 }
castlefei 1:92f42e198925 77
castlefei 1:92f42e198925 78 /* Compute the dimension need to include enough information of raw data.
castlefei 1:92f42e198925 79 * form large index to small index, since the val is sorted from small to large.
castlefei 1:92f42e198925 80 * in some cases, just decide the number of dimension, instead of compute it.
castlefei 1:92f42e198925 81 */
castlefei 1:92f42e198925 82 int ComputDim(MatrixXd &val)
castlefei 1:92f42e198925 83 {
castlefei 1:92f42e198925 84 int dim;
castlefei 1:92f42e198925 85 double sum = 0;
castlefei 1:92f42e198925 86 for (int i = val.rows() - 1; i >= 0;--i)
castlefei 1:92f42e198925 87 {
castlefei 1:92f42e198925 88 sum += val(i, 0);
castlefei 1:92f42e198925 89 dim = i;
castlefei 1:92f42e198925 90 if (sum / val.sum()>=0.8)//80% of the information
castlefei 1:92f42e198925 91 break;
castlefei 1:92f42e198925 92 }
castlefei 1:92f42e198925 93 return val.rows() - dim;
castlefei 1:92f42e198925 94 }
castlefei 1:92f42e198925 95
castlefei 0:44701eab0261 96
castlefei 0:44701eab0261 97 // main() runs in its own thread in the OS
castlefei 0:44701eab0261 98 int main()
castlefei 0:44701eab0261 99 {
castlefei 0:44701eab0261 100 //new mpu(data,clk,address),in constructor addr7bit<<1
castlefei 0:44701eab0261 101 mpu9250 *mpu = new mpu9250(p26,p27,addr7bit);
castlefei 0:44701eab0261 102 //scale of acc and gyro
castlefei 0:44701eab0261 103 mpu->initMPU9250(0x00,0x00);
castlefei 0:44701eab0261 104
castlefei 0:44701eab0261 105 float AccRead[3];
castlefei 0:44701eab0261 106 float GyroRead[3];
castlefei 0:44701eab0261 107 float TempRead[1];
castlefei 0:44701eab0261 108
castlefei 1:92f42e198925 109
castlefei 1:92f42e198925 110 MatrixXd acc_raw(3,0);
castlefei 1:92f42e198925 111 Vector3d acc_new;
castlefei 1:92f42e198925 112 MatrixXd C;
castlefei 1:92f42e198925 113 MatrixXd vec, val;
castlefei 1:92f42e198925 114 int dim = 1; //dimension of PCA
castlefei 1:92f42e198925 115
castlefei 0:44701eab0261 116 while (true) {
castlefei 0:44701eab0261 117
castlefei 0:44701eab0261 118 //Blink LED and wait 1 seconds
castlefei 0:44701eab0261 119 led1 = !led1;
castlefei 0:44701eab0261 120 thread_sleep_for(SLEEP_TIME);
castlefei 0:44701eab0261 121 //read and convert date
castlefei 0:44701eab0261 122 mpu->ReadConvertAll(AccRead,GyroRead,TempRead);
castlefei 0:44701eab0261 123 printf("acc value is (%f,%f,%f).\n\r",AccRead[0],AccRead[1],AccRead[2]);
castlefei 0:44701eab0261 124 printf("gyro value is (%f,%f,%f).\n\r",GyroRead[0],GyroRead[1],GyroRead[2]);
castlefei 0:44701eab0261 125 printf("temp value is %f.\n\r",TempRead[0]);
castlefei 0:44701eab0261 126
castlefei 1:92f42e198925 127 //append new data to matrix acc_raw
castlefei 1:92f42e198925 128 //adding the columns
castlefei 1:92f42e198925 129 acc_new << AccRead[0],AccRead[1],AccRead[2];
castlefei 1:92f42e198925 130 acc_raw.conservativeResize(acc_raw.rows(), acc_raw.cols()+1);
castlefei 1:92f42e198925 131 acc_raw.col(acc_raw.cols()-1) = acc_new;
castlefei 1:92f42e198925 132
castlefei 1:92f42e198925 133 cout << "acc_raw:" << acc_raw << endl;
castlefei 1:92f42e198925 134
castlefei 1:92f42e198925 135 //run PCA
castlefei 1:92f42e198925 136 MatrixXd X1=featurnormail(acc_raw);
castlefei 1:92f42e198925 137 ComComputeCov(X1, C);
castlefei 1:92f42e198925 138 ComputEig(C, vec, val);
castlefei 1:92f42e198925 139 //select dim num of eigenvector from right to left. right is important
castlefei 1:92f42e198925 140 //compute the result array
castlefei 1:92f42e198925 141 MatrixXd res = vec.rightCols(dim).transpose()*X1;
castlefei 1:92f42e198925 142
castlefei 1:92f42e198925 143 //show the result after PCA
castlefei 1:92f42e198925 144 cout << "result" << res << endl;
castlefei 0:44701eab0261 145 }
castlefei 0:44701eab0261 146 }