try constant-position model on kalman filter

danielgilbers · Sep 23, 2024 · 417e64c · 417e64c
1 parent 6d762b2
commit 417e64c
Show file tree

Hide file tree

Showing 2 changed files with 41 additions and 30 deletions.
diff --git a/js/Position.js b/js/Position.js
@@ -80,15 +80,8 @@ export function getGroundAcceleration (accel, yaw, pitch, roll) {
 
 export function kFilter (arr) {
   const kFilter = new KalmanFilter({
-    observation: {
-      sensorDimension: 3,
-      name: 'sensor'
-    },
-    dynamic: {
-      name: 'constant-speed', // observation.sensorDimension * 2 == state.dimension
-      timeStep: 0.1,
-      covariance: [3, 3, 3, 4, 4, 4]// equivalent to diag([3, 3, 3, 4, 4, 4])
-    }
+    observation: 3,
+    dynamic: 'constant-position'
   })
   const res = kFilter.filterAll(arr)
   return res

diff --git a/js/sensor.js b/js/sensor.js
@@ -2,25 +2,31 @@
 
 import { kFilter, getGroundAcceleration } from './Position.js'
 
-let orientationArray = new Array(3)
-let accelerationArray = new Array(3)
+const debug = false
+
+let orientationArray = []
+let accelerationArray = []
 let globalX = 0
 let globalY = 0
-const lastValues = []
 
-// handleOrientation({ alpha: 69, beta: 32, gamma: 80, webkitCompassHeading: 359 })
-// handleMotion()
+if (debug) {
+  handleOrientation({ alpha: 69, beta: 32, gamma: 80, webkitCompassHeading: 359 })
+  handleMotion()
+}
 function handleOrientation (event) {
-  orientationArray = addValue([event.webkitCompassHeading, event.beta, event.gamma])
-  // orientationArray = addValue([30, 14, 69])
+  if (debug) {
+    orientationArray = addValue([30, 14, 69], orientationArray)
+  } else {
+    orientationArray = addValue([event.webkitCompassHeading, event.beta, event.gamma], orientationArray)
+  }
   updateFieldIfNotNull('Orientation_a', event.alpha)
   updateFieldIfNotNull('Orientation_b', event.beta)
   updateFieldIfNotNull('Orientation_g', event.gamma)
   updateFieldIfNotNull('Orientation_compass', event.webkitCompassHeading)
   incrementEventCount()
 }
 
-function addValue (newValue) {
+function addValue (newValue, lastValues = []) {
   if (lastValues.length >= 100) {
     lastValues.shift()
   }
@@ -39,11 +45,22 @@ function updateFieldIfNotNull (fieldName, value, precision = 10) {
 }
 
 function handleMotion (event) {
-  accelerationArray = addValue([event.acceleration.x, event.acceleration.y, event.acceleration.z])
-  // accelerationArray = addValue([0.2, 0.1, 0.5])
+  if (debug) {
+    accelerationArray = addValue([0.2, 0.1, 0.5], accelerationArray)
+    accelerationArray = addValue([0.2, 0.1, 0.5], accelerationArray)
+  } else {
+    accelerationArray = addValue([event.acceleration.x, event.acceleration.y, event.acceleration.z], accelerationArray)
+  }
+
+  console.log(accelerationArray)
+  console.log(orientationArray)
+
   const acceleration = kFilter(accelerationArray)
   const orientation = kFilter(orientationArray)
 
+  console.log(acceleration)
+  console.log(orientation)
+
   const accel = acceleration[acceleration.length - 1]
   const yaw = orientation[orientation.length - 1][0]
   const pitch = orientation[orientation.length - 1][1]
@@ -58,20 +75,21 @@ function handleMotion (event) {
   updateFieldIfNotNull('X_position', globalX)
   updateFieldIfNotNull('Y_position', globalY)
 
-  updateFieldIfNotNull('Accelerometer_gx', event.accelerationIncludingGravity.x)
-  updateFieldIfNotNull('Accelerometer_gy', event.accelerationIncludingGravity.y)
-  updateFieldIfNotNull('Accelerometer_gz', event.accelerationIncludingGravity.z)
+  if (!debug) {
+    updateFieldIfNotNull('Accelerometer_gx', event.accelerationIncludingGravity.x)
+    updateFieldIfNotNull('Accelerometer_gy', event.accelerationIncludingGravity.y)
+    updateFieldIfNotNull('Accelerometer_gz', event.accelerationIncludingGravity.z)
 
-  updateFieldIfNotNull('Accelerometer_x', event.acceleration.x)
-  updateFieldIfNotNull('Accelerometer_y', event.acceleration.y)
-  updateFieldIfNotNull('Accelerometer_z', event.acceleration.z)
+    updateFieldIfNotNull('Accelerometer_x', event.acceleration.x)
+    updateFieldIfNotNull('Accelerometer_y', event.acceleration.y)
+    updateFieldIfNotNull('Accelerometer_z', event.acceleration.z)
 
-  updateFieldIfNotNull('Accelerometer_i', event.interval, 2)
-
-  updateFieldIfNotNull('Gyroscope_z', event.rotationRate.alpha)
-  updateFieldIfNotNull('Gyroscope_x', event.rotationRate.beta)
-  updateFieldIfNotNull('Gyroscope_y', event.rotationRate.gamma)
+    updateFieldIfNotNull('Accelerometer_i', event.interval, 2)
 
+    updateFieldIfNotNull('Gyroscope_z', event.rotationRate.alpha)
+    updateFieldIfNotNull('Gyroscope_x', event.rotationRate.beta)
+    updateFieldIfNotNull('Gyroscope_y', event.rotationRate.gamma)
+  }
   incrementEventCount()
 }