try different kalman filter implementation

js/Position.js

@@ -1,7 +1,7 @@
 /* global kalmanFilter */
 'use strict'
 
-const { KalmanFilter } = kalmanFilter
+// const { KalmanFilter } = kalmanFilter
 
 function toRadians (deg) {
   return deg * (Math.PI / 180)
@@ -85,3 +85,94 @@ export function kFilter (arr) {
   const res = kFilter.filterAll(arr)
   return res
 }
+
+export class KalmanFilter {
+  constructor (dt, processNoise, measurementNoise, estimationError) {
+    this.dt = dt // Zeitintervall zwischen Messungen
+
+    // Zustandsvariablen
+    this.x = 0 // Position in x-Richtung
+    this.y = 0 // Position in y-Richtung
+    this.vx = 0 // Geschwindigkeit in x-Richtung
+    this.vy = 0 // Geschwindigkeit in y-Richtung
+
+    // Kovarianzmatrix
+    this.P = [
+      [1, 0, 0, 0], // Unsicherheit in x, vx, y, vy
+      [0, 1, 0, 0],
+      [0, 0, 1, 0],
+      [0, 0, 0, 1]
+    ]
+
+    // Prozessrauschen
+    this.Q = [
+      [processNoise, 0, 0, 0],
+      [0, processNoise, 0, 0],
+      [0, 0, processNoise, 0],
+      [0, 0, 0, processNoise]
+    ]
+
+    // Messrauschen
+    this.R = [
+      [measurementNoise, 0],
+      [0, measurementNoise]
+    ]
+
+    // Messung
+    this.Z = [0, 0] // Beschleunigungsmessung in x und y
+
+    // Unsicherheit im Zustand
+    this.estimationError = estimationError
+  }
+
+  predict () {
+    // Vorhersage der neuen Position und Geschwindigkeit basierend auf dem letzten Zustand
+    this.x += this.vx * this.dt
+    this.y += this.vy * this.dt
+
+    // Update Geschwindigkeit basierend auf Beschleunigung (aus dem letzten Schritt)
+    this.vx += this.Z[0] * this.dt
+    this.vy += this.Z[1] * this.dt
+
+    // Unsicherheit der Vorhersage erhöhen (Prozessrauschen hinzufügen)
+    this.P[0][0] += this.Q[0][0]
+    this.P[1][1] += this.Q[1][1]
+    this.P[2][2] += this.Q[2][2]
+    this.P[3][3] += this.Q[3][3]
+  }
+
+  update (ax, ay) {
+    // Messung aktualisieren
+    this.Z[0] = ax // Aktuelle Beschleunigung in x-Richtung
+    this.Z[1] = ay // Aktuelle Beschleunigung in y-Richtung
+
+    // Kalman Gain berechnen
+    const S = [
+      [this.P[0][0] + this.R[0][0], 0],
+      [0, this.P[2][2] + this.R[1][1]]
+    ]
+
+    const K = [
+      [this.P[0][0] / S[0][0], 0],
+      [this.P[1][1] / S[0][0], 0],
+      [0, this.P[2][2] / S[1][1]],
+      [0, this.P[3][3] / S[1][1]]
+    ]
+
+    // Zustandsvariablen aktualisieren
+    this.x += K[0][0] * (ax - this.vx)
+    this.vx += K[1][0] * (ax - this.vx)
+    this.y += K[2][1] * (ay - this.vy)
+    this.vy += K[3][1] * (ay - this.vy)
+
+    // Kovarianzmatrix aktualisieren
+    this.P[0][0] *= (1 - K[0][0])
+    this.P[1][1] *= (1 - K[1][0])
+    this.P[2][2] *= (1 - K[2][1])
+    this.P[3][3] *= (1 - K[3][1])
+  }
+
+  getPosition () {
+    return { x: this.x, y: this.y }
+  }
+}

js/sensor.js

@@ -1,6 +1,6 @@
 /* global DeviceMotionEvent */
 
-import { kFilter, getGroundAcceleration } from './Position.js'
+import { getGroundAcceleration, KalmanFilter } from './Position.js'
 
 const debug = false
 
@@ -10,6 +10,15 @@ let globalX = 0
 let globalY = 0
 let globalAX = 0
 let globalAY = 0
+let position = { x: 0, y: 0 }
+
+// Beispielaufruf
+const dt = 0.001 // Zeitintervall (z.B. 100 ms)
+const processNoise = 0.1 // Prozessrauschen
+const measurementNoise = 0.1 // Messrauschen
+const estimationError = 0.1 // Anfangsfehlerabschätzung
+
+const kalman = new KalmanFilter(dt, processNoise, measurementNoise, estimationError)
 
 if (debug) {
   handleOrientation({ alpha: 69, beta: 32, gamma: 80, webkitCompassHeading: 359 })
@@ -55,31 +64,35 @@ function handleMotion (event) {
     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 acceleration = kFilter(accelerationArray)
+  // const orientation = kFilter(orientationArray)
 
+  /*
   const accel = acceleration[acceleration.length - 1]
   const yaw = orientation[orientation.length - 1][0]
   const pitch = orientation[orientation.length - 1][1]
   const roll = orientation[orientation.length - 1][2]
+  */
+  const accel = accelerationArray[accelerationArray.length - 1]
+  const yaw = orientationArray[orientationArray.length - 1][0]
+  const pitch = orientationArray[orientationArray.length - 1][1]
+  const roll = orientationArray[orientationArray.length - 1][2]
   const groundAccel = getGroundAcceleration(accel, yaw, pitch, roll)
 
   if (!isNaN(groundAccel.ax)) {
     const intervall = 0.02
+
+    // Beispiel: Sensordaten verwenden
+    position = updateKalmanFilter(groundAccel.ax, groundAccel.ay)
+
     globalAX = globalAX + groundAccel.ax * intervall
     globalAY = globalAY + groundAccel.ay * intervall
     globalX = globalX + globalAX * intervall + 0.5 * groundAccel.ax * Math.pow(intervall, 2)
     globalY = globalY + globalAY * intervall + 0.5 * groundAccel.ay * Math.pow(intervall, 2)
   }
 
-  updateFieldIfNotNull('X_position', globalX)
-  updateFieldIfNotNull('Y_position', globalY)
+  updateFieldIfNotNull('X_position', position.x)
+  updateFieldIfNotNull('Y_position', position.y)
 
   if (!debug) {
     updateFieldIfNotNull('Accelerometer_gx', event.accelerationIncludingGravity.x)
@@ -128,3 +141,12 @@ demoButton.onclick = function (e) {
     isRunning = true
   }
 }
+
+// In der Schleife, wo du deine Sensoren liest:
+function updateKalmanFilter (ax, ay) {
+  kalman.predict()
+  kalman.update(ax, ay)
+  const position = kalman.getPosition()
+  console.log(`Position: x=${position.x}, y=${position.y}`)
+  return position
+}