Update vcs.xml, AndroidManifest.xml, BleScanner.kt, and 4 more files

Sensor Fusion

App v2/app/src/main/AndroidManifest.xml

@@ -18,6 +18,7 @@
     <!-- Für BLE-Positionierung benötigt-->
     <uses-permission android:name="android.permission.ACCESS_FINE_LOCATION" />
     <uses-permission android:name="android.permission.ACCESS_COARSE_LOCATION" />
+    <uses-permission android:name="android.permission.ACTIVITY_RECOGNITION" />
 
 
 

App v2/app/src/main/java/com/erik/bleindoorpositioning/ble/BleScanner.kt

@@ -14,15 +14,18 @@ import com.erik.bleindoorpositioning.model.PositionResult
 import com.erik.bleindoorpositioning.model.ScannedBeacon
 import com.erik.bleindoorpositioning.positioning.BeaconFilterManager
 import com.erik.bleindoorpositioning.positioning.KalmanFilter
+import com.erik.bleindoorpositioning.positioning.MovingAverageFilter
 import com.erik.bleindoorpositioning.positioning.PositionCalculator
+import com.erik.bleindoorpositioning.positioning.PositionKalmanFilter
 import com.erik.bleindoorpositioning.positioning.PositionSmoother
+import com.erik.bleindoorpositioning.positioning.SensorFusionManager
 import kotlinx.coroutines.*
 import kotlinx.coroutines.flow.MutableStateFlow
 import kotlinx.coroutines.flow.StateFlow
 import kotlinx.coroutines.flow.asStateFlow
 import java.util.concurrent.TimeUnit
 
-class BleScanner(context: Context, private val configRepository: ConfigRepository, private val coroutineScope: CoroutineScope) {
+class BleScanner(private val context: Context, private val configRepository: ConfigRepository, private val coroutineScope: CoroutineScope) {
 
     companion object {
         private const val TAG = "BleScanner"
@@ -52,18 +55,34 @@ class BleScanner(context: Context, private val configRepository: ConfigRepositor
     private var retryAttempt = 0
 
     // RSSI-Filterung
-    private val filterManager = BeaconFilterManager {
-        KalmanFilter(
-            processNoise = 0.5,       // TODO: Kalibrieren
-            measurementNoise = 10.0   // TODO: Kalibrieren
-        )
+    private val filterManager = BeaconFilterManager { // Filter für einzelne Beacon-RSSI-Werte
+        MovingAverageFilter(windowSize = 20) // 20 Messpunkte für stabile Mittelwertbildung
     }
 
     // Positionsberechnung (wird bei jedem Scan-Start neu initialisiert)
     private var positionCalculator: PositionCalculator? = null
 
     // Positions-Glättung
-    private val positionSmoother = PositionSmoother(alpha = 0.15)
+    private val positionSmoother = PositionSmoother(alpha = 0.25) // Reaktiver, da EKF schon glättet
+
+    // Sensor-Fusion (EKF + PDR)
+    private val positionKalmanFilter = PositionKalmanFilter(processNoise = 0.5) // Standard: Balance zwischen PDR und BLE
+    private var isEkfInitialized = false
+    private var sensorFusionManager: SensorFusionManager? = null
+    private val lock = Any()
+
+    init {
+        sensorFusionManager = SensorFusionManager(context) { heading, stepLength ->
+            synchronized(lock) {
+                positionKalmanFilter.predict(heading, stepLength)
+                val fusedPos = positionKalmanFilter.getPosition()
+                val smoothed = positionSmoother.update(fusedPos)
+                _position.value?.let { prev ->
+                    _position.value = prev.copy(position = smoothed)
+                }
+            }
+        }
+    }
 
     // Öffentliche State Flows
     private val _beacons = MutableStateFlow<Map<Int, ScannedBeacon>>(emptyMap())
@@ -109,10 +128,18 @@ class BleScanner(context: Context, private val configRepository: ConfigRepositor
                 val rssiMap = freshBeacons.mapValues { it.value.filteredRssi }
                 val rawResult = positionCalculator?.calculate(rssiMap)
 
-                // Position glätten
-                _position.value = rawResult?.let { raw ->
-                    val smoothed = positionSmoother.update(raw.position)
-                    raw.copy(position = smoothed)
+                // Sensor-Fusion: BLE dominiert, PDR ergänzt zwischen Updates
+                rawResult?.let { raw ->
+                    synchronized(lock) {
+                        if (!isEkfInitialized) {
+                            positionKalmanFilter.initialize(raw.position)
+                            isEkfInitialized = true
+                        }
+                        positionKalmanFilter.update(raw.position, measurementNoise = 2.0)
+                        val fusedPosition = positionKalmanFilter.getPosition()
+                        val smoothed = positionSmoother.update(fusedPosition)
+                        _position.value = raw.copy(position = smoothed)
+                    }
                 }
 
                 Log.d(TAG, "Beacon ${beacon.minor}: " +
@@ -211,6 +238,7 @@ class BleScanner(context: Context, private val configRepository: ConfigRepositor
 
         scanner?.startScan(null, settings, scanCallback)
         _isScanning.value = true
+        sensorFusionManager?.start()
         Log.d(TAG, "Scanning started")
 
         // Start proactive scan restart job
@@ -238,6 +266,8 @@ class BleScanner(context: Context, private val configRepository: ConfigRepositor
         val config = configRepository.loadConfig()
         if (config.beacons.size < 3) {
             Log.w(TAG, "Weniger als 3 Beacons konfiguriert, Positionsberechnung nicht möglich")
+            positionCalculator = null
+            return
         }
         positionCalculator = PositionCalculator(
             rssiAtOneMeter = BeaconData.RSSI_AT_ONE_METER,
@@ -262,7 +292,12 @@ class BleScanner(context: Context, private val configRepository: ConfigRepositor
         retryAttempt = 0
         _isScanning.value = false
         filterManager.reset()
-        positionSmoother.reset()
+        synchronized(lock) {
+            positionSmoother.reset()
+            positionKalmanFilter.reset()
+            isEkfInitialized = false
+        }
+        sensorFusionManager?.stop()
         Log.d(TAG, "Scanning stopped")
     }
 }

App v2/app/src/main/java/com/erik/bleindoorpositioning/positioning/PositionKalmanFilter.kt

@@ -0,0 +1,179 @@
+package com.erik.bleindoorpositioning.positioning
+
+import com.erik.bleindoorpositioning.model.Position
+import kotlin.math.cos
+import kotlin.math.sin
+
+/**
+ * Ein einfacher Erweiterter Kalman-Filter (EKF) zur Schätzung der 2D-Position.
+ * Dieser Filter fusioniert relative Positionsänderungen aus PDR (Pedestrian Dead Reckoning)
+ * mit absoluten Positionen aus BLE-Messungen.
+ *
+ * Der Zustandsvektor ist [x, y]. Die Geschwindigkeit ist nicht Teil des Zustands.
+ *
+ * @param processNoise Die Varianz des Prozessrauschens für die Vorhersage.
+ *                     Dies repräsentiert die Unsicherheit des Schrittmodells.
+ */
+class PositionKalmanFilter(processNoise: Double = 0.5) {
+
+    // Zustandsvektor: [x, y]
+    private var state = doubleArrayOf(0.0, 0.0)
+
+    // Kovarianzmatrix P (2x2)
+    private var covariance = arrayOf(
+        doubleArrayOf(1.0, 0.0),
+        doubleArrayOf(0.0, 1.0)
+    )
+
+    // Prozessrauschkovarianz Q (2x2)
+    private val processNoiseQ = arrayOf(
+        doubleArrayOf(processNoise, 0.0),
+        doubleArrayOf(0.0, processNoise)
+    )
+
+    /**
+     * Initialisiert den Filterzustand mit einer Startposition.
+     * Setzt auch die Kovarianz auf einen Anfangswert zurück, der eine hohe Unsicherheit darstellt.
+     *
+     * @param startPosition Die anfängliche Position.
+     */
+    fun initialize(startPosition: Position) {
+        state[0] = startPosition.x
+        state[1] = startPosition.y
+        // Setze die Kovarianz auf einen Zustand hoher anfänglicher Unsicherheit zurück
+        covariance = arrayOf(
+            doubleArrayOf(1.0, 0.0),
+            doubleArrayOf(0.0, 1.0)
+        )
+    }
+
+    /**
+     * Setzt den Filterzustand und die Kovarianz auf die Standardwerte zurück.
+     */
+    fun reset() {
+        state = doubleArrayOf(0.0, 0.0)
+        covariance = arrayOf(
+            doubleArrayOf(1.0, 0.0),
+            doubleArrayOf(0.0, 1.0)
+        )
+    }
+
+    /**
+     * Der Vorhersageschritt (Prediction).
+     * Aktualisiert den Zustand basierend auf einem Bewegungsschritt (z. B. von PDR).
+     *
+     * @param heading Die Bewegungsrichtung im Bogenmaß.
+     * @param stepLength Die Länge des Schrittes in Metern.
+     */
+    fun predict(heading: Double, stepLength: Double) {
+        // --- Zustand vorhersagen ---
+        // x_new = x + stepLength * sin(heading)
+        // y_new = y + stepLength * cos(heading)
+        state[0] += stepLength * sin(heading)
+        state[1] += stepLength * cos(heading)
+
+        // --- Kovarianz vorhersagen ---
+        // P_pred = F * P * F^T + Q
+        // Die Jacobi-Matrix F des Prozessmodells ist hier die Einheitsmatrix,
+        // da die neue Position linear von der alten abhängt (x_new = x + delta_x).
+        // Daher vereinfacht sich die Formel zu: P_pred = P + Q.
+        covariance[0][0] += processNoiseQ[0][0]
+        covariance[1][1] += processNoiseQ[1][1]
+    }
+
+    /**
+     * Der Korrekturschritt (Update).
+     * Korrigiert den Zustand basierend auf einer absoluten Positionsmessung (z. B. von BLE).
+     *
+     * @param blePosition Die gemessene Position.
+     * @param measurementNoise Die Varianz des Messrauschens. Repräsentiert die
+     *                         Unsicherheit der BLE-Positionsmessung.
+     */
+    fun update(blePosition: Position, measurementNoise: Double = 2.0) {
+        // --- Messrauschkovarianz R ---
+        val r = arrayOf(
+            doubleArrayOf(measurementNoise, 0.0),
+            doubleArrayOf(0.0, measurementNoise)
+        )
+
+        // --- Messmodell H ---
+        // Das Messmodell ist linear und direkt, da die Messung [x_ble, y_ble]
+        // direkt dem Zustand [x, y] entspricht.
+        // H ist daher die 2x2-Einheitsmatrix.
+
+        // --- Innovations-Kovarianz S ---
+        // S = H * P * H^T + R
+        // Da H die Einheitsmatrix ist: S = P + R
+        val s = arrayOf(
+            doubleArrayOf(covariance[0][0] + r[0][0], covariance[0][1]),
+            doubleArrayOf(covariance[1][0], covariance[1][1] + r[1][1])
+        )
+
+        // --- Inverse von S berechnen ---
+        val detS = s[0][0] * s[1][1] - s[0][1] * s[1][0]
+        if (detS == 0.0) return // Singularität vermeiden
+        val invDetS = 1.0 / detS
+        val sInverse = arrayOf(
+            doubleArrayOf(s[1][1] * invDetS, -s[0][1] * invDetS),
+            doubleArrayOf(-s[1][0] * invDetS, s[0][0] * invDetS)
+        )
+
+        // --- Kalman-Gewinn K ---
+        // K = P * H^T * S^-1
+        // Da H die Einheitsmatrix ist: K = P * S^-1
+        val k = arrayOf(
+            doubleArrayOf(
+                covariance[0][0] * sInverse[0][0] + covariance[0][1] * sInverse[1][0],
+                covariance[0][0] * sInverse[0][1] + covariance[0][1] * sInverse[1][1]
+            ),
+            doubleArrayOf(
+                covariance[1][0] * sInverse[0][0] + covariance[1][1] * sInverse[1][0],
+                covariance[1][0] * sInverse[0][1] + covariance[1][1] * sInverse[1][1]
+            )
+        )
+
+        // --- Zustand aktualisieren ---
+        // state_new = state + K * (z - H * state)
+        // Da H die Einheitsmatrix ist: state_new = state + K * (z - state)
+        val measurement = doubleArrayOf(blePosition.x, blePosition.y)
+        val innovation = doubleArrayOf(measurement[0] - state[0], measurement[1] - state[1])
+
+        state[0] += k[0][0] * innovation[0] + k[0][1] * innovation[1]
+        state[1] += k[1][0] * innovation[0] + k[1][1] * innovation[1]
+
+        // --- Kovarianz aktualisieren ---
+        // P_new = (I - K * H) * P
+        // Da H die Einheitsmatrix ist: P_new = (I - K) * P
+        val iMinusK = arrayOf(
+            doubleArrayOf(1.0 - k[0][0], -k[0][1]),
+            doubleArrayOf(-k[1][0], 1.0 - k[1][1])
+        )
+
+        val newCovariance = arrayOf(
+            doubleArrayOf(
+                iMinusK[0][0] * covariance[0][0] + iMinusK[0][1] * covariance[1][0],
+                iMinusK[0][0] * covariance[0][1] + iMinusK[0][1] * covariance[1][1]
+            ),
+            doubleArrayOf(
+                iMinusK[1][0] * covariance[0][0] + iMinusK[1][1] * covariance[1][0],
+                iMinusK[1][0] * covariance[0][1] + iMinusK[1][1] * covariance[1][1]
+            )
+        )
+        // Minimale Kovarianz erzwingen, damit der Filter nie zu "sicher" wird
+        // und immer auf neue Messungen reagiert
+        val minCovariance = 0.1
+        newCovariance[0][0] = newCovariance[0][0].coerceAtLeast(minCovariance)
+        newCovariance[1][1] = newCovariance[1][1].coerceAtLeast(minCovariance)
+
+        covariance = newCovariance
+    }
+
+    /**
+     * Gibt die aktuell geschätzte Position zurück.
+     *
+     * @return Die fusionierte Position als [Position]-Objekt.
+     */
+    fun getPosition(): Position {
+        return Position(state[0], state[1])
+    }
+}

App v2/app/src/main/java/com/erik/bleindoorpositioning/positioning/SensorFusionManager.kt

@@ -0,0 +1,189 @@
+package com.erik.bleindoorpositioning.positioning
+
+import android.content.Context
+import android.hardware.Sensor
+import android.hardware.SensorEvent
+import android.hardware.SensorEventListener
+import android.hardware.SensorManager
+import android.util.Log
+import kotlin.math.atan2
+import kotlin.math.sqrt
+
+/**
+ * Callback-Interface für Schritt-Events
+ * @param heading Bewegungsrichtung in Radiant (0 = Nord, π/2 = Ost)
+ * @param stepLength Schrittlänge in Metern
+ */
+fun interface StepListener {
+    fun onStep(heading: Double, stepLength: Double)
+}
+
+/**
+ * SensorFusionManager für Pedestrian Dead Reckoning (PDR)
+ *
+ * Nutzt Android-Sensoren:
+ * - TYPE_STEP_DETECTOR: Schritterkennung
+ * - TYPE_ROTATION_VECTOR: Geräteausrichtung (Azimuth/Heading)
+ * - TYPE_LINEAR_ACCELERATION: Stillstandserkennung + dynamische Schrittlänge
+ *
+ * @param context Android Context für SensorManager-Zugriff
+ * @param listener Callback für Schritt-Events
+ */
+class SensorFusionManager(
+    context: Context,
+    private val listener: StepListener
+) : SensorEventListener {
+
+    private val sensorManager: SensorManager = context.getSystemService(Context.SENSOR_SERVICE) as SensorManager
+
+    // Sensoren
+    private val stepDetector: Sensor? = sensorManager.getDefaultSensor(Sensor.TYPE_STEP_DETECTOR)
+    private val rotationVector: Sensor? = sensorManager.getDefaultSensor(Sensor.TYPE_ROTATION_VECTOR)
+    private val linearAcceleration: Sensor? = sensorManager.getDefaultSensor(Sensor.TYPE_LINEAR_ACCELERATION)
+
+    val isAvailable: Boolean = (stepDetector != null && rotationVector != null && linearAcceleration != null)
+
+    // State
+    private var currentHeading: Double = 0.0 // Azimuth in Radiant
+    private val accelerationWindow = ArrayDeque<Float>(ACCELERATION_WINDOW_SIZE)
+    private var lastAccelerationMagnitude: Float = 0f
+
+    // Konfiguration
+    private val baseStepLength: Double = 0.7 // Meter
+    private val minStepLength: Double = 0.5 // Meter
+    private val maxStepLength: Double = 1.0 // Meter
+
+    init {
+        if (!isAvailable) {
+            val missingSensors = mutableListOf<String>()
+            if (stepDetector == null) missingSensors.add("TYPE_STEP_DETECTOR")
+            if (rotationVector == null) missingSensors.add("TYPE_ROTATION_VECTOR")
+            if (linearAcceleration == null) missingSensors.add("TYPE_LINEAR_ACCELERATION")
+            Log.w(TAG, "SensorFusionManager is not available. Missing sensors: ${missingSensors.joinToString()}")
+        }
+    }
+
+    /**
+     * Startet Sensor-Listeners
+     */
+    fun start() {
+        if (!isAvailable) return
+        stepDetector?.let {
+            sensorManager.registerListener(this, it, SensorManager.SENSOR_DELAY_FASTEST)
+        }
+        rotationVector?.let {
+            sensorManager.registerListener(this, it, SensorManager.SENSOR_DELAY_GAME)
+        }
+        linearAcceleration?.let {
+            sensorManager.registerListener(this, it, SensorManager.SENSOR_DELAY_FASTEST)
+        }
+    }
+
+    /**
+     * Stoppt Sensor-Listeners
+     */
+    fun stop() {
+        if (!isAvailable) return
+        sensorManager.unregisterListener(this)
+        accelerationWindow.clear()
+    }
+
+    override fun onSensorChanged(event: SensorEvent) {
+        when (event.sensor.type) {
+            Sensor.TYPE_STEP_DETECTOR -> handleStepDetected()
+            Sensor.TYPE_ROTATION_VECTOR -> handleRotationVector(event.values)
+            Sensor.TYPE_LINEAR_ACCELERATION -> handleLinearAcceleration(event.values)
+        }
+    }
+
+    override fun onAccuracyChanged(sensor: Sensor?, accuracy: Int) {
+        // Nicht benötigt für PDR
+    }
+
+    /**
+     * Verarbeitet Schritt-Event
+     * Prüft Stillstand und berechnet dynamische Schrittlänge
+     */
+    private fun handleStepDetected() {
+        // Stillstandserkennung: Varianz der Acceleration prüfen
+        if (isStationary()) {
+            return // Kein Step wenn Nutzer steht
+        }
+
+        // Dynamische Schrittlänge basierend auf Acceleration-Magnitude
+        val stepLength = calculateStepLength(lastAccelerationMagnitude)
+
+        listener.onStep(currentHeading, stepLength)
+    }
+
+    /**
+     * Extrahiert Azimuth (Heading) aus Rotation Vector
+     * @param values Rotation Vector [x, y, z, optional scalar]
+     */
+    private fun handleRotationVector(values: FloatArray) {
+        val rotationMatrix = FloatArray(9)
+        SensorManager.getRotationMatrixFromVector(rotationMatrix, values)
+
+        val orientation = FloatArray(3)
+        SensorManager.getOrientation(rotationMatrix, orientation)
+
+        // orientation[0] = Azimuth (um Z-Achse, -π bis π)
+        // 0 = Nord, π/2 = Ost, -π/2 = West, ±π = Süd
+        currentHeading = orientation[0].toDouble()
+    }
+
+    /**
+     * Verarbeitet Linear Acceleration für Stillstandserkennung
+     * @param values [x, y, z] Acceleration in m/s² (ohne Gravitation)
+     */
+    private fun handleLinearAcceleration(values: FloatArray) {
+        val magnitude = sqrt(
+            values[0] * values[0] +
+            values[1] * values[1] +
+            values[2] * values[2]
+        )
+
+        lastAccelerationMagnitude = magnitude
+
+        // Sliding Window für Varianz-Berechnung
+        if (accelerationWindow.size >= ACCELERATION_WINDOW_SIZE) {
+            accelerationWindow.removeFirst()
+        }
+        accelerationWindow.addLast(magnitude)
+    }
+
+    /**
+     * Prüft ob Nutzer stillsteht (geringe Varianz der Acceleration)
+     * @return true wenn stillstehend
+     */
+    private fun isStationary(): Boolean {
+        if (accelerationWindow.size < ACCELERATION_WINDOW_SIZE) {
+            return false // Noch nicht genug Daten
+        }
+
+        val mean = accelerationWindow.average()
+        val variance = accelerationWindow.map { (it - mean) * (it - mean) }.average()
+
+        return variance < STATIONARY_VARIANCE_THRESHOLD
+    }
+
+    /**
+     * Berechnet dynamische Schrittlänge basierend auf Acceleration
+     * Höhere Beschleunigung = längerer Schritt (Laufen vs. Gehen)
+     * @param accelerationMagnitude Betrag der Linear Acceleration in m/s²
+     * @return Schrittlänge in Metern (zwischen minStepLength und maxStepLength)
+     */
+    private fun calculateStepLength(accelerationMagnitude: Float): Double {
+        // Mapping: 0-2 m/s² (Gehen) -> 0.5-0.7m, 2-5 m/s² (schnelles Gehen/Laufen) -> 0.7-1.0m
+        val normalized = (accelerationMagnitude / 5.0).coerceIn(0.0, 1.0)
+        val stepLength = baseStepLength + normalized * (maxStepLength - baseStepLength)
+
+        return stepLength.coerceIn(minStepLength, maxStepLength)
+    }
+
+    companion object {
+        private const val TAG = "SensorFusionManager"
+        private const val ACCELERATION_WINDOW_SIZE = 10 // Anzahl Samples für Varianz
+        private const val STATIONARY_VARIANCE_THRESHOLD = 0.05 // m²/s⁴ (experimentell anpassen)
+    }
+}

App v2/app/src/main/java/com/erik/bleindoorpositioning/ui/screens/PositioningScreen.kt

@@ -41,6 +41,9 @@ fun PositioningScreen(
 ) {
     val permissions = buildList {
         add(Manifest.permission.ACCESS_FINE_LOCATION)
+        if (Build.VERSION.SDK_INT >= Build.VERSION_CODES.Q) {
+            add(Manifest.permission.ACTIVITY_RECOGNITION)
+        }
         if (Build.VERSION.SDK_INT >= Build.VERSION_CODES.S) {
             add(Manifest.permission.BLUETOOTH_SCAN)
             add(Manifest.permission.BLUETOOTH_CONNECT)