added magnitude

refactored plotting
refactored some code

java/src/main/java/Main.java

@@ -1,4 +1,6 @@
 import bpmEstimation.*;
+import uk.me.berndporr.iirj.Butterworth;
+import utilities.Plot;
 import utilities.Utils;
 
 import java.awt.*;
@@ -6,6 +8,7 @@ import java.io.BufferedReader;
 import java.io.File;
 import java.io.FileReader;
 import java.io.IOException;
+import java.util.Arrays;
 import java.util.LinkedList;
 import java.util.stream.IntStream;
 
@@ -16,33 +19,46 @@ import java.util.stream.IntStream;
 public class Main {
 
     public static void main(String [ ] args) {
-        //File folder = new File("/home/toni/Documents/programme/dirigent/measurements/lgWear");
+        //File folder = new File("/home/toni/Documents/programme/dirigent/measurements/2017.06/lgWear");
-        File folder = new File("/home/toni/Documents/programme/dirigent/measurements/peter_failed");
+        //File folder = new File("/home/toni/Documents/programme/dirigent/measurements/peter_failed");
+        File folder = new File("/home/toni/Documents/programme/dirigent/measurements/2018.06/frank/mSensor");
         File[] listOfFiles = folder.listFiles();
+        Arrays.sort(listOfFiles);
 
-        Utils.ShowPNG windowRaw = new Utils.ShowPNG();
+
-        Utils.ShowPNG windowAuto = new Utils.ShowPNG();
+//        //TODO: write debug class, that is able to simply draw images...
-        Utils.ShowPNG windowPeaksX = new Utils.ShowPNG();
+//        Utils.ShowPNG windowRaw = new Utils.ShowPNG();
-        Utils.ShowPNG windowPeaksY = new Utils.ShowPNG();
+//        Utils.ShowPNG windowAuto = new Utils.ShowPNG();
-        Utils.ShowPNG windowPeaksZ = new Utils.ShowPNG();
+//        Utils.ShowPNG windowAutoButter = new Utils.ShowPNG();
+//        Utils.ShowPNG windowPeaksX = new Utils.ShowPNG();
+//        Utils.ShowPNG windowPeaksY = new Utils.ShowPNG();
+//        Utils.ShowPNG windowPeaksZ = new Utils.ShowPNG();
 
 
         // iterate trough files in measurements folder
         for (File file : listOfFiles) {
             if (file.isFile() && file.getName().contains(".csv")) {
 
-                //TODO: mach Fenster genau 6 sekunden groß. Egal wie viele Samples.
+                //TODO: Die Raw Sensordaten sollte man vielleicht etwas glätten. Sieh Aufnahmen von Frank!
-                AccelerometerWindowBuffer accWindowBuffer = new AccelerometerWindowBuffer(6000, 1500);
+                AccelerometerWindowBuffer accWindowBuffer = new AccelerometerWindowBuffer(6000, 750);
                 BpmEstimator bpmEstimator = new BpmEstimator(accWindowBuffer, 0, 5000);
+                Butterworth butterLowpass = new Butterworth();
 
                 //read the file line by line
                 try (BufferedReader br = new BufferedReader(new FileReader(file))) {
+
+                    //read the first three lines and print out what file it is!
+                    String comment = br.readLine();
+                    br.readLine();
+                    br.readLine();
+                    System.out.println(comment);
+
                     for (String line; (line = br.readLine()) != null; ) {
                         // process the line.
                         String[] measurement = line.split(";");
 
                         //if linear acc
-                        if(measurement[1].equals("10")){
+                        if(measurement[1].equals("3")){
                             long ts = Long.parseLong(measurement[0]);
                             double x = Double.parseDouble(measurement[2]);
                             double y = Double.parseDouble(measurement[3]);
@@ -53,86 +69,132 @@ public class Main {
                         //do calculation stuff
                         if(accWindowBuffer.isNextWindowReady()){
 
-                            double curBpm = bpmEstimator.estimate();
+                            LinkedList<Double> bpmList = new LinkedList<>();
-                            //System.out.println("BPM: " + curBpm);
 
-                            double sampleRate = 20;
+                            // Calculate the BPM for different window sizes
-                            AccelerometerInterpolator acInterp = new AccelerometerInterpolator(accWindowBuffer, sampleRate);
+                            double bpm60 = bpmEstimator.estimate();
-                            int peakWidth = (int) Math.round(250 / sampleRate);
+                            double bpm85 = bpmEstimator.estimate(3500, 750);
+                            double bpm110 = bpmEstimator.estimate(2600, 750);
+                            double bpm135 = bpmEstimator.estimate(2000, 750);
+                            double bpm160 = bpmEstimator.estimate(1600,750);
+                            double bpm200 = bpmEstimator.estimate(1200, 750);
 
-                            //print raw x,y,z
+                            System.out.println("--------------------------------------------------");
-                            double[] dTs = IntStream.range(0, accWindowBuffer.getTs().length).mapToDouble(i -> accWindowBuffer.getTs()[i]).toArray();
-                            double[] dTsInterp = IntStream.range(0, acInterp.getTs().length).mapToDouble(i -> acInterp.getTs()[i]).toArray();
-                            Plot plotRaw = Plot.plot(Plot.plotOpts().
-                                    title("Raw Acc Data").
-                                    legend(Plot.LegendFormat.BOTTOM)).
-                                    series("x", Plot.data().xy(dTsInterp, acInterp.getX()), Plot.seriesOpts().color(Color.RED)).
-                                    series("y", Plot.data().xy(dTsInterp, acInterp.getY()), Plot.seriesOpts().color(Color.BLUE)).
-                                    series("z", Plot.data().xy(dTsInterp, acInterp.getZ()), Plot.seriesOpts().color(Color.GREEN));
 
-                            windowRaw.set(plotRaw.draw());
+                            bpmList.add(bpm60);
+                            bpmList.add(bpm85);
+                            bpmList.add(bpm110);
+                            bpmList.add(bpm135);
+                            bpmList.add(bpm160);
+                            bpmList.add(bpm200);
 
-                            //auto corr
+//                            //TODO: plot the different autocorrelation of different window sizes
-                            double[] xAutoCorr = new AutoCorrelation(acInterp.getX(), accWindowBuffer.size()).getCorr();
+//                            // Draw the data for the complete 6 second window
-                            double[] yAutoCorr = new AutoCorrelation(acInterp.getY(), accWindowBuffer.size()).getCorr();
+//                            double sampleRate = 4;
-                            double[] zAutoCorr = new AutoCorrelation(acInterp.getZ(), accWindowBuffer.size()).getCorr();
+//                            AccelerometerInterpolator acInterp = new AccelerometerInterpolator(accWindowBuffer, sampleRate);
-
+//                            int peakWidth = (int) Math.round(250 / sampleRate);
-                            //print autocorr
+//
-                            int[] tmp = IntStream.rangeClosed(-((xAutoCorr.length - 1)/2), ((xAutoCorr.length - 1)/2)).toArray();
+//                            //print raw x,y,z
-                            double[] rangeAuto = IntStream.range(0, tmp.length).mapToDouble(i -> tmp[i]).toArray();
+//                            double[] dTs = IntStream.range(0, accWindowBuffer.getTs().length).mapToDouble(i -> accWindowBuffer.getTs()[i]).toArray();
-                            Plot plotCorr = Plot.plot(Plot.plotOpts().
+//                            double[] dTsInterp = IntStream.range(0, acInterp.getTs().length).mapToDouble(i -> acInterp.getTs()[i]).toArray();
-                                    title("Auto Correlation").
+//                            Plot plotRaw = Plot.plot(Plot.plotOpts().
-                                    legend(Plot.LegendFormat.BOTTOM)).
+//                                    title("Raw Acc Data").
-                                    series("x", Plot.data().xy(rangeAuto, xAutoCorr), Plot.seriesOpts().color(Color.RED)).
+//                                    legend(Plot.LegendFormat.BOTTOM)).
-                                    series("y", Plot.data().xy(rangeAuto, yAutoCorr), Plot.seriesOpts().color(Color.BLUE)).
+//                                    series("x", Plot.data().xy(dTsInterp, acInterp.getX()), Plot.seriesOpts().color(Color.RED)).
-                                    series("z", Plot.data().xy(rangeAuto, zAutoCorr), Plot.seriesOpts().color(Color.GREEN));
+//                                    series("y", Plot.data().xy(dTsInterp, acInterp.getY()), Plot.seriesOpts().color(Color.BLUE)).
-
+//                                    series("z", Plot.data().xy(dTsInterp, acInterp.getZ()), Plot.seriesOpts().color(Color.GREEN));
-                            windowAuto.set(plotCorr.draw());
+//
-
+//                            windowRaw.set(plotRaw.draw());
-                            Peaks pX = new Peaks(xAutoCorr, peakWidth, 0.1f, 0, false);
+//
-                            int xOffset = xAutoCorr.length / 2;
+//                            //auto corr
-                            LinkedList<Integer> peaksX = pX.getPeaksIdx();
+//                            double[] xAutoCorr = new AutoCorrelation(acInterp.getX(), accWindowBuffer.size()).getCorr();
-
+//                            double[] yAutoCorr = new AutoCorrelation(acInterp.getY(), accWindowBuffer.size()).getCorr();
-                            double[] dPeaksXX = IntStream.range(0, peaksX.size()).mapToDouble(i -> (peaksX.get(i) - xOffset)).toArray();//peaks.stream().mapToDouble(i->i).toArray();
+//                            double[] zAutoCorr = new AutoCorrelation(acInterp.getZ(), accWindowBuffer.size()).getCorr();
-                            double[] dPeaksXY = IntStream.range(0, peaksX.size()).mapToDouble(i -> (xAutoCorr[peaksX.get(i)])).toArray();
+//
-                            Plot plotPeaksX = Plot.plot(Plot.plotOpts().
+//                            //print autocorr raw
-                                    title("Peak Detection on X").
+//                            int[] tmp = IntStream.rangeClosed(-((xAutoCorr.length - 1)/2), ((xAutoCorr.length - 1)/2)).toArray();
-                                    legend(Plot.LegendFormat.BOTTOM)).
+//                            double[] rangeAuto = IntStream.range(0, tmp.length).mapToDouble(i -> tmp[i]).toArray();
-                                    series("x", Plot.data().xy(rangeAuto, xAutoCorr), Plot.seriesOpts().color(Color.RED)).
+//                            Plot plotCorr = Plot.plot(Plot.plotOpts().
-                                    series("Peaks", Plot.data().xy(dPeaksXX, dPeaksXY), Plot.seriesOpts().color(Color.CYAN).
+//                                    title("Auto Correlation").
-                                            marker(Plot.Marker.DIAMOND).line(Plot.Line.NONE));
+//                                    legend(Plot.LegendFormat.BOTTOM)).
-
+//                                    series("x", Plot.data().xy(rangeAuto, xAutoCorr), Plot.seriesOpts().color(Color.RED)).
-                            windowPeaksX.set(plotPeaksX.draw());
+//                                    series("y", Plot.data().xy(rangeAuto, yAutoCorr), Plot.seriesOpts().color(Color.BLUE)).
-
+//                                    series("z", Plot.data().xy(rangeAuto, zAutoCorr), Plot.seriesOpts().color(Color.GREEN));
-                            Peaks pY = new Peaks(yAutoCorr, peakWidth, 0.1f, 0, false);
+//
-                            int yOffset = yAutoCorr.length / 2;
+//                            windowAuto.set(plotCorr.draw());
-                            LinkedList<Integer> peaksY = pY.getPeaksIdx();
+//
-
+//                            //print autocorr butter
-                            double[] dPeaksYX = IntStream.range(0, peaksY.size()).mapToDouble(i -> (peaksY.get(i) - yOffset)).toArray();//peaks.stream().mapToDouble(i->i).toArray();
+//                            // butterworth lowpass filter, cutoff at 3 hz (~180 bpm)
-                            double[] dPeaksYY = IntStream.range(0, peaksY.size()).mapToDouble(i -> (yAutoCorr[peaksY.get(i)])).toArray();
+//                            butterLowpass.lowPass(1,(1/(sampleRate / 1000))/2, 1);
-                            Plot plotPeaksY = Plot.plot(Plot.plotOpts().
+//
-                                    title("Peak Detection on Y").
+//                            int n = acInterp.getX().length;
-                                    legend(Plot.LegendFormat.BOTTOM)).
+//                            double[] xButter = new double[n];
-                                    series("x", Plot.data().xy(rangeAuto, yAutoCorr), Plot.seriesOpts().color(Color.RED)).
+//                            double[] yButter = new double[n];;
-                                    series("Peaks", Plot.data().xy(dPeaksYX, dPeaksYY), Plot.seriesOpts().color(Color.CYAN).
+//                            double[] zButter = new double[n];;
-                                            marker(Plot.Marker.DIAMOND).line(Plot.Line.NONE));
+//                            for(int i = 0; i < acInterp.getX().length; ++i){
-
+//                                //xButter[i] = butterLowpass.filter(acInterp.getX()[i]);
-                            windowPeaksY.set(plotPeaksY.draw());
+//                                //yButter[i] = butterLowpass.filter(acInterp.getY()[i]);
-
+//                                zButter[i] = butterLowpass.filter(acInterp.getZ()[i]);
-                            Peaks pZ = new Peaks(zAutoCorr, peakWidth, 0.1f, 0, false);
+//                            }
-                            int zOffset = zAutoCorr.length / 2;
+//
-                            LinkedList<Integer> peaksZ = pZ.getPeaksIdx();
+//                            //double[] xAutoCorrButter = new AutoCorrelation(xButter, accWindowBuffer.size()).getCorr();
-
+//                            //double[] yAutoCorrButter = new AutoCorrelation(yButter, accWindowBuffer.size()).getCorr();
-                            double[] dPeaksZX = IntStream.range(0, peaksZ.size()).mapToDouble(i -> (peaksZ.get(i) - zOffset)).toArray();//peaks.stream().mapToDouble(i->i).toArray();
+//                            double[] zAutoCorrButter = new AutoCorrelation(zButter, accWindowBuffer.size()).getCorr();
-                            double[] dPeaksZY = IntStream.range(0, peaksZ.size()).mapToDouble(i -> (zAutoCorr[peaksZ.get(i)])).toArray();
+//
-                            Plot plotPeaksZ = Plot.plot(Plot.plotOpts().
+//                            Plot plotCorrButter = Plot.plot(Plot.plotOpts().
-                                    title("Peak Detection on Z").
+//                                    title("Auto Correlation Butter").
-                                    legend(Plot.LegendFormat.BOTTOM)).
+//                                    legend(Plot.LegendFormat.BOTTOM)).
-                                    series("x", Plot.data().xy(rangeAuto, zAutoCorr), Plot.seriesOpts().color(Color.RED)).
+//                                    //series("x", utilities.Plot.data().xy(rangeAuto, xAutoCorrButter), utilities.Plot.seriesOpts().color(Color.RED)).
-                                    series("Peaks", Plot.data().xy(dPeaksZX, dPeaksZY), Plot.seriesOpts().color(Color.CYAN).
+//                                    //series("y", utilities.Plot.data().xy(rangeAuto, yAutoCorrButter), utilities.Plot.seriesOpts().color(Color.BLUE)).
-                                            marker(Plot.Marker.DIAMOND).line(Plot.Line.NONE));
+//                                    series("z", Plot.data().xy(rangeAuto, zAutoCorrButter), Plot.seriesOpts().color(Color.GREEN));
-
+//
-                            windowPeaksZ.set(plotPeaksZ.draw());
+//                            windowAutoButter.set(plotCorrButter.draw());
+//
+//                            //Print peaks
+//                            Peaks pX = new Peaks(xAutoCorr, peakWidth, 0.1f, 0, false);
+//                            int xOffset = xAutoCorr.length / 2;
+//                            LinkedList<Integer> peaksX = pX.getPeaksIdx();
+//
+//                            double[] dPeaksXX = IntStream.range(0, peaksX.size()).mapToDouble(i -> (peaksX.get(i) - xOffset)).toArray();//peaks.stream().mapToDouble(i->i).toArray();
+//                            double[] dPeaksXY = IntStream.range(0, peaksX.size()).mapToDouble(i -> (xAutoCorr[peaksX.get(i)])).toArray();
+//                            Plot plotPeaksX = Plot.plot(Plot.plotOpts().
+//                                    title("Peak Detection on X").
+//                                    legend(Plot.LegendFormat.BOTTOM)).
+//                                    series("x", Plot.data().xy(rangeAuto, xAutoCorr), Plot.seriesOpts().color(Color.RED)).
+//                                    series("Peaks", Plot.data().xy(dPeaksXX, dPeaksXY), Plot.seriesOpts().color(Color.CYAN).
+//                                            marker(Plot.Marker.DIAMOND).line(Plot.Line.NONE));
+//
+//                            windowPeaksX.set(plotPeaksX.draw());
+//
+//                            Peaks pY = new Peaks(yAutoCorr, peakWidth, 0.1f, 0, false);
+//                            int yOffset = yAutoCorr.length / 2;
+//                            LinkedList<Integer> peaksY = pY.getPeaksIdx();
+//
+//                            double[] dPeaksYX = IntStream.range(0, peaksY.size()).mapToDouble(i -> (peaksY.get(i) - yOffset)).toArray();//peaks.stream().mapToDouble(i->i).toArray();
+//                            double[] dPeaksYY = IntStream.range(0, peaksY.size()).mapToDouble(i -> (yAutoCorr[peaksY.get(i)])).toArray();
+//                            Plot plotPeaksY = Plot.plot(Plot.plotOpts().
+//                                    title("Peak Detection on Y").
+//                                    legend(Plot.LegendFormat.BOTTOM)).
+//                                    series("x", Plot.data().xy(rangeAuto, yAutoCorr), Plot.seriesOpts().color(Color.RED)).
+//                                    series("Peaks", Plot.data().xy(dPeaksYX, dPeaksYY), Plot.seriesOpts().color(Color.CYAN).
+//                                            marker(Plot.Marker.DIAMOND).line(Plot.Line.NONE));
+//
+//                            windowPeaksY.set(plotPeaksY.draw());
+//
+//                            Peaks pZ = new Peaks(zAutoCorr, peakWidth, 0.1f, 0, false);
+//                            int zOffset = zAutoCorr.length / 2;
+//                            LinkedList<Integer> peaksZ = pZ.getPeaksIdx();
+//
+//                            double[] dPeaksZX = IntStream.range(0, peaksZ.size()).mapToDouble(i -> (peaksZ.get(i) - zOffset)).toArray();//peaks.stream().mapToDouble(i->i).toArray();
+//                            double[] dPeaksZY = IntStream.range(0, peaksZ.size()).mapToDouble(i -> (zAutoCorr[peaksZ.get(i)])).toArray();
+//                            Plot plotPeaksZ = Plot.plot(Plot.plotOpts().
+//                                    title("Peak Detection on Z").
+//                                    legend(Plot.LegendFormat.BOTTOM)).
+//                                    series("x", Plot.data().xy(rangeAuto, zAutoCorr), Plot.seriesOpts().color(Color.RED)).
+//                                    series("Peaks", Plot.data().xy(dPeaksZX, dPeaksZY), Plot.seriesOpts().color(Color.CYAN).
+//                                            marker(Plot.Marker.DIAMOND).line(Plot.Line.NONE));
+//
+//                            windowPeaksZ.set(plotPeaksZ.draw());
 
                             //fill hols improve peaks
 
@@ -141,8 +203,6 @@ public class Main {
                             //System.out.println("BPM-Y: " + pY.getBPM(bpmEstimator.getSampleRate_ms()));
                             //System.out.println("BPM-Z: " + pZ.getBPM(bpmEstimator.getSampleRate_ms()));
 
-                            //todo: kleiner fenstergrößen testen. so ist doch etwas langsam auf der Uhr.
-
                             int dummyForBreakpoint = 0;
                         }
                     }
@@ -158,6 +218,12 @@ public class Main {
                 }
 
             }
+
+            try {
+                System.in.read();
+            } catch (IOException e) {
+                e.printStackTrace();
+            }
         }
     }
 }

java/src/main/java/bpmEstimation/AccelerometerWindowBuffer.java

@@ -2,14 +2,15 @@ package bpmEstimation;
 
 import utilities.Utils;
 import java.util.ArrayList;
+import java.util.List;
 
 /**
  * Created by toni on 15/12/17.
  */
 public class AccelerometerWindowBuffer extends ArrayList<AccelerometerData> {
 
-    private static int mWindowSize; // in ms
+    private final int mWindowSize; // in ms
-    private static int mOverlapSize; // in ms
+    private final int mOverlapSize; // in ms
     private long mOverlapCounter;
 
     public AccelerometerWindowBuffer(int windowSize, int overlap){
@@ -18,37 +19,55 @@ public class AccelerometerWindowBuffer extends ArrayList<AccelerometerData> {
         mOverlapCounter = 0;
     }
 
+    public AccelerometerWindowBuffer(List<AccelerometerData> list, int overlap){
+        mWindowSize = list.size();
+        mOverlapSize = overlap;
+        mOverlapCounter = 0;
+        super.addAll(list);
+    }
+
     //TODO: add exception handling. falseArgument if ad has no numeric x,y,z
     public boolean add(AccelerometerData ad){
+        synchronized (this){
 
-        //do not add duplicates!
+            //do not add duplicates!
-        if(!isEmpty() && getYongest().equals(ad)){
+            if(!isEmpty() && getYongest().equals(ad)){
-            return false;
+                return false;
+            }
+
+            // current - last to increment overlap time
+            if(!isEmpty()){
+                mOverlapCounter += ad.ts - getYongest().ts;
+            }
+
+            //add element
+            boolean r = super.add(ad);
+            removeOldElements();
+
+            return r;
         }
+    }
 
-        // current - last to increment overlap time
+    private void removeOldElements(){
-        if(!isEmpty()){
+        synchronized (this) {
-            mOverlapCounter += ad.ts - getYongest().ts;
+            if (!isEmpty()) {
-        }
+                if ((getYongest().ts - getOldest().ts) > mWindowSize) {
 
-        //add element
+                    long oldestTime = getYongest().ts - mWindowSize;
-        boolean r = super.add(ad);
+                    for (int i = 0; i < size(); ++i) {
-        if ((getYongest().ts - getOldest().ts) > mWindowSize){
+                        if (get(i).ts > oldestTime) {
-
+                            break;
-            long oldestTime = getYongest().ts - mWindowSize;
+                        }
-            for(int i = 0; i < size(); ++i) {
+                        super.remove(i);
-                if (get(i).ts > oldestTime) {
+                    }
-                    break;
                 }
-                remove(i);
             }
         }
-        return r;
     }
 
     public boolean isNextWindowReady(){
         if(!isEmpty()){
-            if(((getYongest().ts - getOldest().ts) > mWindowSize / 2) && mOverlapCounter > mOverlapSize){
+            if(((getYongest().ts - getOldest().ts) > mWindowSize / 4) && mOverlapCounter > mOverlapSize){
                 mOverlapCounter = 0;
 
                 return true;
@@ -57,6 +76,31 @@ public class AccelerometerWindowBuffer extends ArrayList<AccelerometerData> {
         return false;
     }
 
+    public static AccelerometerWindowBuffer getNewInstance(AccelerometerWindowBuffer buffer, int size, int overlap) {
+
+        double sampleRate = ((buffer.getYongest().ts - buffer.getOldest().ts) / buffer.size());
+
+        //if current size is smaller then wanted size, start at 0 and provide smaller list
+        int start = 0;
+        if ((buffer.getYongest().ts - buffer.getOldest().ts) > size) {
+            start = (int) Math.round(buffer.size() - (size / sampleRate));
+        }
+
+        // start should not be negative, this can happen due to rounding errors.
+        start = start < 0 ? 0 : start;
+
+        List<AccelerometerData> syncList;
+        synchronized (buffer) {
+            syncList = buffer.subList(start, buffer.size());
+        }
+
+        return new AccelerometerWindowBuffer(syncList, overlap);
+    }
+
+    public static AccelerometerWindowBuffer getNewInstance(AccelerometerWindowBuffer buffer) {
+        return new AccelerometerWindowBuffer(buffer, buffer.getOverlapSize());
+    }
+
     public AccelerometerData getYongest() {
         return get(size() - 1);
     }
@@ -84,4 +128,6 @@ public class AccelerometerWindowBuffer extends ArrayList<AccelerometerData> {
     public int getOverlapSize(){
         return mOverlapSize;
     }
+
+    public int getWindowSize(){return mWindowSize; }
 }

java/src/main/java/bpmEstimation/BpmEstimator.java

@@ -1,9 +1,13 @@
 package bpmEstimation;
 
+import static utilities.Utils.DEBUG_MODE;
+
 import utilities.MovingFilter;
 import utilities.SimpleKalman;
 import utilities.Utils;
 
+import uk.me.berndporr.iirj.*;
+
 import java.util.LinkedList;
 
 /**
@@ -17,6 +21,7 @@ public class BpmEstimator {
     private LinkedList<Double> mBpmHistory_X;
     private LinkedList<Double> mBpmHistory_Y;
     private LinkedList<Double> mBpmHistory_Z;
+    private LinkedList<Double> mBpmHistory_Mag;
 
     private LinkedList<Double> mBpmHistory;
     private int mResetCounter;
@@ -25,6 +30,13 @@ public class BpmEstimator {
     private MovingFilter mMvg;
     //private SimpleKalman mKalman;
 
+    private Butterworth mButter_X;
+    private Butterworth mButter_Y;
+    private Butterworth mButter_Z;
+    private Butterworth mButter_Mag;
+
+    //Debugging stuff
+    private Utils.DebugPlotter plotter;
 
     public BpmEstimator(AccelerometerWindowBuffer windowBuffer, double sampleRate_ms, int resetAfter_ms){
         mBuffer = windowBuffer;
@@ -33,39 +45,107 @@ public class BpmEstimator {
         mBpmHistory_X = new LinkedList<>();
         mBpmHistory_Y = new LinkedList<>();
         mBpmHistory_Z = new LinkedList<>();
+        mBpmHistory_Mag = new LinkedList<>();
 
         mBpmHistory = new LinkedList<>();
         mResetCounter = 0;
         mResetLimit_ms = resetAfter_ms;
 
-        mMvg = new MovingFilter(10);
+        mMvg = new MovingFilter(2);
         //mKalman = new SimpleKalman();
+
+        mButter_X = new Butterworth();
+        mButter_Y = new Butterworth();
+        mButter_Z = new Butterworth();
+        mButter_Mag = new Butterworth();
+
+        if(DEBUG_MODE){
+            plotter = new Utils.DebugPlotter();
+        }
     }
 
     public double estimate(){
+        return estimate(mBuffer.getWindowSize(), mBuffer.getOverlapSize());
+    }
+
+    public double estimate(int length_ms, int overlap_ms){
+
+        AccelerometerWindowBuffer tmpBuffer = AccelerometerWindowBuffer.getNewInstance(mBuffer, length_ms, overlap_ms);
 
         double sampleRate = mSampleRate_ms;
         if(sampleRate <= 0){
-            sampleRate = Math.round(Utils.mean(Utils.diff(mBuffer.getTs())));
+            sampleRate = Math.round(Utils.mean(Utils.diff(tmpBuffer.getTs())));
         }
 
-        AccelerometerInterpolator interp = new AccelerometerInterpolator(mBuffer, sampleRate);
+        assert sampleRate != 0 : "samplerate is zero";
 
-        double[] xAutoCorr = new AutoCorrelation(interp.getX(), mBuffer.size()).getCorr();
+        // interpolate
-        double[] yAutoCorr = new AutoCorrelation(interp.getY(), mBuffer.size()).getCorr();
+        AccelerometerInterpolator interp = new AccelerometerInterpolator(tmpBuffer, sampleRate);
-        double[] zAutoCorr = new AutoCorrelation(interp.getZ(), mBuffer.size()).getCorr();
+        double[] magRaw = Utils.magnitude(interp);
+
+        //todo: aufräumen. funktion die eine achse bekommt und aus ihr dann die peaks zurück gibt. für debuggen gleich zeichenfunktionen dazu.
+
+        // butterworth lowpass filter, cutoff at 3 hz (~180 bpm)
+        mButter_X.lowPass(1,(1/(sampleRate / 1000))/2, 1);
+        mButter_Y.lowPass(1,(1/(sampleRate / 1000))/2, 1);
+        mButter_Z.lowPass(1,(1/(sampleRate / 1000))/2, 1);
+
+        int n = interp.getX().length;
+        double[] xButter = new double[n];
+        double[] yButter = new double[n];;
+        double[] zButter = new double[n];;
+        for(int i = 0; i < interp.getX().length; ++i){
+            xButter[i] = mButter_X.filter(interp.getX()[i]);
+            yButter[i] = mButter_Y.filter(interp.getY()[i]);
+            zButter[i] = mButter_Z.filter(interp.getZ()[i]);
+        }
+
+        //TODO: compare with own mButter_Mag.filter
+        double[] magButter = Utils.magnitude(xButter, yButter, zButter);
+
+        //auto correlation
+        double[] xAutoCorr = new AutoCorrelation(xButter, tmpBuffer.size()).getCorr();
+        double[] yAutoCorr = new AutoCorrelation(yButter, tmpBuffer.size()).getCorr();
+        double[] zAutoCorr = new AutoCorrelation(zButter, tmpBuffer.size()).getCorr();
+        double[] magAutoCorr = new AutoCorrelation(magRaw, tmpBuffer.size()).getCorr();
 
         //find a peak within range of 250 ms
         int peakWidth = (int) Math.round(250 / sampleRate);
-        Peaks pX = new Peaks(xAutoCorr, peakWidth, 0.1f, 0, false);
+        Peaks xPeaks = new Peaks(xAutoCorr, peakWidth, 0.1f, 0, false);
-        Peaks pY = new Peaks(yAutoCorr, peakWidth, 0.1f, 0, false);
+        Peaks yPeaks = new Peaks(yAutoCorr, peakWidth, 0.1f, 0, false);
-        Peaks pZ = new Peaks(zAutoCorr, peakWidth, 0.1f, 0, false);
+        Peaks zPeaks = new Peaks(zAutoCorr, peakWidth, 0.1f, 0, false);
+        Peaks magPeaks = new Peaks(magAutoCorr, peakWidth, 0.1f, 0, false);
 
-        mBpmHistory_X.add(pX.getBPM(sampleRate));
+        mBpmHistory_X.add(xPeaks.getBPM(sampleRate));
-        mBpmHistory_Y.add(pY.getBPM(sampleRate));
+        mBpmHistory_Y.add(yPeaks.getBPM(sampleRate));
-        mBpmHistory_Z.add(pZ.getBPM(sampleRate));
+        mBpmHistory_Z.add(zPeaks.getBPM(sampleRate));
+        mBpmHistory_Mag.add(magPeaks.getBPM(sampleRate));
 
-        double estimatedBPM = getBestBpmEstimation(pX, pY, pZ);
+
+        if(DEBUG_MODE){
+            plotter.setPlotRawX(interp.getTs(), interp.getX());
+            plotter.setPlotRawY(interp.getTs(), interp.getY());
+            plotter.setPlotRawZ(interp.getTs(), interp.getZ());
+            plotter.setPlotRawMag(interp.getTs(), magRaw);
+
+            plotter.setPlotButterX(interp.getTs(), xButter);
+            plotter.setPlotButterY(interp.getTs(), yButter);
+            plotter.setPlotButterZ(interp.getTs(), zButter);
+            plotter.setPlotButterMag(interp.getTs(), magButter);
+
+            plotter.setPlotCorrX(xAutoCorr, xPeaks);
+            plotter.setPlotCorrY(yAutoCorr, yPeaks);
+            plotter.setPlotCorrZ(zAutoCorr, zPeaks);
+            plotter.setPlotCorrMag(magAutoCorr, magPeaks);
+
+            //printout the current BPM
+            System.out.println(length_ms + "; x: " + mBpmHistory_X.getLast()
+                                        + "; y: " + mBpmHistory_Y.getLast()
+                                        + "; z: " + mBpmHistory_Z.getLast()
+                                        + "; mag: " + mBpmHistory_Mag.getLast());
+        }
+
+        double estimatedBPM = getBestBpmEstimation(xPeaks, yPeaks, zPeaks);
         if(estimatedBPM != -1){
 
             //moving avg (lohnt dann, wenn wir viele daten haben)
@@ -86,7 +166,8 @@ public class BpmEstimator {
             if(++mResetCounter > resetAfter){
                 mBpmHistory.clear();
 
-                mBuffer.clear();
+                //TODO: send signal to clear from outside this function should just return the bpm
+                //mBuffer.clear();
                 mMvg.clear();
                 mResetCounter = 0;
             }
@@ -105,6 +186,8 @@ public class BpmEstimator {
         return Utils.median(mBpmHistory);
     }
 
+    //TODO: die einzelnen achsen cleverer kombinieren. bspw. bei Peter brauchen wir zwei Achsen in einer Bewegung.
+    //TODO: Vielleicht die einzelnen Kombinationen / Magnitudes der Achsen noch mit einbeziehen. Also xy, xz, yz
     private double getBestBpmEstimation(Peaks peaksX, Peaks peaksY, Peaks peaksZ) throws IllegalArgumentException {
 
         int cntNumAxis = 0;

java/src/main/java/utilities/Utils.java

@@ -1,5 +1,7 @@
 package utilities;
 
+import bpmEstimation.AccelerometerInterpolator;
+import bpmEstimation.AccelerometerWindowBuffer;
 import bpmEstimation.Peaks;
 import javax.swing.*;
 import java.awt.*;
@@ -9,9 +11,13 @@ import java.util.Arrays;
 import java.util.Collections;
 import java.util.Comparator;
 import java.util.LinkedList;
+import java.util.stream.IntStream;
 
 //TODO: change from double to generic type
 public class Utils {
+
+    public static final boolean DEBUG_MODE = true;
+
     public static double getDistance(double x1, double y1, double x2, double y2) {
         return (double) Math.sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
     }
@@ -164,6 +170,37 @@ public class Utils {
         return newArray;
     }
 
+    public static double magnitude(double x, double y, double z){
+        return Math.sqrt((x*x) + (y*y) + (z*z));
+    }
+
+    public static double[] magnitude(double[] arrayX, double[] arrayY, double[] arrayZ){
+
+        //check of all arrays have the same size, if not crop to smallest one
+        int n = arrayZ.length;
+        if(arrayX.length < arrayY.length && arrayX.length < arrayZ.length){
+            n = arrayX.length;
+        } else if (arrayY.length < arrayZ.length){
+            n = arrayY.length;
+        }
+
+        double[] output = new double[n];
+        for(int i = 0; i < n; ++i){
+            output[i] = magnitude(arrayX[i], arrayY[i], arrayZ[i]);
+        }
+
+        return output;
+    }
+
+    public static double[] magnitude(AccelerometerInterpolator interpolator){
+        return magnitude(interpolator.getX(), interpolator.getY(), interpolator.getZ());
+    }
+
+    //TODO: this could be added to AccelerometerData.. saves some time, better design.
+    public static double[] magnitude(AccelerometerWindowBuffer buffer){
+        return magnitude(buffer.getX(), buffer.getY(), buffer.getZ());
+    }
+
     @SuppressWarnings("serial")
     public static class ShowPNG extends JFrame
     {
@@ -178,7 +215,55 @@ public class Utils {
             this.setVisible(true);
         }
 
-        public void set(BufferedImage bi){
+        public void plotData(String title, String name, long[] ts, double[] data){
+
+            double[] dTs = IntStream.range(0, ts.length).mapToDouble(i -> ts[i]).toArray();
+            Plot plot = Plot.plot(Plot.plotOpts().
+                    title(title).
+                    legend(Plot.LegendFormat.BOTTOM)).
+                    series(name, Plot.data().xy(dTs, data), Plot.seriesOpts().color(Color.RED));
+
+            this.set(plot.draw());
+        }
+
+        public void plotCorr(String title, String name, double[] data, int scale){
+
+            int[] tmp = IntStream.rangeClosed(-((data.length - 1)/scale), ((data.length - 1)/scale)).toArray();
+            double[] range = IntStream.range(0, tmp.length).mapToDouble(i -> tmp[i]).toArray();
+            Plot plot = Plot.plot(Plot.plotOpts().
+                    title(title).
+                    legend(Plot.LegendFormat.BOTTOM)).
+                    series(name, Plot.data().xy(range, data), Plot.seriesOpts().color(Color.RED));
+
+            this.set(plot.draw());
+        }
+
+        public void plotCorrWithPeaks(String title, String name, double[] corr, int scale, Peaks peaks, boolean isCorrelation){
+
+            int[] tmp = IntStream.rangeClosed(-((corr.length - 1)/scale), ((corr.length - 1)/scale)).toArray();
+            double[] range= IntStream.range(0, tmp.length).mapToDouble(i -> tmp[i]).toArray();
+
+            //if we have a correlation, shift the peaks into negative by half the correlation size
+            int tmpOffset = 0;
+            if(isCorrelation){
+                tmpOffset = corr.length / 2;
+            }
+            int offset = tmpOffset;
+
+            LinkedList<Integer> peaksZ = peaks.getPeaksIdx();
+            double[] dPeaksZX = IntStream.range(0, peaksZ.size()).mapToDouble(i -> (peaksZ.get(i) - offset)).toArray();//peaks.stream().mapToDouble(i->i).toArray();
+            double[] dPeaksZY = IntStream.range(0, peaksZ.size()).mapToDouble(i -> (corr[peaksZ.get(i)])).toArray();
+            Plot plot = Plot.plot(Plot.plotOpts().
+                    title(title).
+                    legend(Plot.LegendFormat.BOTTOM)).
+                    series(name, Plot.data().xy(range, corr), Plot.seriesOpts().color(Color.RED)).
+                    series("Peaks", Plot.data().xy(dPeaksZX, dPeaksZY), Plot.seriesOpts().color(Color.CYAN).
+                            marker(Plot.Marker.DIAMOND).line(Plot.Line.NONE));
+
+            this.set(plot.draw());
+        }
+
+        private void set(BufferedImage bi){
 
             mIcon = new ImageIcon(bi);
             mLabel.setVisible(false);
@@ -187,4 +272,87 @@ public class Utils {
         }
     }
 
+    public static class DebugPlotter {
+
+        //what we want to draw
+        private Utils.ShowPNG plotRawX;
+        private Utils.ShowPNG plotRawY;
+        private Utils.ShowPNG plotRawZ;
+        private Utils.ShowPNG plotRawMag;
+        private Utils.ShowPNG plotButterX;
+        private Utils.ShowPNG plotButterY;
+        private Utils.ShowPNG plotButterZ;
+        private Utils.ShowPNG plotButterMag;
+        private Utils.ShowPNG plotCorrX;
+        private Utils.ShowPNG plotCorrY;
+        private Utils.ShowPNG plotCorrZ;
+        private Utils.ShowPNG plotCorrMag;
+
+        public DebugPlotter(){
+
+            plotRawX = new Utils.ShowPNG();
+            plotRawY = new Utils.ShowPNG();
+            plotRawZ = new Utils.ShowPNG();
+            plotRawMag = new Utils.ShowPNG();
+            plotButterX = new Utils.ShowPNG();
+            plotButterY = new Utils.ShowPNG();
+            plotButterZ = new Utils.ShowPNG();
+            plotButterMag = new Utils.ShowPNG();
+            plotCorrX = new Utils.ShowPNG();
+            plotCorrY = new Utils.ShowPNG();
+            plotCorrZ = new Utils.ShowPNG();
+            plotCorrMag = new Utils.ShowPNG();
+        }
+
+        public void setPlotRawX(long[] ts, double[] data){
+            plotRawX.plotData("Raw Data X", "x", ts, data);
+        }
+
+        public void setPlotRawY(long[] ts, double[] data){
+            plotRawY.plotData("Raw Data Y", "y", ts, data);
+        }
+
+        public void setPlotRawZ(long[] ts, double[] data){
+            plotRawZ.plotData("Raw Data Z", "z", ts, data);
+        }
+
+        public void setPlotRawMag(long[] ts, double[] data){
+            plotRawMag.plotData("Raw Data Magnitude", "mag", ts, data);
+        }
+
+        public void setPlotButterX(long[] ts, double[] data){
+            plotButterX.plotData("Butter Data X", "x", ts, data);
+        }
+
+        public void setPlotButterY(long[] ts, double[] data){
+            plotButterY.plotData("Butter Data Y", "z", ts, data);
+        }
+
+        public void setPlotButterZ(long[] ts, double[] data){
+            plotButterZ.plotData("Butter Data Z", "y", ts, data);
+        }
+
+        public void setPlotButterMag(long[] ts, double[] data){
+            plotButterMag.plotData("Butter Data Mag", "mag", ts, data);
+        }
+
+        public void setPlotCorrX(double[] data, Peaks peaks){
+            plotCorrX.plotCorrWithPeaks("Autocorr X", "x", data,2, peaks, true);
+        }
+
+        public void setPlotCorrY(double[] data, Peaks peaks){
+            plotCorrY.plotCorrWithPeaks("Autocorr Y", "y", data,2, peaks, true);
+        }
+
+        public void setPlotCorrZ(double[] data, Peaks peaks){
+            plotCorrZ.plotCorrWithPeaks("Autocorr Z", "z", data,2, peaks, true);
+        }
+
+        public void setPlotCorrMag(double[] data, Peaks peaks){
+            plotCorrMag.plotCorrWithPeaks("Autocorr Mag", "mag", data,2, peaks, true);
+        }
+
+
+    }
+
 }