refactor

js/Position.js

@@ -4,108 +4,99 @@
 const { KalmanFilter } = kalmanFilter
 
 const magnitudeArrayLength = 5
-
 const position = { lat: 0, lng: 0 }
-let lastIndex, xFiltered, yFiltered, zFiltered, magnitude, lastOrientation
 const magnitudeArray = []
-
 const stepLength = 0.7 // Schrittweite in Metern
 const stepThreshold = 2.3
 
-const rotationBias = 240
-
-function toRadians (deg) {
-  return deg * (Math.PI / 180)
+/**
+ * Konvertiere Winkel in Radiant
+ * @param {*} degrees Winkel in Grad
+ * @returns Radiant
+ */
+function toRadians (degrees) {
+  return degrees * (Math.PI / 180)
 }
 
-function rotateVector (accel, yaw, pitch, roll) {
+function rotateAcceleration (x, y, z, yaw, pitch, roll) {
   const theta = toRadians(yaw)
   const beta = toRadians(pitch)
   const gamma = toRadians(roll)
 
-  // Rotationsmatrix R_yaw
-  const R_yaw = [
+  // Rotationsmatrix Yaw
+  const rYaw = [
     [Math.cos(theta), -Math.sin(theta), 0],
     [Math.sin(theta), Math.cos(theta), 0],
     [0, 0, 1]
   ]
 
-  // Rotationsmatrix R_pitch
-  const R_pitch = [
+  // Rotationsmatrix Pitch
+  const rPitch = [
     [1, 0, 0],
     [0, Math.cos(beta), -Math.sin(beta)],
     [0, Math.sin(beta), Math.cos(beta)]
   ]
 
-  // Rotationsmatrix R_roll
-  const R_roll = [
+  // Rotationsmatrix Roll
+  const rRoll = [
     [Math.cos(gamma), 0, Math.sin(gamma)],
     [0, 1, 0],
     [-Math.sin(gamma), 0, Math.cos(gamma)]
   ]
 
+  // Beschleunigungsvektor [x, y, z]
+  const accel = [x, y, z]
+
   // Multipliziere die Rotationsmatrizen
-  const R = multiplyMatrices(multiplyMatrices(R_yaw, R_pitch), R_roll)
+  const R = multiplyMatrices(multiplyMatrices(rYaw, rPitch), rRoll)
 
   // Beschleunigung in globale Koordinaten umrechnen
-  return multiplyMatrixVector(R, accel)
+  return multiplyMatrixAndVector(R, accel)
 }
 
 function multiplyMatrices (A, B) {
-  const result = []
-  for (let i = 0; i < A.length; i++) {
-    result[i] = []
-    for (let j = 0; j < B[0].length; j++) {
-      result[i][j] = 0
-      for (let k = 0; k < A[0].length; k++) {
-        result[i][j] += A[i][k] * B[k][j]
-      }
-    }
-  }
-  return result
-}
+  const result = [
+    [0, 0, 0],
+    [0, 0, 0],
+    [0, 0, 0]
+  ]
 
-function multiplyMatrixVector (matrix, vector) {
-  const result = []
-  for (let i = 0; i < matrix.length; i++) {
-    result[i] = 0
-    for (let j = 0; j < vector.length; j++) {
-      result[i] += matrix[i][j] * vector[j]
+  for (let i = 0; i < 3; i++) {
+    for (let j = 0; j < 3; j++) {
+      result[i][j] = A[i][0] * B[0][j] + A[i][1] * B[1][j] + A[i][2] * B[2][j]
     }
   }
+
   return result
 }
 
-export function getGroundAcceleration (accel, yaw, pitch, roll) {
-  // Berechne die globale Beschleunigung
-  const globalAccel = rotateVector(accel, yaw, pitch, roll)
+function multiplyMatrixAndVector (matrix, vector) {
+  const result = [0, 0, 0]
 
-  // Extrahiere die x- und y-Komponenten der globalen Beschleunigung
-  const groundAccel = {
-    ax: globalAccel[0], // Beschleunigung entlang der globalen x-Achse
-    ay: globalAccel[1] // Beschleunigung entlang der globalen y-Achse
+  for (let i = 0; i < 3; i++) {
+    result[i] = matrix[i][0] * vector[0] + matrix[i][1] * vector[1] + matrix[i][2] * vector[2]
   }
 
-  return groundAccel
+  return result
 }
 
 export function kFilter (arr) {
   const kFilter = new KalmanFilter({
     observation: {
       name: 'sensor',
       sensorDimension: 6,
-      sensorCovariance: [3, 3, 3, 3, 3, 3]
+      sensorCovariance: [1, 1, 1, 1, 1, 1]
     },
     dynamic: {
       name: 'constant-speed'
-      // covariance: [0.3, 5] // 0.3, 30
+     covariance: [0.1, 0.5]
     }
   })
   const res = kFilter.filterAll(arr)
   return res
 }
 
-export function detectPeak (data) {
+function detectPeak (data) {
   const len = data.length
   if (len < 3) return false
 
@@ -117,31 +108,34 @@ export function detectPeak (data) {
   return (beforeLast > last && beforeLast > twoBeforeLast && beforeLast > stepThreshold)
 }
 
-export function calculatePosition (motionArray, userPosition) {
+export function calculatePosition (motionArray, userPosition, bias) {
+  const rotationBias = 360 - bias
   position.lat = userPosition.lat
   position.lng = userPosition.lng
-  const filteredArrays = kFilter(motionArray).map((element) => [element[0], element[1], element[2], element[3], element[4], element[5]]) // Wende den Filter an
+  const filteredArrays = kFilter(motionArray)
 
-  lastIndex = filteredArrays.length - 1
-  xFiltered = filteredArrays[lastIndex][0]
-  yFiltered = filteredArrays[lastIndex][1]
-  zFiltered = filteredArrays[lastIndex][2]
+  const lastIndex = filteredArrays.length - 1
+  const [xFiltered, yFiltered, zFiltered] = filteredArrays[lastIndex].slice(0, 3)
 
-  lastOrientation = motionArray[lastIndex][3] + rotationBias
-  magnitude = Math.sqrt(xFiltered * xFiltered + yFiltered * yFiltered + zFiltered * zFiltered)
+  const lastOrientation = motionArray[lastIndex][3] + rotationBias
+  const magnitude = Math.sqrt(xFiltered ** 2 + yFiltered ** 2 + zFiltered ** 2)
   if (magnitudeArray.length >= magnitudeArrayLength) {
     magnitudeArray.shift()
   }
   magnitudeArray.push(magnitude)
 
   if (detectPeak(magnitudeArray)) {
-    updatePosition()
-    return position
+    return updatePosition(lastOrientation)
   }
 }
 
-function updatePosition () {
-  const directionRad = lastOrientation * (Math.PI / 180)
-  position.lat += stepLength * Math.cos(directionRad)
-  position.lng += stepLength * Math.sin(directionRad)
+function updatePosition (lastOrientation) {
+  const directionRad = toRadians(lastOrientation)
+  const cosDirection = Math.cos(directionRad)
+  const sinDirection = Math.sin(directionRad)
+
+  position.lat += stepLength * cosDirection
+  position.lng += stepLength * sinDirection
+
+  return position
 }

js/map.js

@@ -269,7 +269,7 @@ function handleOrientation (event) {
   const orientation = 360 - event.webkitCompassHeading
   map.setBearing(orientation + bias)
   motionArray[motionArray.length - 1].push(event.webkitCompassHeading, event.beta, event.gamma)
-  const newPosition = calculatePosition(motionArray, userPosition)
+  const newPosition = calculatePosition(motionArray, userPosition, bias)
   userPosition = L.latLng(newPosition.lat, newPosition.lng)
   circle.setLatLng(userPosition)
   map.flyTo(userPosition, 1)