diff --git a/android/src/main/java/com/dumon/plugin/geolocation/fusion/SensorFusionManager.kt b/android/src/main/java/com/dumon/plugin/geolocation/fusion/SensorFusionManager.kt deleted file mode 100644 index 08a8d68..0000000 --- a/android/src/main/java/com/dumon/plugin/geolocation/fusion/SensorFusionManager.kt +++ /dev/null @@ -1,83 +0,0 @@ -package com.dumon.plugin.geolocation.fusion - -import android.util.Log -import kotlin.math.* - -class SensorFusionManager( - private val onFusedPositionUpdate: (Double, Double) -> Unit = { _, _ -> } -) { - - // Kalman filter state - private var latEstimate = 0.0 - private var lonEstimate = 0.0 - private var latErrorEstimate = 1.0 - private var lonErrorEstimate = 1.0 - - private val processNoiseBase = 0.01 - private val measurementNoise = 3.0 // typical GPS error in meters - - private var isFirstUpdate = true - private var lastUpdateTimestamp: Long = 0L - - private var lastSpeed = 0.0 - private var lastHeadingRad = 0.0 - - fun updateGpsPosition(lat: Double, lon: Double) { - val currentTimestamp = System.currentTimeMillis() - val dtSeconds = if (lastUpdateTimestamp > 0) { - (currentTimestamp - lastUpdateTimestamp) / 1000.0 - } else { - 1.0 - } - lastUpdateTimestamp = currentTimestamp - - val processNoise = processNoiseBase * dtSeconds - - if (isFirstUpdate) { - latEstimate = lat - lonEstimate = lon - isFirstUpdate = false - } else { - // Kalman update for latitude - val latKalmanGain = latErrorEstimate / (latErrorEstimate + measurementNoise) - latEstimate += latKalmanGain * (lat - latEstimate) - latErrorEstimate = (1 - latKalmanGain) * latErrorEstimate + processNoise - - // Kalman update for longitude - val lonKalmanGain = lonErrorEstimate / (lonErrorEstimate + measurementNoise) - lonEstimate += lonKalmanGain * (lon - lonEstimate) - lonErrorEstimate = (1 - lonKalmanGain) * lonErrorEstimate + processNoise - } - - Log.d("SENSOR_FUSION", "Fused Lat: $latEstimate, Lon: $lonEstimate") - - onFusedPositionUpdate(latEstimate, lonEstimate) - } - - fun updateMotionEstimate(speed: Double, headingRad: Double) { - lastSpeed = speed - lastHeadingRad = headingRad - } - - fun predictForwardPosition(secondsAhead: Double): Pair { - val distance = lastSpeed * secondsAhead - val R = 6371000.0 - - val deltaLat = (distance * cos(lastHeadingRad)) / R - val deltaLon = (distance * sin(lastHeadingRad)) / (R * cos(Math.toRadians(latEstimate))) - - val predictedLat = latEstimate + Math.toDegrees(deltaLat) - val predictedLon = lonEstimate + Math.toDegrees(deltaLon) - - return Pair(predictedLat, predictedLon) - } - - fun reset() { - latEstimate = 0.0 - lonEstimate = 0.0 - latErrorEstimate = 1.0 - lonErrorEstimate = 1.0 - isFirstUpdate = true - lastUpdateTimestamp = 0L - } -} \ No newline at end of file