wengki/dumon-geolocation
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Debuging added

Browse Source
This commit is contained in:
wengki81 2025-06-17 23:01:40 +08:00
parent a4ca0d1913
commit 5f46f6ab21
1 changed files with 0 additions and 83 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

83
android/src/main/java/com/dumon/plugin/geolocation/fusion/SensorFusionManager.kt
View File

@ -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<Double, Double> {
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
}
}