From 256c94dc35204b4660209cac687d3f9c6a5985b9 Mon Sep 17 00:00:00 2001 From: HuangerzJ <125708330+HuangerzJ@users.noreply.github.com> Date: Mon, 23 Mar 2026 17:08:56 +0000 Subject: [PATCH 01/52] dummy sensing fusion --- .../fragment/RecordingFragment.java | 33 ++- .../sensors/PositionFusionEngine.java | 277 ++++++++++++++++++ .../sensors/PositionFusionEstimate.java | 31 ++ .../sensors/SensorEventHandler.java | 29 +- .../PositionMe/sensors/SensorFusion.java | 58 +++- .../PositionMe/sensors/SensorState.java | 6 + .../sensors/WifiPositionManager.java | 26 +- 7 files changed, 443 insertions(+), 17 deletions(-) create mode 100644 app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java create mode 100644 app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEstimate.java diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/RecordingFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/RecordingFragment.java index 340843ca..8ca06126 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/RecordingFragment.java +++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/RecordingFragment.java @@ -264,23 +264,28 @@ private void updateUIandPosition() { float elevationVal = sensorFusion.getElevation(); elevation.setText(getString(R.string.elevation, String.format("%.1f", elevationVal))); - // Current location - // Convert PDR coordinates to actual LatLng if you have a known starting lat/lon - // Or simply pass relative data for the TrajectoryMapFragment to handle - // For example: - float[] latLngArray = sensorFusion.getGNSSLatitude(true); - if (latLngArray != null) { - LatLng oldLocation = trajectoryMapFragment.getCurrentLocation(); // or store locally - LatLng newLocation = UtilFunctions.calculateNewPos( - oldLocation == null ? new LatLng(latLngArray[0], latLngArray[1]) : oldLocation, - new float[]{ pdrValues[0] - previousPosX, pdrValues[1] - previousPosY } - ); - - // Pass the location + orientation to the map + // Current location: use fused estimate when available, otherwise keep PDR dead-reckoning fallback. + LatLng fusedLocation = sensorFusion.getFusedLatLng(); + if (fusedLocation != null) { if (trajectoryMapFragment != null) { - trajectoryMapFragment.updateUserLocation(newLocation, + trajectoryMapFragment.updateUserLocation( + fusedLocation, (float) Math.toDegrees(sensorFusion.passOrientation())); } + } else { + float[] latLngArray = sensorFusion.getGNSSLatitude(true); + if (latLngArray != null) { + LatLng oldLocation = trajectoryMapFragment.getCurrentLocation(); + LatLng newLocation = UtilFunctions.calculateNewPos( + oldLocation == null ? new LatLng(latLngArray[0], latLngArray[1]) : oldLocation, + new float[]{pdrValues[0] - previousPosX, pdrValues[1] - previousPosY} + ); + + if (trajectoryMapFragment != null) { + trajectoryMapFragment.updateUserLocation(newLocation, + (float) Math.toDegrees(sensorFusion.passOrientation())); + } + } } // GNSS logic if you want to show GNSS error, etc. diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java new file mode 100644 index 00000000..35b11cde --- /dev/null +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java @@ -0,0 +1,277 @@ +package com.openpositioning.PositionMe.sensors; + +import com.google.android.gms.maps.model.LatLng; + +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; +import java.util.Random; + +/** + * SIR particle filter fusion engine in local East/North coordinates. + * + *

The filter predicts with step-based PDR displacement and updates particle + * weights with GNSS/WiFi absolute fixes. Resampling is triggered when + * the effective particle count falls below a threshold.

+ */ +public class PositionFusionEngine { + + private static final double EARTH_RADIUS_M = 6378137.0; + + private static final int PARTICLE_COUNT = 220; + private static final double RESAMPLE_RATIO = 0.5; + private static final double PDR_NOISE_STD_M = 0.45; + private static final double INIT_STD_M = 2.0; + private static final double ROUGHEN_STD_M = 0.15; + private static final double WIFI_SIGMA_M = 8.0; + private static final double EPS = 1e-300; + + private final float floorHeightMeters; + private final Random random = new Random(); + + // Local tangent frame anchor + private double anchorLatDeg; + private double anchorLonDeg; + private boolean hasAnchor; + + private final List particles = new ArrayList<>(PARTICLE_COUNT); + private int fallbackFloor; + + private static final class Particle { + double xEast; + double yNorth; + int floor; + double weight; + } + + public PositionFusionEngine(float floorHeightMeters) { + this.floorHeightMeters = floorHeightMeters > 0f ? floorHeightMeters : 4f; + } + + public synchronized void reset(double latDeg, double lonDeg, int initialFloor) { + anchorLatDeg = latDeg; + anchorLonDeg = lonDeg; + hasAnchor = true; + + fallbackFloor = initialFloor; + initParticlesAtOrigin(initialFloor); + } + + public synchronized void updatePdrDisplacement(float dxEastMeters, float dyNorthMeters) { + if (!hasAnchor || particles.isEmpty()) { + return; + } + + for (Particle p : particles) { + p.xEast += dxEastMeters + random.nextGaussian() * PDR_NOISE_STD_M; + p.yNorth += dyNorthMeters + random.nextGaussian() * PDR_NOISE_STD_M; + } + } + + public synchronized void updateGnss(double latDeg, double lonDeg, float accuracyMeters) { + double sigma = Math.max(accuracyMeters, 3.0f); + applyAbsoluteFix(latDeg, lonDeg, sigma, null); + } + + public synchronized void updateWifi(double latDeg, double lonDeg, int wifiFloor) { + applyAbsoluteFix(latDeg, lonDeg, WIFI_SIGMA_M, wifiFloor); + } + + public synchronized void updateElevation(float elevationMeters) { + int floorFromBarometer = Math.round(elevationMeters / floorHeightMeters); + fallbackFloor = floorFromBarometer; + if (!particles.isEmpty()) { + for (Particle p : particles) { + p.floor = floorFromBarometer; + } + } + } + + public synchronized PositionFusionEstimate getEstimate() { + if (!hasAnchor || particles.isEmpty()) { + return new PositionFusionEstimate(null, fallbackFloor, false); + } + + double meanX = 0.0; + double meanY = 0.0; + Map floorWeights = new HashMap<>(); + + for (Particle p : particles) { + meanX += p.weight * p.xEast; + meanY += p.weight * p.yNorth; + floorWeights.put(p.floor, floorWeights.getOrDefault(p.floor, 0.0) + p.weight); + } + + int bestFloor = fallbackFloor; + double bestFloorWeight = -1.0; + for (Map.Entry entry : floorWeights.entrySet()) { + if (entry.getValue() > bestFloorWeight) { + bestFloor = entry.getKey(); + bestFloorWeight = entry.getValue(); + } + } + + LatLng latLng = toLatLng(meanX, meanY); + return new PositionFusionEstimate(latLng, bestFloor, true); + } + + private void applyAbsoluteFix(double latDeg, double lonDeg, double sigmaMeters, Integer floorHint) { + if (!hasAnchor) { + reset(latDeg, lonDeg, 0); + return; + } + + if (particles.isEmpty()) { + initParticlesAtOrigin(fallbackFloor); + } + + double[] z = toLocal(latDeg, lonDeg); + + double sigma2 = sigmaMeters * sigmaMeters; + double maxLogWeight = Double.NEGATIVE_INFINITY; + double[] logWeights = new double[particles.size()]; + + for (int i = 0; i < particles.size(); i++) { + Particle p = particles.get(i); + double dx = p.xEast - z[0]; + double dy = p.yNorth - z[1]; + double distance2 = dx * dx + dy * dy; + double logLikelihood = -0.5 * (distance2 / sigma2); + + if (floorHint != null) { + // Soft floor gating: keep mismatch possible, but less probable. + logLikelihood += (p.floor == floorHint) ? Math.log(0.90) : Math.log(0.10); + } + + double logWeight = Math.log(Math.max(p.weight, EPS)) + logLikelihood; + logWeights[i] = logWeight; + if (logWeight > maxLogWeight) { + maxLogWeight = logWeight; + } + } + + double sumW = 0.0; + for (int i = 0; i < particles.size(); i++) { + double normalized = Math.exp(logWeights[i] - maxLogWeight); + particles.get(i).weight = Math.max(normalized, EPS); + sumW += particles.get(i).weight; + } + + if (sumW <= 0.0) { + reinitializeAroundMeasurement(z[0], z[1], floorHint != null ? floorHint : fallbackFloor); + return; + } + + for (Particle p : particles) { + p.weight /= sumW; + } + + if (floorHint != null) { + fallbackFloor = floorHint; + } + + double effectiveN = computeEffectiveSampleSize(); + if (effectiveN < PARTICLE_COUNT * RESAMPLE_RATIO) { + resampleSystematic(); + roughenParticles(); + } + } + + private void initParticlesAtOrigin(int initialFloor) { + particles.clear(); + double w = 1.0 / PARTICLE_COUNT; + for (int i = 0; i < PARTICLE_COUNT; i++) { + Particle p = new Particle(); + p.xEast = random.nextGaussian() * INIT_STD_M; + p.yNorth = random.nextGaussian() * INIT_STD_M; + p.floor = initialFloor; + p.weight = w; + particles.add(p); + } + } + + private void reinitializeAroundMeasurement(double x, double y, int floor) { + particles.clear(); + double w = 1.0 / PARTICLE_COUNT; + for (int i = 0; i < PARTICLE_COUNT; i++) { + Particle p = new Particle(); + p.xEast = x + random.nextGaussian() * INIT_STD_M; + p.yNorth = y + random.nextGaussian() * INIT_STD_M; + p.floor = floor; + p.weight = w; + particles.add(p); + } + } + + private double computeEffectiveSampleSize() { + double sumSquared = 0.0; + for (Particle p : particles) { + sumSquared += p.weight * p.weight; + } + if (sumSquared <= 0.0) { + return 0.0; + } + return 1.0 / sumSquared; + } + + private void resampleSystematic() { + List resampled = new ArrayList<>(PARTICLE_COUNT); + double step = 1.0 / PARTICLE_COUNT; + double u = random.nextDouble() * step; + double cdf = particles.get(0).weight; + int idx = 0; + + for (int m = 0; m < PARTICLE_COUNT; m++) { + double threshold = u + m * step; + while (threshold > cdf && idx < particles.size() - 1) { + idx++; + cdf += particles.get(idx).weight; + } + + Particle src = particles.get(idx); + Particle copy = new Particle(); + copy.xEast = src.xEast; + copy.yNorth = src.yNorth; + copy.floor = src.floor; + copy.weight = step; + resampled.add(copy); + } + + particles.clear(); + particles.addAll(resampled); + } + + private void roughenParticles() { + for (Particle p : particles) { + p.xEast += random.nextGaussian() * ROUGHEN_STD_M; + p.yNorth += random.nextGaussian() * ROUGHEN_STD_M; + } + } + + private double[] toLocal(double latDeg, double lonDeg) { + double lat0Rad = Math.toRadians(anchorLatDeg); + double dLat = Math.toRadians(latDeg - anchorLatDeg); + double dLon = Math.toRadians(lonDeg - anchorLonDeg); + + double east = dLon * EARTH_RADIUS_M * Math.cos(lat0Rad); + double north = dLat * EARTH_RADIUS_M; + return new double[]{east, north}; + } + + private LatLng toLatLng(double eastMeters, double northMeters) { + double lat0Rad = Math.toRadians(anchorLatDeg); + + double dLat = northMeters / EARTH_RADIUS_M; + double cosLat = Math.cos(lat0Rad); + if (Math.abs(cosLat) < 1e-9) { + cosLat = 1e-9; + } + double dLon = eastMeters / (EARTH_RADIUS_M * cosLat); + + double lat = anchorLatDeg + Math.toDegrees(dLat); + double lon = anchorLonDeg + Math.toDegrees(dLon); + + return new LatLng(lat, lon); + } +} diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEstimate.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEstimate.java new file mode 100644 index 00000000..f1ee5cad --- /dev/null +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEstimate.java @@ -0,0 +1,31 @@ +package com.openpositioning.PositionMe.sensors; + +import com.google.android.gms.maps.model.LatLng; + +/** + * Immutable snapshot of the fused position estimate. + */ +public class PositionFusionEstimate { + + private final LatLng latLng; + private final int floor; + private final boolean available; + + public PositionFusionEstimate(LatLng latLng, int floor, boolean available) { + this.latLng = latLng; + this.floor = floor; + this.available = available; + } + + public LatLng getLatLng() { + return latLng; + } + + public int getFloor() { + return floor; + } + + public boolean isAvailable() { + return available; + } +} diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java index 639fc5c2..222b1a05 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java @@ -22,6 +22,10 @@ */ public class SensorEventHandler { + public interface PdrStepListener { + void onPdrStep(float dxEastMeters, float dyNorthMeters, long relativeTimestampMs); + } + private static final float ALPHA = 0.8f; private static final long LARGE_GAP_THRESHOLD_MS = 500; @@ -29,6 +33,7 @@ public class SensorEventHandler { private final PdrProcessing pdrProcessing; private final PathView pathView; private final TrajectoryRecorder recorder; + private final PdrStepListener pdrStepListener; // Timestamp tracking private final HashMap lastEventTimestamps = new HashMap<>(); @@ -38,6 +43,9 @@ public class SensorEventHandler { // Acceleration magnitude buffer between steps private final List accelMagnitude = new ArrayList<>(); + private float lastPdrX = 0f; + private float lastPdrY = 0f; + private boolean hasPdrReference = false; /** * Creates a new SensorEventHandler. @@ -50,12 +58,14 @@ public class SensorEventHandler { */ public SensorEventHandler(SensorState state, PdrProcessing pdrProcessing, PathView pathView, TrajectoryRecorder recorder, - long bootTime) { + long bootTime, + PdrStepListener pdrStepListener) { this.state = state; this.pdrProcessing = pdrProcessing; this.pathView = pathView; this.recorder = recorder; this.bootTime = bootTime; + this.pdrStepListener = pdrStepListener; } /** @@ -171,6 +181,20 @@ public void handleSensorEvent(SensorEvent sensorEvent) { state.orientation[0] ); + float dx = 0f; + float dy = 0f; + if (hasPdrReference) { + dx = newCords[0] - lastPdrX; + dy = newCords[1] - lastPdrY; + } + lastPdrX = newCords[0]; + lastPdrY = newCords[1]; + hasPdrReference = true; + + if (pdrStepListener != null && hasPdrReference) { + pdrStepListener.onPdrStep(dx, dy, stepTime); + } + this.accelMagnitude.clear(); if (recorder.isRecording()) { @@ -203,5 +227,8 @@ public void logSensorFrequencies() { */ void resetBootTime(long newBootTime) { this.bootTime = newBootTime; + this.hasPdrReference = false; + this.lastPdrX = 0f; + this.lastPdrY = 0f; } } diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java index aeb6386a..282243d5 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -63,6 +63,7 @@ public class SensorFusion implements SensorEventListener { private SensorEventHandler eventHandler; private TrajectoryRecorder recorder; private WifiPositionManager wifiPositionManager; + private PositionFusionEngine fusionEngine; // Movement sensor instances (lifecycle managed here) private MovementSensor accelerometerSensor; @@ -151,6 +152,7 @@ public void setContext(Context context) { this.pdrProcessing = new PdrProcessing(context); this.pathView = new PathView(context, null); WiFiPositioning wiFiPositioning = new WiFiPositioning(context); + this.fusionEngine = new PositionFusionEngine(settings.getInt("floor_height", 4)); // Create internal modules this.recorder = new TrajectoryRecorder(appContext, state, serverCommunications, settings); @@ -162,7 +164,17 @@ public void setContext(Context context) { long bootTime = SystemClock.uptimeMillis(); this.eventHandler = new SensorEventHandler( - state, pdrProcessing, pathView, recorder, bootTime); + state, pdrProcessing, pathView, recorder, bootTime, + (dxEastMeters, dyNorthMeters, relativeTimestampMs) -> { + fusionEngine.updatePdrDisplacement(dxEastMeters, dyNorthMeters); + fusionEngine.updateElevation(state.elevation); + updateFusedState(); + }); + + this.wifiPositionManager.setWifiFixListener((wifiLocation, floor) -> { + fusionEngine.updateWifi(wifiLocation.latitude, wifiLocation.longitude, floor); + updateFusedState(); + }); // Register WiFi observer on WifiPositionManager (not on SensorFusion) this.wifiProcessor = new WifiDataProcessor(context); @@ -490,6 +502,10 @@ public float[] getGNSSLatitude(boolean start) { public void setStartGNSSLatitude(float[] startPosition) { state.startLocation[0] = startPosition[0]; state.startLocation[1] = startPosition[1]; + if (fusionEngine != null) { + fusionEngine.reset(startPosition[0], startPosition[1], 0); + updateFusedState(); + } } /** @@ -614,6 +630,23 @@ public LatLng getLatLngWifiPositioning() { return wifiPositionManager.getLatLngWifiPositioning(); } + /** + * Returns the best fused location estimate, if available. + */ + public LatLng getFusedLatLng() { + if (!state.fusedAvailable) { + return null; + } + return new LatLng(state.fusedLatitude, state.fusedLongitude); + } + + /** + * Returns the current fused floor estimate. + */ + public int getFusedFloor() { + return state.fusedFloor; + } + /** * Returns the current floor the user is on, obtained using WiFi positioning. * @@ -644,9 +677,32 @@ class MyLocationListener implements LocationListener { public void onLocationChanged(@NonNull Location location) { state.latitude = (float) location.getLatitude(); state.longitude = (float) location.getLongitude(); + if (fusionEngine != null) { + fusionEngine.updateGnss( + location.getLatitude(), + location.getLongitude(), + location.getAccuracy()); + updateFusedState(); + } recorder.addGnssData(location); } } + private void updateFusedState() { + if (fusionEngine == null) { + return; + } + PositionFusionEstimate estimate = fusionEngine.getEstimate(); + if (!estimate.isAvailable() || estimate.getLatLng() == null) { + state.fusedAvailable = false; + return; + } + + state.fusedLatitude = (float) estimate.getLatLng().latitude; + state.fusedLongitude = (float) estimate.getLatLng().longitude; + state.fusedFloor = estimate.getFloor(); + state.fusedAvailable = true; + } + //endregion } diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorState.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorState.java index 1554f61d..e5dbdadc 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorState.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorState.java @@ -37,6 +37,12 @@ public class SensorState { public volatile float longitude; public final float[] startLocation = new float[2]; + // Fused position estimate + public volatile float fusedLatitude; + public volatile float fusedLongitude; + public volatile int fusedFloor; + public volatile boolean fusedAvailable; + // Step counting public volatile int stepCounter; } diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java index 1edfd68a..da815693 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java @@ -22,11 +22,16 @@ */ public class WifiPositionManager implements Observer { + public interface WifiFixListener { + void onWifiFix(LatLng wifiLocation, int floor); + } + private static final String WIFI_FINGERPRINT = "wf"; private final WiFiPositioning wiFiPositioning; private final TrajectoryRecorder recorder; private List wifiList; + private WifiFixListener wifiFixListener; /** * Creates a new WifiPositionManager. @@ -65,7 +70,19 @@ private void createWifiPositioningRequest() { } JSONObject wifiFingerPrint = new JSONObject(); wifiFingerPrint.put(WIFI_FINGERPRINT, wifiAccessPoints); - this.wiFiPositioning.request(wifiFingerPrint); + this.wiFiPositioning.request(wifiFingerPrint, new WiFiPositioning.VolleyCallback() { + @Override + public void onSuccess(LatLng wifiLocation, int floor) { + if (wifiFixListener != null && wifiLocation != null) { + wifiFixListener.onWifiFix(wifiLocation, floor); + } + } + + @Override + public void onError(String message) { + Log.e("WifiPositionManager", "WiFi positioning request failed: " + message); + } + }); } catch (JSONException e) { Log.e("jsonErrors", "Error creating json object" + e.toString()); } @@ -124,4 +141,11 @@ public int getWifiFloor() { public List getWifiList() { return this.wifiList; } + + /** + * Registers a listener receiving WiFi absolute fixes for downstream fusion. + */ + public void setWifiFixListener(WifiFixListener wifiFixListener) { + this.wifiFixListener = wifiFixListener; + } } From 1c260e99be3c04700a1880c6233f7517abdcb929 Mon Sep 17 00:00:00 2001 From: HuangerzJ <125708330+HuangerzJ@users.noreply.github.com> Date: Tue, 24 Mar 2026 15:52:13 +0000 Subject: [PATCH 02/52] Logging to test PF --- .../sensors/PositionFusionEngine.java | 103 ++++++++++++++++++ 1 file changed, 103 insertions(+) diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java index 35b11cde..afd6cc95 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java @@ -1,10 +1,13 @@ package com.openpositioning.PositionMe.sensors; +import android.util.Log; + import com.google.android.gms.maps.model.LatLng; import java.util.ArrayList; import java.util.HashMap; import java.util.List; +import java.util.Locale; import java.util.Map; import java.util.Random; @@ -17,6 +20,9 @@ */ public class PositionFusionEngine { + private static final String TAG = "PositionFusionPF"; + private static final boolean DEBUG_LOGS = true; + private static final double EARTH_RADIUS_M = 6378137.0; private static final int PARTICLE_COUNT = 220; @@ -37,6 +43,7 @@ public class PositionFusionEngine { private final List particles = new ArrayList<>(PARTICLE_COUNT); private int fallbackFloor; + private long updateCounter; private static final class Particle { double xEast; @@ -56,6 +63,11 @@ public synchronized void reset(double latDeg, double lonDeg, int initialFloor) { fallbackFloor = initialFloor; initParticlesAtOrigin(initialFloor); + if (DEBUG_LOGS) { + Log.i(TAG, String.format(Locale.US, + "Reset anchor=(%.7f, %.7f) floor=%d particles=%d", + latDeg, lonDeg, initialFloor, PARTICLE_COUNT)); + } } public synchronized void updatePdrDisplacement(float dxEastMeters, float dyNorthMeters) { @@ -67,14 +79,30 @@ public synchronized void updatePdrDisplacement(float dxEastMeters, float dyNorth p.xEast += dxEastMeters + random.nextGaussian() * PDR_NOISE_STD_M; p.yNorth += dyNorthMeters + random.nextGaussian() * PDR_NOISE_STD_M; } + + if (DEBUG_LOGS) { + Log.d(TAG, String.format(Locale.US, + "Predict dPDR=(%.2fE, %.2fN) noiseStd=%.2f", + dxEastMeters, dyNorthMeters, PDR_NOISE_STD_M)); + } } public synchronized void updateGnss(double latDeg, double lonDeg, float accuracyMeters) { double sigma = Math.max(accuracyMeters, 3.0f); + if (DEBUG_LOGS) { + Log.d(TAG, String.format(Locale.US, + "GNSS update lat=%.7f lon=%.7f acc=%.2f sigma=%.2f", + latDeg, lonDeg, accuracyMeters, sigma)); + } applyAbsoluteFix(latDeg, lonDeg, sigma, null); } public synchronized void updateWifi(double latDeg, double lonDeg, int wifiFloor) { + if (DEBUG_LOGS) { + Log.d(TAG, String.format(Locale.US, + "WiFi update lat=%.7f lon=%.7f floor=%d sigma=%.2f", + latDeg, lonDeg, wifiFloor, WIFI_SIGMA_M)); + } applyAbsoluteFix(latDeg, lonDeg, WIFI_SIGMA_M, wifiFloor); } @@ -127,6 +155,7 @@ private void applyAbsoluteFix(double latDeg, double lonDeg, double sigmaMeters, } double[] z = toLocal(latDeg, lonDeg); + double effectiveBefore = computeEffectiveSampleSize(); double sigma2 = sigmaMeters * sigmaMeters; double maxLogWeight = Double.NEGATIVE_INFINITY; @@ -172,10 +201,15 @@ private void applyAbsoluteFix(double latDeg, double lonDeg, double sigmaMeters, } double effectiveN = computeEffectiveSampleSize(); + boolean resampled = false; if (effectiveN < PARTICLE_COUNT * RESAMPLE_RATIO) { resampleSystematic(); roughenParticles(); + resampled = true; } + + updateCounter++; + logUpdateSummary(z[0], z[1], sigmaMeters, floorHint, effectiveBefore, effectiveN, resampled); } private void initParticlesAtOrigin(int initialFloor) { @@ -274,4 +308,73 @@ private LatLng toLatLng(double eastMeters, double northMeters) { return new LatLng(lat, lon); } + + private void logUpdateSummary(double zEast, double zNorth, + double sigmaMeters, + Integer floorHint, + double effectiveBefore, + double effectiveAfter, + boolean resampled) { + if (!DEBUG_LOGS || particles.isEmpty()) { + return; + } + + double minW = Double.POSITIVE_INFINITY; + double maxW = 0.0; + double entropy = 0.0; + double meanX = 0.0; + double meanY = 0.0; + int bestFloor = fallbackFloor; + Map floorWeights = new HashMap<>(); + + Particle bestParticle = particles.get(0); + for (Particle p : particles) { + minW = Math.min(minW, p.weight); + maxW = Math.max(maxW, p.weight); + if (p.weight > bestParticle.weight) { + bestParticle = p; + } + + if (p.weight > 0.0) { + entropy -= p.weight * Math.log(p.weight); + } + + meanX += p.weight * p.xEast; + meanY += p.weight * p.yNorth; + floorWeights.put(p.floor, floorWeights.getOrDefault(p.floor, 0.0) + p.weight); + } + + double bestFloorWeight = -1.0; + for (Map.Entry entry : floorWeights.entrySet()) { + if (entry.getValue() > bestFloorWeight) { + bestFloorWeight = entry.getValue(); + bestFloor = entry.getKey(); + } + } + + double entropyNorm = entropy / Math.log(PARTICLE_COUNT); + String source = floorHint == null ? "GNSS" : "WiFi"; + Log.i(TAG, String.format(Locale.US, + "u=%d src=%s z=(%.2fE,%.2fN) sigma=%.2f floorHint=%s Neff=%.1f->%.1f resampled=%s w[min=%.5f max=%.5f Hn=%.3f] mean=(%.2fE,%.2fN) bestP=(%.2fE,%.2fN,f=%d,w=%.5f) bestFloor=%d(%.3f)", + updateCounter, + source, + zEast, + zNorth, + sigmaMeters, + floorHint == null ? "-" : String.valueOf(floorHint), + effectiveBefore, + effectiveAfter, + String.valueOf(resampled), + minW, + maxW, + entropyNorm, + meanX, + meanY, + bestParticle.xEast, + bestParticle.yNorth, + bestParticle.floor, + bestParticle.weight, + bestFloor, + bestFloorWeight)); + } } From 8a8afd8b465c1aa2fbfeec51731f868970612875 Mon Sep 17 00:00:00 2001 From: HuangerzJ <125708330+HuangerzJ@users.noreply.github.com> Date: Tue, 24 Mar 2026 16:02:59 +0000 Subject: [PATCH 03/52] Map matching --- .../sensors/PositionFusionEngine.java | 314 +++++++++++++++++- .../PositionMe/sensors/SensorFusion.java | 13 +- 2 files changed, 320 insertions(+), 7 deletions(-) diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java index afd6cc95..bcdee534 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java @@ -3,6 +3,7 @@ import android.util.Log; import com.google.android.gms.maps.model.LatLng; +import com.openpositioning.PositionMe.data.remote.FloorplanApiClient; import java.util.ArrayList; import java.util.HashMap; @@ -32,6 +33,8 @@ public class PositionFusionEngine { private static final double ROUGHEN_STD_M = 0.15; private static final double WIFI_SIGMA_M = 8.0; private static final double EPS = 1e-300; + private static final double CONNECTOR_RADIUS_M = 3.0; + private static final double LIFT_HORIZONTAL_MAX_M = 0.50; private final float floorHeightMeters; private final Random random = new Random(); @@ -44,6 +47,9 @@ public class PositionFusionEngine { private final List particles = new ArrayList<>(PARTICLE_COUNT); private int fallbackFloor; private long updateCounter; + private String activeBuildingName; + private final Map floorConstraints = new HashMap<>(); + private double recentStepMotionMeters; private static final class Particle { double xEast; @@ -52,6 +58,32 @@ private static final class Particle { double weight; } + private static final class Point2D { + final double x; + final double y; + + Point2D(double x, double y) { + this.x = x; + this.y = y; + } + } + + private static final class Segment { + final Point2D a; + final Point2D b; + + Segment(Point2D a, Point2D b) { + this.a = a; + this.b = b; + } + } + + private static final class FloorConstraint { + final List walls = new ArrayList<>(); + final List stairs = new ArrayList<>(); + final List lifts = new ArrayList<>(); + } + public PositionFusionEngine(float floorHeightMeters) { this.floorHeightMeters = floorHeightMeters > 0f ? floorHeightMeters : 4f; } @@ -75,15 +107,28 @@ public synchronized void updatePdrDisplacement(float dxEastMeters, float dyNorth return; } + recentStepMotionMeters = Math.hypot(dxEastMeters, dyNorthMeters); + int blockedByWall = 0; + for (Particle p : particles) { - p.xEast += dxEastMeters + random.nextGaussian() * PDR_NOISE_STD_M; - p.yNorth += dyNorthMeters + random.nextGaussian() * PDR_NOISE_STD_M; + double oldX = p.xEast; + double oldY = p.yNorth; + double candidateX = oldX + dxEastMeters + random.nextGaussian() * PDR_NOISE_STD_M; + double candidateY = oldY + dyNorthMeters + random.nextGaussian() * PDR_NOISE_STD_M; + + if (crossesWall(p.floor, oldX, oldY, candidateX, candidateY)) { + blockedByWall++; + continue; + } + + p.xEast = candidateX; + p.yNorth = candidateY; } if (DEBUG_LOGS) { Log.d(TAG, String.format(Locale.US, - "Predict dPDR=(%.2fE, %.2fN) noiseStd=%.2f", - dxEastMeters, dyNorthMeters, PDR_NOISE_STD_M)); + "Predict dPDR=(%.2fE, %.2fN) noiseStd=%.2f blockedByWall=%d", + dxEastMeters, dyNorthMeters, PDR_NOISE_STD_M, blockedByWall)); } } @@ -106,16 +151,106 @@ public synchronized void updateWifi(double latDeg, double lonDeg, int wifiFloor) applyAbsoluteFix(latDeg, lonDeg, WIFI_SIGMA_M, wifiFloor); } - public synchronized void updateElevation(float elevationMeters) { + public synchronized void updateElevation(float elevationMeters, boolean elevatorLikely) { int floorFromBarometer = Math.round(elevationMeters / floorHeightMeters); fallbackFloor = floorFromBarometer; if (!particles.isEmpty()) { for (Particle p : particles) { - p.floor = floorFromBarometer; + if (p.floor == floorFromBarometer) { + continue; + } + + int step = floorFromBarometer > p.floor ? 1 : -1; + int nextFloor = p.floor + step; + if (canUseConnector(p.floor, p.xEast, p.yNorth, elevatorLikely)) { + p.floor = nextFloor; + } } } } + public synchronized void updateMapMatchingContext( + double currentLatDeg, + double currentLonDeg, + List buildings) { + if (!hasAnchor || buildings == null || buildings.isEmpty()) { + floorConstraints.clear(); + activeBuildingName = null; + return; + } + + FloorplanApiClient.BuildingInfo containing = null; + LatLng current = new LatLng(currentLatDeg, currentLonDeg); + for (FloorplanApiClient.BuildingInfo b : buildings) { + List outline = b.getOutlinePolygon(); + if (outline != null && outline.size() >= 3 && pointInPolygon(current, outline)) { + containing = b; + break; + } + } + + if (containing == null) { + floorConstraints.clear(); + activeBuildingName = null; + return; + } + + if (containing.getName().equals(activeBuildingName) && !floorConstraints.isEmpty()) { + return; + } + + Map parsed = new HashMap<>(); + List floorShapes = containing.getFloorShapesList(); + for (int i = 0; i < floorShapes.size(); i++) { + FloorplanApiClient.FloorShapes floor = floorShapes.get(i); + Integer logicalFloor = parseLogicalFloor(floor, i); + if (logicalFloor == null) { + continue; + } + FloorConstraint constraint = parsed.get(logicalFloor); + if (constraint == null) { + constraint = new FloorConstraint(); + parsed.put(logicalFloor, constraint); + } + + for (FloorplanApiClient.MapShapeFeature feature : floor.getFeatures()) { + String type = feature.getIndoorType(); + List> parts = feature.getParts(); + if (parts == null || parts.isEmpty()) { + continue; + } + + if ("wall".equals(type)) { + for (List part : parts) { + addWallSegments(part, constraint.walls); + } + } else if ("stairs".equals(type) || "lift".equals(type)) { + for (List part : parts) { + Point2D center = toLocalCentroid(part); + if (center == null) { + continue; + } + if ("stairs".equals(type)) { + constraint.stairs.add(center); + } else { + constraint.lifts.add(center); + } + } + } + } + } + + floorConstraints.clear(); + floorConstraints.putAll(parsed); + activeBuildingName = containing.getName(); + if (DEBUG_LOGS) { + Log.i(TAG, String.format(Locale.US, + "Map matching enabled for building=%s floors=%d", + activeBuildingName, + floorConstraints.size())); + } + } + public synchronized PositionFusionEstimate getEstimate() { if (!hasAnchor || particles.isEmpty()) { return new PositionFusionEstimate(null, fallbackFloor, false); @@ -309,6 +444,173 @@ private LatLng toLatLng(double eastMeters, double northMeters) { return new LatLng(lat, lon); } + private boolean crossesWall(int floor, double x0, double y0, double x1, double y1) { + FloorConstraint fc = floorConstraints.get(floor); + if (fc == null || fc.walls.isEmpty()) { + return false; + } + + Point2D a = new Point2D(x0, y0); + Point2D b = new Point2D(x1, y1); + for (Segment wall : fc.walls) { + if (segmentsIntersect(a, b, wall.a, wall.b)) { + return true; + } + } + return false; + } + + private boolean canUseConnector(int floor, double x, double y, boolean elevatorLikely) { + if (floorConstraints.isEmpty()) { + return true; + } + + FloorConstraint fc = floorConstraints.get(floor); + if (fc == null) { + return true; + } + + if (elevatorLikely && recentStepMotionMeters <= LIFT_HORIZONTAL_MAX_M) { + if (!fc.lifts.isEmpty()) { + return isNearAny(fc.lifts, x, y, CONNECTOR_RADIUS_M); + } + return false; + } + + if (!fc.stairs.isEmpty()) { + return isNearAny(fc.stairs, x, y, CONNECTOR_RADIUS_M); + } + + // If stairs are not mapped for this floor, do not hard-block transitions. + return true; + } + + private boolean isNearAny(List points, double x, double y, double radius) { + double r2 = radius * radius; + for (Point2D p : points) { + double dx = p.x - x; + double dy = p.y - y; + if (dx * dx + dy * dy <= r2) { + return true; + } + } + return false; + } + + private void addWallSegments(List points, List out) { + if (points == null || points.size() < 2) { + return; + } + for (int i = 0; i < points.size() - 1; i++) { + Point2D a = toLocalPoint(points.get(i)); + Point2D b = toLocalPoint(points.get(i + 1)); + if (a != null && b != null) { + out.add(new Segment(a, b)); + } + } + } + + private Point2D toLocalPoint(LatLng latLng) { + if (latLng == null) { + return null; + } + double[] local = toLocal(latLng.latitude, latLng.longitude); + return new Point2D(local[0], local[1]); + } + + private Point2D toLocalCentroid(List points) { + if (points == null || points.isEmpty()) { + return null; + } + + double sx = 0.0; + double sy = 0.0; + int count = 0; + for (LatLng latLng : points) { + Point2D p = toLocalPoint(latLng); + if (p == null) { + continue; + } + sx += p.x; + sy += p.y; + count++; + } + + if (count == 0) { + return null; + } + return new Point2D(sx / count, sy / count); + } + + private Integer parseLogicalFloor(FloorplanApiClient.FloorShapes floor, int index) { + if (floor == null) { + return null; + } + + String display = floor.getDisplayName() == null ? "" : floor.getDisplayName().trim(); + String upper = display.toUpperCase(Locale.US); + + if ("LG".equals(upper) || "L".equals(upper)) { + return -1; + } + if ("G".equals(upper) || "GROUND".equals(upper)) { + return 0; + } + + try { + return Integer.parseInt(display); + } catch (Exception ignored) { + // Fall back to index mapping when display name is not numeric. + } + + return index; + } + + private boolean pointInPolygon(LatLng point, List polygon) { + boolean inside = false; + for (int i = 0, j = polygon.size() - 1; i < polygon.size(); j = i++) { + double xi = polygon.get(i).longitude; + double yi = polygon.get(i).latitude; + double xj = polygon.get(j).longitude; + double yj = polygon.get(j).latitude; + + boolean intersect = ((yi > point.latitude) != (yj > point.latitude)) + && (point.longitude + < (xj - xi) * (point.latitude - yi) / (yj - yi + 1e-12) + xi); + if (intersect) { + inside = !inside; + } + } + return inside; + } + + private boolean segmentsIntersect(Point2D p1, Point2D p2, Point2D q1, Point2D q2) { + double o1 = orientation(p1, p2, q1); + double o2 = orientation(p1, p2, q2); + double o3 = orientation(q1, q2, p1); + double o4 = orientation(q1, q2, p2); + + if ((o1 > 0) != (o2 > 0) && (o3 > 0) != (o4 > 0)) { + return true; + } + + return (Math.abs(o1) < 1e-9 && onSegment(p1, q1, p2)) + || (Math.abs(o2) < 1e-9 && onSegment(p1, q2, p2)) + || (Math.abs(o3) < 1e-9 && onSegment(q1, p1, q2)) + || (Math.abs(o4) < 1e-9 && onSegment(q1, p2, q2)); + } + + private double orientation(Point2D a, Point2D b, Point2D c) { + return (b.x - a.x) * (c.y - a.y) - (b.y - a.y) * (c.x - a.x); + } + + private boolean onSegment(Point2D a, Point2D b, Point2D c) { + return b.x >= Math.min(a.x, c.x) - 1e-9 + && b.x <= Math.max(a.x, c.x) + 1e-9 + && b.y >= Math.min(a.y, c.y) - 1e-9 + && b.y <= Math.max(a.y, c.y) + 1e-9; + } + private void logUpdateSummary(double zEast, double zNorth, double sigmaMeters, Integer floorHint, diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java index 282243d5..16b2e52f 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -167,7 +167,7 @@ public void setContext(Context context) { state, pdrProcessing, pathView, recorder, bootTime, (dxEastMeters, dyNorthMeters, relativeTimestampMs) -> { fusionEngine.updatePdrDisplacement(dxEastMeters, dyNorthMeters); - fusionEngine.updateElevation(state.elevation); + fusionEngine.updateElevation(state.elevation, state.elevator); updateFusedState(); }); @@ -434,6 +434,13 @@ public void setFloorplanBuildings(List building } floorplanBuildingCache.put(building.getName(), building); } + + if (fusionEngine != null) { + fusionEngine.updateMapMatchingContext( + state.latitude, + state.longitude, + getFloorplanBuildings()); + } } /** @@ -678,6 +685,10 @@ public void onLocationChanged(@NonNull Location location) { state.latitude = (float) location.getLatitude(); state.longitude = (float) location.getLongitude(); if (fusionEngine != null) { + fusionEngine.updateMapMatchingContext( + location.getLatitude(), + location.getLongitude(), + getFloorplanBuildings()); fusionEngine.updateGnss( location.getLatitude(), location.getLongitude(), From 4d920dc7a885b18fdc9995231b713447586f3eb0 Mon Sep 17 00:00:00 2001 From: HuangerzJ <125708330+HuangerzJ@users.noreply.github.com> Date: Tue, 24 Mar 2026 16:54:28 +0000 Subject: [PATCH 04/52] dummy data display --- .../fragment/RecordingFragment.java | 18 ++ .../fragment/TrajectoryMapFragment.java | 183 ++++++++++++++++-- .../res/layout/fragment_trajectory_map.xml | 13 ++ app/src/main/res/values/strings.xml | 2 + 4 files changed, 197 insertions(+), 19 deletions(-) diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/RecordingFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/RecordingFragment.java index 8ca06126..813b0ba3 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/RecordingFragment.java +++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/RecordingFragment.java @@ -83,6 +83,7 @@ public class RecordingFragment extends Fragment { private float distance = 0f; private float previousPosX = 0f; private float previousPosY = 0f; + private LatLng lastWifiObservation; // References to the child map fragment private TrajectoryMapFragment trajectoryMapFragment; @@ -307,6 +308,23 @@ private void updateUIandPosition() { } } + if (trajectoryMapFragment != null) { + LatLng wifiLocation = sensorFusion.getLatLngWifiPositioning(); + if (wifiLocation != null && (lastWifiObservation == null + || !lastWifiObservation.equals(wifiLocation))) { + trajectoryMapFragment.updateWiFiObservation(wifiLocation); + lastWifiObservation = wifiLocation; + } + + float[] startLatLng = sensorFusion.getGNSSLatitude(true); + if (startLatLng != null && !(startLatLng[0] == 0f && startLatLng[1] == 0f)) { + LatLng pdrAbsolute = UtilFunctions.calculateNewPos( + new LatLng(startLatLng[0], startLatLng[1]), + new float[]{pdrValues[0], pdrValues[1]}); + trajectoryMapFragment.updatePdrObservation(pdrAbsolute); + } + } + // Update previous previousPosX = pdrValues[0]; previousPosY = pdrValues[1]; diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java index 479ea51b..b9ba2fcb 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java +++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java @@ -57,16 +57,29 @@ public class TrajectoryMapFragment extends Fragment { + private static final int MAX_OBSERVATION_MARKERS = 20; + private static final long TRAJECTORY_APPEND_MIN_INTERVAL_MS = 1000; + private static final double TRAJECTORY_APPEND_MIN_METERS = 0.60; + private static final double SMOOTHING_ALPHA = 0.30; + private GoogleMap gMap; // Google Maps instance private LatLng currentLocation; // Stores the user's current location private Marker orientationMarker; // Marker representing user's heading private Marker gnssMarker; // GNSS position marker // Keep test point markers so they can be cleared when recording ends private final List testPointMarkers = new ArrayList<>(); + private final List gnssObservationMarkers = new ArrayList<>(); + private final List wifiObservationMarkers = new ArrayList<>(); + private final List pdrObservationMarkers = new ArrayList<>(); private Polyline polyline; // Polyline representing user's movement path private boolean isRed = true; // Tracks whether the polyline color is red private boolean isGnssOn = false; // Tracks if GNSS tracking is enabled + private boolean isSmoothingOn = false; + private boolean showObservationMarkers = true; + private long lastTrajectoryAppendMs = 0L; + private LatLng lastTrajectoryPoint; + private LatLng smoothedLocation; private Polyline gnssPolyline; // Polyline for GNSS path private LatLng lastGnssLocation = null; // Stores the last GNSS location @@ -91,6 +104,8 @@ public class TrajectoryMapFragment extends Fragment { private SwitchMaterial gnssSwitch; private SwitchMaterial autoFloorSwitch; + private SwitchMaterial smoothingSwitch; + private SwitchMaterial observationsSwitch; private com.google.android.material.floatingactionbutton.FloatingActionButton floorUpButton, floorDownButton; private TextView floorLabel; @@ -120,6 +135,8 @@ public void onViewCreated(@NonNull View view, switchMapSpinner = view.findViewById(R.id.mapSwitchSpinner); gnssSwitch = view.findViewById(R.id.gnssSwitch); autoFloorSwitch = view.findViewById(R.id.autoFloor); + smoothingSwitch = view.findViewById(R.id.smoothingSwitch); + observationsSwitch = view.findViewById(R.id.observationsSwitch); floorUpButton = view.findViewById(R.id.floorUpButton); floorDownButton = view.findViewById(R.id.floorDownButton); floorLabel = view.findViewById(R.id.floorLabel); @@ -168,6 +185,22 @@ public void onMapReady(@NonNull GoogleMap googleMap) { } }); + if (smoothingSwitch != null) { + smoothingSwitch.setOnCheckedChangeListener((buttonView, isChecked) -> { + isSmoothingOn = isChecked; + smoothedLocation = null; + }); + } + + if (observationsSwitch != null) { + observationsSwitch.setOnCheckedChangeListener((buttonView, isChecked) -> { + showObservationMarkers = isChecked; + if (!isChecked) { + clearObservationMarkers(); + } + }); + } + // Color switch switchColorButton.setOnClickListener(v -> { if (polyline != null) { @@ -310,27 +343,39 @@ public void onNothingSelected(AdapterView parent) {} public void updateUserLocation(@NonNull LatLng newLocation, float orientation) { if (gMap == null) return; + LatLng displayLocation = newLocation; + if (isSmoothingOn) { + if (smoothedLocation == null) { + smoothedLocation = newLocation; + } else { + smoothedLocation = smoothLocation(smoothedLocation, newLocation); + } + displayLocation = smoothedLocation; + } else { + smoothedLocation = null; + } + // Keep track of current location LatLng oldLocation = this.currentLocation; - this.currentLocation = newLocation; + this.currentLocation = displayLocation; // If no marker, create it if (orientationMarker == null) { orientationMarker = gMap.addMarker(new MarkerOptions() - .position(newLocation) + .position(displayLocation) .flat(true) .title("Current Position") .icon(BitmapDescriptorFactory.fromBitmap( UtilFunctions.getBitmapFromVector(requireContext(), R.drawable.ic_baseline_navigation_24))) ); - gMap.moveCamera(CameraUpdateFactory.newLatLngZoom(newLocation, 19f)); + gMap.moveCamera(CameraUpdateFactory.newLatLngZoom(displayLocation, 19f)); } else { // Update marker position + orientation - orientationMarker.setPosition(newLocation); + orientationMarker.setPosition(displayLocation); orientationMarker.setRotation(orientation); // Move camera a bit - gMap.moveCamera(CameraUpdateFactory.newLatLng(newLocation)); + gMap.moveCamera(CameraUpdateFactory.newLatLng(displayLocation)); } // Extend polyline if movement occurred @@ -340,24 +385,12 @@ public void updateUserLocation(@NonNull LatLng newLocation, float orientation) { polyline.setPoints(points); }*/ // Extend polyline - if (polyline != null) { - List points = new ArrayList<>(polyline.getPoints()); - - // First position fix: add the first polyline point - if (oldLocation == null) { - points.add(newLocation); - polyline.setPoints(points); - } else if (!oldLocation.equals(newLocation)) { - // Subsequent movement: append a new polyline point - points.add(newLocation); - polyline.setPoints(points); - } - } + maybeAppendTrajectoryPoint(oldLocation, displayLocation); // Update indoor map overlay if (indoorMapManager != null) { - indoorMapManager.setCurrentLocation(newLocation); + indoorMapManager.setCurrentLocation(displayLocation); setFloorControlsVisibility(indoorMapManager.getIsIndoorMapSet() ? View.VISIBLE : View.GONE); } } @@ -420,6 +453,13 @@ public void updateGNSS(@NonNull LatLng gnssLocation) { if (gMap == null) return; if (!isGnssOn) return; + addObservationMarker( + gnssObservationMarkers, + gnssLocation, + BitmapDescriptorFactory.HUE_AZURE, + "GNSS", + true); + if (gnssMarker == null) { // Create the GNSS marker for the first time gnssMarker = gMap.addMarker(new MarkerOptions() @@ -442,6 +482,30 @@ public void updateGNSS(@NonNull LatLng gnssLocation) { } } + /** + * Adds a WiFi observation marker (last N points). + */ + public void updateWiFiObservation(@NonNull LatLng wifiLocation) { + addObservationMarker( + wifiObservationMarkers, + wifiLocation, + BitmapDescriptorFactory.HUE_GREEN, + "WiFi", + false); + } + + /** + * Adds a PDR observation marker (last N points). + */ + public void updatePdrObservation(@NonNull LatLng pdrLocation) { + addObservationMarker( + pdrObservationMarkers, + pdrLocation, + BitmapDescriptorFactory.HUE_ORANGE, + "PDR", + false); + } + /** * Remove GNSS marker if user toggles it off @@ -502,12 +566,16 @@ public void clearMapAndReset() { } lastGnssLocation = null; currentLocation = null; + smoothedLocation = null; + lastTrajectoryPoint = null; + lastTrajectoryAppendMs = 0L; // Clear test point markers for (Marker m : testPointMarkers) { m.remove(); } testPointMarkers.clear(); + clearObservationMarkers(); // Re-create empty polylines with your chosen colors @@ -523,6 +591,83 @@ public void clearMapAndReset() { } } + private void maybeAppendTrajectoryPoint(@Nullable LatLng oldLocation, + @NonNull LatLng newLocation) { + if (polyline == null) { + return; + } + + List points = new ArrayList<>(polyline.getPoints()); + if (oldLocation == null || points.isEmpty()) { + points.add(newLocation); + polyline.setPoints(points); + lastTrajectoryPoint = newLocation; + lastTrajectoryAppendMs = SystemClock.elapsedRealtime(); + return; + } + + long now = SystemClock.elapsedRealtime(); + double moved = lastTrajectoryPoint == null + ? UtilFunctions.distanceBetweenPoints(oldLocation, newLocation) + : UtilFunctions.distanceBetweenPoints(lastTrajectoryPoint, newLocation); + + if ((now - lastTrajectoryAppendMs) >= TRAJECTORY_APPEND_MIN_INTERVAL_MS + || moved >= TRAJECTORY_APPEND_MIN_METERS) { + points.add(newLocation); + polyline.setPoints(points); + lastTrajectoryPoint = newLocation; + lastTrajectoryAppendMs = now; + } + } + + private LatLng smoothLocation(@NonNull LatLng prev, @NonNull LatLng cur) { + double lat = prev.latitude + SMOOTHING_ALPHA * (cur.latitude - prev.latitude); + double lon = prev.longitude + SMOOTHING_ALPHA * (cur.longitude - prev.longitude); + return new LatLng(lat, lon); + } + + private void addObservationMarker(@NonNull List bucket, + @NonNull LatLng location, + float hue, + @NonNull String title, + boolean respectGnssSwitch) { + if (gMap == null || !showObservationMarkers) { + return; + } + if (respectGnssSwitch && !isGnssOn) { + return; + } + + Marker marker = gMap.addMarker(new MarkerOptions() + .position(location) + .title(title) + .icon(BitmapDescriptorFactory.defaultMarker(hue)) + .alpha(0.65f)); + + if (marker == null) { + return; + } + + bucket.add(marker); + while (bucket.size() > MAX_OBSERVATION_MARKERS) { + Marker stale = bucket.remove(0); + stale.remove(); + } + } + + private void clearObservationMarkers() { + clearMarkerBucket(gnssObservationMarkers); + clearMarkerBucket(wifiObservationMarkers); + clearMarkerBucket(pdrObservationMarkers); + } + + private void clearMarkerBucket(@NonNull List bucket) { + for (Marker m : bucket) { + m.remove(); + } + bucket.clear(); + } + /** * Draw the building polygon on the map *

diff --git a/app/src/main/res/layout/fragment_trajectory_map.xml b/app/src/main/res/layout/fragment_trajectory_map.xml index a72425bf..ad0a90f6 100644 --- a/app/src/main/res/layout/fragment_trajectory_map.xml +++ b/app/src/main/res/layout/fragment_trajectory_map.xml @@ -43,6 +43,19 @@ android:layout_height="wrap_content" android:text="@string/auto_floor" /> + + + + Floor Up button Choose Map ❇️ Auto Floor + Smooth Display + Show Last N Obs GNSS error: Satellite Normal From d1a91e3919aa1aab067db223d5df6855ee93d7cc Mon Sep 17 00:00:00 2001 From: HuangerzJ <125708330+HuangerzJ@users.noreply.github.com> Date: Wed, 25 Mar 2026 11:11:50 +0000 Subject: [PATCH 05/52] data display v2 --- .../fragment/TrajectoryMapFragment.java | 104 ++++++------------ .../res/layout/fragment_trajectory_map.xml | 13 --- app/src/main/res/values/strings.xml | 2 - 3 files changed, 33 insertions(+), 86 deletions(-) diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java index b9ba2fcb..e54dd4e2 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java +++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java @@ -60,7 +60,7 @@ public class TrajectoryMapFragment extends Fragment { private static final int MAX_OBSERVATION_MARKERS = 20; private static final long TRAJECTORY_APPEND_MIN_INTERVAL_MS = 1000; private static final double TRAJECTORY_APPEND_MIN_METERS = 0.60; - private static final double SMOOTHING_ALPHA = 0.30; + private static final double OBSERVATION_CIRCLE_RADIUS_M = 1.4; private GoogleMap gMap; // Google Maps instance private LatLng currentLocation; // Stores the user's current location @@ -68,18 +68,15 @@ public class TrajectoryMapFragment extends Fragment { private Marker gnssMarker; // GNSS position marker // Keep test point markers so they can be cleared when recording ends private final List testPointMarkers = new ArrayList<>(); - private final List gnssObservationMarkers = new ArrayList<>(); - private final List wifiObservationMarkers = new ArrayList<>(); - private final List pdrObservationMarkers = new ArrayList<>(); + private final List gnssObservationCircles = new ArrayList<>(); + private final List wifiObservationCircles = new ArrayList<>(); + private final List pdrObservationCircles = new ArrayList<>(); private Polyline polyline; // Polyline representing user's movement path private boolean isRed = true; // Tracks whether the polyline color is red private boolean isGnssOn = false; // Tracks if GNSS tracking is enabled - private boolean isSmoothingOn = false; - private boolean showObservationMarkers = true; private long lastTrajectoryAppendMs = 0L; private LatLng lastTrajectoryPoint; - private LatLng smoothedLocation; private Polyline gnssPolyline; // Polyline for GNSS path private LatLng lastGnssLocation = null; // Stores the last GNSS location @@ -104,8 +101,6 @@ public class TrajectoryMapFragment extends Fragment { private SwitchMaterial gnssSwitch; private SwitchMaterial autoFloorSwitch; - private SwitchMaterial smoothingSwitch; - private SwitchMaterial observationsSwitch; private com.google.android.material.floatingactionbutton.FloatingActionButton floorUpButton, floorDownButton; private TextView floorLabel; @@ -135,8 +130,6 @@ public void onViewCreated(@NonNull View view, switchMapSpinner = view.findViewById(R.id.mapSwitchSpinner); gnssSwitch = view.findViewById(R.id.gnssSwitch); autoFloorSwitch = view.findViewById(R.id.autoFloor); - smoothingSwitch = view.findViewById(R.id.smoothingSwitch); - observationsSwitch = view.findViewById(R.id.observationsSwitch); floorUpButton = view.findViewById(R.id.floorUpButton); floorDownButton = view.findViewById(R.id.floorDownButton); floorLabel = view.findViewById(R.id.floorLabel); @@ -185,22 +178,6 @@ public void onMapReady(@NonNull GoogleMap googleMap) { } }); - if (smoothingSwitch != null) { - smoothingSwitch.setOnCheckedChangeListener((buttonView, isChecked) -> { - isSmoothingOn = isChecked; - smoothedLocation = null; - }); - } - - if (observationsSwitch != null) { - observationsSwitch.setOnCheckedChangeListener((buttonView, isChecked) -> { - showObservationMarkers = isChecked; - if (!isChecked) { - clearObservationMarkers(); - } - }); - } - // Color switch switchColorButton.setOnClickListener(v -> { if (polyline != null) { @@ -344,16 +321,6 @@ public void updateUserLocation(@NonNull LatLng newLocation, float orientation) { if (gMap == null) return; LatLng displayLocation = newLocation; - if (isSmoothingOn) { - if (smoothedLocation == null) { - smoothedLocation = newLocation; - } else { - smoothedLocation = smoothLocation(smoothedLocation, newLocation); - } - displayLocation = smoothedLocation; - } else { - smoothedLocation = null; - } // Keep track of current location LatLng oldLocation = this.currentLocation; @@ -454,10 +421,10 @@ public void updateGNSS(@NonNull LatLng gnssLocation) { if (!isGnssOn) return; addObservationMarker( - gnssObservationMarkers, + gnssObservationCircles, gnssLocation, - BitmapDescriptorFactory.HUE_AZURE, - "GNSS", + Color.argb(220, 33, 150, 243), + Color.argb(80, 33, 150, 243), true); if (gnssMarker == null) { @@ -487,10 +454,10 @@ public void updateGNSS(@NonNull LatLng gnssLocation) { */ public void updateWiFiObservation(@NonNull LatLng wifiLocation) { addObservationMarker( - wifiObservationMarkers, + wifiObservationCircles, wifiLocation, - BitmapDescriptorFactory.HUE_GREEN, - "WiFi", + Color.argb(220, 67, 160, 71), + Color.argb(80, 67, 160, 71), false); } @@ -499,10 +466,10 @@ public void updateWiFiObservation(@NonNull LatLng wifiLocation) { */ public void updatePdrObservation(@NonNull LatLng pdrLocation) { addObservationMarker( - pdrObservationMarkers, + pdrObservationCircles, pdrLocation, - BitmapDescriptorFactory.HUE_ORANGE, - "PDR", + Color.argb(220, 251, 140, 0), + Color.argb(80, 251, 140, 0), false); } @@ -566,7 +533,6 @@ public void clearMapAndReset() { } lastGnssLocation = null; currentLocation = null; - smoothedLocation = null; lastTrajectoryPoint = null; lastTrajectoryAppendMs = 0L; @@ -620,50 +586,46 @@ private void maybeAppendTrajectoryPoint(@Nullable LatLng oldLocation, } } - private LatLng smoothLocation(@NonNull LatLng prev, @NonNull LatLng cur) { - double lat = prev.latitude + SMOOTHING_ALPHA * (cur.latitude - prev.latitude); - double lon = prev.longitude + SMOOTHING_ALPHA * (cur.longitude - prev.longitude); - return new LatLng(lat, lon); - } - - private void addObservationMarker(@NonNull List bucket, + private void addObservationMarker(@NonNull List bucket, @NonNull LatLng location, - float hue, - @NonNull String title, + int strokeColor, + int fillColor, boolean respectGnssSwitch) { - if (gMap == null || !showObservationMarkers) { + if (gMap == null) { return; } if (respectGnssSwitch && !isGnssOn) { return; } - Marker marker = gMap.addMarker(new MarkerOptions() - .position(location) - .title(title) - .icon(BitmapDescriptorFactory.defaultMarker(hue)) - .alpha(0.65f)); + Circle circle = gMap.addCircle(new CircleOptions() + .center(location) + .radius(OBSERVATION_CIRCLE_RADIUS_M) + .strokeWidth(2f) + .strokeColor(strokeColor) + .fillColor(fillColor) + .zIndex(3f)); - if (marker == null) { + if (circle == null) { return; } - bucket.add(marker); + bucket.add(circle); while (bucket.size() > MAX_OBSERVATION_MARKERS) { - Marker stale = bucket.remove(0); + Circle stale = bucket.remove(0); stale.remove(); } } private void clearObservationMarkers() { - clearMarkerBucket(gnssObservationMarkers); - clearMarkerBucket(wifiObservationMarkers); - clearMarkerBucket(pdrObservationMarkers); + clearObservationCircles(gnssObservationCircles); + clearObservationCircles(wifiObservationCircles); + clearObservationCircles(pdrObservationCircles); } - private void clearMarkerBucket(@NonNull List bucket) { - for (Marker m : bucket) { - m.remove(); + private void clearObservationCircles(@NonNull List bucket) { + for (Circle c : bucket) { + c.remove(); } bucket.clear(); } diff --git a/app/src/main/res/layout/fragment_trajectory_map.xml b/app/src/main/res/layout/fragment_trajectory_map.xml index ad0a90f6..a72425bf 100644 --- a/app/src/main/res/layout/fragment_trajectory_map.xml +++ b/app/src/main/res/layout/fragment_trajectory_map.xml @@ -43,19 +43,6 @@ android:layout_height="wrap_content" android:text="@string/auto_floor" /> - - - - Floor Up button Choose Map ❇️ Auto Floor - Smooth Display - Show Last N Obs GNSS error: Satellite Normal From f385387f414a8f7964b92705fd727b2eb0e895f2 Mon Sep 17 00:00:00 2001 From: HuangerzJ <125708330+HuangerzJ@users.noreply.github.com> Date: Wed, 25 Mar 2026 11:27:21 +0000 Subject: [PATCH 06/52] different indoor map colour for clarity --- .../PositionMe/utils/IndoorMapManager.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java b/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java index f8058603..8b16df94 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java @@ -55,10 +55,10 @@ public class IndoorMapManager { public static final float MURCHISON_FLOOR_HEIGHT = 4.0F; // Colours for different indoor feature types - private static final int WALL_STROKE = Color.argb(200, 80, 80, 80); - private static final int ROOM_STROKE = Color.argb(180, 33, 150, 243); - private static final int ROOM_FILL = Color.argb(40, 33, 150, 243); - private static final int DEFAULT_STROKE = Color.argb(150, 100, 100, 100); + private static final int WALL_STROKE = Color.argb(220, 25, 118, 210); + private static final int ROOM_STROKE = Color.argb(210, 33, 150, 243); + private static final int ROOM_FILL = Color.argb(60, 33, 150, 243); + private static final int DEFAULT_STROKE = Color.argb(170, 66, 165, 245); /** * Constructor to set the map instance. From a133ba42b73160dea7e67fffd814ab142bf80f07 Mon Sep 17 00:00:00 2001 From: HuangerzJ <125708330+HuangerzJ@users.noreply.github.com> Date: Wed, 25 Mar 2026 11:55:40 +0000 Subject: [PATCH 07/52] further logging and fixing the bestFloor functionality --- .../sensors/PositionFusionEngine.java | 90 +++++++++++++++++-- .../PositionMe/sensors/SensorFusion.java | 12 +++ .../PositionMe/utils/IndoorMapManager.java | 30 ++++++- 3 files changed, 125 insertions(+), 7 deletions(-) diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java index bcdee534..76268075 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java @@ -26,12 +26,17 @@ public class PositionFusionEngine { private static final double EARTH_RADIUS_M = 6378137.0; - private static final int PARTICLE_COUNT = 220; + private static final int PARTICLE_COUNT = 300; private static final double RESAMPLE_RATIO = 0.5; - private static final double PDR_NOISE_STD_M = 0.45; + private static final double PDR_NOISE_STD_M = 0.55; private static final double INIT_STD_M = 2.0; private static final double ROUGHEN_STD_M = 0.15; - private static final double WIFI_SIGMA_M = 8.0; + private static final double WIFI_SIGMA_M = 5.5; + private static final double INDOOR_GNSS_SIGMA_MULTIPLIER = 2.0; + private static final double INDOOR_GNSS_MIN_SIGMA_M = 10.0; + private static final double FLOOR_HINT_MIN_SUPPORT = 0.08; + private static final double FLOOR_HINT_INJECTION_FRACTION = 0.25; + private static final double FLOOR_HINT_INJECTION_STD_M = 1.2; private static final double EPS = 1e-300; private static final double CONNECTOR_RADIUS_M = 3.0; private static final double LIFT_HORIZONTAL_MAX_M = 0.50; @@ -134,10 +139,14 @@ public synchronized void updatePdrDisplacement(float dxEastMeters, float dyNorth public synchronized void updateGnss(double latDeg, double lonDeg, float accuracyMeters) { double sigma = Math.max(accuracyMeters, 3.0f); + boolean indoors = activeBuildingName != null && !activeBuildingName.isEmpty(); + if (indoors) { + sigma = Math.max(sigma * INDOOR_GNSS_SIGMA_MULTIPLIER, INDOOR_GNSS_MIN_SIGMA_M); + } if (DEBUG_LOGS) { Log.d(TAG, String.format(Locale.US, - "GNSS update lat=%.7f lon=%.7f acc=%.2f sigma=%.2f", - latDeg, lonDeg, accuracyMeters, sigma)); + "GNSS update lat=%.7f lon=%.7f acc=%.2f sigma=%.2f indoors=%s", + latDeg, lonDeg, accuracyMeters, sigma, String.valueOf(indoors))); } applyAbsoluteFix(latDeg, lonDeg, sigma, null); } @@ -154,6 +163,8 @@ public synchronized void updateWifi(double latDeg, double lonDeg, int wifiFloor) public synchronized void updateElevation(float elevationMeters, boolean elevatorLikely) { int floorFromBarometer = Math.round(elevationMeters / floorHeightMeters); fallbackFloor = floorFromBarometer; + int blockedTransitions = 0; + int allowedTransitions = 0; if (!particles.isEmpty()) { for (Particle p : particles) { if (p.floor == floorFromBarometer) { @@ -164,9 +175,21 @@ public synchronized void updateElevation(float elevationMeters, boolean elevator int nextFloor = p.floor + step; if (canUseConnector(p.floor, p.xEast, p.yNorth, elevatorLikely)) { p.floor = nextFloor; + allowedTransitions++; + } else { + blockedTransitions++; } } } + + if (DEBUG_LOGS && (allowedTransitions > 0 || blockedTransitions > 0)) { + Log.d(TAG, String.format(Locale.US, + "Elevation floor target=%d elevator=%s transitions allowed=%d blocked=%d", + floorFromBarometer, + String.valueOf(elevatorLikely), + allowedTransitions, + blockedTransitions)); + } } public synchronized void updateMapMatchingContext( @@ -238,6 +261,18 @@ public synchronized void updateMapMatchingContext( } } } + + if (DEBUG_LOGS) { + Log.d(TAG, String.format(Locale.US, + "Map floor parsed building=%s idx=%d display=%s logical=%d walls=%d stairs=%d lifts=%d", + containing.getName(), + i, + floor.getDisplayName(), + logicalFloor, + constraint.walls.size(), + constraint.stairs.size(), + constraint.lifts.size())); + } } floorConstraints.clear(); @@ -290,7 +325,10 @@ private void applyAbsoluteFix(double latDeg, double lonDeg, double sigmaMeters, } double[] z = toLocal(latDeg, lonDeg); - double effectiveBefore = computeEffectiveSampleSize(); + if (floorHint != null) { + injectFloorSupportIfNeeded(floorHint, z[0], z[1]); + } + double effectiveBefore = computeEffectiveSampleSize(); double sigma2 = sigmaMeters * sigmaMeters; double maxLogWeight = Double.NEGATIVE_INFINITY; @@ -347,6 +385,46 @@ private void applyAbsoluteFix(double latDeg, double lonDeg, double sigmaMeters, logUpdateSummary(z[0], z[1], sigmaMeters, floorHint, effectiveBefore, effectiveN, resampled); } + private void injectFloorSupportIfNeeded(int floorHint, double zEast, double zNorth) { + double floorSupport = floorSupportWeight(floorHint); + if (floorSupport >= FLOOR_HINT_MIN_SUPPORT) { + return; + } + + int injectCount = Math.max(1, + (int) Math.round(PARTICLE_COUNT * FLOOR_HINT_INJECTION_FRACTION)); + List indices = new ArrayList<>(particles.size()); + for (int i = 0; i < particles.size(); i++) { + indices.add(i); + } + indices.sort((a, b) -> Double.compare(particles.get(a).weight, particles.get(b).weight)); + + for (int i = 0; i < injectCount && i < indices.size(); i++) { + Particle p = particles.get(indices.get(i)); + p.floor = floorHint; + p.xEast = zEast + random.nextGaussian() * FLOOR_HINT_INJECTION_STD_M; + p.yNorth = zNorth + random.nextGaussian() * FLOOR_HINT_INJECTION_STD_M; + } + + if (DEBUG_LOGS) { + Log.i(TAG, String.format(Locale.US, + "Floor support injection hint=%d supportBefore=%.3f injectCount=%d", + floorHint, + floorSupport, + injectCount)); + } + } + + private double floorSupportWeight(int floor) { + double sum = 0.0; + for (Particle p : particles) { + if (p.floor == floor) { + sum += p.weight; + } + } + return sum; + } + private void initParticlesAtOrigin(int initialFloor) { particles.clear(); double w = 1.0 / PARTICLE_COUNT; diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java index 16b2e52f..1a3b0f7f 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -11,6 +11,7 @@ import android.os.Handler; import android.os.Looper; import android.os.SystemClock; +import android.util.Log; import android.widget.Toast; import androidx.annotation.NonNull; @@ -51,6 +52,7 @@ public class SensorFusion implements SensorEventListener { //region Static variables private static final SensorFusion sensorFusion = new SensorFusion(); + private static final String TAG = "SensorFusion"; //endregion //region Instance variables @@ -433,6 +435,16 @@ public void setFloorplanBuildings(List building continue; } floorplanBuildingCache.put(building.getName(), building); + + List floors = building.getFloorShapesList(); + Log.i(TAG, "Floorplan cache building=" + building.getName() + + " floors=" + floors.size()); + for (int i = 0; i < floors.size(); i++) { + FloorplanApiClient.FloorShapes floor = floors.get(i); + Log.d(TAG, "Floorplan floor index=" + i + + " display=" + floor.getDisplayName() + + " features=" + floor.getFeatures().size()); + } } if (fusionEngine != null) { diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java b/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java index 8b16df94..5efc2ab6 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java @@ -161,14 +161,27 @@ public int getAutoFloorBias() { public void setCurrentFloor(int newFloor, boolean autoFloor) { if (currentFloorShapes == null || currentFloorShapes.isEmpty()) return; + int requestedFloor = newFloor; + int bias = getAutoFloorBias(); + if (autoFloor) { - newFloor += getAutoFloorBias(); + newFloor += bias; + Log.d(TAG, "Auto-floor request logical=" + requestedFloor + + " bias=" + bias + " -> index=" + newFloor); } if (newFloor >= 0 && newFloor < currentFloorShapes.size() && newFloor != this.currentFloor) { this.currentFloor = newFloor; + Log.i(TAG, "Set floor index=" + newFloor + + " display=" + getCurrentFloorDisplayName() + + " totalFloors=" + currentFloorShapes.size()); drawFloorShapes(newFloor); + } else { + Log.d(TAG, "Ignored floor change request requested=" + requestedFloor + + " mappedIndex=" + newFloor + + " current=" + this.currentFloor + + " totalFloors=" + currentFloorShapes.size()); } } @@ -230,11 +243,23 @@ private void setBuildingOverlay() { SensorFusion.getInstance().getFloorplanBuilding(apiName); if (building != null) { currentFloorShapes = building.getFloorShapesList(); + Log.i(TAG, "Loaded floorplan building=" + apiName + + " floors=" + (currentFloorShapes == null ? 0 : currentFloorShapes.size())); + if (currentFloorShapes != null) { + for (int i = 0; i < currentFloorShapes.size(); i++) { + FloorplanApiClient.FloorShapes floorShapes = currentFloorShapes.get(i); + Log.d(TAG, "Floor index=" + i + " display=" + floorShapes.getDisplayName() + + " features=" + floorShapes.getFeatures().size()); + } + } } if (currentFloorShapes != null && !currentFloorShapes.isEmpty()) { drawFloorShapes(currentFloor); isIndoorMapSet = true; + Log.i(TAG, "Indoor overlay enabled building=" + apiName + + " startFloorIndex=" + currentFloor + + " display=" + getCurrentFloorDisplayName()); } } else if (!inAnyBuilding && isIndoorMapSet) { @@ -243,6 +268,7 @@ private void setBuildingOverlay() { currentBuilding = BUILDING_NONE; currentFloor = 0; currentFloorShapes = null; + Log.i(TAG, "Indoor overlay disabled (left mapped buildings)"); } } catch (Exception ex) { Log.e(TAG, "Error with overlay: " + ex.toString()); @@ -262,6 +288,8 @@ private void drawFloorShapes(int floorIndex) { || floorIndex >= currentFloorShapes.size()) return; FloorplanApiClient.FloorShapes floor = currentFloorShapes.get(floorIndex); + Log.d(TAG, "Draw floor index=" + floorIndex + " display=" + floor.getDisplayName() + + " featureCount=" + floor.getFeatures().size()); for (FloorplanApiClient.MapShapeFeature feature : floor.getFeatures()) { String geoType = feature.getGeometryType(); String indoorType = feature.getIndoorType(); From 1bb0d489df70d1e2a1c855b7eb48a52e87826d28 Mon Sep 17 00:00:00 2001 From: HuangerzJ <125708330+HuangerzJ@users.noreply.github.com> Date: Wed, 25 Mar 2026 12:50:22 +0000 Subject: [PATCH 08/52] Showing GNSS points --- .../presentation/fragment/RecordingFragment.java | 7 ++++--- .../presentation/fragment/TrajectoryMapFragment.java | 5 +++-- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/RecordingFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/RecordingFragment.java index 813b0ba3..be53566d 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/RecordingFragment.java +++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/RecordingFragment.java @@ -292,16 +292,17 @@ private void updateUIandPosition() { // GNSS logic if you want to show GNSS error, etc. float[] gnss = sensorFusion.getSensorValueMap().get(SensorTypes.GNSSLATLONG); if (gnss != null && trajectoryMapFragment != null) { - // If user toggles showing GNSS in the map, call e.g. + LatLng gnssLocation = new LatLng(gnss[0], gnss[1]); + // Always forward GNSS observations so GNSS circles are shown regardless of toggle. + trajectoryMapFragment.updateGNSS(gnssLocation); + if (trajectoryMapFragment.isGnssEnabled()) { - LatLng gnssLocation = new LatLng(gnss[0], gnss[1]); LatLng currentLoc = trajectoryMapFragment.getCurrentLocation(); if (currentLoc != null) { double errorDist = UtilFunctions.distanceBetweenPoints(currentLoc, gnssLocation); gnssError.setVisibility(View.VISIBLE); gnssError.setText(String.format(getString(R.string.gnss_error) + "%.2fm", errorDist)); } - trajectoryMapFragment.updateGNSS(gnssLocation); } else { gnssError.setVisibility(View.GONE); trajectoryMapFragment.clearGNSS(); diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java index e54dd4e2..a6f52490 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java +++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java @@ -418,14 +418,15 @@ public void addTestPointMarker(int index, long timestampMs, @NonNull LatLng posi */ public void updateGNSS(@NonNull LatLng gnssLocation) { if (gMap == null) return; - if (!isGnssOn) return; addObservationMarker( gnssObservationCircles, gnssLocation, Color.argb(220, 33, 150, 243), Color.argb(80, 33, 150, 243), - true); + false); + + if (!isGnssOn) return; if (gnssMarker == null) { // Create the GNSS marker for the first time From ada43dffc08494a2f6d93079b79d07e931b59d30 Mon Sep 17 00:00:00 2001 From: tommyj0 Date: Thu, 26 Mar 2026 15:44:46 +0000 Subject: [PATCH 09/52] ordering fixed --- .../PositionMe/utils/IndoorMapManager.java | 89 ++++++++++++++++++- 1 file changed, 88 insertions(+), 1 deletion(-) diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java b/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java index 5efc2ab6..b2feddf7 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java @@ -13,7 +13,11 @@ import com.openpositioning.PositionMe.sensors.SensorFusion; import java.util.ArrayList; +import java.util.Collections; import java.util.List; +import java.util.Locale; +import java.util.regex.Matcher; +import java.util.regex.Pattern; /** * Manages indoor floor map display for all supported buildings @@ -59,6 +63,7 @@ public class IndoorMapManager { private static final int ROOM_STROKE = Color.argb(210, 33, 150, 243); private static final int ROOM_FILL = Color.argb(60, 33, 150, 243); private static final int DEFAULT_STROKE = Color.argb(170, 66, 165, 245); + private static final Pattern FLOOR_NUMBER_PATTERN = Pattern.compile("-?\\d+"); /** * Constructor to set the map instance. @@ -242,7 +247,7 @@ private void setBuildingOverlay() { FloorplanApiClient.BuildingInfo building = SensorFusion.getInstance().getFloorplanBuilding(apiName); if (building != null) { - currentFloorShapes = building.getFloorShapesList(); + currentFloorShapes = normalizeFloorOrder(building.getFloorShapesList()); Log.i(TAG, "Loaded floorplan building=" + apiName + " floors=" + (currentFloorShapes == null ? 0 : currentFloorShapes.size())); if (currentFloorShapes != null) { @@ -254,6 +259,15 @@ private void setBuildingOverlay() { } } + if (currentFloorShapes != null && !currentFloorShapes.isEmpty()) { + int groundFloorIndex = findFloorIndexForLogicalFloor(0); + if (groundFloorIndex >= 0) { + currentFloor = groundFloorIndex; + } else if (currentFloor < 0 || currentFloor >= currentFloorShapes.size()) { + currentFloor = 0; + } + } + if (currentFloorShapes != null && !currentFloorShapes.isEmpty()) { drawFloorShapes(currentFloor); isIndoorMapSet = true; @@ -351,6 +365,79 @@ private int getFillColor(String indoorType) { return Color.TRANSPARENT; } + private List normalizeFloorOrder( + List input) { + if (input == null || input.isEmpty()) { + return input; + } + + List ordered = new ArrayList<>(input); + Collections.sort(ordered, (a, b) -> { + Integer floorA = logicalFloorFromDisplayName(a == null ? null : a.getDisplayName()); + Integer floorB = logicalFloorFromDisplayName(b == null ? null : b.getDisplayName()); + + if (floorA != null && floorB != null) { + return Integer.compare(floorA, floorB); + } + if (floorA != null) { + return -1; + } + if (floorB != null) { + return 1; + } + return 0; + }); + return ordered; + } + + private int findFloorIndexForLogicalFloor(int logicalFloor) { + if (currentFloorShapes == null || currentFloorShapes.isEmpty()) { + return -1; + } + for (int i = 0; i < currentFloorShapes.size(); i++) { + FloorplanApiClient.FloorShapes floorShapes = currentFloorShapes.get(i); + Integer candidate = logicalFloorFromDisplayName( + floorShapes == null ? null : floorShapes.getDisplayName()); + if (candidate != null && candidate == logicalFloor) { + return i; + } + } + return -1; + } + + private Integer logicalFloorFromDisplayName(String displayName) { + if (displayName == null) { + return null; + } + + String normalized = displayName.trim().toUpperCase(Locale.US).replace(" ", ""); + if (normalized.isEmpty()) { + return null; + } + + if ("LG".equals(normalized) || "L".equals(normalized) || "LOWERGROUND".equals(normalized)) { + return -1; + } + if ("G".equals(normalized) || "GF".equals(normalized) + || "GROUND".equals(normalized) || "GROUNDFLOOR".equals(normalized)) { + return 0; + } + + if (normalized.startsWith("F") || normalized.startsWith("L")) { + normalized = normalized.substring(1); + } + + Matcher matcher = FLOOR_NUMBER_PATTERN.matcher(normalized); + if (matcher.matches()) { + try { + return Integer.parseInt(normalized); + } catch (NumberFormatException ignored) { + return null; + } + } + return null; + } + /** * Detects which building the user is currently in. * Checks floorplan API outline polygons first; falls back to legacy From 21e62fd12f0e4c99223e6d32428e0f94e8e1dcb5 Mon Sep 17 00:00:00 2001 From: tommyj0 Date: Thu, 26 Mar 2026 15:59:23 +0000 Subject: [PATCH 10/52] orientation handler --- .../sensors/SensorEventHandler.java | 41 +++++++++++++++++++ 1 file changed, 41 insertions(+) diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java index 222b1a05..15f279df 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java @@ -28,6 +28,9 @@ public interface PdrStepListener { private static final float ALPHA = 0.8f; private static final long LARGE_GAP_THRESHOLD_MS = 500; + private static final float FLIP_DETECTION_MIN_DEG = 150f; + private static final float FLIP_DETECTION_MAX_DEG = 210f; + private static final float FLIP_GYRO_MAX_RAD_PER_SEC = 0.8f; private final SensorState state; private final PdrProcessing pdrProcessing; @@ -46,6 +49,8 @@ public interface PdrStepListener { private float lastPdrX = 0f; private float lastPdrY = 0f; private boolean hasPdrReference = false; + private boolean hasPreviousHeading = false; + private float previousHeadingRad = 0f; /** * Creates a new SensorEventHandler. @@ -153,6 +158,26 @@ public void handleSensorEvent(SensorEvent sensorEvent) { float[] rotationVectorDCM = new float[9]; SensorManager.getRotationMatrixFromVector(rotationVectorDCM, state.rotation); SensorManager.getOrientation(rotationVectorDCM, state.orientation); + + float correctedAzimuth = state.orientation[0]; + if (hasPreviousHeading) { + float deltaDeg = Math.abs((float) Math.toDegrees( + shortestAngularDistance(previousHeadingRad, correctedAzimuth))); + float gyroNorm = (float) Math.sqrt( + state.angularVelocity[0] * state.angularVelocity[0] + + state.angularVelocity[1] * state.angularVelocity[1] + + state.angularVelocity[2] * state.angularVelocity[2]); + + if (deltaDeg >= FLIP_DETECTION_MIN_DEG + && deltaDeg <= FLIP_DETECTION_MAX_DEG + && gyroNorm <= FLIP_GYRO_MAX_RAD_PER_SEC) { + correctedAzimuth = normalizeAngleRad((float) (correctedAzimuth + Math.PI)); + } + } + + state.orientation[0] = correctedAzimuth; + previousHeadingRad = correctedAzimuth; + hasPreviousHeading = true; break; case Sensor.TYPE_STEP_DETECTOR: @@ -209,6 +234,20 @@ public void handleSensorEvent(SensorEvent sensorEvent) { } } + private static float normalizeAngleRad(float angle) { + while (angle > Math.PI) { + angle -= (float) (2.0 * Math.PI); + } + while (angle <= -Math.PI) { + angle += (float) (2.0 * Math.PI); + } + return angle; + } + + private static float shortestAngularDistance(float from, float to) { + return normalizeAngleRad(to - from); + } + /** * Utility function to log the event frequency of each sensor. * Call this periodically for debugging purposes. @@ -230,5 +269,7 @@ void resetBootTime(long newBootTime) { this.hasPdrReference = false; this.lastPdrX = 0f; this.lastPdrY = 0f; + this.hasPreviousHeading = false; + this.previousHeadingRad = 0f; } } From 6b4971722d848058be098f20cf739f428c584b4e Mon Sep 17 00:00:00 2001 From: tommyj0 Date: Thu, 26 Mar 2026 16:50:37 +0000 Subject: [PATCH 11/52] added pdr feedback from fusion --- .../sensors/SensorEventHandler.java | 5 +- .../PositionMe/sensors/SensorFusion.java | 23 ++++++++ .../PositionMe/utils/PdrProcessing.java | 53 +++++++++++++++++-- 3 files changed, 75 insertions(+), 6 deletions(-) diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java index 15f279df..ae1ceee4 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java @@ -205,12 +205,13 @@ public void handleSensorEvent(SensorEvent sensorEvent) { this.accelMagnitude, state.orientation[0] ); + float[] correctionDelta = this.pdrProcessing.consumePendingFeedbackDelta(); float dx = 0f; float dy = 0f; if (hasPdrReference) { - dx = newCords[0] - lastPdrX; - dy = newCords[1] - lastPdrY; + dx = newCords[0] - lastPdrX - correctionDelta[0]; + dy = newCords[1] - lastPdrY - correctionDelta[1]; } lastPdrX = newCords[0]; lastPdrY = newCords[1]; diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java index 1a3b0f7f..6f461610 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -24,6 +24,7 @@ import com.openpositioning.PositionMe.utils.PathView; import com.openpositioning.PositionMe.utils.PdrProcessing; import com.openpositioning.PositionMe.utils.TrajectoryValidator; +import com.openpositioning.PositionMe.utils.UtilFunctions; import com.openpositioning.PositionMe.data.remote.ServerCommunications; import java.util.ArrayList; @@ -725,6 +726,28 @@ private void updateFusedState() { state.fusedLongitude = (float) estimate.getLatLng().longitude; state.fusedFloor = estimate.getFloor(); state.fusedAvailable = true; + + // Feed the fused absolute estimate back into local PDR coordinates to limit long-term drift. + applyFusionFeedbackToPdr(estimate.getLatLng()); + } + + private void applyFusionFeedbackToPdr(@NonNull LatLng fusedLatLng) { + if (pdrProcessing == null) { + return; + } + + float startLat = state.startLocation[0]; + float startLon = state.startLocation[1]; + if (startLat == 0f && startLon == 0f) { + return; + } + + double dLat = fusedLatLng.latitude - startLat; + double dLon = fusedLatLng.longitude - startLon; + + float targetY = (float) UtilFunctions.degreesToMetersLat(dLat); + float targetX = (float) UtilFunctions.degreesToMetersLng(dLon, startLat); + pdrProcessing.applyFusionFeedback(targetX, targetY); } //endregion diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java b/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java index 9765b044..86a24d32 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java @@ -38,6 +38,8 @@ public class PdrProcessing { // Threshold under which movement is considered non-existent private static final float epsilon = 0.18f; private static final int MIN_REQUIRED_SAMPLES = 2; + private static final float FUSION_FEEDBACK_GAIN = 0.15f; + private static final float FUSION_FEEDBACK_MAX_STEP_M = 0.8f; //endregion //region Instance variables @@ -52,6 +54,10 @@ public class PdrProcessing { // Current 2D position coordinates private float positionX; private float positionY; + private float correctionX; + private float correctionY; + private float pendingCorrectionX; + private float pendingCorrectionY; // Vertical movement calculation private Float[] startElevationBuffer; @@ -102,6 +108,10 @@ public PdrProcessing(Context context) { // Initial position and elevation - starts from zero this.positionX = 0f; this.positionY = 0f; + this.correctionX = 0f; + this.correctionY = 0f; + this.pendingCorrectionX = 0f; + this.pendingCorrectionY = 0f; this.elevation = 0f; @@ -141,7 +151,7 @@ public PdrProcessing(Context context) { */ public float[] updatePdr(long currentStepEnd, List accelMagnitudeOvertime, float headingRad) { if (accelMagnitudeOvertime == null || accelMagnitudeOvertime.size() < MIN_REQUIRED_SAMPLES) { - return new float[]{this.positionX, this.positionY}; // Return current position without update + return new float[]{this.positionX + this.correctionX, this.positionY + this.correctionY}; // - TODO - temporary solution of the empty list issue } @@ -151,7 +161,7 @@ public float[] updatePdr(long currentStepEnd, List accelMagnitudeOvertim // check if accelMagnitudeOvertime is empty if (accelMagnitudeOvertime == null || accelMagnitudeOvertime.isEmpty()) { // return current position, do not update - return new float[]{this.positionX, this.positionY}; + return new float[]{this.positionX + this.correctionX, this.positionY + this.correctionY}; } // Calculate step length @@ -175,7 +185,7 @@ public float[] updatePdr(long currentStepEnd, List accelMagnitudeOvertim this.positionY += y; // return current position - return new float[]{this.positionX, this.positionY}; + return new float[]{this.positionX + this.correctionX, this.positionY + this.correctionY}; } /** @@ -268,11 +278,42 @@ private float weibergMinMax(List accelMagnitude) { * @return float array of size 2, with the X and Y coordinates respectively. */ public float[] getPDRMovement() { - float [] pdrPosition= new float[] {positionX,positionY}; + float [] pdrPosition= new float[] {positionX + correctionX, positionY + correctionY}; return pdrPosition; } + public void applyFusionFeedback(float targetX, float targetY) { + float correctedX = this.positionX + this.correctionX; + float correctedY = this.positionY + this.correctionY; + + float errorX = targetX - correctedX; + float errorY = targetY - correctedY; + + float deltaX = clamp(errorX * FUSION_FEEDBACK_GAIN, + -FUSION_FEEDBACK_MAX_STEP_M, + FUSION_FEEDBACK_MAX_STEP_M); + float deltaY = clamp(errorY * FUSION_FEEDBACK_GAIN, + -FUSION_FEEDBACK_MAX_STEP_M, + FUSION_FEEDBACK_MAX_STEP_M); + + this.correctionX += deltaX; + this.correctionY += deltaY; + this.pendingCorrectionX += deltaX; + this.pendingCorrectionY += deltaY; + } + + public float[] consumePendingFeedbackDelta() { + float[] delta = new float[]{pendingCorrectionX, pendingCorrectionY}; + pendingCorrectionX = 0f; + pendingCorrectionY = 0f; + return delta; + } + + private static float clamp(float value, float min, float max) { + return Math.max(min, Math.min(max, value)); + } + /** * Get the current elevation as calculated by the PDR class. * @@ -370,6 +411,10 @@ public void resetPDR() { // Initial position and elevation - starts from zero this.positionX = 0f; this.positionY = 0f; + this.correctionX = 0f; + this.correctionY = 0f; + this.pendingCorrectionX = 0f; + this.pendingCorrectionY = 0f; this.elevation = 0f; if(this.settings.getBoolean("overwrite_constants", false)) { From f391336fba2fad530e5e71ead8d5ad7c70709751 Mon Sep 17 00:00:00 2001 From: tommyj0 Date: Fri, 27 Mar 2026 14:21:51 +0000 Subject: [PATCH 12/52] feedback turned off for now, added legend for data points --- .../sensors/SensorEventHandler.java | 23 +++- .../PositionMe/sensors/SensorFusion.java | 5 +- .../PositionMe/sensors/WiFiPositioning.java | 130 ++++++++++-------- .../sensors/WifiPositionManager.java | 62 ++++++++- .../PositionMe/utils/PdrProcessing.java | 18 ++- .../main/res/drawable/legend_circle_blue.xml | 5 + .../main/res/drawable/legend_circle_green.xml | 5 + .../res/drawable/legend_circle_orange.xml | 5 + .../res/layout/fragment_trajectory_map.xml | 95 +++++++++++++ 9 files changed, 281 insertions(+), 67 deletions(-) create mode 100644 app/src/main/res/drawable/legend_circle_blue.xml create mode 100644 app/src/main/res/drawable/legend_circle_green.xml create mode 100644 app/src/main/res/drawable/legend_circle_orange.xml diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java index ae1ceee4..e2688c99 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java @@ -31,6 +31,10 @@ public interface PdrStepListener { private static final float FLIP_DETECTION_MIN_DEG = 150f; private static final float FLIP_DETECTION_MAX_DEG = 210f; private static final float FLIP_GYRO_MAX_RAD_PER_SEC = 0.8f; + private static final float FLIP_DETECTION_STRICT_MIN_DEG = 170f; + private static final float FLIP_DETECTION_STRICT_MAX_DEG = 190f; + private static final float FLIP_GYRO_STRICT_MAX_RAD_PER_SEC = 0.3f; + private static final int FLIP_HYSTERESIS_MS = 500; private final SensorState state; private final PdrProcessing pdrProcessing; @@ -51,6 +55,7 @@ public interface PdrStepListener { private boolean hasPdrReference = false; private boolean hasPreviousHeading = false; private float previousHeadingRad = 0f; + private long lastFlipCorrectionTime = 0; /** * Creates a new SensorEventHandler. @@ -168,10 +173,21 @@ public void handleSensorEvent(SensorEvent sensorEvent) { + state.angularVelocity[1] * state.angularVelocity[1] + state.angularVelocity[2] * state.angularVelocity[2]); - if (deltaDeg >= FLIP_DETECTION_MIN_DEG - && deltaDeg <= FLIP_DETECTION_MAX_DEG - && gyroNorm <= FLIP_GYRO_MAX_RAD_PER_SEC) { + long timeSinceLastFlip = currentTime - lastFlipCorrectionTime; + boolean hysteresisOk = timeSinceLastFlip >= FLIP_HYSTERESIS_MS; + + if (hysteresisOk + && deltaDeg >= FLIP_DETECTION_STRICT_MIN_DEG + && deltaDeg <= FLIP_DETECTION_STRICT_MAX_DEG + && gyroNorm <= FLIP_GYRO_STRICT_MAX_RAD_PER_SEC) { correctedAzimuth = normalizeAngleRad((float) (correctedAzimuth + Math.PI)); + lastFlipCorrectionTime = currentTime; + Log.w("SensorFusion", "180-flip detected and corrected: deltaDeg=" + deltaDeg + + " gyroNorm=" + gyroNorm); + } else if (!hysteresisOk && deltaDeg >= FLIP_DETECTION_STRICT_MIN_DEG + && deltaDeg <= FLIP_DETECTION_STRICT_MAX_DEG) { + Log.d("SensorFusion", "Flip pattern detected but hysteresis active; timeSinceLastFlip=" + + timeSinceLastFlip + "ms"); } } @@ -272,5 +288,6 @@ void resetBootTime(long newBootTime) { this.lastPdrY = 0f; this.hasPreviousHeading = false; this.previousHeadingRad = 0f; + this.lastFlipCorrectionTime = 0; } } diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java index 6f461610..9f420579 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -54,6 +54,7 @@ public class SensorFusion implements SensorEventListener { //region Static variables private static final SensorFusion sensorFusion = new SensorFusion(); private static final String TAG = "SensorFusion"; + private static final boolean ENABLE_PDR_FEEDBACK = false; //endregion //region Instance variables @@ -728,7 +729,9 @@ private void updateFusedState() { state.fusedAvailable = true; // Feed the fused absolute estimate back into local PDR coordinates to limit long-term drift. - applyFusionFeedbackToPdr(estimate.getLatLng()); + if (ENABLE_PDR_FEEDBACK) { + applyFusionFeedbackToPdr(estimate.getLatLng()); + } } private void applyFusionFeedbackToPdr(@NonNull LatLng fusedLatLng) { diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/WiFiPositioning.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/WiFiPositioning.java index dbf809dd..3b7af45c 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/WiFiPositioning.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/WiFiPositioning.java @@ -4,6 +4,7 @@ import com.android.volley.Request; import com.android.volley.RequestQueue; +import com.android.volley.VolleyError; import com.android.volley.toolbox.JsonObjectRequest; import com.android.volley.toolbox.Volley; import com.google.android.gms.maps.model.LatLng; @@ -30,8 +31,11 @@ public class WiFiPositioning { // Queue for storing the POST requests made private RequestQueue requestQueue; - // URL for WiFi positioning API - private static final String url="https://openpositioning.org/api/position/fine"; + // Try fine first, then fallback to coarse when fine cannot localize this fingerprint. + private static final String[] URL_CANDIDATES = new String[] { + "https://openpositioning.org/api/position/fine", + "https://openpositioning.org/api/position/coarse" + }; /** * Getter for the WiFi positioning coordinates obtained using openpositioning API @@ -81,39 +85,7 @@ public WiFiPositioning(Context context){ * @param jsonWifiFeatures WiFi Fingerprint from device */ public void request(JSONObject jsonWifiFeatures) { - // Creating the POST request using WiFi fingerprint (a JSON object) - JsonObjectRequest jsonObjectRequest = new JsonObjectRequest( - Request.Method.POST, url, jsonWifiFeatures, - // Parses the response to obtain the WiFi location and WiFi floor - response -> { - try { - wifiLocation = new LatLng(response.getDouble("lat"),response.getDouble("lon")); - floor = response.getInt("floor"); - } catch (JSONException e) { - // Error log to keep record of errors (for secure programming and maintainability) - Log.e("jsonErrors","Error parsing response: "+e.getMessage()+" "+ response); - } - }, - // Handles the errors obtained from the POST request - error -> { - // Validation Error - if (error.networkResponse!=null && error.networkResponse.statusCode==422){ - Log.e("WiFiPositioning", "Validation Error "+ error.getMessage()); - } - // Other Errors - else{ - // When Response code is available - if (error.networkResponse!=null) { - Log.e("WiFiPositioning","Response Code: " + error.networkResponse.statusCode + ", " + error.getMessage()); - } - else{ - Log.e("WiFiPositioning","Error message: " + error.getMessage()); - } - } - } - ); - // Adds the request to the request queue - requestQueue.add(jsonObjectRequest); + requestWithRetry(jsonWifiFeatures, 0, null); } @@ -132,44 +104,88 @@ public void request(JSONObject jsonWifiFeatures) { * @param callback callback function to allow user to use location when ready */ public void request( JSONObject jsonWifiFeatures, final VolleyCallback callback) { - // Creating the POST request using WiFi fingerprint (a JSON object) + requestWithRetry(jsonWifiFeatures, 0, callback); + } + + private void requestWithRetry(JSONObject jsonWifiFeatures, + int urlIndex, + final VolleyCallback callback) { + if (urlIndex >= URL_CANDIDATES.length) { + String message = "WiFi positioning failed for all URL candidates"; + Log.e("WiFiPositioning", message); + if (callback != null) { + callback.onError(message); + } + return; + } + + final String url = URL_CANDIDATES[urlIndex]; + Log.d("WiFiPositioning", "Request URL=" + url); JsonObjectRequest jsonObjectRequest = new JsonObjectRequest( - Request.Method.POST, url, jsonWifiFeatures, + Request.Method.POST, url, jsonWifiFeatures, response -> { try { - Log.d("jsonObject",response.toString()); - wifiLocation = new LatLng(response.getDouble("lat"),response.getDouble("lon")); + Log.d("WiFiPositioning", "Response URL=" + url + " body=" + response); + wifiLocation = new LatLng(response.getDouble("lat"), response.getDouble("lon")); floor = response.getInt("floor"); - callback.onSuccess(wifiLocation,floor); + if (callback != null) { + callback.onSuccess(wifiLocation, floor); + } } catch (JSONException e) { - Log.e("jsonErrors","Error parsing response: "+e.getMessage()+" "+ response); - callback.onError("Error parsing response: " + e.getMessage()); + String msg = "Error parsing response from " + url + ": " + e.getMessage(); + Log.e("WiFiPositioning", msg + " " + response); + if (callback != null) { + callback.onError(msg); + } } }, error -> { - // Validation Error - if (error.networkResponse!=null && error.networkResponse.statusCode==422){ - Log.e("WiFiPositioning", "Validation Error "+ error.getMessage()); - callback.onError( "Validation Error (422): "+ error.getMessage()); - } - // Other Errors - else{ - // When Response code is available - if (error.networkResponse!=null) { - Log.e("WiFiPositioning","Response Code: " + error.networkResponse.statusCode + ", " + error.getMessage()); - callback.onError("Response Code: " + error.networkResponse.statusCode + ", " + error.getMessage()); + int code = error.networkResponse != null ? error.networkResponse.statusCode : -1; + String responseBody = extractErrorBody(error); + Log.e("WiFiPositioning", + "Request failed URL=" + url + + " code=" + code + + " message=" + error.getMessage() + + " body=" + responseBody); + + if (code == 404 && responseBody.contains("Position unknown")) { + // Endpoint is reachable, but this fingerprint is not known at current granularity. + // Fallback from fine -> coarse if available. + if (urlIndex + 1 < URL_CANDIDATES.length) { + requestWithRetry(jsonWifiFeatures, urlIndex + 1, callback); + return; } - else{ - Log.e("WiFiPositioning","Error message: " + error.getMessage()); - callback.onError("Error message: " + error.getMessage()); + if (callback != null) { + callback.onError("Position unknown"); } + return; + } + + if (code == 404 && urlIndex + 1 < URL_CANDIDATES.length) { + requestWithRetry(jsonWifiFeatures, urlIndex + 1, callback); + return; + } + + if (callback != null) { + callback.onError("Response Code: " + code + ", " + + error.getMessage() + ", body=" + responseBody); } } ); - // Adds the request to the request queue requestQueue.add(jsonObjectRequest); } + private String extractErrorBody(VolleyError error) { + if (error == null || error.networkResponse == null || error.networkResponse.data == null) { + return ""; + } + try { + return new String(error.networkResponse.data, "UTF-8"); + } catch (Exception ignored) { + return ""; + } + } + /** * Interface defined for the callback to access response obtained after POST request */ diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java index da815693..50373786 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java @@ -66,7 +66,15 @@ private void createWifiPositioningRequest() { try { JSONObject wifiAccessPoints = new JSONObject(); for (Wifi data : this.wifiList) { - wifiAccessPoints.put(String.valueOf(data.getBssid()), data.getLevel()); + String bssidKey = getBssidKey(data); + if (bssidKey == null) { + continue; + } + wifiAccessPoints.put(bssidKey, data.getLevel()); + } + if (wifiAccessPoints.length() == 0) { + Log.w("WifiPositionManager", "Skipping WiFi positioning request: no valid BSSID keys"); + return; } JSONObject wifiFingerPrint = new JSONObject(); wifiFingerPrint.put(WIFI_FINGERPRINT, wifiAccessPoints); @@ -95,7 +103,15 @@ private void createWifiPositionRequestCallback() { try { JSONObject wifiAccessPoints = new JSONObject(); for (Wifi data : this.wifiList) { - wifiAccessPoints.put(String.valueOf(data.getBssid()), data.getLevel()); + String bssidKey = getBssidKey(data); + if (bssidKey == null) { + continue; + } + wifiAccessPoints.put(bssidKey, data.getLevel()); + } + if (wifiAccessPoints.length() == 0) { + Log.w("WifiPositionManager", "Skipping WiFi callback request: no valid BSSID keys"); + return; } JSONObject wifiFingerPrint = new JSONObject(); wifiFingerPrint.put(WIFI_FINGERPRINT, wifiAccessPoints); @@ -148,4 +164,46 @@ public List getWifiList() { public void setWifiFixListener(WifiFixListener wifiFixListener) { this.wifiFixListener = wifiFixListener; } + + private String getBssidKey(Wifi wifi) { + String bssidString = wifi.getBssidString(); + if (bssidString != null && !bssidString.trim().isEmpty()) { + String normalizedHex = normalizeMacToHex(bssidString.trim()); + if (normalizedHex != null) { + long macValue = Long.parseUnsignedLong(normalizedHex, 16); + return Long.toUnsignedString(macValue); + } + } + + // Fallback: convert packed long (lower 48 bits) to unsigned decimal string. + long mac = wifi.getBssid() & 0x0000FFFFFFFFFFFFL; + return Long.toUnsignedString(mac); + } + + private String normalizeMacToHex(String value) { + if (value == null) { + return null; + } + String normalized = value.replace(":", "").replace("-", "").toUpperCase(); + if (!isValidHexMac(normalized)) { + return null; + } + return normalized; + } + + private boolean isValidHexMac(String value) { + if (value.length() != 12) { + return false; + } + for (int i = 0; i < value.length(); i++) { + char c = value.charAt(i); + boolean digit = c >= '0' && c <= '9'; + boolean upperHex = c >= 'A' && c <= 'F'; + if (!digit && !upperHex) { + return false; + } + } + return true; + } + } diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java b/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java index 86a24d32..59cfcaf0 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java @@ -38,8 +38,10 @@ public class PdrProcessing { // Threshold under which movement is considered non-existent private static final float epsilon = 0.18f; private static final int MIN_REQUIRED_SAMPLES = 2; - private static final float FUSION_FEEDBACK_GAIN = 0.15f; - private static final float FUSION_FEEDBACK_MAX_STEP_M = 0.8f; + private static final float FUSION_FEEDBACK_GAIN_NEAR = 0.22f; + private static final float FUSION_FEEDBACK_GAIN_MEDIUM = 0.35f; + private static final float FUSION_FEEDBACK_GAIN_FAR = 0.50f; + private static final float FUSION_FEEDBACK_MAX_STEP_M = 1.8f; //endregion //region Instance variables @@ -290,10 +292,18 @@ public void applyFusionFeedback(float targetX, float targetY) { float errorX = targetX - correctedX; float errorY = targetY - correctedY; - float deltaX = clamp(errorX * FUSION_FEEDBACK_GAIN, + float errorDist = (float) Math.hypot(errorX, errorY); + float gain = FUSION_FEEDBACK_GAIN_NEAR; + if (errorDist >= 4.0f) { + gain = FUSION_FEEDBACK_GAIN_FAR; + } else if (errorDist >= 1.5f) { + gain = FUSION_FEEDBACK_GAIN_MEDIUM; + } + + float deltaX = clamp(errorX * gain, -FUSION_FEEDBACK_MAX_STEP_M, FUSION_FEEDBACK_MAX_STEP_M); - float deltaY = clamp(errorY * FUSION_FEEDBACK_GAIN, + float deltaY = clamp(errorY * gain, -FUSION_FEEDBACK_MAX_STEP_M, FUSION_FEEDBACK_MAX_STEP_M); diff --git a/app/src/main/res/drawable/legend_circle_blue.xml b/app/src/main/res/drawable/legend_circle_blue.xml new file mode 100644 index 00000000..28a8f242 --- /dev/null +++ b/app/src/main/res/drawable/legend_circle_blue.xml @@ -0,0 +1,5 @@ + + + + diff --git a/app/src/main/res/drawable/legend_circle_green.xml b/app/src/main/res/drawable/legend_circle_green.xml new file mode 100644 index 00000000..23820537 --- /dev/null +++ b/app/src/main/res/drawable/legend_circle_green.xml @@ -0,0 +1,5 @@ + + + + diff --git a/app/src/main/res/drawable/legend_circle_orange.xml b/app/src/main/res/drawable/legend_circle_orange.xml new file mode 100644 index 00000000..a57069fe --- /dev/null +++ b/app/src/main/res/drawable/legend_circle_orange.xml @@ -0,0 +1,5 @@ + + + + diff --git a/app/src/main/res/layout/fragment_trajectory_map.xml b/app/src/main/res/layout/fragment_trajectory_map.xml index a72425bf..107108a7 100644 --- a/app/src/main/res/layout/fragment_trajectory_map.xml +++ b/app/src/main/res/layout/fragment_trajectory_map.xml @@ -66,6 +66,101 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Date: Fri, 27 Mar 2026 16:11:33 +0000 Subject: [PATCH 13/52] using francisco suggested method of orientation --- .../sensors/PositionFusionEngine.java | 101 ++++++++++++++++-- 1 file changed, 95 insertions(+), 6 deletions(-) diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java index 76268075..eb6d4589 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java @@ -40,6 +40,11 @@ public class PositionFusionEngine { private static final double EPS = 1e-300; private static final double CONNECTOR_RADIUS_M = 3.0; private static final double LIFT_HORIZONTAL_MAX_M = 0.50; + private static final double ORIENTATION_BIAS_LEARN_RATE = 0.20; + private static final double ORIENTATION_BIAS_MAX_STEP_RAD = Math.toRadians(6.0); + private static final double ORIENTATION_BIAS_MAX_ABS_RAD = Math.toRadians(45.0); + private static final double ORIENTATION_BIAS_MIN_STEP_M = 0.35; + private static final double ORIENTATION_BIAS_MIN_INNOVATION_M = 0.50; private final float floorHeightMeters; private final Random random = new Random(); @@ -55,6 +60,9 @@ public class PositionFusionEngine { private String activeBuildingName; private final Map floorConstraints = new HashMap<>(); private double recentStepMotionMeters; + private double recentStepEastMeters; + private double recentStepNorthMeters; + private double headingBiasRad; private static final class Particle { double xEast; @@ -99,11 +107,16 @@ public synchronized void reset(double latDeg, double lonDeg, int initialFloor) { hasAnchor = true; fallbackFloor = initialFloor; + headingBiasRad = 0.0; + recentStepEastMeters = 0.0; + recentStepNorthMeters = 0.0; + recentStepMotionMeters = 0.0; initParticlesAtOrigin(initialFloor); if (DEBUG_LOGS) { Log.i(TAG, String.format(Locale.US, - "Reset anchor=(%.7f, %.7f) floor=%d particles=%d", - latDeg, lonDeg, initialFloor, PARTICLE_COUNT)); + "Reset anchor=(%.7f, %.7f) floor=%d particles=%d headingBiasDeg=%.2f", + latDeg, lonDeg, initialFloor, PARTICLE_COUNT, + Math.toDegrees(headingBiasRad))); } } @@ -112,14 +125,19 @@ public synchronized void updatePdrDisplacement(float dxEastMeters, float dyNorth return; } + recentStepEastMeters = dxEastMeters; + recentStepNorthMeters = dyNorthMeters; recentStepMotionMeters = Math.hypot(dxEastMeters, dyNorthMeters); + double[] correctedStep = rotateVector(dxEastMeters, dyNorthMeters, headingBiasRad); + double correctedDx = correctedStep[0]; + double correctedDy = correctedStep[1]; int blockedByWall = 0; for (Particle p : particles) { double oldX = p.xEast; double oldY = p.yNorth; - double candidateX = oldX + dxEastMeters + random.nextGaussian() * PDR_NOISE_STD_M; - double candidateY = oldY + dyNorthMeters + random.nextGaussian() * PDR_NOISE_STD_M; + double candidateX = oldX + correctedDx + random.nextGaussian() * PDR_NOISE_STD_M; + double candidateY = oldY + correctedDy + random.nextGaussian() * PDR_NOISE_STD_M; if (crossesWall(p.floor, oldX, oldY, candidateX, candidateY)) { blockedByWall++; @@ -132,8 +150,12 @@ public synchronized void updatePdrDisplacement(float dxEastMeters, float dyNorth if (DEBUG_LOGS) { Log.d(TAG, String.format(Locale.US, - "Predict dPDR=(%.2fE, %.2fN) noiseStd=%.2f blockedByWall=%d", - dxEastMeters, dyNorthMeters, PDR_NOISE_STD_M, blockedByWall)); + "Predict dPDRraw=(%.2fE, %.2fN) dPDRcorr=(%.2fE, %.2fN) headingBiasDeg=%.2f noiseStd=%.2f blockedByWall=%d", + dxEastMeters, dyNorthMeters, + correctedDx, correctedDy, + Math.toDegrees(headingBiasRad), + PDR_NOISE_STD_M, + blockedByWall)); } } @@ -325,6 +347,12 @@ private void applyAbsoluteFix(double latDeg, double lonDeg, double sigmaMeters, } double[] z = toLocal(latDeg, lonDeg); + double priorMeanEast = 0.0; + double priorMeanNorth = 0.0; + for (Particle p : particles) { + priorMeanEast += p.weight * p.xEast; + priorMeanNorth += p.weight * p.yNorth; + } if (floorHint != null) { injectFloorSupportIfNeeded(floorHint, z[0], z[1]); } @@ -381,10 +409,59 @@ private void applyAbsoluteFix(double latDeg, double lonDeg, double sigmaMeters, resampled = true; } + double innovationEast = z[0] - priorMeanEast; + double innovationNorth = z[1] - priorMeanNorth; + updateOrientationBiasFromInnovation(innovationEast, innovationNorth, floorHint == null ? "GNSS" : "WiFi"); + updateCounter++; logUpdateSummary(z[0], z[1], sigmaMeters, floorHint, effectiveBefore, effectiveN, resampled); } + private void updateOrientationBiasFromInnovation(double innovationEast, + double innovationNorth, + String source) { + if (recentStepMotionMeters < ORIENTATION_BIAS_MIN_STEP_M) { + return; + } + + double innovationNorm = Math.hypot(innovationEast, innovationNorth); + if (innovationNorm < ORIENTATION_BIAS_MIN_INNOVATION_M) { + return; + } + + double stepNorm2 = recentStepEastMeters * recentStepEastMeters + + recentStepNorthMeters * recentStepNorthMeters; + if (stepNorm2 < 1e-6) { + return; + } + + // cross(step, innovation) tells whether absolute fixes lie left/right of step heading. + double cross = recentStepEastMeters * innovationNorth + - recentStepNorthMeters * innovationEast; + double rawBiasDelta = ORIENTATION_BIAS_LEARN_RATE * (cross / stepNorm2); + double boundedBiasDelta = clamp(rawBiasDelta, + -ORIENTATION_BIAS_MAX_STEP_RAD, + ORIENTATION_BIAS_MAX_STEP_RAD); + + headingBiasRad = clamp(headingBiasRad + boundedBiasDelta, + -ORIENTATION_BIAS_MAX_ABS_RAD, + ORIENTATION_BIAS_MAX_ABS_RAD); + + if (DEBUG_LOGS) { + Log.d(TAG, String.format(Locale.US, + "HeadingBias update src=%s innovation=(%.2fE,%.2fN)|%.2fm step=(%.2fE,%.2fN)|%.2fm deltaDeg=%.2f biasDeg=%.2f", + source, + innovationEast, + innovationNorth, + innovationNorm, + recentStepEastMeters, + recentStepNorthMeters, + recentStepMotionMeters, + Math.toDegrees(boundedBiasDelta), + Math.toDegrees(headingBiasRad))); + } + } + private void injectFloorSupportIfNeeded(int floorHint, double zEast, double zNorth) { double floorSupport = floorSupportWeight(floorHint); if (floorSupport >= FLOOR_HINT_MIN_SUPPORT) { @@ -496,6 +573,18 @@ private void roughenParticles() { } } + private static double[] rotateVector(double east, double north, double angleRad) { + double cos = Math.cos(angleRad); + double sin = Math.sin(angleRad); + double rotatedEast = east * cos - north * sin; + double rotatedNorth = east * sin + north * cos; + return new double[]{rotatedEast, rotatedNorth}; + } + + private static double clamp(double value, double min, double max) { + return Math.max(min, Math.min(max, value)); + } + private double[] toLocal(double latDeg, double lonDeg) { double lat0Rad = Math.toRadians(anchorLatDeg); double dLat = Math.toRadians(latDeg - anchorLatDeg); From 62ee10741f30e21ca619656e5c0c3fed80e46eb0 Mon Sep 17 00:00:00 2001 From: HuangerzJ <125708330+HuangerzJ@users.noreply.github.com> Date: Sat, 28 Mar 2026 14:25:32 +0000 Subject: [PATCH 14/52] Removed GNSS downweighting and Floor injection support --- .../sensors/PositionFusionEngine.java | 56 +------------------ 1 file changed, 2 insertions(+), 54 deletions(-) diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java index eb6d4589..d0a360ea 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java @@ -32,11 +32,6 @@ public class PositionFusionEngine { private static final double INIT_STD_M = 2.0; private static final double ROUGHEN_STD_M = 0.15; private static final double WIFI_SIGMA_M = 5.5; - private static final double INDOOR_GNSS_SIGMA_MULTIPLIER = 2.0; - private static final double INDOOR_GNSS_MIN_SIGMA_M = 10.0; - private static final double FLOOR_HINT_MIN_SUPPORT = 0.08; - private static final double FLOOR_HINT_INJECTION_FRACTION = 0.25; - private static final double FLOOR_HINT_INJECTION_STD_M = 1.2; private static final double EPS = 1e-300; private static final double CONNECTOR_RADIUS_M = 3.0; private static final double LIFT_HORIZONTAL_MAX_M = 0.50; @@ -161,14 +156,10 @@ public synchronized void updatePdrDisplacement(float dxEastMeters, float dyNorth public synchronized void updateGnss(double latDeg, double lonDeg, float accuracyMeters) { double sigma = Math.max(accuracyMeters, 3.0f); - boolean indoors = activeBuildingName != null && !activeBuildingName.isEmpty(); - if (indoors) { - sigma = Math.max(sigma * INDOOR_GNSS_SIGMA_MULTIPLIER, INDOOR_GNSS_MIN_SIGMA_M); - } if (DEBUG_LOGS) { Log.d(TAG, String.format(Locale.US, - "GNSS update lat=%.7f lon=%.7f acc=%.2f sigma=%.2f indoors=%s", - latDeg, lonDeg, accuracyMeters, sigma, String.valueOf(indoors))); + "GNSS update lat=%.7f lon=%.7f acc=%.2f sigma=%.2f", + latDeg, lonDeg, accuracyMeters, sigma)); } applyAbsoluteFix(latDeg, lonDeg, sigma, null); } @@ -353,9 +344,6 @@ private void applyAbsoluteFix(double latDeg, double lonDeg, double sigmaMeters, priorMeanEast += p.weight * p.xEast; priorMeanNorth += p.weight * p.yNorth; } - if (floorHint != null) { - injectFloorSupportIfNeeded(floorHint, z[0], z[1]); - } double effectiveBefore = computeEffectiveSampleSize(); double sigma2 = sigmaMeters * sigmaMeters; @@ -462,46 +450,6 @@ private void updateOrientationBiasFromInnovation(double innovationEast, } } - private void injectFloorSupportIfNeeded(int floorHint, double zEast, double zNorth) { - double floorSupport = floorSupportWeight(floorHint); - if (floorSupport >= FLOOR_HINT_MIN_SUPPORT) { - return; - } - - int injectCount = Math.max(1, - (int) Math.round(PARTICLE_COUNT * FLOOR_HINT_INJECTION_FRACTION)); - List indices = new ArrayList<>(particles.size()); - for (int i = 0; i < particles.size(); i++) { - indices.add(i); - } - indices.sort((a, b) -> Double.compare(particles.get(a).weight, particles.get(b).weight)); - - for (int i = 0; i < injectCount && i < indices.size(); i++) { - Particle p = particles.get(indices.get(i)); - p.floor = floorHint; - p.xEast = zEast + random.nextGaussian() * FLOOR_HINT_INJECTION_STD_M; - p.yNorth = zNorth + random.nextGaussian() * FLOOR_HINT_INJECTION_STD_M; - } - - if (DEBUG_LOGS) { - Log.i(TAG, String.format(Locale.US, - "Floor support injection hint=%d supportBefore=%.3f injectCount=%d", - floorHint, - floorSupport, - injectCount)); - } - } - - private double floorSupportWeight(int floor) { - double sum = 0.0; - for (Particle p : particles) { - if (p.floor == floor) { - sum += p.weight; - } - } - return sum; - } - private void initParticlesAtOrigin(int initialFloor) { particles.clear(); double w = 1.0 / PARTICLE_COUNT; From a90de60db3c5102d5894239410f12040d52a9c57 Mon Sep 17 00:00:00 2001 From: HuangerzJ <125708330+HuangerzJ@users.noreply.github.com> Date: Sat, 28 Mar 2026 16:14:05 +0000 Subject: [PATCH 15/52] added a smoothing output filter --- .../sensors/PositionFusionEngine.java | 100 ++++++++++++++++-- 1 file changed, 93 insertions(+), 7 deletions(-) diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java index d0a360ea..f9652f54 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java @@ -32,11 +32,19 @@ public class PositionFusionEngine { private static final double INIT_STD_M = 2.0; private static final double ROUGHEN_STD_M = 0.15; private static final double WIFI_SIGMA_M = 5.5; + private static final double FLOOR_HINT_MIN_SUPPORT = 0.08; + private static final double FLOOR_HINT_INJECTION_FRACTION = 0.25; + private static final double FLOOR_HINT_INJECTION_STD_M = 1.2; + private static final double OUTLIER_GATE_SIGMA_MULT_GNSS = 2.8; + private static final double OUTLIER_GATE_SIGMA_MULT_WIFI = 2.3; + private static final double OUTLIER_GATE_MIN_M = 6.0; + private static final double MAX_OUTLIER_SIGMA_SCALE = 4.0; + private static final double OUTPUT_SMOOTHING_ALPHA = 0.20; private static final double EPS = 1e-300; private static final double CONNECTOR_RADIUS_M = 3.0; private static final double LIFT_HORIZONTAL_MAX_M = 0.50; - private static final double ORIENTATION_BIAS_LEARN_RATE = 0.20; - private static final double ORIENTATION_BIAS_MAX_STEP_RAD = Math.toRadians(6.0); + private static final double ORIENTATION_BIAS_LEARN_RATE = 0.10; + private static final double ORIENTATION_BIAS_MAX_STEP_RAD = Math.toRadians(4.0); private static final double ORIENTATION_BIAS_MAX_ABS_RAD = Math.toRadians(45.0); private static final double ORIENTATION_BIAS_MIN_STEP_M = 0.35; private static final double ORIENTATION_BIAS_MIN_INNOVATION_M = 0.50; @@ -58,6 +66,9 @@ public class PositionFusionEngine { private double recentStepEastMeters; private double recentStepNorthMeters; private double headingBiasRad; + private double smoothedEastMeters; + private double smoothedNorthMeters; + private boolean hasSmoothedEstimate; private static final class Particle { double xEast; @@ -106,6 +117,7 @@ public synchronized void reset(double latDeg, double lonDeg, int initialFloor) { recentStepEastMeters = 0.0; recentStepNorthMeters = 0.0; recentStepMotionMeters = 0.0; + hasSmoothedEstimate = false; initParticlesAtOrigin(initialFloor); if (DEBUG_LOGS) { Log.i(TAG, String.format(Locale.US, @@ -323,7 +335,16 @@ public synchronized PositionFusionEstimate getEstimate() { } } - LatLng latLng = toLatLng(meanX, meanY); + if (!hasSmoothedEstimate) { + smoothedEastMeters = meanX; + smoothedNorthMeters = meanY; + hasSmoothedEstimate = true; + } else { + smoothedEastMeters += OUTPUT_SMOOTHING_ALPHA * (meanX - smoothedEastMeters); + smoothedNorthMeters += OUTPUT_SMOOTHING_ALPHA * (meanY - smoothedNorthMeters); + } + + LatLng latLng = toLatLng(smoothedEastMeters, smoothedNorthMeters); return new PositionFusionEstimate(latLng, bestFloor, true); } @@ -344,9 +365,36 @@ private void applyAbsoluteFix(double latDeg, double lonDeg, double sigmaMeters, priorMeanEast += p.weight * p.xEast; priorMeanNorth += p.weight * p.yNorth; } + + if (floorHint != null) { + injectFloorSupportIfNeeded(floorHint, z[0], z[1]); + } + + double innovationEast = z[0] - priorMeanEast; + double innovationNorth = z[1] - priorMeanNorth; + double innovationDistance = Math.hypot(innovationEast, innovationNorth); + double gateSigmaMultiplier = floorHint == null + ? OUTLIER_GATE_SIGMA_MULT_GNSS + : OUTLIER_GATE_SIGMA_MULT_WIFI; + double gateMeters = Math.max(gateSigmaMultiplier * sigmaMeters, OUTLIER_GATE_MIN_M); + double effectiveSigma = sigmaMeters; + if (innovationDistance > gateMeters) { + double sigmaScale = Math.min(innovationDistance / gateMeters, MAX_OUTLIER_SIGMA_SCALE); + effectiveSigma = sigmaMeters * sigmaScale; + if (DEBUG_LOGS) { + Log.w(TAG, String.format(Locale.US, + "Outlier damping src=%s innovation=%.2fm gate=%.2fm sigma %.2f->%.2f", + floorHint == null ? "GNSS" : "WiFi", + innovationDistance, + gateMeters, + sigmaMeters, + effectiveSigma)); + } + } + double effectiveBefore = computeEffectiveSampleSize(); - double sigma2 = sigmaMeters * sigmaMeters; + double sigma2 = effectiveSigma * effectiveSigma; double maxLogWeight = Double.NEGATIVE_INFINITY; double[] logWeights = new double[particles.size()]; @@ -397,12 +445,10 @@ private void applyAbsoluteFix(double latDeg, double lonDeg, double sigmaMeters, resampled = true; } - double innovationEast = z[0] - priorMeanEast; - double innovationNorth = z[1] - priorMeanNorth; updateOrientationBiasFromInnovation(innovationEast, innovationNorth, floorHint == null ? "GNSS" : "WiFi"); updateCounter++; - logUpdateSummary(z[0], z[1], sigmaMeters, floorHint, effectiveBefore, effectiveN, resampled); + logUpdateSummary(z[0], z[1], effectiveSigma, floorHint, effectiveBefore, effectiveN, resampled); } private void updateOrientationBiasFromInnovation(double innovationEast, @@ -450,6 +496,46 @@ private void updateOrientationBiasFromInnovation(double innovationEast, } } + private void injectFloorSupportIfNeeded(int floorHint, double zEast, double zNorth) { + double floorSupport = floorSupportWeight(floorHint); + if (floorSupport >= FLOOR_HINT_MIN_SUPPORT) { + return; + } + + int injectCount = Math.max(1, + (int) Math.round(PARTICLE_COUNT * FLOOR_HINT_INJECTION_FRACTION)); + List indices = new ArrayList<>(particles.size()); + for (int i = 0; i < particles.size(); i++) { + indices.add(i); + } + indices.sort((a, b) -> Double.compare(particles.get(a).weight, particles.get(b).weight)); + + for (int i = 0; i < injectCount && i < indices.size(); i++) { + Particle p = particles.get(indices.get(i)); + p.floor = floorHint; + p.xEast = zEast + random.nextGaussian() * FLOOR_HINT_INJECTION_STD_M; + p.yNorth = zNorth + random.nextGaussian() * FLOOR_HINT_INJECTION_STD_M; + } + + if (DEBUG_LOGS) { + Log.i(TAG, String.format(Locale.US, + "Floor support injection hint=%d supportBefore=%.3f injectCount=%d", + floorHint, + floorSupport, + injectCount)); + } + } + + private double floorSupportWeight(int floor) { + double sum = 0.0; + for (Particle p : particles) { + if (p.floor == floor) { + sum += p.weight; + } + } + return sum; + } + private void initParticlesAtOrigin(int initialFloor) { particles.clear(); double w = 1.0 / PARTICLE_COUNT; From 52ed55a31a041360d7f05af5402ee43dea97f978 Mon Sep 17 00:00:00 2001 From: HuangerzJ <125708330+HuangerzJ@users.noreply.github.com> Date: Sat, 28 Mar 2026 16:23:31 +0000 Subject: [PATCH 16/52] re-added gnss downweighting --- .../PositionMe/sensors/PositionFusionEngine.java | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java index f9652f54..2ea2f957 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java @@ -32,6 +32,8 @@ public class PositionFusionEngine { private static final double INIT_STD_M = 2.0; private static final double ROUGHEN_STD_M = 0.15; private static final double WIFI_SIGMA_M = 5.5; + private static final double INDOOR_GNSS_SIGMA_MULTIPLIER = 2.0; + private static final double INDOOR_GNSS_MIN_SIGMA_M = 10.0; private static final double FLOOR_HINT_MIN_SUPPORT = 0.08; private static final double FLOOR_HINT_INJECTION_FRACTION = 0.25; private static final double FLOOR_HINT_INJECTION_STD_M = 1.2; @@ -168,10 +170,14 @@ public synchronized void updatePdrDisplacement(float dxEastMeters, float dyNorth public synchronized void updateGnss(double latDeg, double lonDeg, float accuracyMeters) { double sigma = Math.max(accuracyMeters, 3.0f); + boolean indoors = activeBuildingName != null && !activeBuildingName.isEmpty(); + if (indoors) { + sigma = Math.max(sigma * INDOOR_GNSS_SIGMA_MULTIPLIER, INDOOR_GNSS_MIN_SIGMA_M); + } if (DEBUG_LOGS) { Log.d(TAG, String.format(Locale.US, - "GNSS update lat=%.7f lon=%.7f acc=%.2f sigma=%.2f", - latDeg, lonDeg, accuracyMeters, sigma)); + "GNSS update lat=%.7f lon=%.7f acc=%.2f sigma=%.2f indoors=%s", + latDeg, lonDeg, accuracyMeters, sigma, String.valueOf(indoors))); } applyAbsoluteFix(latDeg, lonDeg, sigma, null); } From 7133efba00bb3119dc430dc55ecdcd5e7494baf1 Mon Sep 17 00:00:00 2001 From: HuangerzJ <125708330+HuangerzJ@users.noreply.github.com> Date: Sat, 28 Mar 2026 16:52:16 +0000 Subject: [PATCH 17/52] Removed GNSS downweight again --- .../PositionMe/sensors/PositionFusionEngine.java | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java index 2ea2f957..f9652f54 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java @@ -32,8 +32,6 @@ public class PositionFusionEngine { private static final double INIT_STD_M = 2.0; private static final double ROUGHEN_STD_M = 0.15; private static final double WIFI_SIGMA_M = 5.5; - private static final double INDOOR_GNSS_SIGMA_MULTIPLIER = 2.0; - private static final double INDOOR_GNSS_MIN_SIGMA_M = 10.0; private static final double FLOOR_HINT_MIN_SUPPORT = 0.08; private static final double FLOOR_HINT_INJECTION_FRACTION = 0.25; private static final double FLOOR_HINT_INJECTION_STD_M = 1.2; @@ -170,14 +168,10 @@ public synchronized void updatePdrDisplacement(float dxEastMeters, float dyNorth public synchronized void updateGnss(double latDeg, double lonDeg, float accuracyMeters) { double sigma = Math.max(accuracyMeters, 3.0f); - boolean indoors = activeBuildingName != null && !activeBuildingName.isEmpty(); - if (indoors) { - sigma = Math.max(sigma * INDOOR_GNSS_SIGMA_MULTIPLIER, INDOOR_GNSS_MIN_SIGMA_M); - } if (DEBUG_LOGS) { Log.d(TAG, String.format(Locale.US, - "GNSS update lat=%.7f lon=%.7f acc=%.2f sigma=%.2f indoors=%s", - latDeg, lonDeg, accuracyMeters, sigma, String.valueOf(indoors))); + "GNSS update lat=%.7f lon=%.7f acc=%.2f sigma=%.2f", + latDeg, lonDeg, accuracyMeters, sigma)); } applyAbsoluteFix(latDeg, lonDeg, sigma, null); } From 6700ca2d3f2f0e710c7a110fe005a6acec8a45ab Mon Sep 17 00:00:00 2001 From: tommyj0 Date: Sat, 28 Mar 2026 17:36:34 +0000 Subject: [PATCH 18/52] removed redundant pdr feedback --- .../fragment/TrajectoryMapFragment.java | 78 ++++++++++++++++--- .../sensors/SensorEventHandler.java | 5 +- .../PositionMe/sensors/SensorFusion.java | 26 ------- .../PositionMe/utils/PdrProcessing.java | 63 +-------------- .../res/layout/fragment_trajectory_map.xml | 1 + 5 files changed, 76 insertions(+), 97 deletions(-) diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java index a6f52490..6033c8c9 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java +++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java @@ -61,6 +61,8 @@ public class TrajectoryMapFragment extends Fragment { private static final long TRAJECTORY_APPEND_MIN_INTERVAL_MS = 1000; private static final double TRAJECTORY_APPEND_MIN_METERS = 0.60; private static final double OBSERVATION_CIRCLE_RADIUS_M = 1.4; + private static final long GNSS_OBSERVATION_TTL_MS = 15000; + private static final long WIFI_OBSERVATION_TTL_MS = 20000; private GoogleMap gMap; // Google Maps instance private LatLng currentLocation; // Stores the user's current location @@ -71,6 +73,9 @@ public class TrajectoryMapFragment extends Fragment { private final List gnssObservationCircles = new ArrayList<>(); private final List wifiObservationCircles = new ArrayList<>(); private final List pdrObservationCircles = new ArrayList<>(); + private final List gnssObservationTimesMs = new ArrayList<>(); + private final List wifiObservationTimesMs = new ArrayList<>(); + private final List pdrObservationTimesMs = new ArrayList<>(); private Polyline polyline; // Polyline representing user's movement path private boolean isRed = true; // Tracks whether the polyline color is red @@ -202,6 +207,9 @@ public void onMapReady(@NonNull GoogleMap googleMap) { stopAutoFloor(); } }); + if (autoFloorSwitch.isChecked()) { + startAutoFloor(); + } floorUpButton.setOnClickListener(v -> { // If user manually changes floor, turn off auto floor @@ -320,6 +328,9 @@ public void onNothingSelected(AdapterView parent) {} public void updateUserLocation(@NonNull LatLng newLocation, float orientation) { if (gMap == null) return; + // Expire stale GNSS/WiFi observation points even when no new fixes arrive. + pruneExpiredObservations(); + LatLng displayLocation = newLocation; // Keep track of current location @@ -421,10 +432,12 @@ public void updateGNSS(@NonNull LatLng gnssLocation) { addObservationMarker( gnssObservationCircles, + gnssObservationTimesMs, gnssLocation, Color.argb(220, 33, 150, 243), Color.argb(80, 33, 150, 243), - false); + false, + GNSS_OBSERVATION_TTL_MS); if (!isGnssOn) return; @@ -456,10 +469,12 @@ public void updateGNSS(@NonNull LatLng gnssLocation) { public void updateWiFiObservation(@NonNull LatLng wifiLocation) { addObservationMarker( wifiObservationCircles, + wifiObservationTimesMs, wifiLocation, Color.argb(220, 67, 160, 71), Color.argb(80, 67, 160, 71), - false); + false, + WIFI_OBSERVATION_TTL_MS); } /** @@ -468,10 +483,12 @@ public void updateWiFiObservation(@NonNull LatLng wifiLocation) { public void updatePdrObservation(@NonNull LatLng pdrLocation) { addObservationMarker( pdrObservationCircles, + pdrObservationTimesMs, pdrLocation, Color.argb(220, 251, 140, 0), Color.argb(80, 251, 140, 0), - false); + false, + Long.MAX_VALUE); } @@ -514,7 +531,7 @@ private void updateFloorLabel() { public void clearMapAndReset() { stopAutoFloor(); if (autoFloorSwitch != null) { - autoFloorSwitch.setChecked(false); + autoFloorSwitch.setChecked(true); } if (polyline != null) { polyline.remove(); @@ -588,10 +605,12 @@ private void maybeAppendTrajectoryPoint(@Nullable LatLng oldLocation, } private void addObservationMarker(@NonNull List bucket, + @NonNull List timesBucket, @NonNull LatLng location, int strokeColor, int fillColor, - boolean respectGnssSwitch) { + boolean respectGnssSwitch, + long ttlMs) { if (gMap == null) { return; } @@ -599,6 +618,8 @@ private void addObservationMarker(@NonNull List bucket, return; } + pruneExpiredObservationCircles(bucket, timesBucket, ttlMs); + Circle circle = gMap.addCircle(new CircleOptions() .center(location) .radius(OBSERVATION_CIRCLE_RADIUS_M) @@ -612,23 +633,62 @@ private void addObservationMarker(@NonNull List bucket, } bucket.add(circle); + timesBucket.add(SystemClock.elapsedRealtime()); while (bucket.size() > MAX_OBSERVATION_MARKERS) { Circle stale = bucket.remove(0); stale.remove(); + if (!timesBucket.isEmpty()) { + timesBucket.remove(0); + } + } + } + + private void pruneExpiredObservations() { + long now = SystemClock.elapsedRealtime(); + pruneExpiredObservationCircles(gnssObservationCircles, gnssObservationTimesMs, + GNSS_OBSERVATION_TTL_MS, now); + pruneExpiredObservationCircles(wifiObservationCircles, wifiObservationTimesMs, + WIFI_OBSERVATION_TTL_MS, now); + } + + private void pruneExpiredObservationCircles(@NonNull List bucket, + @NonNull List timesBucket, + long ttlMs) { + pruneExpiredObservationCircles(bucket, timesBucket, ttlMs, SystemClock.elapsedRealtime()); + } + + private void pruneExpiredObservationCircles(@NonNull List bucket, + @NonNull List timesBucket, + long ttlMs, + long nowMs) { + if (ttlMs == Long.MAX_VALUE) { + return; + } + + while (!bucket.isEmpty() && !timesBucket.isEmpty()) { + long ageMs = nowMs - timesBucket.get(0); + if (ageMs <= ttlMs) { + break; + } + Circle stale = bucket.remove(0); + stale.remove(); + timesBucket.remove(0); } } private void clearObservationMarkers() { - clearObservationCircles(gnssObservationCircles); - clearObservationCircles(wifiObservationCircles); - clearObservationCircles(pdrObservationCircles); + clearObservationCircles(gnssObservationCircles, gnssObservationTimesMs); + clearObservationCircles(wifiObservationCircles, wifiObservationTimesMs); + clearObservationCircles(pdrObservationCircles, pdrObservationTimesMs); } - private void clearObservationCircles(@NonNull List bucket) { + private void clearObservationCircles(@NonNull List bucket, + @NonNull List timesBucket) { for (Circle c : bucket) { c.remove(); } bucket.clear(); + timesBucket.clear(); } /** diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java index e2688c99..d9adc9f5 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java @@ -221,13 +221,12 @@ public void handleSensorEvent(SensorEvent sensorEvent) { this.accelMagnitude, state.orientation[0] ); - float[] correctionDelta = this.pdrProcessing.consumePendingFeedbackDelta(); float dx = 0f; float dy = 0f; if (hasPdrReference) { - dx = newCords[0] - lastPdrX - correctionDelta[0]; - dy = newCords[1] - lastPdrY - correctionDelta[1]; + dx = newCords[0] - lastPdrX; + dy = newCords[1] - lastPdrY; } lastPdrX = newCords[0]; lastPdrY = newCords[1]; diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java index 9f420579..1a3b0f7f 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -24,7 +24,6 @@ import com.openpositioning.PositionMe.utils.PathView; import com.openpositioning.PositionMe.utils.PdrProcessing; import com.openpositioning.PositionMe.utils.TrajectoryValidator; -import com.openpositioning.PositionMe.utils.UtilFunctions; import com.openpositioning.PositionMe.data.remote.ServerCommunications; import java.util.ArrayList; @@ -54,7 +53,6 @@ public class SensorFusion implements SensorEventListener { //region Static variables private static final SensorFusion sensorFusion = new SensorFusion(); private static final String TAG = "SensorFusion"; - private static final boolean ENABLE_PDR_FEEDBACK = false; //endregion //region Instance variables @@ -727,30 +725,6 @@ private void updateFusedState() { state.fusedLongitude = (float) estimate.getLatLng().longitude; state.fusedFloor = estimate.getFloor(); state.fusedAvailable = true; - - // Feed the fused absolute estimate back into local PDR coordinates to limit long-term drift. - if (ENABLE_PDR_FEEDBACK) { - applyFusionFeedbackToPdr(estimate.getLatLng()); - } - } - - private void applyFusionFeedbackToPdr(@NonNull LatLng fusedLatLng) { - if (pdrProcessing == null) { - return; - } - - float startLat = state.startLocation[0]; - float startLon = state.startLocation[1]; - if (startLat == 0f && startLon == 0f) { - return; - } - - double dLat = fusedLatLng.latitude - startLat; - double dLon = fusedLatLng.longitude - startLon; - - float targetY = (float) UtilFunctions.degreesToMetersLat(dLat); - float targetX = (float) UtilFunctions.degreesToMetersLng(dLon, startLat); - pdrProcessing.applyFusionFeedback(targetX, targetY); } //endregion diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java b/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java index 59cfcaf0..115d369c 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java @@ -38,10 +38,6 @@ public class PdrProcessing { // Threshold under which movement is considered non-existent private static final float epsilon = 0.18f; private static final int MIN_REQUIRED_SAMPLES = 2; - private static final float FUSION_FEEDBACK_GAIN_NEAR = 0.22f; - private static final float FUSION_FEEDBACK_GAIN_MEDIUM = 0.35f; - private static final float FUSION_FEEDBACK_GAIN_FAR = 0.50f; - private static final float FUSION_FEEDBACK_MAX_STEP_M = 1.8f; //endregion //region Instance variables @@ -56,10 +52,6 @@ public class PdrProcessing { // Current 2D position coordinates private float positionX; private float positionY; - private float correctionX; - private float correctionY; - private float pendingCorrectionX; - private float pendingCorrectionY; // Vertical movement calculation private Float[] startElevationBuffer; @@ -110,10 +102,6 @@ public PdrProcessing(Context context) { // Initial position and elevation - starts from zero this.positionX = 0f; this.positionY = 0f; - this.correctionX = 0f; - this.correctionY = 0f; - this.pendingCorrectionX = 0f; - this.pendingCorrectionY = 0f; this.elevation = 0f; @@ -153,7 +141,7 @@ public PdrProcessing(Context context) { */ public float[] updatePdr(long currentStepEnd, List accelMagnitudeOvertime, float headingRad) { if (accelMagnitudeOvertime == null || accelMagnitudeOvertime.size() < MIN_REQUIRED_SAMPLES) { - return new float[]{this.positionX + this.correctionX, this.positionY + this.correctionY}; + return new float[]{this.positionX, this.positionY}; // - TODO - temporary solution of the empty list issue } @@ -163,7 +151,7 @@ public float[] updatePdr(long currentStepEnd, List accelMagnitudeOvertim // check if accelMagnitudeOvertime is empty if (accelMagnitudeOvertime == null || accelMagnitudeOvertime.isEmpty()) { // return current position, do not update - return new float[]{this.positionX + this.correctionX, this.positionY + this.correctionY}; + return new float[]{this.positionX, this.positionY}; } // Calculate step length @@ -187,7 +175,7 @@ public float[] updatePdr(long currentStepEnd, List accelMagnitudeOvertim this.positionY += y; // return current position - return new float[]{this.positionX + this.correctionX, this.positionY + this.correctionY}; + return new float[]{this.positionX, this.positionY}; } /** @@ -280,50 +268,11 @@ private float weibergMinMax(List accelMagnitude) { * @return float array of size 2, with the X and Y coordinates respectively. */ public float[] getPDRMovement() { - float [] pdrPosition= new float[] {positionX + correctionX, positionY + correctionY}; + float [] pdrPosition= new float[] {positionX, positionY}; return pdrPosition; } - public void applyFusionFeedback(float targetX, float targetY) { - float correctedX = this.positionX + this.correctionX; - float correctedY = this.positionY + this.correctionY; - - float errorX = targetX - correctedX; - float errorY = targetY - correctedY; - - float errorDist = (float) Math.hypot(errorX, errorY); - float gain = FUSION_FEEDBACK_GAIN_NEAR; - if (errorDist >= 4.0f) { - gain = FUSION_FEEDBACK_GAIN_FAR; - } else if (errorDist >= 1.5f) { - gain = FUSION_FEEDBACK_GAIN_MEDIUM; - } - - float deltaX = clamp(errorX * gain, - -FUSION_FEEDBACK_MAX_STEP_M, - FUSION_FEEDBACK_MAX_STEP_M); - float deltaY = clamp(errorY * gain, - -FUSION_FEEDBACK_MAX_STEP_M, - FUSION_FEEDBACK_MAX_STEP_M); - - this.correctionX += deltaX; - this.correctionY += deltaY; - this.pendingCorrectionX += deltaX; - this.pendingCorrectionY += deltaY; - } - - public float[] consumePendingFeedbackDelta() { - float[] delta = new float[]{pendingCorrectionX, pendingCorrectionY}; - pendingCorrectionX = 0f; - pendingCorrectionY = 0f; - return delta; - } - - private static float clamp(float value, float min, float max) { - return Math.max(min, Math.min(max, value)); - } - /** * Get the current elevation as calculated by the PDR class. * @@ -421,10 +370,6 @@ public void resetPDR() { // Initial position and elevation - starts from zero this.positionX = 0f; this.positionY = 0f; - this.correctionX = 0f; - this.correctionY = 0f; - this.pendingCorrectionX = 0f; - this.pendingCorrectionY = 0f; this.elevation = 0f; if(this.settings.getBoolean("overwrite_constants", false)) { diff --git a/app/src/main/res/layout/fragment_trajectory_map.xml b/app/src/main/res/layout/fragment_trajectory_map.xml index 107108a7..adf57b1d 100644 --- a/app/src/main/res/layout/fragment_trajectory_map.xml +++ b/app/src/main/res/layout/fragment_trajectory_map.xml @@ -41,6 +41,7 @@ android:id="@+id/autoFloor" android:layout_width="wrap_content" android:layout_height="wrap_content" + android:checked="true" android:text="@string/auto_floor" /> Date: Sat, 28 Mar 2026 17:40:28 +0000 Subject: [PATCH 19/52] autofloor is on by default --- .../presentation/fragment/TrajectoryMapFragment.java | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java index 6033c8c9..513b7254 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java +++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java @@ -164,6 +164,10 @@ public void onMapReady(@NonNull GoogleMap googleMap) { drawBuildingPolygon(); + if (autoFloorSwitch != null && autoFloorSwitch.isChecked()) { + startAutoFloor(); + } + Log.d("TrajectoryMapFragment", "onMapReady: Map is ready!"); @@ -533,6 +537,7 @@ public void clearMapAndReset() { if (autoFloorSwitch != null) { autoFloorSwitch.setChecked(true); } + startAutoFloor(); if (polyline != null) { polyline.remove(); polyline = null; @@ -791,6 +796,9 @@ private void drawBuildingPolygon() { * of consistent readings). */ private void startAutoFloor() { + if (autoFloorHandler != null && autoFloorTask != null) { + return; + } if (autoFloorHandler == null) { autoFloorHandler = new Handler(Looper.getMainLooper()); } @@ -844,6 +852,7 @@ private void stopAutoFloor() { if (autoFloorHandler != null && autoFloorTask != null) { autoFloorHandler.removeCallbacks(autoFloorTask); } + autoFloorTask = null; lastCandidateFloor = Integer.MIN_VALUE; lastCandidateTime = 0; Log.d(TAG, "Auto-floor stopped"); From 68052e9983393d82b13fbd5cf45053ee72753c7d Mon Sep 17 00:00:00 2001 From: tommyj0 Date: Sun, 29 Mar 2026 13:54:23 +0100 Subject: [PATCH 20/52] local pdr updates --- .../fragment/TrajectoryMapFragment.java | 40 ++++++++++++++++++- 1 file changed, 39 insertions(+), 1 deletion(-) diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java index 513b7254..58db29a0 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java +++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java @@ -86,6 +86,8 @@ public class TrajectoryMapFragment extends Fragment { private Polyline gnssPolyline; // Polyline for GNSS path private LatLng lastGnssLocation = null; // Stores the last GNSS location + private LatLng lastPdrLocationForViz = null; // Previous PDR position for delta calculation + private LatLng lastFusionLocationForViz = null; // Previous fusion position for delta calculation private LatLng pendingCameraPosition = null; // Stores pending camera movement private boolean hasPendingCameraMove = false; // Tracks if camera needs to move @@ -483,16 +485,50 @@ public void updateWiFiObservation(@NonNull LatLng wifiLocation) { /** * Adds a PDR observation marker (last N points). + * Visualized as: currentLocation + (delta_pdr - delta_fusion) to show only the + * current discrepancy between PDR and fusion, without cumulative drift. */ public void updatePdrObservation(@NonNull LatLng pdrLocation) { + if (currentLocation == null) return; + + // Calculate deltas from last positions + double pdrDeltaLat = 0; + double pdrDeltaLng = 0; + double fusionDeltaLat = 0; + double fusionDeltaLng = 0; + + if (lastPdrLocationForViz != null) { + pdrDeltaLat = pdrLocation.latitude - lastPdrLocationForViz.latitude; + pdrDeltaLng = pdrLocation.longitude - lastPdrLocationForViz.longitude; + } + + if (lastFusionLocationForViz != null) { + fusionDeltaLat = currentLocation.latitude - lastFusionLocationForViz.latitude; + fusionDeltaLng = currentLocation.longitude - lastFusionLocationForViz.longitude; + } + + // PDR discrepancy: how much PDR moved vs fusion in this step + double discrepancyLat = pdrDeltaLat - fusionDeltaLat; + double discrepancyLng = pdrDeltaLng - fusionDeltaLng; + + // Visualize at: currentLocation + discrepancy + LatLng visualPdrLocation = new LatLng( + currentLocation.latitude + discrepancyLat, + currentLocation.longitude + discrepancyLng + ); + addObservationMarker( pdrObservationCircles, pdrObservationTimesMs, - pdrLocation, + visualPdrLocation, Color.argb(220, 251, 140, 0), Color.argb(80, 251, 140, 0), false, Long.MAX_VALUE); + + // Update tracking positions for next call + lastPdrLocationForViz = pdrLocation; + lastFusionLocationForViz = currentLocation; } @@ -538,6 +574,8 @@ public void clearMapAndReset() { autoFloorSwitch.setChecked(true); } startAutoFloor(); + lastPdrLocationForViz = null; + lastFusionLocationForViz = null; if (polyline != null) { polyline.remove(); polyline = null; From 34edc9f8594840e9ad4bbde0e2b8ebb7cf420a1c Mon Sep 17 00:00:00 2001 From: tommyj0 Date: Sun, 29 Mar 2026 15:00:40 +0100 Subject: [PATCH 21/52] dark mode implemented, including system default --- app/src/main/AndroidManifest.xml | 2 +- .../presentation/activity/MainActivity.java | 6 +-- .../activity/RecordingActivity.java | 2 + .../presentation/activity/ReplayActivity.java | 2 + .../fragment/SettingsFragment.java | 11 +++++ .../fragment/TrajectoryMapFragment.java | 3 +- .../PositionMe/utils/ThemePreferences.java | 48 +++++++++++++++++++ .../res/drawable/bg_map_switch_spinner.xml | 13 +++++ app/src/main/res/layout/activity_main.xml | 5 +- .../res/layout/fragment_trajectory_map.xml | 2 +- .../main/res/layout/item_map_type_spinner.xml | 13 +++++ .../layout/item_map_type_spinner_dropdown.xml | 14 ++++++ app/src/main/res/menu/menu_items.xml | 2 +- app/src/main/res/values-night/themes.xml | 4 +- app/src/main/res/values/arrays.xml | 12 +++++ app/src/main/res/values/strings.xml | 2 + app/src/main/res/values/themes.xml | 4 +- app/src/main/res/xml/root_preferences.xml | 14 ++++++ 18 files changed, 144 insertions(+), 15 deletions(-) create mode 100644 app/src/main/java/com/openpositioning/PositionMe/utils/ThemePreferences.java create mode 100644 app/src/main/res/drawable/bg_map_switch_spinner.xml create mode 100644 app/src/main/res/layout/item_map_type_spinner.xml create mode 100644 app/src/main/res/layout/item_map_type_spinner_dropdown.xml diff --git a/app/src/main/AndroidManifest.xml b/app/src/main/AndroidManifest.xml index 6398bce9..98eed0c5 100644 --- a/app/src/main/AndroidManifest.xml +++ b/app/src/main/AndroidManifest.xml @@ -85,7 +85,7 @@ android:label="@string/app_name" android:roundIcon="@mipmap/ic_launcher_icon_map_round" android:supportsRtl="true" - android:theme="@style/Theme.Material3.DayNight.NoActionBar" + android:theme="@style/Theme.App" android:requestLegacyExternalStorage="true" > diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/activity/MainActivity.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/activity/MainActivity.java index 3dde48cd..6d31aeb6 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/presentation/activity/MainActivity.java +++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/activity/MainActivity.java @@ -15,7 +15,6 @@ import androidx.annotation.NonNull; import androidx.appcompat.app.AlertDialog; import androidx.appcompat.app.AppCompatActivity; -import androidx.appcompat.app.AppCompatDelegate; import androidx.appcompat.widget.Toolbar; import androidx.core.content.ContextCompat; @@ -34,6 +33,7 @@ import com.openpositioning.PositionMe.sensors.SensorFusion; import com.openpositioning.PositionMe.service.SensorCollectionService; import com.openpositioning.PositionMe.utils.PermissionManager; +import com.openpositioning.PositionMe.utils.ThemePreferences; import java.util.Objects; @@ -90,7 +90,7 @@ public class MainActivity extends AppCompatActivity implements Observer { @Override protected void onCreate(Bundle savedInstanceState) { super.onCreate(savedInstanceState); - AppCompatDelegate.setDefaultNightMode(AppCompatDelegate.MODE_NIGHT_NO); + ThemePreferences.applyThemeFromPreferences(this); setContentView(R.layout.activity_main); // Set up navigation and fragments @@ -102,8 +102,6 @@ protected void onCreate(Bundle savedInstanceState) { Toolbar toolbar = findViewById(R.id.main_toolbar); setSupportActionBar(toolbar); toolbar.showOverflowMenu(); - toolbar.setBackgroundColor(ContextCompat.getColor(getApplicationContext(), R.color.md_theme_light_surface)); - toolbar.setTitleTextColor(ContextCompat.getColor(getApplicationContext(), R.color.black)); toolbar.setNavigationIcon(R.drawable.ic_baseline_back_arrow); // Set up back action with NavigationUI diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/activity/RecordingActivity.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/activity/RecordingActivity.java index 9497848e..c739998a 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/presentation/activity/RecordingActivity.java +++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/activity/RecordingActivity.java @@ -16,6 +16,7 @@ import com.openpositioning.PositionMe.presentation.fragment.StartLocationFragment; import com.openpositioning.PositionMe.presentation.fragment.RecordingFragment; import com.openpositioning.PositionMe.presentation.fragment.CorrectionFragment; +import com.openpositioning.PositionMe.utils.ThemePreferences; /** @@ -48,6 +49,7 @@ public class RecordingActivity extends AppCompatActivity { @Override protected void onCreate(@Nullable Bundle savedInstanceState) { super.onCreate(savedInstanceState); + ThemePreferences.applyThemeFromPreferences(this); setContentView(R.layout.activity_recording); if (savedInstanceState == null) { diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/activity/ReplayActivity.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/activity/ReplayActivity.java index c6a30472..438984e6 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/presentation/activity/ReplayActivity.java +++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/activity/ReplayActivity.java @@ -10,6 +10,7 @@ import com.openpositioning.PositionMe.R; import com.openpositioning.PositionMe.presentation.fragment.ReplayFragment; import com.openpositioning.PositionMe.presentation.fragment.StartLocationFragment; +import com.openpositioning.PositionMe.utils.ThemePreferences; /** @@ -51,6 +52,7 @@ public class ReplayActivity extends AppCompatActivity { @Override protected void onCreate(@Nullable Bundle savedInstanceState) { super.onCreate(savedInstanceState); + ThemePreferences.applyThemeFromPreferences(this); setContentView(R.layout.activity_replay); // Get the trajectory file path from the Intent filePath = getIntent().getStringExtra(EXTRA_TRAJECTORY_FILE_PATH); diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/SettingsFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/SettingsFragment.java index c1f6501c..f2d983d3 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/SettingsFragment.java +++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/SettingsFragment.java @@ -4,9 +4,11 @@ import android.text.InputType; import androidx.preference.EditTextPreference; +import androidx.preference.ListPreference; import androidx.preference.PreferenceFragmentCompat; import com.openpositioning.PositionMe.R; +import com.openpositioning.PositionMe.utils.ThemePreferences; /** * SettingsFragment that inflates and displays the preferences (settings). @@ -25,6 +27,7 @@ public class SettingsFragment extends PreferenceFragmentCompat { private EditTextPreference epsilon; private EditTextPreference accelFilter; private EditTextPreference wifiInterval; + private ListPreference themeMode; /** * {@inheritDoc} @@ -53,5 +56,13 @@ public void onCreatePreferences(Bundle savedInstanceState, String rootKey) { wifiInterval.setOnBindEditTextListener(editText -> editText.setInputType( InputType.TYPE_CLASS_NUMBER)); + themeMode = findPreference(ThemePreferences.KEY_THEME_MODE); + if (themeMode != null) { + themeMode.setOnPreferenceChangeListener((preference, newValue) -> { + ThemePreferences.applyThemeMode(String.valueOf(newValue)); + return true; + }); + } + } } \ No newline at end of file diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java index 58db29a0..40bca82f 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java +++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java @@ -297,9 +297,10 @@ private void initMapTypeSpinner() { }; ArrayAdapter adapter = new ArrayAdapter<>( requireContext(), - android.R.layout.simple_spinner_dropdown_item, + R.layout.item_map_type_spinner, maps ); + adapter.setDropDownViewResource(R.layout.item_map_type_spinner_dropdown); switchMapSpinner.setAdapter(adapter); switchMapSpinner.setOnItemSelectedListener(new AdapterView.OnItemSelectedListener() { diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/ThemePreferences.java b/app/src/main/java/com/openpositioning/PositionMe/utils/ThemePreferences.java new file mode 100644 index 00000000..88d18a9e --- /dev/null +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/ThemePreferences.java @@ -0,0 +1,48 @@ +package com.openpositioning.PositionMe.utils; + +import android.content.Context; +import android.content.SharedPreferences; + +import androidx.appcompat.app.AppCompatDelegate; +import androidx.preference.PreferenceManager; + +public final class ThemePreferences { + + public static final String KEY_THEME_MODE = "theme_mode"; + public static final String THEME_LIGHT = "light"; + public static final String THEME_DARK = "dark"; + public static final String THEME_SYSTEM = "system"; + + private ThemePreferences() { + // Utility class + } + + public static void applyThemeFromPreferences(Context context) { + SharedPreferences prefs = PreferenceManager.getDefaultSharedPreferences(context); + String themeMode; + if (prefs.contains(KEY_THEME_MODE)) { + themeMode = prefs.getString(KEY_THEME_MODE, THEME_SYSTEM); + } else if (prefs.contains("dark_mode")) { + // Backward compatibility: migrate old boolean dark_mode if present. + boolean darkModeEnabled = prefs.getBoolean("dark_mode", false); + themeMode = darkModeEnabled ? THEME_DARK : THEME_LIGHT; + prefs.edit().putString(KEY_THEME_MODE, themeMode).apply(); + } else { + // Fresh install or unset preference: follow system by default. + themeMode = THEME_SYSTEM; + prefs.edit().putString(KEY_THEME_MODE, themeMode).apply(); + } + + applyThemeMode(themeMode); + } + + public static void applyThemeMode(String themeMode) { + if (THEME_DARK.equals(themeMode)) { + AppCompatDelegate.setDefaultNightMode(AppCompatDelegate.MODE_NIGHT_YES); + } else if (THEME_LIGHT.equals(themeMode)) { + AppCompatDelegate.setDefaultNightMode(AppCompatDelegate.MODE_NIGHT_NO); + } else { + AppCompatDelegate.setDefaultNightMode(AppCompatDelegate.MODE_NIGHT_FOLLOW_SYSTEM); + } + } +} diff --git a/app/src/main/res/drawable/bg_map_switch_spinner.xml b/app/src/main/res/drawable/bg_map_switch_spinner.xml new file mode 100644 index 00000000..397208c6 --- /dev/null +++ b/app/src/main/res/drawable/bg_map_switch_spinner.xml @@ -0,0 +1,13 @@ + + + + + + + diff --git a/app/src/main/res/layout/activity_main.xml b/app/src/main/res/layout/activity_main.xml index 0a3ceda2..12e91881 100644 --- a/app/src/main/res/layout/activity_main.xml +++ b/app/src/main/res/layout/activity_main.xml @@ -3,7 +3,6 @@ xmlns:android="http://schemas.android.com/apk/res/android" xmlns:app="http://schemas.android.com/apk/res-auto" xmlns:tools="http://schemas.android.com/tools" - android:theme="@style/Theme.AppCompat.Light.NoActionBar" android:layout_width="match_parent" android:layout_height="match_parent" tools:context=".presentation.activity.MainActivity"> @@ -13,10 +12,10 @@ android:id="@+id/main_toolbar" android:layout_width="0dp" android:layout_height="?attr/actionBarSize" - android:background="?attr/colorPrimary" + android:background="?attr/colorSurface" android:elevation="4dp" android:title="@string/app_name" - android:titleTextColor="@color/md_theme_light_onPrimary" + android:titleTextColor="?attr/colorOnSurface" app:layout_constraintTop_toTopOf="parent" app:layout_constraintStart_toStartOf="parent" app:layout_constraintEnd_toEndOf="parent" diff --git a/app/src/main/res/layout/fragment_trajectory_map.xml b/app/src/main/res/layout/fragment_trajectory_map.xml index adf57b1d..721c7a1e 100644 --- a/app/src/main/res/layout/fragment_trajectory_map.xml +++ b/app/src/main/res/layout/fragment_trajectory_map.xml @@ -59,7 +59,7 @@ android:layout_height="48dp" android:layout_marginStart="0dp" android:layout_marginTop="8dp" - android:background="@android:color/white" + android:background="@drawable/bg_map_switch_spinner" android:spinnerMode="dropdown" app:layout_constraintTop_toTopOf="parent" android:elevation="5dp"/> diff --git a/app/src/main/res/layout/item_map_type_spinner.xml b/app/src/main/res/layout/item_map_type_spinner.xml new file mode 100644 index 00000000..c0d7af01 --- /dev/null +++ b/app/src/main/res/layout/item_map_type_spinner.xml @@ -0,0 +1,13 @@ + + diff --git a/app/src/main/res/layout/item_map_type_spinner_dropdown.xml b/app/src/main/res/layout/item_map_type_spinner_dropdown.xml new file mode 100644 index 00000000..baed7f62 --- /dev/null +++ b/app/src/main/res/layout/item_map_type_spinner_dropdown.xml @@ -0,0 +1,14 @@ + + diff --git a/app/src/main/res/menu/menu_items.xml b/app/src/main/res/menu/menu_items.xml index 350b8441..7cb278ea 100644 --- a/app/src/main/res/menu/menu_items.xml +++ b/app/src/main/res/menu/menu_items.xml @@ -6,7 +6,7 @@ android:id="@+id/settingsFragment" android:icon="@drawable/ic_baseline_settings_24" android:title="@string/settings_title" - android:iconTint="@color/md_theme_light_onPrimary" + android:iconTint="?attr/colorOnSurface" app:showAsAction="ifRoom" /> diff --git a/app/src/main/res/values-night/themes.xml b/app/src/main/res/values-night/themes.xml index f4253019..65e81026 100644 --- a/app/src/main/res/values-night/themes.xml +++ b/app/src/main/res/values-night/themes.xml @@ -3,7 +3,7 @@ \ No newline at end of file diff --git a/app/src/main/res/values/arrays.xml b/app/src/main/res/values/arrays.xml index 2200d5e5..a5679d28 100644 --- a/app/src/main/res/values/arrays.xml +++ b/app/src/main/res/values/arrays.xml @@ -32,4 +32,16 @@ cell wifi + + + System default + Light + Dark + + + + system + light + dark + \ No newline at end of file diff --git a/app/src/main/res/values/strings.xml b/app/src/main/res/values/strings.xml index 125d27c3..65810a44 100644 --- a/app/src/main/res/values/strings.xml +++ b/app/src/main/res/values/strings.xml @@ -60,6 +60,8 @@ Settings + Appearance + Theme Sensors Sync User information diff --git a/app/src/main/res/values/themes.xml b/app/src/main/res/values/themes.xml index f4253019..65e81026 100644 --- a/app/src/main/res/values/themes.xml +++ b/app/src/main/res/values/themes.xml @@ -3,7 +3,7 @@ \ No newline at end of file diff --git a/app/src/main/res/xml/root_preferences.xml b/app/src/main/res/xml/root_preferences.xml index 65793d88..f053f1fc 100644 --- a/app/src/main/res/xml/root_preferences.xml +++ b/app/src/main/res/xml/root_preferences.xml @@ -1,6 +1,20 @@ + + + + + + From 39be0e5ce8aca81601d8d9143748601f251836a3 Mon Sep 17 00:00:00 2001 From: tommyj0 Date: Sun, 29 Mar 2026 15:13:20 +0100 Subject: [PATCH 22/52] removed indoor positioning button --- .../presentation/fragment/HomeFragment.java | 8 -------- .../main/res/layout-small/fragment_home.xml | 19 +------------------ app/src/main/res/layout/fragment_home.xml | 19 +------------------ 3 files changed, 2 insertions(+), 44 deletions(-) diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/HomeFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/HomeFragment.java index 654c4bfd..4d576657 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/HomeFragment.java +++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/HomeFragment.java @@ -12,7 +12,6 @@ import android.widget.Button; import android.widget.TextView; import android.util.Log; -import android.widget.Toast; import androidx.annotation.NonNull; import androidx.annotation.Nullable; @@ -54,7 +53,6 @@ public class HomeFragment extends Fragment implements OnMapReadyCallback { private Button start; private Button measurements; private Button files; - private Button indoorButton; private TextView gnssStatusTextView; // For the map @@ -121,12 +119,6 @@ public void onViewCreated(@NonNull View view, @Nullable Bundle savedInstanceStat Navigation.findNavController(v).navigate(action); }); - // Indoor Positioning button - indoorButton = view.findViewById(R.id.indoorButton); - indoorButton.setOnClickListener(v -> { - Toast.makeText(requireContext(), R.string.indoor_mode_hint, Toast.LENGTH_SHORT).show(); - }); - // TextView to display GNSS disabled message gnssStatusTextView = view.findViewById(R.id.gnssStatusTextView); diff --git a/app/src/main/res/layout-small/fragment_home.xml b/app/src/main/res/layout-small/fragment_home.xml index bd713b67..1964fa93 100644 --- a/app/src/main/res/layout-small/fragment_home.xml +++ b/app/src/main/res/layout-small/fragment_home.xml @@ -80,7 +80,7 @@ app:layout_constraintTop_toBottomOf="@id/mapFragmentContainer" app:layout_constraintStart_toStartOf="parent" app:layout_constraintEnd_toEndOf="parent" - app:layout_constraintBottom_toTopOf="@id/indoorButton"> + app:layout_constraintBottom_toBottomOf="parent"> - - - diff --git a/app/src/main/res/layout/fragment_home.xml b/app/src/main/res/layout/fragment_home.xml index 661afbb3..2f8e2d4c 100644 --- a/app/src/main/res/layout/fragment_home.xml +++ b/app/src/main/res/layout/fragment_home.xml @@ -81,7 +81,7 @@ app:layout_constraintTop_toBottomOf="@id/mapFragmentContainer" app:layout_constraintStart_toStartOf="parent" app:layout_constraintEnd_toEndOf="parent" - app:layout_constraintBottom_toTopOf="@id/indoorButton"> + app:layout_constraintBottom_toBottomOf="parent"> - - - From 6e77cd16b064399a3c4648992a1c3814c7595ef9 Mon Sep 17 00:00:00 2001 From: tommyj0 Date: Sun, 29 Mar 2026 15:53:05 +0100 Subject: [PATCH 23/52] removed floor injection --- .../sensors/PositionFusionEngine.java | 47 ------------------- 1 file changed, 47 deletions(-) diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java index f9652f54..ca8043c3 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java @@ -32,9 +32,6 @@ public class PositionFusionEngine { private static final double INIT_STD_M = 2.0; private static final double ROUGHEN_STD_M = 0.15; private static final double WIFI_SIGMA_M = 5.5; - private static final double FLOOR_HINT_MIN_SUPPORT = 0.08; - private static final double FLOOR_HINT_INJECTION_FRACTION = 0.25; - private static final double FLOOR_HINT_INJECTION_STD_M = 1.2; private static final double OUTLIER_GATE_SIGMA_MULT_GNSS = 2.8; private static final double OUTLIER_GATE_SIGMA_MULT_WIFI = 2.3; private static final double OUTLIER_GATE_MIN_M = 6.0; @@ -366,10 +363,6 @@ private void applyAbsoluteFix(double latDeg, double lonDeg, double sigmaMeters, priorMeanNorth += p.weight * p.yNorth; } - if (floorHint != null) { - injectFloorSupportIfNeeded(floorHint, z[0], z[1]); - } - double innovationEast = z[0] - priorMeanEast; double innovationNorth = z[1] - priorMeanNorth; double innovationDistance = Math.hypot(innovationEast, innovationNorth); @@ -496,46 +489,6 @@ private void updateOrientationBiasFromInnovation(double innovationEast, } } - private void injectFloorSupportIfNeeded(int floorHint, double zEast, double zNorth) { - double floorSupport = floorSupportWeight(floorHint); - if (floorSupport >= FLOOR_HINT_MIN_SUPPORT) { - return; - } - - int injectCount = Math.max(1, - (int) Math.round(PARTICLE_COUNT * FLOOR_HINT_INJECTION_FRACTION)); - List indices = new ArrayList<>(particles.size()); - for (int i = 0; i < particles.size(); i++) { - indices.add(i); - } - indices.sort((a, b) -> Double.compare(particles.get(a).weight, particles.get(b).weight)); - - for (int i = 0; i < injectCount && i < indices.size(); i++) { - Particle p = particles.get(indices.get(i)); - p.floor = floorHint; - p.xEast = zEast + random.nextGaussian() * FLOOR_HINT_INJECTION_STD_M; - p.yNorth = zNorth + random.nextGaussian() * FLOOR_HINT_INJECTION_STD_M; - } - - if (DEBUG_LOGS) { - Log.i(TAG, String.format(Locale.US, - "Floor support injection hint=%d supportBefore=%.3f injectCount=%d", - floorHint, - floorSupport, - injectCount)); - } - } - - private double floorSupportWeight(int floor) { - double sum = 0.0; - for (Particle p : particles) { - if (p.floor == floor) { - sum += p.weight; - } - } - return sum; - } - private void initParticlesAtOrigin(int initialFloor) { particles.clear(); double w = 1.0 / PARTICLE_COUNT; From 58197ccc88f7fd0ca2fee9c93beee01021959b89 Mon Sep 17 00:00:00 2001 From: tommyj0 Date: Sun, 29 Mar 2026 16:54:36 +0100 Subject: [PATCH 24/52] replay with corrected pos --- .../PositionMe/data/local/TrajParser.java | 78 +++++++++++++++---- .../PositionMe/sensors/SensorFusion.java | 12 +++ .../sensors/TrajectoryRecorder.java | 19 +++++ 3 files changed, 92 insertions(+), 17 deletions(-) diff --git a/app/src/main/java/com/openpositioning/PositionMe/data/local/TrajParser.java b/app/src/main/java/com/openpositioning/PositionMe/data/local/TrajParser.java index 2d2b1cbf..a2e2e01a 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/data/local/TrajParser.java +++ b/app/src/main/java/com/openpositioning/PositionMe/data/local/TrajParser.java @@ -104,6 +104,12 @@ private static class GnssRecord { public double latitude, longitude; // GNSS coordinates } + /** Represents a fused/corrected position record from protobuf corrected_positions. */ + private static class CorrectedRecord { + public long relativeTimestamp; + public double latitude, longitude; + } + /** * Parses trajectory data from a JSON file and reconstructs a list of replay points. * @@ -150,43 +156,71 @@ public static List parseTrajectoryData(String filePath, Context con List imuList = parseImuData(root.getAsJsonArray("imuData")); List pdrList = parsePdrData(root.getAsJsonArray("pdrData")); List gnssList = parseGnssData(root.getAsJsonArray("gnssData")); + JsonArray correctedArray = root.has("correctedPositions") + ? root.getAsJsonArray("correctedPositions") + : root.getAsJsonArray("corrected_positions"); + List correctedList = parseCorrectedData(correctedArray); Log.i(TAG, "Parsed data - IMU: " + imuList.size() + " records, PDR: " - + pdrList.size() + " records, GNSS: " + gnssList.size() + " records"); + + pdrList.size() + " records, GNSS: " + gnssList.size() + " records" + + ", Corrected: " + correctedList.size() + " records"); - for (int i = 0; i < pdrList.size(); i++) { - PdrRecord pdr = pdrList.get(i); + if (!correctedList.isEmpty()) { + for (int i = 0; i < correctedList.size(); i++) { + CorrectedRecord corrected = correctedList.get(i); - ImuRecord closestImu = findClosestImuRecord(imuList, pdr.relativeTimestamp); - float orientationDeg = closestImu != null ? computeOrientationFromRotationVector( + ImuRecord closestImu = findClosestImuRecord(imuList, corrected.relativeTimestamp); + float orientationDeg = closestImu != null ? computeOrientationFromRotationVector( closestImu.rotationVectorX, closestImu.rotationVectorY, closestImu.rotationVectorZ, closestImu.rotationVectorW, context - ) : 0f; + ) : 0f; + + LatLng correctedLocation = new LatLng(corrected.latitude, corrected.longitude); + + GnssRecord closestGnss = findClosestGnssRecord(gnssList, corrected.relativeTimestamp); + LatLng gnssLocation = closestGnss != null ? + new LatLng(closestGnss.latitude, closestGnss.longitude) : null; + + result.add(new ReplayPoint(correctedLocation, gnssLocation, orientationDeg, + 0f, corrected.relativeTimestamp)); + } + } else { + for (int i = 0; i < pdrList.size(); i++) { + PdrRecord pdr = pdrList.get(i); - float speed = 0f; - if (i > 0) { + ImuRecord closestImu = findClosestImuRecord(imuList, pdr.relativeTimestamp); + float orientationDeg = closestImu != null ? computeOrientationFromRotationVector( + closestImu.rotationVectorX, + closestImu.rotationVectorY, + closestImu.rotationVectorZ, + closestImu.rotationVectorW, + context + ) : 0f; + + float speed = 0f; + if (i > 0) { PdrRecord prev = pdrList.get(i - 1); double dt = (pdr.relativeTimestamp - prev.relativeTimestamp) / 1000.0; double dx = pdr.x - prev.x; double dy = pdr.y - prev.y; double distance = Math.sqrt(dx * dx + dy * dy); if (dt > 0) speed = (float) (distance / dt); - } + } + double lat = originLat + pdr.y * 1E-5; + double lng = originLng + pdr.x * 1E-5; + LatLng pdrLocation = new LatLng(lat, lng); - double lat = originLat + pdr.y * 1E-5; - double lng = originLng + pdr.x * 1E-5; - LatLng pdrLocation = new LatLng(lat, lng); - - GnssRecord closestGnss = findClosestGnssRecord(gnssList, pdr.relativeTimestamp); - LatLng gnssLocation = closestGnss != null ? + GnssRecord closestGnss = findClosestGnssRecord(gnssList, pdr.relativeTimestamp); + LatLng gnssLocation = closestGnss != null ? new LatLng(closestGnss.latitude, closestGnss.longitude) : null; - result.add(new ReplayPoint(pdrLocation, gnssLocation, orientationDeg, - 0f, pdr.relativeTimestamp)); + result.add(new ReplayPoint(pdrLocation, gnssLocation, orientationDeg, + speed, pdr.relativeTimestamp)); + } } Collections.sort(result, Comparator.comparingLong(rp -> rp.timestamp)); @@ -219,6 +253,16 @@ private static List parsePdrData(JsonArray pdrArray) { pdrList.add(record); } return pdrList; +}/** Parses corrected (fused) data from JSON. */ +private static List parseCorrectedData(JsonArray correctedArray) { + List correctedList = new ArrayList<>(); + if (correctedArray == null) return correctedList; + Gson gson = new Gson(); + for (int i = 0; i < correctedArray.size(); i++) { + CorrectedRecord record = gson.fromJson(correctedArray.get(i), CorrectedRecord.class); + correctedList.add(record); + } + return correctedList; }/** Parses GNSS data from JSON. */ private static List parseGnssData(JsonArray gnssArray) { List gnssList = new ArrayList<>(); diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java index 1a3b0f7f..b673e8f3 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -725,6 +725,18 @@ private void updateFusedState() { state.fusedLongitude = (float) estimate.getLatLng().longitude; state.fusedFloor = estimate.getFloor(); state.fusedAvailable = true; + + if (recorder != null && recorder.isRecording()) { + long relativeTimestamp = SystemClock.uptimeMillis() - recorder.getBootTime(); + if (relativeTimestamp < 0) { + relativeTimestamp = 0; + } + recorder.addCorrectedPosition( + relativeTimestamp, + estimate.getLatLng().latitude, + estimate.getLatLng().longitude, + estimate.getFloor()); + } } //endregion diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/TrajectoryRecorder.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/TrajectoryRecorder.java index 8c771758..1926705c 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/TrajectoryRecorder.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/TrajectoryRecorder.java @@ -250,6 +250,25 @@ public void addPdrData(long relativeTimestamp, float x, float y) { } } + /** + * Adds a fused/corrected position entry to the trajectory. + */ + public void addCorrectedPosition(long relativeTimestamp, + double latitude, + double longitude, + int floor) { + if (trajectory == null || !saveRecording) return; + + Traj.GNSSPosition.Builder corrected = Traj.GNSSPosition.newBuilder() + .setRelativeTimestamp(relativeTimestamp) + .setLatitude(latitude) + .setLongitude(longitude) + .setAltitude(0.0); + + corrected.setFloor(String.valueOf(floor)); + trajectory.addCorrectedPositions(corrected); + } + /** * Adds a GNSS reading to the trajectory. */ From 8959b1b89675350fee856d12ce793fd1373d7f17 Mon Sep 17 00:00:00 2001 From: tommyj0 Date: Mon, 30 Mar 2026 13:08:59 +0100 Subject: [PATCH 25/52] added comments --- .../sensors/PositionFusionEngine.java | 47 +++++++++++++++++++ .../sensors/PositionFusionEstimate.java | 11 +++++ 2 files changed, 58 insertions(+) diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java index ca8043c3..ac136ea5 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java @@ -104,6 +104,9 @@ public PositionFusionEngine(float floorHeightMeters) { this.floorHeightMeters = floorHeightMeters > 0f ? floorHeightMeters : 4f; } + /** + * Re-anchors the local tangent frame and reinitializes all particles. + */ public synchronized void reset(double latDeg, double lonDeg, int initialFloor) { anchorLatDeg = latDeg; anchorLonDeg = lonDeg; @@ -124,6 +127,12 @@ public synchronized void reset(double latDeg, double lonDeg, int initialFloor) { } } + /** + * Prediction step: propagate particles using step displacement + process noise. + * + *

When a predicted segment intersects an indoor wall, motion is blocked for + * that particle to keep trajectories inside mapped traversable space.

+ */ public synchronized void updatePdrDisplacement(float dxEastMeters, float dyNorthMeters) { if (!hasAnchor || particles.isEmpty()) { return; @@ -163,6 +172,9 @@ public synchronized void updatePdrDisplacement(float dxEastMeters, float dyNorth } } + /** + * GNSS measurement update. Accuracy is converted into measurement sigma. + */ public synchronized void updateGnss(double latDeg, double lonDeg, float accuracyMeters) { double sigma = Math.max(accuracyMeters, 3.0f); if (DEBUG_LOGS) { @@ -173,6 +185,9 @@ public synchronized void updateGnss(double latDeg, double lonDeg, float accuracy applyAbsoluteFix(latDeg, lonDeg, sigma, null); } + /** + * WiFi absolute-fix update with fixed sigma and floor hint support. + */ public synchronized void updateWifi(double latDeg, double lonDeg, int wifiFloor) { if (DEBUG_LOGS) { Log.d(TAG, String.format(Locale.US, @@ -182,6 +197,12 @@ public synchronized void updateWifi(double latDeg, double lonDeg, int wifiFloor) applyAbsoluteFix(latDeg, lonDeg, WIFI_SIGMA_M, wifiFloor); } + /** + * Floor-transition update from barometer/elevator cues. + * + *

Transitions are allowed only near mapped stairs/lifts when those + * connectors are available for the floor.

+ */ public synchronized void updateElevation(float elevationMeters, boolean elevatorLikely) { int floorFromBarometer = Math.round(elevationMeters / floorHeightMeters); fallbackFloor = floorFromBarometer; @@ -214,6 +235,9 @@ public synchronized void updateElevation(float elevationMeters, boolean elevator } } + /** + * Updates indoor map-matching constraints for the currently containing building. + */ public synchronized void updateMapMatchingContext( double currentLatDeg, double currentLonDeg, @@ -308,6 +332,9 @@ public synchronized void updateMapMatchingContext( } } + /** + * Returns the current fused estimate as the weighted particle mean. + */ public synchronized PositionFusionEstimate getEstimate() { if (!hasAnchor || particles.isEmpty()) { return new PositionFusionEstimate(null, fallbackFloor, false); @@ -345,6 +372,9 @@ public synchronized PositionFusionEstimate getEstimate() { return new PositionFusionEstimate(latLng, bestFloor, true); } + /** + * Absolute-fix measurement update (GNSS/WiFi): reweight, normalize and resample. + */ private void applyAbsoluteFix(double latDeg, double lonDeg, double sigmaMeters, Integer floorHint) { if (!hasAnchor) { reset(latDeg, lonDeg, 0); @@ -363,6 +393,7 @@ private void applyAbsoluteFix(double latDeg, double lonDeg, double sigmaMeters, priorMeanNorth += p.weight * p.yNorth; } + // Innovation is measured against the prior weighted mean in local EN coordinates. double innovationEast = z[0] - priorMeanEast; double innovationNorth = z[1] - priorMeanNorth; double innovationDistance = Math.hypot(innovationEast, innovationNorth); @@ -371,6 +402,7 @@ private void applyAbsoluteFix(double latDeg, double lonDeg, double sigmaMeters, : OUTLIER_GATE_SIGMA_MULT_WIFI; double gateMeters = Math.max(gateSigmaMultiplier * sigmaMeters, OUTLIER_GATE_MIN_M); double effectiveSigma = sigmaMeters; + // Outlier damping: inflate sigma instead of discarding large residual fixes. if (innovationDistance > gateMeters) { double sigmaScale = Math.min(innovationDistance / gateMeters, MAX_OUTLIER_SIGMA_SCALE); effectiveSigma = sigmaMeters * sigmaScale; @@ -502,6 +534,7 @@ private void initParticlesAtOrigin(int initialFloor) { } } + /** Re-seeds particles around the latest absolute measurement when weights collapse. */ private void reinitializeAroundMeasurement(double x, double y, int floor) { particles.clear(); double w = 1.0 / PARTICLE_COUNT; @@ -526,6 +559,7 @@ private double computeEffectiveSampleSize() { return 1.0 / sumSquared; } + /** Systematic resampling used when effective particle count drops too low. */ private void resampleSystematic() { List resampled = new ArrayList<>(PARTICLE_COUNT); double step = 1.0 / PARTICLE_COUNT; @@ -560,6 +594,7 @@ private void roughenParticles() { } } + /** Applies heading-bias correction to a step vector in local EN coordinates. */ private static double[] rotateVector(double east, double north, double angleRad) { double cos = Math.cos(angleRad); double sin = Math.sin(angleRad); @@ -572,6 +607,7 @@ private static double clamp(double value, double min, double max) { return Math.max(min, Math.min(max, value)); } + /** Converts WGS84 coordinates to local East/North meters around the anchor. */ private double[] toLocal(double latDeg, double lonDeg) { double lat0Rad = Math.toRadians(anchorLatDeg); double dLat = Math.toRadians(latDeg - anchorLatDeg); @@ -582,6 +618,7 @@ private double[] toLocal(double latDeg, double lonDeg) { return new double[]{east, north}; } + /** Converts local East/North meters back to WGS84 coordinates. */ private LatLng toLatLng(double eastMeters, double northMeters) { double lat0Rad = Math.toRadians(anchorLatDeg); @@ -598,6 +635,7 @@ private LatLng toLatLng(double eastMeters, double northMeters) { return new LatLng(lat, lon); } + /** Returns true when segment (x0,y0)->(x1,y1) intersects any mapped wall segment. */ private boolean crossesWall(int floor, double x0, double y0, double x1, double y1) { FloorConstraint fc = floorConstraints.get(floor); if (fc == null || fc.walls.isEmpty()) { @@ -614,6 +652,9 @@ private boolean crossesWall(int floor, double x0, double y0, double x1, double y return false; } + /** + * Validates whether a floor transition is plausible at the particle position. + */ private boolean canUseConnector(int floor, double x, double y, boolean elevatorLikely) { if (floorConstraints.isEmpty()) { return true; @@ -651,6 +692,7 @@ private boolean isNearAny(List points, double x, double y, double radiu return false; } + /** Converts polyline points from API geometry into local wall segments. */ private void addWallSegments(List points, List out) { if (points == null || points.size() < 2) { return; @@ -672,6 +714,7 @@ private Point2D toLocalPoint(LatLng latLng) { return new Point2D(local[0], local[1]); } + /** Computes a centroid in local meters for stairs/lift connector features. */ private Point2D toLocalCentroid(List points) { if (points == null || points.isEmpty()) { return null; @@ -696,6 +739,7 @@ private Point2D toLocalCentroid(List points) { return new Point2D(sx / count, sy / count); } + /** Maps floor display labels (e.g. G, LG) to numeric logical floors. */ private Integer parseLogicalFloor(FloorplanApiClient.FloorShapes floor, int index) { if (floor == null) { return null; @@ -720,6 +764,7 @@ private Integer parseLogicalFloor(FloorplanApiClient.FloorShapes floor, int inde return index; } + /** Point-in-polygon test in lat/lon space for containing-building detection. */ private boolean pointInPolygon(LatLng point, List polygon) { boolean inside = false; for (int i = 0, j = polygon.size() - 1; i < polygon.size(); j = i++) { @@ -738,6 +783,7 @@ private boolean pointInPolygon(LatLng point, List polygon) { return inside; } + /** Robust segment intersection test with collinearity handling. */ private boolean segmentsIntersect(Point2D p1, Point2D p2, Point2D q1, Point2D q2) { double o1 = orientation(p1, p2, q1); double o2 = orientation(p1, p2, q2); @@ -758,6 +804,7 @@ private double orientation(Point2D a, Point2D b, Point2D c) { return (b.x - a.x) * (c.y - a.y) - (b.y - a.y) * (c.x - a.x); } + /** Inclusive collinearity-bound check used by segment intersection. */ private boolean onSegment(Point2D a, Point2D b, Point2D c) { return b.x >= Math.min(a.x, c.x) - 1e-9 && b.x <= Math.max(a.x, c.x) + 1e-9 diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEstimate.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEstimate.java index f1ee5cad..9ef97302 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEstimate.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEstimate.java @@ -4,6 +4,9 @@ /** * Immutable snapshot of the fused position estimate. + * + *

This object is intentionally tiny and read-only so UI code can safely + * consume it on every refresh without sharing mutable filter state.

*/ public class PositionFusionEstimate { @@ -11,20 +14,28 @@ public class PositionFusionEstimate { private final int floor; private final boolean available; + /** + * @param latLng fused position in global coordinates, null when unavailable + * @param floor inferred floor index + * @param available true when the estimate is valid for consumption + */ public PositionFusionEstimate(LatLng latLng, int floor, boolean available) { this.latLng = latLng; this.floor = floor; this.available = available; } + /** Returns the fused global position, or null when unavailable. */ public LatLng getLatLng() { return latLng; } + /** Returns the inferred floor value for this estimate. */ public int getFloor() { return floor; } + /** Returns whether the estimate contains a usable position. */ public boolean isAvailable() { return available; } From 4eab52265f9bba83d228a6b488c60497b7de0993 Mon Sep 17 00:00:00 2001 From: evmorfiaa Date: Mon, 30 Mar 2026 15:29:33 +0100 Subject: [PATCH 26/52] collapsable UI --- .../fragment/TrajectoryMapFragment.java | 35 ++- .../res/drawable-night/switch_label_bg.xml | 6 + app/src/main/res/drawable/ic_chevron_up.xml | 10 + app/src/main/res/drawable/switch_label_bg.xml | 6 + .../res/layout/fragment_trajectory_map.xml | 266 ++++++++++++------ .../main/res/layout/item_map_type_spinner.xml | 10 +- app/src/main/res/values-night/colors.xml | 4 + app/src/main/res/values/colors.xml | 5 + 8 files changed, 232 insertions(+), 110 deletions(-) create mode 100644 app/src/main/res/drawable-night/switch_label_bg.xml create mode 100644 app/src/main/res/drawable/ic_chevron_up.xml create mode 100644 app/src/main/res/drawable/switch_label_bg.xml diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java index 40bca82f..aef63fe5 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java +++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java @@ -140,7 +140,25 @@ public void onViewCreated(@NonNull View view, floorUpButton = view.findViewById(R.id.floorUpButton); floorDownButton = view.findViewById(R.id.floorDownButton); floorLabel = view.findViewById(R.id.floorLabel); - switchColorButton = view.findViewById(R.id.lineColorButton); + switchColorButton = null; // color button removed from layout + + // Collapsible left panel + View leftPanelContent = view.findViewById(R.id.leftPanelContent); + TextView leftToggle = view.findViewById(R.id.leftPanelToggle); + leftToggle.setOnClickListener(v -> { + boolean visible = leftPanelContent.getVisibility() == View.VISIBLE; + leftPanelContent.setVisibility(visible ? View.GONE : View.VISIBLE); + leftToggle.setText(visible ? "▼" : "▲"); + }); + + // Collapsible right panel + View rightPanelContent = view.findViewById(R.id.rightPanelContent); + TextView rightToggle = view.findViewById(R.id.rightPanelToggle); + rightToggle.setOnClickListener(v -> { + boolean visible = rightPanelContent.getVisibility() == View.VISIBLE; + rightPanelContent.setVisibility(visible ? View.GONE : View.VISIBLE); + rightToggle.setText(visible ? "▼" : "▲"); + }); // Setup floor up/down UI hidden initially until we know there's an indoor map setFloorControlsVisibility(View.GONE); @@ -189,20 +207,7 @@ public void onMapReady(@NonNull GoogleMap googleMap) { } }); - // Color switch - switchColorButton.setOnClickListener(v -> { - if (polyline != null) { - if (isRed) { - switchColorButton.setBackgroundColor(Color.BLACK); - polyline.setColor(Color.BLACK); - isRed = false; - } else { - switchColorButton.setBackgroundColor(Color.RED); - polyline.setColor(Color.RED); - isRed = true; - } - } - }); + // color button removed // Auto-floor toggle: start/stop periodic floor evaluation sensorFusion = SensorFusion.getInstance(); diff --git a/app/src/main/res/drawable-night/switch_label_bg.xml b/app/src/main/res/drawable-night/switch_label_bg.xml new file mode 100644 index 00000000..cae8dc14 --- /dev/null +++ b/app/src/main/res/drawable-night/switch_label_bg.xml @@ -0,0 +1,6 @@ + + + + + diff --git a/app/src/main/res/drawable/ic_chevron_up.xml b/app/src/main/res/drawable/ic_chevron_up.xml new file mode 100644 index 00000000..0748bbf8 --- /dev/null +++ b/app/src/main/res/drawable/ic_chevron_up.xml @@ -0,0 +1,10 @@ + + + + diff --git a/app/src/main/res/drawable/switch_label_bg.xml b/app/src/main/res/drawable/switch_label_bg.xml new file mode 100644 index 00000000..104eec3d --- /dev/null +++ b/app/src/main/res/drawable/switch_label_bg.xml @@ -0,0 +1,6 @@ + + + + + diff --git a/app/src/main/res/layout/fragment_trajectory_map.xml b/app/src/main/res/layout/fragment_trajectory_map.xml index 721c7a1e..e178e423 100644 --- a/app/src/main/res/layout/fragment_trajectory_map.xml +++ b/app/src/main/res/layout/fragment_trajectory_map.xml @@ -12,14 +12,15 @@ android:layout_width="match_parent" android:layout_height="match_parent" /> - + @@ -28,53 +29,85 @@ android:layout_width="wrap_content" android:layout_height="wrap_content" android:orientation="vertical" - android:padding="8dp" - android:gravity="center"> + android:padding="8dp"> - + + android:orientation="horizontal" + android:gravity="start"> - + - + + + + android:orientation="vertical" + android:gravity="center" + android:layout_marginTop="4dp"> - + + + + + + - + @@ -82,83 +115,136 @@ - - - + + android:gravity="center_vertical" + android:baselineAligned="false" + android:background="@drawable/switch_label_bg" + android:paddingStart="6dp" + android:paddingEnd="6dp" + android:paddingTop="4dp" + android:paddingBottom="4dp"> - + + android:text="▲" + android:textSize="14sp" + android:textStyle="bold" + android:textColor="@color/glass_text" + android:layout_marginStart="8dp" + android:padding="4dp" + android:background="@android:color/transparent" + android:contentDescription="Toggle legend panel" /> + - + + android:orientation="vertical" + android:layout_marginTop="6dp"> - + + - + + + + + + - + android:orientation="horizontal" + android:layout_marginBottom="4dp" + android:gravity="center_vertical" + android:background="@drawable/switch_label_bg" + android:paddingStart="6dp" + android:paddingEnd="6dp" + android:paddingTop="4dp" + android:paddingBottom="4dp"> - - + - + + - + - + android:orientation="horizontal" + android:gravity="center_vertical" + android:background="@drawable/switch_label_bg" + android:paddingStart="6dp" + android:paddingEnd="6dp" + android:paddingTop="4dp" + android:paddingBottom="4dp"> + + + + + + @@ -190,7 +276,7 @@ android:paddingTop="6dp" android:paddingBottom="6dp" android:text="G" - android:textColor="@color/md_theme_onSurface" + android:textColor="@color/glass_text" android:textSize="18sp" android:textStyle="bold" app:layout_constraintBottom_toTopOf="@id/floorDownButton" diff --git a/app/src/main/res/layout/item_map_type_spinner.xml b/app/src/main/res/layout/item_map_type_spinner.xml index c0d7af01..17d7fe40 100644 --- a/app/src/main/res/layout/item_map_type_spinner.xml +++ b/app/src/main/res/layout/item_map_type_spinner.xml @@ -4,10 +4,10 @@ android:layout_height="wrap_content" android:ellipsize="end" android:maxLines="1" - android:paddingStart="12dp" - android:paddingTop="8dp" - android:paddingEnd="12dp" - android:paddingBottom="8dp" + android:paddingStart="10dp" + android:paddingTop="4dp" + android:paddingEnd="10dp" + android:paddingBottom="4dp" android:textAlignment="viewStart" android:textColor="?attr/colorOnSurface" - android:textSize="14sp" /> + android:textSize="12sp" /> diff --git a/app/src/main/res/values-night/colors.xml b/app/src/main/res/values-night/colors.xml index 76fa7d44..988c10b0 100644 --- a/app/src/main/res/values-night/colors.xml +++ b/app/src/main/res/values-night/colors.xml @@ -1,4 +1,8 @@ + + #44C0C0C0 + #44FFFFFF + #EEEEEE #86D1EA #003642 #004E5F diff --git a/app/src/main/res/values/colors.xml b/app/src/main/res/values/colors.xml index 3f28e98d..8dc1c5e2 100644 --- a/app/src/main/res/values/colors.xml +++ b/app/src/main/res/values/colors.xml @@ -14,6 +14,11 @@ #FF001080 #FE0000 + + #55FFFFFF + #33000000 + #1A1A1A + #6750A4 #FFFFFF From 4f170453d39e0c05e253e42e05293fbcc26470ff Mon Sep 17 00:00:00 2001 From: tommyj0 Date: Mon, 30 Mar 2026 15:30:38 +0100 Subject: [PATCH 27/52] wait on startup --- .../fragment/StartLocationFragment.java | 127 +++++++++++++++++- .../res/layout/fragment_startlocation.xml | 15 +++ app/src/main/res/values/strings.xml | 3 + 3 files changed, 142 insertions(+), 3 deletions(-) diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/StartLocationFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/StartLocationFragment.java index 0951e85a..c602ddf1 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/StartLocationFragment.java +++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/StartLocationFragment.java @@ -1,7 +1,9 @@ package com.openpositioning.PositionMe.presentation.fragment; import android.graphics.Color; +import android.os.Handler; import android.os.Bundle; +import android.os.Looper; import android.util.Log; import android.view.LayoutInflater; import android.view.View; @@ -51,9 +53,11 @@ public class StartLocationFragment extends Fragment { private static final String TAG = "StartLocationFragment"; + private static final long INITIAL_ESTIMATE_POLL_MS = 500L; // UI elements private Button button; + private Button loadingButton; private TextView instructionText; private View buildingInfoCard; private TextView buildingNameText; @@ -73,6 +77,22 @@ public class StartLocationFragment extends Fragment { private final List buildingPolygons = new ArrayList<>(); private final Map floorplanBuildingMap = new HashMap<>(); private Polygon selectedPolygon; + private boolean userAdjustedStartPosition; + private boolean initialEstimateApplied; + private boolean floorplanRequestInFlight; + private boolean floorplanDataReady; + + private final Handler initialEstimateHandler = new Handler(Looper.getMainLooper()); + private final Runnable initialEstimatePoller = new Runnable() { + @Override + public void run() { + if (!isAdded()) { + return; + } + updateSetButtonAvailability(); + initialEstimateHandler.postDelayed(this, INITIAL_ESTIMATE_POLL_MS); + } + }; // Vector shapes drawn as floor plan preview (cleared when switching buildings) private final List previewPolygons = new ArrayList<>(); @@ -161,6 +181,7 @@ public void onMarkerDragStart(Marker marker) {} public void onMarkerDragEnd(Marker marker) { startPosition[0] = (float) marker.getPosition().latitude; startPosition[1] = (float) marker.getPosition().longitude; + userAdjustedStartPosition = true; } @Override @@ -183,6 +204,9 @@ public void onMarkerDrag(Marker marker) {} */ private void requestBuildingData() { FloorplanApiClient apiClient = new FloorplanApiClient(); + floorplanRequestInFlight = true; + floorplanDataReady = false; + updateSetButtonAvailability(); // Collect observed WiFi AP MAC addresses from latest scan List observedMacs = new ArrayList<>(); @@ -201,14 +225,20 @@ private void requestBuildingData() { new FloorplanApiClient.FloorplanCallback() { @Override public void onSuccess(List buildings) { - if (!isAdded() || mMap == null) return; - + floorplanRequestInFlight = false; + floorplanDataReady = true; sensorFusion.setFloorplanBuildings(buildings); floorplanBuildingMap.clear(); for (FloorplanApiClient.BuildingInfo building : buildings) { floorplanBuildingMap.put(building.getName(), building); } + if (!isAdded() || mMap == null) { + return; + } + + updateSetButtonAvailability(); + if (buildings.isEmpty()) { Log.d(TAG, "No buildings returned by API"); if (instructionText != null) { @@ -222,9 +252,12 @@ public void onSuccess(List buildings) { @Override public void onFailure(String error) { - if (!isAdded()) return; + floorplanRequestInFlight = false; + floorplanDataReady = true; sensorFusion.setFloorplanBuildings(new ArrayList<>()); floorplanBuildingMap.clear(); + if (!isAdded()) return; + updateSetButtonAvailability(); Log.e(TAG, "Floorplan API failed: " + error); } }); @@ -306,6 +339,7 @@ private void onBuildingSelected(String buildingName, Polygon polygon) { } startPosition[0] = (float) center.latitude; startPosition[1] = (float) center.longitude; + userAdjustedStartPosition = true; // Zoom to the building mMap.animateCamera(CameraUpdateFactory.newLatLngZoom(center, 20f)); @@ -315,6 +349,7 @@ private void onBuildingSelected(String buildingName, Polygon polygon) { // Update UI with building name updateBuildingInfoDisplay(buildingName); + updateSetButtonAvailability(); Log.d(TAG, "Building selected: " + buildingName); } @@ -454,11 +489,23 @@ public void onViewCreated(@NonNull View view, @Nullable Bundle savedInstanceStat super.onViewCreated(view, savedInstanceState); this.button = view.findViewById(R.id.startLocationDone); + this.loadingButton = view.findViewById(R.id.startLocationLoading); this.instructionText = view.findViewById(R.id.correctionInfoView); this.buildingInfoCard = view.findViewById(R.id.buildingInfoCard); this.buildingNameText = view.findViewById(R.id.buildingNameText); + updateSetButtonAvailability(); + if (requireActivity() instanceof RecordingActivity) { + initialEstimateHandler.post(initialEstimatePoller); + } + this.button.setOnClickListener(v -> { + if (requireActivity() instanceof RecordingActivity + && !(hasInitialEstimate() && hasRequiredFloorplanDataForRecording())) { + updateSetButtonAvailability(); + return; + } + float chosenLat = startPosition[0]; float chosenLon = startPosition[1]; @@ -483,4 +530,78 @@ public void onViewCreated(@NonNull View view, @Nullable Bundle savedInstanceStat } }); } + + @Override + public void onDestroyView() { + initialEstimateHandler.removeCallbacks(initialEstimatePoller); + super.onDestroyView(); + } + + private boolean hasInitialEstimate() { + return sensorFusion.getFusedLatLng() != null; + } + + private void updateSetButtonAvailability() { + if (button == null || !isAdded()) { + return; + } + + boolean isReplayFlow = requireActivity() instanceof ReplayActivity; + boolean hasEstimate = hasInitialEstimate(); + boolean hasFloorplan = hasRequiredFloorplanDataForRecording(); + boolean ready = isReplayFlow || (hasEstimate && hasFloorplan); + + button.setEnabled(ready); + button.setAlpha(ready ? 1.0f : 0.5f); + if (loadingButton != null) { + loadingButton.setVisibility(ready ? View.GONE : View.VISIBLE); + } + button.setVisibility(ready ? View.VISIBLE : View.GONE); + + if (!isReplayFlow && instructionText != null) { + if (!hasEstimate) { + instructionText.setText(R.string.initialEstimateWaiting); + } else if (!hasFloorplan) { + instructionText.setText(R.string.floorplanLoadingWaiting); + } else { + instructionText.setText(R.string.buildingSelectionInstructions); + } + } + + applyInitialEstimateToMarkerIfNeeded(ready, isReplayFlow); + } + + private boolean hasRequiredFloorplanDataForRecording() { + if (requireActivity() instanceof ReplayActivity) { + return true; + } + + if (selectedBuildingId != null && !selectedBuildingId.isEmpty()) { + FloorplanApiClient.BuildingInfo selectedBuilding = + sensorFusion.getFloorplanBuilding(selectedBuildingId); + return selectedBuilding != null + && selectedBuilding.getFloorShapesList() != null + && !selectedBuilding.getFloorShapesList().isEmpty(); + } + + return floorplanDataReady && !floorplanRequestInFlight; + } + + private void applyInitialEstimateToMarkerIfNeeded(boolean ready, boolean isReplayFlow) { + if (!ready || isReplayFlow || initialEstimateApplied || userAdjustedStartPosition + || mMap == null || startMarker == null) { + return; + } + + LatLng fusedPosition = sensorFusion.getFusedLatLng(); + if (fusedPosition == null) { + return; + } + + startPosition[0] = (float) fusedPosition.latitude; + startPosition[1] = (float) fusedPosition.longitude; + startMarker.setPosition(fusedPosition); + mMap.animateCamera(CameraUpdateFactory.newLatLngZoom(fusedPosition, zoom)); + initialEstimateApplied = true; + } } diff --git a/app/src/main/res/layout/fragment_startlocation.xml b/app/src/main/res/layout/fragment_startlocation.xml index fa9b0931..245bdc9e 100644 --- a/app/src/main/res/layout/fragment_startlocation.xml +++ b/app/src/main/res/layout/fragment_startlocation.xml @@ -74,6 +74,7 @@ android:layout_marginBottom="24dp" android:text="@string/setLocation" android:textSize="24sp" + android:visibility="gone" app:icon="@drawable/ic_baseline_add_location_24" app:iconGravity="start" app:iconSize="30dp" @@ -81,6 +82,20 @@ app:layout_constraintEnd_toEndOf="parent" app:layout_constraintStart_toStartOf="parent" /> +