Applied lowpass filter to sensor readings

appoly · Oct 10, 2018 · 62227ac · 62227ac
1 parent b567ed0
commit 62227ac
Showing 1 changed file with 12 additions and 2 deletions.
diff --git a/arcore-location/src/main/java/uk/co/appoly/arcorelocation/sensor/DeviceOrientation.java b/arcore-location/src/main/java/uk/co/appoly/arcorelocation/sensor/DeviceOrientation.java
@@ -16,6 +16,7 @@
 public class DeviceOrientation implements SensorEventListener {
 
     private SensorManager mSensorManager;
+    private static final float ALPHA = 0.25f;
 
     // Gravity rotational data
     private float gravity[];
@@ -50,10 +51,10 @@ public void onSensorChanged(SensorEvent event) {
 
         switch (event.sensor.getType()) {
             case Sensor.TYPE_MAGNETIC_FIELD:
-                mags = event.values.clone();
+                mags = lowPass(event.values.clone(),mags);
                 break;
             case Sensor.TYPE_ACCELEROMETER:
-                accels = event.values.clone();
+                accels = lowPass(event.values.clone(),accels);
                 break;
         }
 
@@ -87,4 +88,13 @@ public void pause() {
         mSensorManager.unregisterListener(this);
     }
 
+    protected float[] lowPass( float[] input, float[] output ) {
+        if ( output == null ) return input;
+
+        for ( int i=0; i<input.length; i++ ) {
+            output[i] = output[i] + ALPHA * (input[i] - output[i]);
+        }
+        return output;
+    }
+
 }