mix code vision 2. Using the previous algorithm to detect peaks as Nikoleta and Shiyao

Dependencies:   mpu9250_i2c biquadFilter peakdetection Eigen