From df222f01268d272be9a2750121d813e782dde5a3 Mon Sep 17 00:00:00 2001 From: Daniel Gilbers Date: Wed, 25 Sep 2024 10:10:05 +0200 Subject: [PATCH] refactor --- js/Position.js | 112 +++++++++++++++++++++++-------------------------- js/map.js | 2 +- 2 files changed, 54 insertions(+), 60 deletions(-) diff --git a/js/Position.js b/js/Position.js index 21a4f81..48793a3 100644 --- a/js/Position.js +++ b/js/Position.js @@ -4,89 +4,80 @@ 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) { @@ -94,18 +85,18 @@ export function kFilter (arr) { 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 } diff --git a/js/map.js b/js/map.js index a9a9801..a1c4bd4 100644 --- a/js/map.js +++ b/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)