From 4c509636e0c1d4d3c18ed3e9e2192f0dffd36d97 Mon Sep 17 00:00:00 2001 From: Saleh-AlMulla Date: Mon, 16 Mar 2026 15:46:09 +0000 Subject: [PATCH 01/26] Add secrets.properties to gitignore --- .gitignore | 2 ++ secrets.properties | 6 ------ 2 files changed, 2 insertions(+), 6 deletions(-) delete mode 100644 secrets.properties diff --git a/.gitignore b/.gitignore index d4c3a57e..fb017ff3 100644 --- a/.gitignore +++ b/.gitignore @@ -14,3 +14,5 @@ .cxx local.properties /.idea/ +secret.properties +secrets.properties diff --git a/secrets.properties b/secrets.properties deleted file mode 100644 index f0dc54fd..00000000 --- a/secrets.properties +++ /dev/null @@ -1,6 +0,0 @@ -# -# Modify the variables to set your keys -# -MAPS_API_KEY= -OPENPOSITIONING_API_KEY= -OPENPOSITIONING_MASTER_KEY= From 90f6d9cf9a8663d97583e66fea6809dfb8c75e72 Mon Sep 17 00:00:00 2001 From: chaitrad2002-dev Date: Mon, 16 Mar 2026 21:40:58 +0530 Subject: [PATCH 02/26] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index c4a9f02d..659ffb48 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,5 @@ **PositionMe** is an indoor positioning data collection application initially developed for the University of Edinburgh's Embedded Wireless course. The application now includes enhanced features, including **trajectory playback**, improved UI design, and comprehensive location tracking. - +///////// ## Features - **Real-time Sensor Data Collection**: Captures sensor, location, and GNSS data. From 0816d6685b7514988624e02f74ed22d425bc19f7 Mon Sep 17 00:00:00 2001 From: chaitrad2002-dev Date: Wed, 18 Mar 2026 20:44:19 +0530 Subject: [PATCH 03/26] update --- .../fragment/RecordingFragment.java | 29 +++++++---- .../sensors/SensorEventHandler.java | 4 ++ .../PositionMe/sensors/SensorFusion.java | 2 + .../sensors/WifiPositionManager.java | 4 +- .../java/positioning/CoordinateUtils.java | 27 ++++++++++ app/src/main/java/positioning/EKFFilter.java | 38 ++++++++++++++ .../main/java/positioning/FusionManager.java | 52 +++++++++++++++++++ 7 files changed, 145 insertions(+), 11 deletions(-) create mode 100644 app/src/main/java/positioning/CoordinateUtils.java create mode 100644 app/src/main/java/positioning/EKFFilter.java create mode 100644 app/src/main/java/positioning/FusionManager.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..537e2fd4 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 @@ -36,6 +36,7 @@ import java.util.List; import android.widget.Toast; +import com.openpositioning.PositionMe.positioning.FusionManager; @@ -268,21 +269,29 @@ private void updateUIandPosition() { // 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 + // Use fused position if available, otherwise fall back to PDR + double[] fusedPos = FusionManager.getInstance().getBestPosition(); + if (fusedPos != null) { + LatLng newLocation = new LatLng(fusedPos[0], fusedPos[1]); if (trajectoryMapFragment != null) { trajectoryMapFragment.updateUserLocation(newLocation, (float) Math.toDegrees(sensorFusion.passOrientation())); } + } else { + // Fallback to raw PDR until fusion is initialised + 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. float[] gnss = sensorFusion.getSensorValueMap().get(SensorTypes.GNSSLATLONG); if (gnss != null && trajectoryMapFragment != null) { 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..f45b0562 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java @@ -12,6 +12,7 @@ import java.util.ArrayList; import java.util.HashMap; import java.util.List; +import com.openpositioning.PositionMe.positioning.FusionManager; /** * Handles sensor event dispatching for all registered movement sensors. @@ -172,6 +173,9 @@ public void handleSensorEvent(SensorEvent sensorEvent) { ); this.accelMagnitude.clear(); + FusionManager.getInstance().onStep( // ADD THIS + pdrProcessing.getAverageStepLength(), // ADD THIS + state.orientation[0]); // if (recorder.isRecording()) { this.pathView.drawTrajectory(newCords); 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..9b1aaf0d 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -31,6 +31,7 @@ import java.util.Map; import java.util.stream.Collectors; import java.util.stream.Stream; +import com.openpositioning.PositionMe.positioning.FusionManager; /** * The SensorFusion class is the main data gathering and processing class of the application. @@ -645,6 +646,7 @@ public void onLocationChanged(@NonNull Location location) { state.latitude = (float) location.getLatitude(); state.longitude = (float) location.getLongitude(); recorder.addGnssData(location); + FusionManager.getInstance().onGnss(location.getLatitude(), location.getLongitude()); } } 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..92110279 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java @@ -10,6 +10,7 @@ import java.util.List; import java.util.stream.Collectors; import java.util.stream.Stream; +import com.openpositioning.PositionMe.positioning.FusionManager; /** * Manages WiFi scan result processing and WiFi-based positioning requests. @@ -51,7 +52,7 @@ public WifiPositionManager(WiFiPositioning wiFiPositioning, public void update(Object[] wifiList) { this.wifiList = Stream.of(wifiList).map(o -> (Wifi) o).collect(Collectors.toList()); recorder.addWifiFingerprint(this.wifiList); - createWifiPositioningRequest(); + createWifiPositionRequestCallback(); } /** @@ -86,6 +87,7 @@ private void createWifiPositionRequestCallback() { @Override public void onSuccess(LatLng wifiLocation, int floor) { // Handle the success response + createWifiPositionRequestCallback(); } @Override diff --git a/app/src/main/java/positioning/CoordinateUtils.java b/app/src/main/java/positioning/CoordinateUtils.java new file mode 100644 index 00000000..aa85b0df --- /dev/null +++ b/app/src/main/java/positioning/CoordinateUtils.java @@ -0,0 +1,27 @@ +package com.openpositioning.PositionMe.positioning; + +public class CoordinateUtils { + private final double originLat; + private final double originLon; + private final double metersPerLat; + private final double metersPerLon; + + public CoordinateUtils(double originLat, double originLon) { + this.originLat = originLat; + this.originLon = originLon; + this.metersPerLat = 111320.0; + this.metersPerLon = 111320.0 * Math.cos(Math.toRadians(originLat)); + } + + public double[] toLocal(double lat, double lon) { + double east = (lon - originLon) * metersPerLon; + double north = (lat - originLat) * metersPerLat; + return new double[]{east, north}; + } + + public double[] toGlobal(double east, double north) { + double lon = originLon + east / metersPerLon; + double lat = originLat + north / metersPerLat; + return new double[]{lat, lon}; + } +} \ No newline at end of file diff --git a/app/src/main/java/positioning/EKFFilter.java b/app/src/main/java/positioning/EKFFilter.java new file mode 100644 index 00000000..3e58518f --- /dev/null +++ b/app/src/main/java/positioning/EKFFilter.java @@ -0,0 +1,38 @@ +package com.openpositioning.PositionMe.positioning; + +public class EKFFilter { + private double east, north; + private double pEast, pNorth; + + private static final double Q = 0.3; + private static final double R_GNSS = 15.0; + private static final double R_WIFI = 6.0; + + public EKFFilter(double initEast, double initNorth) { + this.east = initEast; + this.north = initNorth; + this.pEast = 100.0; + this.pNorth = 100.0; + } + + public void predict(double dE, double dN) { + east += dE; + north += dN; + pEast += Q; + pNorth += Q; + } + + public void update(double obsEast, double obsNorth, double R) { + double kE = pEast / (pEast + R); + double kN = pNorth / (pNorth + R); + east += kE * (obsEast - east); + north += kN * (obsNorth - north); + pEast *= (1 - kE); + pNorth *= (1 - kN); + } + + public double getEast() { return east; } + public double getNorth() { return north; } + public static double getGnssNoise() { return R_GNSS; } + public static double getWifiNoise() { return R_WIFI; } +} \ No newline at end of file diff --git a/app/src/main/java/positioning/FusionManager.java b/app/src/main/java/positioning/FusionManager.java new file mode 100644 index 00000000..44318f5d --- /dev/null +++ b/app/src/main/java/positioning/FusionManager.java @@ -0,0 +1,52 @@ +package com.openpositioning.PositionMe.positioning; + +import android.util.Log; + +public class FusionManager { + + private static final FusionManager instance = new FusionManager(); + + private EKFFilter ekf; + private CoordinateUtils coords; + private boolean ready = false; + + private FusionManager() {} + + public static FusionManager getInstance() { + return instance; + } + + public void onGnss(double lat, double lon) { + if (!ready) { + coords = new CoordinateUtils(lat, lon); + ekf = new EKFFilter(0, 0); + ready = true; + Log.d("FusionManager", "Initialised at " + lat + ", " + lon); + return; + } + double[] en = coords.toLocal(lat, lon); + ekf.update(en[0], en[1], EKFFilter.getGnssNoise()); + } + + public void onWifi(double lat, double lon) { + if (!ready) return; + double[] en = coords.toLocal(lat, lon); + ekf.update(en[0], en[1], EKFFilter.getWifiNoise()); + } + + public void onStep(double stepLen, double headingRad) { + if (!ready) return; + double dE = stepLen * Math.sin(headingRad); + double dN = stepLen * Math.cos(headingRad); + ekf.predict(dE, dN); + } + + public double[] getBestPosition() { + if (!ready) return null; + return coords.toGlobal(ekf.getEast(), ekf.getNorth()); + } + + public boolean isReady() { + return ready; + } +} \ No newline at end of file From 55003ddcf692bf01c95c0c2a9bba9707749189e4 Mon Sep 17 00:00:00 2001 From: Lane2048 <145272991+Lane2048@users.noreply.github.com> Date: Mon, 23 Mar 2026 15:58:26 +0000 Subject: [PATCH 04/26] Data display for gnss, wifi and pdr Implemented observation-based trajectory visualisation for PDR, GNSS, and WiFi: 1. Added distance-based filtering (1m for PDR, 3m for GNSS) to suppress noisy updates 2. Introduced capped observation markers (PDR: 20, GNSS/WiFi: 5) to prevent map clutter 3. Implemented continuous PDR trajectory polyline (orange) 4. Added WiFi observation integration with graceful null handling 5. Ensured full reset of markers and trajectories between recordings --- .../fragment/RecordingFragment.java | 47 ++- .../fragment/TrajectoryMapFragment.java | 348 +++++++++++++++++- 2 files changed, 379 insertions(+), 16 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 340843ca..60d50704 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 @@ -18,6 +18,7 @@ import android.widget.ImageView; import android.widget.ProgressBar; import android.widget.TextView; +import android.util.Log; import com.google.android.material.button.MaterialButton; import androidx.annotation.NonNull; @@ -34,6 +35,7 @@ import java.util.ArrayList; import java.util.List; +import java.util.Arrays; import android.widget.Toast; @@ -87,11 +89,43 @@ public class RecordingFragment extends Fragment { // References to the child map fragment private TrajectoryMapFragment trajectoryMapFragment; + // ------------------------------------------------------------------------- + // REFRESH LOOP (NEW: WiFi call added here) + // ------------------------------------------------------------------------- + + /** + * Repeating Runnable that drives all real-time UI and map updates at ~200ms intervals. + * + * WHY WiFi IS CALLED HERE (not inside updateUIandPosition): + * WiFi positioning is handled separately from PDR/GNSS because it comes from a network + * API call (openpositioning) rather than a local sensor. It is logically a distinct + * data source and keeps updateUIandPosition() focused on sensor data only. + * + * GRACEFUL NULL HANDLING: + * sensorFusion.getLatLngWifiPositioning() returns null whenever: + * (a) the WiFi API returned HTTP 404 (building not registered / outside range), or + * (b) the API call has not completed yet. + * Before this fix, the null case was silently ignored with no feedback. + * Now it logs "WiFi positioning unavailable" so you can see in logcat exactly when + * WiFi kicks in (the log stops appearing) vs when it is failing (it keeps appearing). + */ + private final Runnable refreshDataTask = new Runnable() { @Override public void run() { updateUIandPosition(); - // Loop again + // NEW: Feed WiFi positioning result to the map if available + LatLng wifiLocation = sensorFusion.getLatLngWifiPositioning(); + if (wifiLocation != null && trajectoryMapFragment != null) { + // WiFi API returned a valid fix — display a green marker on the map + Log.d("DisplayDebug", "Calling updateWifiObservation()"); + trajectoryMapFragment.updateWifiObservation(wifiLocation); + } else { + // WiFi unavailable: either 404, out of range, or API not yet responded. + // This is expected outside the Nucleus/Library buildings. + Log.d("DisplayDebug", "WiFi positioning unavailable (null) — skipping updateWifiObservation"); + } + // Schedule the next refresh cycle refreshDataHandler.postDelayed(refreshDataTask, 200); } }; @@ -253,6 +287,7 @@ private void onAddTestPoint() { */ private void updateUIandPosition() { float[] pdrValues = sensorFusion.getSensorValueMap().get(SensorTypes.PDR); + Log.d("DisplayDebug", "PDR values = " + Arrays.toString(pdrValues)); if (pdrValues == null) return; // Distance @@ -276,26 +311,36 @@ private void updateUIandPosition() { new float[]{ pdrValues[0] - previousPosX, pdrValues[1] - previousPosY } ); + Log.d("DisplayDebug", "PDR newLocation = " + newLocation); + // Pass the location + orientation to the map if (trajectoryMapFragment != null) { + Log.d("DisplayDebug", "Calling updateUserLocation()"); trajectoryMapFragment.updateUserLocation(newLocation, (float) Math.toDegrees(sensorFusion.passOrientation())); + Log.d("DisplayDebug", "Calling updatePdrObservation()"); + trajectoryMapFragment.updatePdrObservation(newLocation); } } // GNSS logic if you want to show GNSS error, etc. float[] gnss = sensorFusion.getSensorValueMap().get(SensorTypes.GNSSLATLONG); + Log.d("DisplayDebug", "GNSS raw = " + Arrays.toString(gnss)); + if (gnss != null && trajectoryMapFragment != null) { // If user toggles showing GNSS in the map, call e.g. if (trajectoryMapFragment.isGnssEnabled()) { LatLng gnssLocation = new LatLng(gnss[0], gnss[1]); + Log.d("DisplayDebug", "GNSS LatLng = " + gnssLocation); LatLng currentLoc = trajectoryMapFragment.getCurrentLocation(); if (currentLoc != null) { double errorDist = UtilFunctions.distanceBetweenPoints(currentLoc, gnssLocation); + Log.d("DisplayDebug", "gnssError showing: dist=" + String.format("%.2f", errorDist) + "m"); gnssError.setVisibility(View.VISIBLE); gnssError.setText(String.format(getString(R.string.gnss_error) + "%.2fm", errorDist)); } trajectoryMapFragment.updateGNSS(gnssLocation); + Log.d("DisplayDebug", "Calling updateGNSS()"); } 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 479ea51b..fa1f9f7f 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 @@ -97,6 +97,134 @@ public class TrajectoryMapFragment extends Fragment { private Button switchColorButton; private Polygon buildingPolygon; + // ------------------------------------------------------------------------- + // NEW FIELDS — observation markers and PDR trajectory (Assignment 2) + // ------------------------------------------------------------------------- + + /** + * Maximum number of observation markers kept on screen per positioning source. + * + * WHY DIFFERENT VALUES: + * PDR updates every ~200ms whenever the user moves, so we allow more markers (6) + * to show a visible recent trail. GNSS and WiFi are sparse (a few fixes per minute), + * so 5 markers is plenty — they won't accumulate fast enough to clutter the map. + * + * The "last N observations" requirement in the assignment brief (section 3.3) is + * satisfied by these caps: old markers are automatically removed as new ones appear. + */ + private static final int MAX_PDR_OBSERVATIONS = 6; + private static final int MAX_GNSS_OBSERVATIONS = 5; + private static final int MAX_WIFI_OBSERVATIONS = 5; + + /** + * Separate lists for each positioning source's observation markers. + * + * WHY SEPARATE LISTS: + * Each source has its own cap (MAX_*_OBSERVATIONS) and its own colour, so they + * must be managed independently. Mixing them into one list would make it impossible + * to remove only GNSS markers when the GNSS switch is toggled off, for example. + * + * Colour coding (as per assignment brief section 3.3): + * PDR → orange (HUE_ORANGE) + * GNSS → blue (HUE_AZURE) + * WiFi → green (HUE_GREEN) + */ + + private final List gnssObservationMarkers = new ArrayList<>(); + private final List wifiObservationMarkers = new ArrayList<>(); + private final List pdrObservationMarkers = new ArrayList<>(); + + + /** + * Orange polyline connecting every accepted PDR position (filtered by distance threshold). + * + * WHY A SEPARATE POLYLINE FROM THE RED ONE: + * The existing red {@code polyline} draws every single raw PDR update (~5 per second), + * including noise when the user is standing still. This new orange polyline only adds + * a point when the user has moved at least PDR_OBSERVATION_MIN_DISTANCE_M, producing + * a much cleaner path. This satisfies the assignment brief requirement to "display the + * full trajectory of fused positions" without the jitter of the raw feed. + */ + + private Polyline pdrTrajectoryPolyline; + /** + * Stores the last position at which a PDR observation marker was placed. + * Used to calculate the distance to the next incoming position and decide + * whether to skip it (too close) or accept it (far enough). + * Null on first call, which means the very first position is always accepted. + */ + private LatLng lastPdrObservationLocation = null; + + /** + * Minimum distance in metres the user must move before a new PDR observation + * marker is placed and the orange trajectory polyline is extended. + * + * WHY 1 METRE: + * PDR updates at ~200ms intervals. Without a threshold, standing still produces + * hundreds of overlapping markers per minute (confirmed: 733 markers in one session + * before this fix). 1m filters out sensor noise while still capturing each step. + * The GNSS equivalent threshold is 3m (in shouldAddGnssObservation) because GNSS + * noise is larger in scale than PDR noise. + */ + + private static final double PDR_OBSERVATION_MIN_DISTANCE_M = 1.0; + + + // ------------------------------------------------------------------------- + // OBSERVATION MARKER HELPERS (NEW) + // ------------------------------------------------------------------------- + + /** + * Places a colour-coded observation marker on the map and enforces the "last N" cap. + * + * HOW THE CAP WORKS: + * Each positioning source (PDR, GNSS, WiFi) has its own list and its own maxSize. + * When a new marker is added and the list exceeds maxSize, the oldest marker is + * removed from both the list and the map. This implements a sliding window of + * the most recent N observations, as required by assignment brief section 3.3: + * "Display colour coded absolute position updates (last N observations)". + * + * WHY A SHARED HELPER METHOD: + * All three sources (PDR, GNSS, WiFi) need the same add-and-cap logic, just with + * different colours and caps. Using one helper avoids duplicating the same 15 lines + * three times and makes it easy to change the logic in one place. + * + * @param position Where to place the marker on the map. + * @param title Label shown when the marker is tapped (e.g. "PDR", "GNSS", "WiFi"). + * @param hue Colour of the marker pin (use BitmapDescriptorFactory.HUE_* constants). + * @param markerList The list for this source (pdrObservationMarkers, gnssObservationMarkers, etc.). + * @param maxSize Maximum number of markers to keep visible for this source. + */ + private void addObservationMarker( + LatLng position, + String title, + float hue, + List markerList, + int maxSize + ) { + if (gMap == null) + { + Log.d("DisplayDebug", "addObservationMarker skipped: gMap == null, title=" + title); + return; + } + + Log.d("DisplayDebug", "addObservationMarker title=" + title + ", pos=" + position); + + Marker marker = gMap.addMarker(new MarkerOptions() + .position(position) + .title(title) + .icon(BitmapDescriptorFactory.defaultMarker(hue))); + + if (marker != null) { + markerList.add(marker); + } + // If the list now exceeds the cap, remove the oldest marker from the map and the list + + if (markerList.size() > maxSize) { + Marker oldest = markerList.remove(0);// remove from front (oldest) + oldest.remove(); // remove from the map + } + } public TrajectoryMapFragment() { // Required empty public constructor @@ -246,6 +374,16 @@ private void initMapSettings(GoogleMap map) { .width(5f) .add() // start empty ); + + // NEW: Orange polyline — filtered PDR trajectory. + // Only extended when user moves >= PDR_OBSERVATION_MIN_DISTANCE_M (1m). + // This is the cleaner, smoother path shown to the user as their walking trail. + // When sensor fusion is integrated, this will be replaced by the fused position path. + pdrTrajectoryPolyline = map.addPolyline(new PolylineOptions() + .color(Color.rgb(255, 165, 0)) + .width(5f) + .add() + ); } @@ -413,35 +551,185 @@ public void addTestPointMarker(int index, long timestampMs, @NonNull LatLng posi } + // ------------------------------------------------------------------------- + // GNSS UPDATE (ORIGINAL, EXTENDED) + // ------------------------------------------------------------------------- + /** - * Called when we want to set or update the GNSS marker position + * Updates the GNSS marker position and conditionally adds a new blue observation dot. + * + * WHAT CHANGED FROM ORIGINAL: + * The original only moved the GNSS marker and extended the blue polyline on every update. + * Now it also calls {@link #addObservationMarker} to place a capped blue dot, but only + * when {@link #shouldAddGnssObservation} confirms the new fix is > 3m from the last one. + * This prevents the blue dots from piling up when GNSS jitters in place indoors. + * + * This method only runs when the GNSS switch in the UI is toggled ON. + * + * @param gnssLocation The raw GNSS position from SensorTypes.GNSSLATLONG. */ public void updateGNSS(@NonNull LatLng gnssLocation) { - if (gMap == null) return; - if (!isGnssOn) return; + Log.d("DisplayDebug", "updateGNSS called: " + gnssLocation); + if (gMap == null) + { + Log.d("DisplayDebug", "updateGNSS skipped: gMap == null"); + return; + } + if (!isGnssOn) { + Log.d("DisplayDebug", "updateGNSS skipped: GNSS switch off"); + return; + } + boolean isNewGnssPoint = shouldAddGnssObservation(gnssLocation); + // Always move the live GNSS marker to the latest fix if (gnssMarker == null) { - // Create the GNSS marker for the first time gnssMarker = gMap.addMarker(new MarkerOptions() .position(gnssLocation) .title("GNSS Position") - .icon(BitmapDescriptorFactory - .defaultMarker(BitmapDescriptorFactory.HUE_AZURE))); - lastGnssLocation = gnssLocation; + .icon(BitmapDescriptorFactory.defaultMarker(BitmapDescriptorFactory.HUE_AZURE))); } else { - // Move existing GNSS marker gnssMarker.setPosition(gnssLocation); + } + + // Only extend the blue polyline and add an observation dot when significantly moved + if (isNewGnssPoint) { + List gnssPoints = new ArrayList<>(gnssPolyline.getPoints()); + gnssPoints.add(gnssLocation); + gnssPolyline.setPoints(gnssPoints); + + addObservationMarker( + gnssLocation, + "GNSS", + BitmapDescriptorFactory.HUE_AZURE, + gnssObservationMarkers, + MAX_GNSS_OBSERVATIONS + ); + + - // Add a segment to the blue GNSS line, if this is a new location - if (lastGnssLocation != null && !lastGnssLocation.equals(gnssLocation)) { - List gnssPoints = new ArrayList<>(gnssPolyline.getPoints()); - gnssPoints.add(gnssLocation); - gnssPolyline.setPoints(gnssPoints); - } lastGnssLocation = gnssLocation; } } + /** + * Decides whether a new GNSS fix is far enough from the last one to warrant + * adding a new observation marker and extending the blue polyline. + * + * WHY 3 METRES: + * GNSS receivers indoors report positions that fluctuate by several metres even + * when the user is stationary, due to signal reflection off walls (multipath). + * A 3m threshold filters out this stationary jitter while still capturing + * meaningful movement. + * + * @param newLocation The incoming GNSS fix to evaluate. + * @return true if a new observation marker should be added, false to skip. + */ + private boolean shouldAddGnssObservation(LatLng newLocation) { + if (lastGnssLocation == null) return true; + + double dist = UtilFunctions.distanceBetweenPoints(lastGnssLocation, newLocation); + return dist > 3.0; // only calculate new observation if distance > 3m + } + + // ------------------------------------------------------------------------- + // PDR OBSERVATION UPDATE (NEW) + // ------------------------------------------------------------------------- + + /** + * Adds a new orange PDR observation marker and extends the orange trajectory polyline, + * but only if the user has moved at least PDR_OBSERVATION_MIN_DISTANCE_M (1m) since + * the last accepted observation. + * + * WHY THIS METHOD EXISTS (was not in the original): + * The original code had no PDR observation display at all — PDR was only used internally + * to move the navigation arrow. This method adds the visible orange dot trail and the + * smooth orange trajectory line required by assignment brief section 3.3. + * + * TWO THINGS HAPPEN WHEN A POINT IS ACCEPTED: + * 1. An orange dot (observation marker) is placed and the oldest dot is removed if + * the count exceeds MAX_PDR_OBSERVATIONS, keeping a rolling trail of recent positions. + * 2. The orange pdrTrajectoryPolyline is extended, building up the full walking path. + * + * Called by RecordingFragment.updateUIandPosition() on every sensor update cycle. + * + * @param pdrLocation The current PDR-computed position in WGS84 coordinates. + */ + public void updatePdrObservation(@NonNull LatLng pdrLocation) { + Log.d("DisplayDebug", "updatePdrObservation called: " + pdrLocation); + if (gMap == null) { + Log.d("DisplayDebug", "updatePdrObservation skipped: gMap == null"); + return; + } + + // Distance threshold: skip this update if the user hasn't moved far enough. + // Without this, standing still produces hundreds of overlapping markers per minute. + + // Only add a new PDR observation marker if the user has moved at least 1 metre + if (lastPdrObservationLocation != null) { + double dist = UtilFunctions.distanceBetweenPoints(lastPdrObservationLocation, pdrLocation); + if (dist < PDR_OBSERVATION_MIN_DISTANCE_M) { + Log.d("DisplayDebug", "updatePdrObservation skipped: too close (" + + String.format("%.2f", dist) + "m < " + PDR_OBSERVATION_MIN_DISTANCE_M + "m)"); + return; + } + } + // Update the reference point for the next distance check + lastPdrObservationLocation = pdrLocation; + + // Place a new orange dot; the helper removes the oldest dot if cap is exceeded + addObservationMarker( + pdrLocation, + "PDR", + BitmapDescriptorFactory.HUE_ORANGE, + pdrObservationMarkers, + MAX_PDR_OBSERVATIONS + ); + + // Extend the orange trajectory polyline with this accepted position. + // This builds the continuous walking path shown on the map. + if (pdrTrajectoryPolyline != null) { + List points = new ArrayList<>(pdrTrajectoryPolyline.getPoints()); + points.add(pdrLocation); + pdrTrajectoryPolyline.setPoints(points); + } + } + + // ------------------------------------------------------------------------- + // WIFI OBSERVATION UPDATE (NEW) + // ------------------------------------------------------------------------- + + /** + * Places a green WiFi observation marker on the map. + * + * WHY THIS METHOD EXISTS (was not in the original): + * The original code had no WiFi position display. This method is called by + * RecordingFragment whenever the openpositioning API returns a valid WiFi fix. + * When inside the Nucleus/Library buildings, WiFi positioning returns a fix + * every few seconds; this method places a green dot at that location. + * + * NOTE: WiFi observations are not distance-filtered (unlike GNSS and PDR) because + * they are already sparse — the API typically only returns a new fix every few seconds, + * and each fix represents a genuinely different computed position, not sensor noise. + * + * @param wifiLocation The position returned by the openpositioning WiFi API. + */ + public void updateWifiObservation(@NonNull LatLng wifiLocation) { + Log.d("DisplayDebug", "updateWifiObservation called: " + wifiLocation); + + if (gMap == null) { + Log.d("DisplayDebug", "updateWifiObservation skipped: gMap == null"); + return; + } + + addObservationMarker( + wifiLocation, + "WiFi", + BitmapDescriptorFactory.HUE_GREEN, + wifiObservationMarkers, + MAX_WIFI_OBSERVATIONS + ); + } + /** * Remove GNSS marker if user toggles it off @@ -509,8 +797,27 @@ public void clearMapAndReset() { } testPointMarkers.clear(); + // NEW: Clear GNSS observation dots + for (Marker m : gnssObservationMarkers) { + m.remove(); + } + gnssObservationMarkers.clear(); + + // NEW: Clear WiFi observation dots + for (Marker m : wifiObservationMarkers) { + m.remove(); + } + wifiObservationMarkers.clear(); + + // NEW: Clear WiFi observation dots + for (Marker m : pdrObservationMarkers) { + m.remove(); + } + pdrObservationMarkers.clear(); + + - // Re-create empty polylines with your chosen colors + // Recreate empty polylines so the new session can start drawing immediately if (gMap != null) { polyline = gMap.addPolyline(new PolylineOptions() .color(Color.RED) @@ -520,6 +827,17 @@ public void clearMapAndReset() { .color(Color.BLUE) .width(5f) .add()); + // NEW: Remove and recreate the orange PDR trajectory polyline + if (pdrTrajectoryPolyline != null) { + pdrTrajectoryPolyline.remove(); + pdrTrajectoryPolyline = null; + } + // Reset the distance filter anchor so the first new point is always accepted + lastPdrObservationLocation = null; + pdrTrajectoryPolyline = gMap.addPolyline(new PolylineOptions() + .color(Color.rgb(255, 165, 0)) + .width(5f) + .add()); } } From cb2ddcdcfd3392e3374f1c320b61a5ba4ce9a338 Mon Sep 17 00:00:00 2001 From: Saleh-AlMulla Date: Mon, 23 Mar 2026 16:05:18 +0000 Subject: [PATCH 05/26] Add particle filter map-matching engine Introduce a MapMatchingEngine and Particle classes implementing a particle-filter based indoor map-matching (PDR propagation, wall/stairs/lift constraints, WiFi/GNSS likelihoods and systematic resampling). Integrate the engine into the app lifecycle: SensorFusion now owns the engine and exposes initialise/getter methods; SensorEventHandler calls predict() on each PDR step (uses new lastPdrX/Y in SensorState); WifiPositionManager forwards WiFi fixes to the engine; StartLocationFragment initialises the filter when recording begins inside a known building. Includes utilities for geometry checks, floor transition logic and estimated position/floor outputs. --- .../mapmatching/MapMatchingEngine.java | 635 ++++++++++++++++++ .../PositionMe/mapmatching/Particle.java | 63 ++ .../fragment/StartLocationFragment.java | 23 + .../sensors/SensorEventHandler.java | 20 +- .../PositionMe/sensors/SensorFusion.java | 23 +- .../PositionMe/sensors/SensorState.java | 4 + .../sensors/WifiPositionManager.java | 12 + 7 files changed, 778 insertions(+), 2 deletions(-) create mode 100644 app/src/main/java/com/openpositioning/PositionMe/mapmatching/MapMatchingEngine.java create mode 100644 app/src/main/java/com/openpositioning/PositionMe/mapmatching/Particle.java diff --git a/app/src/main/java/com/openpositioning/PositionMe/mapmatching/MapMatchingEngine.java b/app/src/main/java/com/openpositioning/PositionMe/mapmatching/MapMatchingEngine.java new file mode 100644 index 00000000..efa0e40d --- /dev/null +++ b/app/src/main/java/com/openpositioning/PositionMe/mapmatching/MapMatchingEngine.java @@ -0,0 +1,635 @@ +package com.openpositioning.PositionMe.mapmatching; + +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.List; +import java.util.Random; + +/** + * Particle filter engine for indoor map matching. + * + *

Uses a set of weighted particles to estimate the user's position and floor. + * Each particle is propagated using PDR displacement, then weighted against map + * constraints (walls, stairs, lifts) and positioning observations (WiFi, GNSS). + * Systematic resampling keeps the particle population healthy.

+ * + *

Map constraint types (from floorplan API):

+ *
    + *
  • wall — impassable barriers; particles crossing them are penalised
    • + *
  • stairs — floor transitions with height change
    • + *
  • lift — floor transitions without significant horizontal displacement
    • + *
+ * + *

Integration points:

+ *
    + *
  • {@link com.openpositioning.PositionMe.sensors.SensorFusion} — owns the engine instance
    • + *
  • {@link com.openpositioning.PositionMe.sensors.SensorEventHandler} — calls + * {@link #predict(float, float, float)} on each PDR step
    • + *
  • {@link com.openpositioning.PositionMe.sensors.WifiPositionManager} — calls + * {@link #updateWifi(double, double, int, float)} on WiFi position updates
    • + *
+ * + * @see Particle individual particle state + */ +public class MapMatchingEngine { + + private static final String TAG = "MapMatchingEngine"; + + // --- Tuneable parameters --- + + /** Number of particles in the filter. */ + private static final int NUM_PARTICLES = 200; + + /** Standard deviation of noise added to PDR step length (metres). */ + private static final double PDR_NOISE_STEP = 0.15; + + /** Standard deviation of noise added to PDR heading (radians). */ + private static final double PDR_NOISE_HEADING = Math.toRadians(8); + + /** Standard deviation for WiFi observation likelihood (metres). */ + private static final double WIFI_SIGMA = 8.0; + + /** Standard deviation for GNSS observation likelihood (metres). */ + private static final double GNSS_SIGMA = 12.0; + + /** Weight penalty multiplier for particles that cross a wall. */ + private static final double WALL_PENALTY = 0.01; + + /** Distance threshold (metres) to consider a particle "near" stairs or lift. */ + private static final double TRANSITION_PROXIMITY = 4.0; + + /** Minimum elevation change (metres) to trigger a floor change attempt. */ + private static final double FLOOR_CHANGE_ELEVATION_THRESHOLD = 2.0; + + /** Effective sample size ratio below which resampling is triggered. */ + private static final double RESAMPLE_THRESHOLD = 0.5; + + /** Spread radius (metres) for initial particle distribution. */ + private static final double INIT_SPREAD = 3.0; + + // --- Coordinate conversion constants --- + // Approximate metres per degree at Edinburgh latitude (~55.92°N) + private static final double METRES_PER_DEG_LAT = 111_320.0; + private static final double METRES_PER_DEG_LNG = + 111_320.0 * Math.cos(Math.toRadians(55.92)); + + // --- State --- + + private final List particles = new ArrayList<>(); + private final Random random = new Random(); + + /** Reference point (latitude, longitude) for local coordinate conversion. */ + private double refLat; + private double refLng; + private boolean initialised = false; + + /** Current floor shapes from the floorplan API for wall/stair/lift checking. */ + private List floorShapes; + + /** Latest elevation relative to start (from barometer via PdrProcessing). */ + private float currentElevation = 0f; + + /** Floor height of the current building (metres). */ + private float floorHeight = 4.0f; + + /** Current estimated floor from the particle filter. */ + private int estimatedFloor = 0; + + /** Whether the engine is actively running. */ + private boolean enabled = false; + + // ===================================================================== + // Initialisation + // ===================================================================== + + /** + * Initialises the particle filter around a starting position. + * + * @param lat starting latitude (WGS84) + * @param lng starting longitude (WGS84) + * @param floor starting floor index + * @param floorHeight floor height of the current building (metres) + * @param shapes per-floor vector shape data from floorplan API (may be null) + */ + public void initialise(double lat, double lng, int floor, + float floorHeight, List shapes) { + this.refLat = lat; + this.refLng = lng; + this.floorHeight = floorHeight; + this.floorShapes = shapes; + this.estimatedFloor = floor; + + particles.clear(); + for (int i = 0; i < NUM_PARTICLES; i++) { + double px = random.nextGaussian() * INIT_SPREAD; + double py = random.nextGaussian() * INIT_SPREAD; + particles.add(new Particle(px, py, floor, 1.0 / NUM_PARTICLES)); + } + + this.initialised = true; + this.enabled = true; + Log.d(TAG, "Initialised with " + NUM_PARTICLES + " particles at (" + + lat + ", " + lng + ") floor " + floor); + } + + /** + * Returns whether the engine has been initialised and is active. + */ + public boolean isActive() { + return initialised && enabled; + } + + /** + * Enables or disables the engine without clearing particles. + */ + public void setEnabled(boolean enabled) { + this.enabled = enabled; + } + + /** + * Resets the engine, clearing all particles and state. + */ + public void reset() { + particles.clear(); + initialised = false; + enabled = false; + currentElevation = 0f; + estimatedFloor = 0; + } + + /** + * Updates the floor shapes used for wall/stair/lift constraints. + * Call this when the user enters a new building. + * + * @param shapes per-floor shape data + */ + public void setFloorShapes(List shapes) { + this.floorShapes = shapes; + } + + /** + * Updates the floor height for the current building. + */ + public void setFloorHeight(float height) { + this.floorHeight = height; + } + + // ===================================================================== + // Prediction step — called on each PDR step + // ===================================================================== + + /** + * Propagates all particles using the PDR displacement and applies map constraints. + * + *

Called from {@code SensorEventHandler} on each detected step. The heading + * is the device orientation in radians (0 = north, clockwise positive).

+ * + * @param stepLength step length in metres from PDR + * @param headingRad heading in radians (north = 0, clockwise) + * @param elevation current relative elevation from barometer (metres) + */ + public void predict(float stepLength, float headingRad, float elevation) { + if (!isActive()) return; + + this.currentElevation = elevation; + + for (Particle p : particles) { + // Add noise to step length and heading + double noisyStep = stepLength + random.nextGaussian() * PDR_NOISE_STEP; + double noisyHeading = headingRad + random.nextGaussian() * PDR_NOISE_HEADING; + + // Clamp step length to reasonable bounds + if (noisyStep < 0) noisyStep = 0; + if (noisyStep > 2.0) noisyStep = 2.0; + + // Convert heading to local XY displacement + // heading: 0=north, pi/2=east => dx = step*sin(h), dy = step*cos(h) + double dx = noisyStep * Math.sin(noisyHeading); + double dy = noisyStep * Math.cos(noisyHeading); + + double oldX = p.getX(); + double oldY = p.getY(); + double newX = oldX + dx; + double newY = oldY + dy; + + // Wall collision check + if (doesCrossWall(oldX, oldY, newX, newY, p.getFloor())) { + // Penalise but don't move — particle stays at old position + p.setWeight(p.getWeight() * WALL_PENALTY); + } else { + p.setX(newX); + p.setY(newY); + } + } + + // Check for floor transitions based on elevation change + checkFloorTransition(); + + // Normalise weights and conditionally resample + normaliseWeights(); + if (effectiveSampleSize() < RESAMPLE_THRESHOLD * NUM_PARTICLES) { + resample(); + } + } + + // ===================================================================== + // Observation updates + // ===================================================================== + + /** + * Updates particle weights based on a WiFi position observation. + * + * @param lat WiFi-estimated latitude + * @param lng WiFi-estimated longitude + * @param wifiFloor WiFi-estimated floor number + * @param accuracy estimated accuracy in metres (used to scale sigma) + */ + public void updateWifi(double lat, double lng, int wifiFloor, float accuracy) { + if (!isActive()) return; + + double obsX = (lng - refLng) * METRES_PER_DEG_LNG; + double obsY = (lat - refLat) * METRES_PER_DEG_LAT; + double sigma = Math.max(WIFI_SIGMA, accuracy); + + for (Particle p : particles) { + double dist = Math.sqrt(Math.pow(p.getX() - obsX, 2) + + Math.pow(p.getY() - obsY, 2)); + double likelihood = gaussianLikelihood(dist, sigma); + + // Floor mismatch penalty + if (p.getFloor() != wifiFloor) { + likelihood *= 0.1; + } + + p.setWeight(p.getWeight() * likelihood); + } + + normaliseWeights(); + if (effectiveSampleSize() < RESAMPLE_THRESHOLD * NUM_PARTICLES) { + resample(); + } + + Log.d(TAG, "WiFi update at (" + lat + ", " + lng + ") floor " + wifiFloor); + } + + /** + * Updates particle weights based on a GNSS position observation. + * + * @param lat GNSS latitude + * @param lng GNSS longitude + * @param accuracy GNSS accuracy in metres + */ + public void updateGnss(double lat, double lng, float accuracy) { + if (!isActive()) return; + + double obsX = (lng - refLng) * METRES_PER_DEG_LNG; + double obsY = (lat - refLat) * METRES_PER_DEG_LAT; + double sigma = Math.max(GNSS_SIGMA, accuracy); + + for (Particle p : particles) { + double dist = Math.sqrt(Math.pow(p.getX() - obsX, 2) + + Math.pow(p.getY() - obsY, 2)); + double likelihood = gaussianLikelihood(dist, sigma); + p.setWeight(p.getWeight() * likelihood); + } + + normaliseWeights(); + } + + // ===================================================================== + // Floor transition logic + // ===================================================================== + + /** + * Checks if the barometric elevation change warrants a floor transition. + * Particles near stairs/lifts are allowed to change floors; others are penalised. + */ + private void checkFloorTransition() { + if (floorShapes == null || floorHeight <= 0) return; + + // Calculate expected floor offset from elevation + int elevationFloorDelta = (int) Math.round(currentElevation / floorHeight); + int targetFloor = estimatedFloor + elevationFloorDelta; + + // Only act if elevation suggests we've moved at least one floor + if (Math.abs(currentElevation) < FLOOR_CHANGE_ELEVATION_THRESHOLD) return; + + for (Particle p : particles) { + if (p.getFloor() == targetFloor) continue; // already on target floor + + boolean nearTransition = isNearStairsOrLift(p.getX(), p.getY(), p.getFloor()); + + if (nearTransition) { + // Allow floor change + p.setFloor(clampFloor(targetFloor)); + // Slight bonus for particles that can transition + p.setWeight(p.getWeight() * 1.2); + } else { + // Penalise particles that should change floor but aren't near stairs/lift + p.setWeight(p.getWeight() * 0.5); + } + } + } + + /** + * Checks if a position is near stairs or lift features on the given floor. + * + * @param x easting in metres + * @param y northing in metres + * @param floor floor index + * @return true if within {@link #TRANSITION_PROXIMITY} of a stair or lift feature + */ + private boolean isNearStairsOrLift(double x, double y, int floor) { + if (floorShapes == null || floor < 0 || floor >= floorShapes.size()) return false; + + FloorplanApiClient.FloorShapes floorData = floorShapes.get(floor); + for (FloorplanApiClient.MapShapeFeature feature : floorData.getFeatures()) { + String type = feature.getIndoorType(); + if ("stairs".equals(type) || "lift".equals(type)) { + for (List part : feature.getParts()) { + for (LatLng point : part) { + double fx = (point.longitude - refLng) * METRES_PER_DEG_LNG; + double fy = (point.latitude - refLat) * METRES_PER_DEG_LAT; + double dist = Math.sqrt(Math.pow(x - fx, 2) + Math.pow(y - fy, 2)); + if (dist < TRANSITION_PROXIMITY) { + return true; + } + } + } + } + } + return false; + } + + /** + * Clamps a floor index to valid bounds. + */ + private int clampFloor(int floor) { + if (floorShapes == null || floorShapes.isEmpty()) return floor; + return Math.max(0, Math.min(floor, floorShapes.size() - 1)); + } + + // ===================================================================== + // Wall collision detection + // ===================================================================== + + /** + * Tests whether moving from (x1,y1) to (x2,y2) crosses any wall segment + * on the given floor. + * + * @return true if the movement line intersects a wall + */ + private boolean doesCrossWall(double x1, double y1, double x2, double y2, int floor) { + if (floorShapes == null || floor < 0 || floor >= floorShapes.size()) return false; + + FloorplanApiClient.FloorShapes floorData = floorShapes.get(floor); + for (FloorplanApiClient.MapShapeFeature feature : floorData.getFeatures()) { + if (!"wall".equals(feature.getIndoorType())) continue; + + for (List part : feature.getParts()) { + for (int i = 0; i < part.size() - 1; i++) { + LatLng a = part.get(i); + LatLng b = part.get(i + 1); + + double ax = (a.longitude - refLng) * METRES_PER_DEG_LNG; + double ay = (a.latitude - refLat) * METRES_PER_DEG_LAT; + double bx = (b.longitude - refLng) * METRES_PER_DEG_LNG; + double by = (b.latitude - refLat) * METRES_PER_DEG_LAT; + + if (segmentsIntersect(x1, y1, x2, y2, ax, ay, bx, by)) { + return true; + } + } + + // For polygons, also check the closing edge + if (part.size() >= 3) { + String geoType = feature.getGeometryType(); + if ("MultiPolygon".equals(geoType) || "Polygon".equals(geoType)) { + LatLng first = part.get(0); + LatLng last = part.get(part.size() - 1); + double fx = (first.longitude - refLng) * METRES_PER_DEG_LNG; + double fy = (first.latitude - refLat) * METRES_PER_DEG_LAT; + double lx = (last.longitude - refLng) * METRES_PER_DEG_LNG; + double ly = (last.latitude - refLat) * METRES_PER_DEG_LAT; + + if (segmentsIntersect(x1, y1, x2, y2, lx, ly, fx, fy)) { + return true; + } + } + } + } + } + return false; + } + + /** + * Tests if two line segments (p1-p2) and (p3-p4) intersect using the + * cross-product orientation method. + * + * @return true if segments intersect + */ + private static boolean segmentsIntersect(double p1x, double p1y, double p2x, double p2y, + double p3x, double p3y, double p4x, double p4y) { + double d1 = crossProduct(p3x, p3y, p4x, p4y, p1x, p1y); + double d2 = crossProduct(p3x, p3y, p4x, p4y, p2x, p2y); + double d3 = crossProduct(p1x, p1y, p2x, p2y, p3x, p3y); + double d4 = crossProduct(p1x, p1y, p2x, p2y, p4x, p4y); + + if (((d1 > 0 && d2 < 0) || (d1 < 0 && d2 > 0)) + && ((d3 > 0 && d4 < 0) || (d3 < 0 && d4 > 0))) { + return true; + } + + // Collinear cases + if (d1 == 0 && onSegment(p3x, p3y, p4x, p4y, p1x, p1y)) return true; + if (d2 == 0 && onSegment(p3x, p3y, p4x, p4y, p2x, p2y)) return true; + if (d3 == 0 && onSegment(p1x, p1y, p2x, p2y, p3x, p3y)) return true; + if (d4 == 0 && onSegment(p1x, p1y, p2x, p2y, p4x, p4y)) return true; + + return false; + } + + /** + * Cross product of vectors (bx-ax, by-ay) and (cx-ax, cy-ay). + */ + private static double crossProduct(double ax, double ay, double bx, double by, + double cx, double cy) { + return (bx - ax) * (cy - ay) - (by - ay) * (cx - ax); + } + + /** + * Checks if point (px,py) lies on segment (ax,ay)-(bx,by), assuming collinearity. + */ + private static boolean onSegment(double ax, double ay, double bx, double by, + double px, double py) { + return Math.min(ax, bx) <= px && px <= Math.max(ax, bx) + && Math.min(ay, by) <= py && py <= Math.max(ay, by); + } + + // ===================================================================== + // Resampling + // ===================================================================== + + /** + * Calculates the effective sample size (ESS) of the particle population. + * ESS = 1 / sum(w_i^2). Low ESS indicates weight degeneracy. + */ + private double effectiveSampleSize() { + double sumSq = 0; + for (Particle p : particles) { + sumSq += p.getWeight() * p.getWeight(); + } + return (sumSq > 0) ? 1.0 / sumSq : 0; + } + + /** + * Systematic resampling: selects particles proportional to their weights + * and replaces the population with copies of selected particles. + */ + private void resample() { + int n = particles.size(); + if (n == 0) return; + + // Build cumulative weight array + double[] cumulative = new double[n]; + cumulative[0] = particles.get(0).getWeight(); + for (int i = 1; i < n; i++) { + cumulative[i] = cumulative[i - 1] + particles.get(i).getWeight(); + } + + List newParticles = new ArrayList<>(n); + double step = 1.0 / n; + double start = random.nextDouble() * step; + + int idx = 0; + for (int i = 0; i < n; i++) { + double target = start + i * step; + while (idx < n - 1 && cumulative[idx] < target) { + idx++; + } + Particle copy = particles.get(idx).copy(); + copy.setWeight(1.0 / n); + newParticles.add(copy); + } + + particles.clear(); + particles.addAll(newParticles); + } + + /** + * Normalises all particle weights to sum to 1. + */ + private void normaliseWeights() { + double sum = 0; + for (Particle p : particles) { + sum += p.getWeight(); + } + if (sum <= 0) { + // Catastrophic weight collapse — reset to uniform + double uniform = 1.0 / particles.size(); + for (Particle p : particles) { + p.setWeight(uniform); + } + return; + } + for (Particle p : particles) { + p.setWeight(p.getWeight() / sum); + } + } + + // ===================================================================== + // Output — estimated position + // ===================================================================== + + /** + * Returns the weighted mean position of all particles as a LatLng. + * + * @return best-estimate position, or null if not initialised + */ + public LatLng getEstimatedPosition() { + if (!isActive() || particles.isEmpty()) return null; + + double weightedX = 0, weightedY = 0; + for (Particle p : particles) { + weightedX += p.getX() * p.getWeight(); + weightedY += p.getY() * p.getWeight(); + } + + double lat = refLat + weightedY / METRES_PER_DEG_LAT; + double lng = refLng + weightedX / METRES_PER_DEG_LNG; + return new LatLng(lat, lng); + } + + /** + * Returns the weighted-majority floor estimate from the particles. + * + * @return estimated floor index + */ + public int getEstimatedFloor() { + if (!isActive() || particles.isEmpty()) return estimatedFloor; + + // Accumulate weight per floor + java.util.Map floorWeights = new java.util.HashMap<>(); + for (Particle p : particles) { + floorWeights.merge(p.getFloor(), p.getWeight(), Double::sum); + } + + // Find floor with highest accumulated weight + int bestFloor = estimatedFloor; + double bestWeight = -1; + for (java.util.Map.Entry entry : floorWeights.entrySet()) { + if (entry.getValue() > bestWeight) { + bestWeight = entry.getValue(); + bestFloor = entry.getKey(); + } + } + + this.estimatedFloor = bestFloor; + return bestFloor; + } + + /** + * Returns the estimated position as local XY coordinates (metres from reference). + * + * @return float[2] with {x, y}, or null if not initialised + */ + public float[] getEstimatedXY() { + if (!isActive() || particles.isEmpty()) return null; + + double weightedX = 0, weightedY = 0; + for (Particle p : particles) { + weightedX += p.getX() * p.getWeight(); + weightedY += p.getY() * p.getWeight(); + } + return new float[]{(float) weightedX, (float) weightedY}; + } + + /** + * Returns the current particle list (read-only snapshot). + * Useful for debug visualisation. + * + * @return list of particle copies + */ + public List getParticles() { + List snapshot = new ArrayList<>(particles.size()); + for (Particle p : particles) { + snapshot.add(p.copy()); + } + return snapshot; + } + + // ===================================================================== + // Utility + // ===================================================================== + + /** + * Gaussian likelihood function: exp(-d^2 / (2 * sigma^2)). + */ + private static double gaussianLikelihood(double distance, double sigma) { + return Math.exp(-(distance * distance) / (2.0 * sigma * sigma)); + } +} diff --git a/app/src/main/java/com/openpositioning/PositionMe/mapmatching/Particle.java b/app/src/main/java/com/openpositioning/PositionMe/mapmatching/Particle.java new file mode 100644 index 00000000..e9e49bb9 --- /dev/null +++ b/app/src/main/java/com/openpositioning/PositionMe/mapmatching/Particle.java @@ -0,0 +1,63 @@ +package com.openpositioning.PositionMe.mapmatching; + +/** + * Represents a single particle in the particle filter for indoor map matching. + * + *

Each particle holds a 2D position in easting/northing (metres), a floor index, + * and a weight reflecting how well the particle agrees with observations and map + * constraints.

+ * + * @see MapMatchingEngine the particle filter that manages a population of particles + */ +public class Particle { + + /** Easting position in metres relative to the reference point. */ + private double x; + + /** Northing position in metres relative to the reference point. */ + private double y; + + /** Floor index (0-based, matching FloorShapes list order). */ + private int floor; + + /** Importance weight of this particle. */ + private double weight; + + /** + * Creates a new particle with the given position, floor, and weight. + * + * @param x easting in metres + * @param y northing in metres + * @param floor floor index + * @param weight importance weight + */ + public Particle(double x, double y, int floor, double weight) { + this.x = x; + this.y = y; + this.floor = floor; + this.weight = weight; + } + + /** + * Creates a deep copy of this particle. + * + * @return a new Particle with the same state + */ + public Particle copy() { + return new Particle(x, y, floor, weight); + } + + // --- Getters and setters --- + + public double getX() { return x; } + public void setX(double x) { this.x = x; } + + public double getY() { return y; } + public void setY(double y) { this.y = y; } + + public int getFloor() { return floor; } + public void setFloor(int floor) { this.floor = floor; } + + public double getWeight() { return weight; } + public void setWeight(double weight) { this.weight = weight; } +} 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..2842216e 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 @@ -37,6 +37,8 @@ import java.util.List; import java.util.Map; +import com.openpositioning.PositionMe.data.remote.FloorplanApiClient; + /** * Fragment for selecting the start location before recording begins. * Displays a Google Map with building outlines fetched from the floorplan API. @@ -474,6 +476,27 @@ public void onViewCreated(@NonNull View view, @Nullable Bundle savedInstanceStat // Write trajectory_id, initial_position and initial heading to protobuf sensorFusion.writeInitialMetadata(); + // Initialise particle filter if inside a known building + if (selectedBuildingId != null) { + FloorplanApiClient.BuildingInfo building = + sensorFusion.getFloorplanBuilding(selectedBuildingId); + if (building != null) { + int startFloor = sensorFusion.getWifiFloor(); + float floorHeight = 4.0f; + switch (selectedBuildingId) { + case "nucleus_building": floorHeight = 4.2f; break; + case "library": floorHeight = 3.6f; break; + case "murchison_house": floorHeight = 4.0f; break; + } + sensorFusion.initialiseMapMatching( + startPosition[0], startPosition[1], + startFloor, floorHeight, + building.getFloorShapesList() + ); + Log.d(TAG, "Map matching initialised for " + selectedBuildingId); + } + } + // Switch to the recording screen ((RecordingActivity) requireActivity()).showRecordingScreen(); 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..8428cf95 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java @@ -13,6 +13,8 @@ import java.util.HashMap; import java.util.List; +import com.openpositioning.PositionMe.mapmatching.MapMatchingEngine; + /** * Handles sensor event dispatching for all registered movement sensors. * @@ -30,6 +32,7 @@ public class SensorEventHandler { private final PathView pathView; private final TrajectoryRecorder recorder; + private final MapMatchingEngine mapMatchingEngine; // Timestamp tracking private final HashMap lastEventTimestamps = new HashMap<>(); private final HashMap eventCounts = new HashMap<>(); @@ -50,12 +53,13 @@ public class SensorEventHandler { */ public SensorEventHandler(SensorState state, PdrProcessing pdrProcessing, PathView pathView, TrajectoryRecorder recorder, - long bootTime) { + long bootTime, MapMatchingEngine mapMatchingEngine) { this.state = state; this.pdrProcessing = pdrProcessing; this.pathView = pathView; this.recorder = recorder; this.bootTime = bootTime; + this.mapMatchingEngine = mapMatchingEngine; } /** @@ -173,6 +177,20 @@ public void handleSensorEvent(SensorEvent sensorEvent) { this.accelMagnitude.clear(); + // Propagate particles using PDR step displacement + if (mapMatchingEngine != null && mapMatchingEngine.isActive()) { + float dx = newCords[0] - state.lastPdrX; + float dy = newCords[1] - state.lastPdrY; + float stepLen = (float) Math.sqrt(dx * dx + dy * dy); + mapMatchingEngine.predict( + stepLen, + state.orientation[0], + state.elevation + ); + state.lastPdrX = newCords[0]; + state.lastPdrY = newCords[1]; + } + if (recorder.isRecording()) { this.pathView.drawTrajectory(newCords); state.stepCounter++; 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..939c3dad 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -32,6 +32,8 @@ import java.util.stream.Collectors; import java.util.stream.Stream; +import com.openpositioning.PositionMe.mapmatching.MapMatchingEngine; + /** * The SensorFusion class is the main data gathering and processing class of the application. * @@ -88,6 +90,8 @@ public class SensorFusion implements SensorEventListener { private PdrProcessing pdrProcessing; private PathView pathView; + private final MapMatchingEngine mapMatchingEngine = new MapMatchingEngine(); + // Sensor registration latency setting long maxReportLatencyNs = 0; @@ -162,7 +166,7 @@ public void setContext(Context context) { long bootTime = SystemClock.uptimeMillis(); this.eventHandler = new SensorEventHandler( - state, pdrProcessing, pathView, recorder, bootTime); + state, pdrProcessing, pathView, recorder, bootTime, mapMatchingEngine); // Register WiFi observer on WifiPositionManager (not on SensorFusion) this.wifiProcessor = new WifiDataProcessor(context); @@ -482,6 +486,23 @@ public float[] getGNSSLatitude(boolean start) { return latLong; } + /** + * Returns the map matching particle filter engine. + */ + public MapMatchingEngine getMapMatchingEngine() { + return mapMatchingEngine; + } + + /** + * Initialises map matching with current position and building data. + * Called when user starts recording inside a building. + */ + public void initialiseMapMatching(double lat, double lng, int floor, + float floorHeight, + java.util.List shapes) { + mapMatchingEngine.initialise(lat, lng, floor, floorHeight, shapes); + } + /** * Setter function for core location data. * 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..a89dd11a 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorState.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorState.java @@ -39,4 +39,8 @@ public class SensorState { // Step counting public volatile int stepCounter; + + // Last PDR position for step length calculation (used by map matching) + public volatile float lastPdrX = 0f; + public volatile float lastPdrY = 0f; } 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..e0203fca 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java @@ -11,6 +11,8 @@ import java.util.stream.Collectors; import java.util.stream.Stream; +import com.openpositioning.PositionMe.mapmatching.MapMatchingEngine; + /** * Manages WiFi scan result processing and WiFi-based positioning requests. * @@ -66,6 +68,16 @@ private void createWifiPositioningRequest() { JSONObject wifiFingerPrint = new JSONObject(); wifiFingerPrint.put(WIFI_FINGERPRINT, wifiAccessPoints); this.wiFiPositioning.request(wifiFingerPrint); + + // Feed WiFi position to map matching engine + LatLng wifiPos = this.wiFiPositioning.getWifiLocation(); + int wifiFloor = this.wiFiPositioning.getFloor(); + if (wifiPos != null) { + MapMatchingEngine engine = SensorFusion.getInstance().getMapMatchingEngine(); + if (engine != null && engine.isActive()) { + engine.updateWifi(wifiPos.latitude, wifiPos.longitude, wifiFloor, 10.0f); + } + } } catch (JSONException e) { Log.e("jsonErrors", "Error creating json object" + e.toString()); } From 927024004830ee04440b1e2df0b9eb3feeb85201 Mon Sep 17 00:00:00 2001 From: Saleh-AlMulla Date: Mon, 23 Mar 2026 16:26:44 +0000 Subject: [PATCH 06/26] Comment out debug Log in MapMatchingEngine Commented out a Log.d debug statement inside the particle prediction loop in MapMatchingEngine to reduce log noise during normal runs while preserving the code for future debugging. --- .../PositionMe/mapmatching/MapMatchingEngine.java | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/app/src/main/java/com/openpositioning/PositionMe/mapmatching/MapMatchingEngine.java b/app/src/main/java/com/openpositioning/PositionMe/mapmatching/MapMatchingEngine.java index efa0e40d..4f2ac6ba 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/mapmatching/MapMatchingEngine.java +++ b/app/src/main/java/com/openpositioning/PositionMe/mapmatching/MapMatchingEngine.java @@ -224,6 +224,11 @@ public void predict(float stepLength, float headingRad, float elevation) { p.setX(newX); p.setY(newY); } + +/* Log.d(TAG, "Predict: particles=" + particles.size() + + " pos=" + getEstimatedPosition() + + " floor=" + getEstimatedFloor());*/ + } // Check for floor transitions based on elevation change From 8cab562eb166a676ae6becc2f0b4ca65211a4500 Mon Sep 17 00:00:00 2001 From: Saleh-AlMulla Date: Mon, 23 Mar 2026 17:02:11 +0000 Subject: [PATCH 07/26] Update TrajectoryMapFragment.java --- .../PositionMe/presentation/fragment/TrajectoryMapFragment.java | 1 - 1 file changed, 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 479ea51b..9802bef6 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 @@ -442,7 +442,6 @@ public void updateGNSS(@NonNull LatLng gnssLocation) { } } - /** * Remove GNSS marker if user toggles it off */ From 804fd8b36e73a5c769c502a4c506628b585d09f9 Mon Sep 17 00:00:00 2001 From: jagsnapuri Date: Sun, 29 Mar 2026 14:38:18 +0100 Subject: [PATCH 08/26] First draft - Particle filter algorithm --- .../sensors/SensorEventHandler.java | 9 +- .../PositionMe/sensors/SensorFusion.java | 6 +- .../sensors/WifiPositionManager.java | 13 +- .../main/java/positioning/FusionManager.java | 52 ------ .../main/java/positioning/ParticleFilter.java | 167 ++++++++++++++++++ 5 files changed, 188 insertions(+), 59 deletions(-) delete mode 100644 app/src/main/java/positioning/FusionManager.java create mode 100644 app/src/main/java/positioning/ParticleFilter.java 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 f45b0562..084620b1 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java @@ -173,9 +173,12 @@ public void handleSensorEvent(SensorEvent sensorEvent) { ); this.accelMagnitude.clear(); - FusionManager.getInstance().onStep( // ADD THIS - pdrProcessing.getAverageStepLength(), // ADD THIS - state.orientation[0]); // + + float avgStepLength = pdrProcessing.getAverageStepLength(); + float heading = state.orientation[0]; + if (!Float.isNaN(avgStepLength) && avgStepLength > 0.05f && !Float.isNaN(heading)) { + FusionManager.getInstance().onStep(avgStepLength, heading); + } if (recorder.isRecording()) { this.pathView.drawTrajectory(newCords); 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 9b1aaf0d..3310076f 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -327,6 +327,7 @@ private void stopWirelessCollectors() { * @see SensorCollectionService */ public void startRecording() { + FusionManager.getInstance().reset(); recorder.startRecording(pdrProcessing); eventHandler.resetBootTime(recorder.getBootTime()); @@ -646,7 +647,10 @@ public void onLocationChanged(@NonNull Location location) { state.latitude = (float) location.getLatitude(); state.longitude = (float) location.getLongitude(); recorder.addGnssData(location); - FusionManager.getInstance().onGnss(location.getLatitude(), location.getLongitude()); + FusionManager.getInstance().onGnss( + location.getLatitude(), + location.getLongitude(), + location.getAccuracy()); } } 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 92110279..fe02ae4f 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java @@ -76,6 +76,9 @@ private void createWifiPositioningRequest() { * Creates a WiFi positioning request using the Volley callback pattern. */ private void createWifiPositionRequestCallback() { + if (this.wifiList == null || this.wifiList.isEmpty()) { + return; + } try { JSONObject wifiAccessPoints = new JSONObject(); for (Wifi data : this.wifiList) { @@ -86,13 +89,17 @@ private void createWifiPositionRequestCallback() { this.wiFiPositioning.request(wifiFingerPrint, new WiFiPositioning.VolleyCallback() { @Override public void onSuccess(LatLng wifiLocation, int floor) { - // Handle the success response - createWifiPositionRequestCallback(); + if (wifiLocation != null) { + FusionManager.getInstance().onWifi( + wifiLocation.latitude, + wifiLocation.longitude + ); + } } @Override public void onError(String message) { - // Handle the error response + Log.e("WifiPositionManager", "WiFi positioning failed: " + message); } }); } catch (JSONException e) { diff --git a/app/src/main/java/positioning/FusionManager.java b/app/src/main/java/positioning/FusionManager.java deleted file mode 100644 index 44318f5d..00000000 --- a/app/src/main/java/positioning/FusionManager.java +++ /dev/null @@ -1,52 +0,0 @@ -package com.openpositioning.PositionMe.positioning; - -import android.util.Log; - -public class FusionManager { - - private static final FusionManager instance = new FusionManager(); - - private EKFFilter ekf; - private CoordinateUtils coords; - private boolean ready = false; - - private FusionManager() {} - - public static FusionManager getInstance() { - return instance; - } - - public void onGnss(double lat, double lon) { - if (!ready) { - coords = new CoordinateUtils(lat, lon); - ekf = new EKFFilter(0, 0); - ready = true; - Log.d("FusionManager", "Initialised at " + lat + ", " + lon); - return; - } - double[] en = coords.toLocal(lat, lon); - ekf.update(en[0], en[1], EKFFilter.getGnssNoise()); - } - - public void onWifi(double lat, double lon) { - if (!ready) return; - double[] en = coords.toLocal(lat, lon); - ekf.update(en[0], en[1], EKFFilter.getWifiNoise()); - } - - public void onStep(double stepLen, double headingRad) { - if (!ready) return; - double dE = stepLen * Math.sin(headingRad); - double dN = stepLen * Math.cos(headingRad); - ekf.predict(dE, dN); - } - - public double[] getBestPosition() { - if (!ready) return null; - return coords.toGlobal(ekf.getEast(), ekf.getNorth()); - } - - public boolean isReady() { - return ready; - } -} \ No newline at end of file diff --git a/app/src/main/java/positioning/ParticleFilter.java b/app/src/main/java/positioning/ParticleFilter.java new file mode 100644 index 00000000..59064050 --- /dev/null +++ b/app/src/main/java/positioning/ParticleFilter.java @@ -0,0 +1,167 @@ +package com.openpositioning.PositionMe.positioning; + +import java.util.ArrayList; +import java.util.List; +import java.util.Random; + +public class ParticleFilter { + + private static class Particle { + double east; + double north; + double weight; + + Particle(double east, double north, double weight) { + this.east = east; + this.north = north; + this.weight = weight; + } + + Particle copy() { + return new Particle(east, north, weight); + } + } + + private static final double DEFAULT_INIT_SIGMA = 4.0; + private static final double MOTION_NOISE_BASE = 0.20; + private static final double MOTION_NOISE_STEP_SCALE = 0.12; + private static final double STEP_NOISE_SIGMA = 0.10; + private static final double HEADING_NOISE_SIGMA = 0.08; + private static final double GNSS_SIGMA_MIN = 3.0; + private static final double WIFI_SIGMA_DEFAULT = 6.0; + + private final Random rng = new Random(42); + private final List particles = new ArrayList<>(); + private final int particleCount; + private boolean initialised = false; + + public ParticleFilter(int particleCount) { + this.particleCount = particleCount; + } + + public boolean isInitialised() { + return initialised; + } + + public void initialise(double east, double north, double sigmaMeters) { + particles.clear(); + double sigma = Math.max(1.0, sigmaMeters > 0 ? sigmaMeters : DEFAULT_INIT_SIGMA); + for (int i = 0; i < particleCount; i++) { + particles.add(new Particle( + east + rng.nextGaussian() * sigma, + north + rng.nextGaussian() * sigma, + 1.0 / particleCount + )); + } + initialised = true; + } + + public void predict(double stepLength, double headingRad) { + if (!initialised || stepLength <= 0.05 || Double.isNaN(stepLength) || Double.isNaN(headingRad)) { + return; + } + + double motionSigma = MOTION_NOISE_BASE + MOTION_NOISE_STEP_SCALE * stepLength; + + for (Particle p : particles) { + double noisyStep = stepLength + rng.nextGaussian() * STEP_NOISE_SIGMA; + double noisyHeading = headingRad + rng.nextGaussian() * HEADING_NOISE_SIGMA; + + double dE = noisyStep * Math.sin(noisyHeading); + double dN = noisyStep * Math.cos(noisyHeading); + + p.east += dE + rng.nextGaussian() * motionSigma; + p.north += dN + rng.nextGaussian() * motionSigma; + } + } + + public void updateGnss(double obsEast, double obsNorth, float accuracyMeters) { + double sigma = Math.max(GNSS_SIGMA_MIN, accuracyMeters); + updateAbsolute(obsEast, obsNorth, sigma); + } + + public void updateWifi(double obsEast, double obsNorth) { + updateAbsolute(obsEast, obsNorth, WIFI_SIGMA_DEFAULT); + } + + private void updateAbsolute(double obsEast, double obsNorth, double sigma) { + if (!initialised) return; + + double sigma2 = sigma * sigma; + double totalWeight = 0.0; + + for (Particle p : particles) { + double dE = p.east - obsEast; + double dN = p.north - obsNorth; + double dist2 = dE * dE + dN * dN; + double likelihood = Math.exp(-0.5 * dist2 / sigma2); + p.weight *= Math.max(likelihood, 1e-12); + totalWeight += p.weight; + } + + if (totalWeight < 1e-20) { + initialise(obsEast, obsNorth, sigma); + return; + } + + normalise(totalWeight); + + if (effectiveSampleSize() < particleCount * 0.5) { + resampleSystematic(); + } + } + + private void normalise(double totalWeight) { + for (Particle p : particles) { + p.weight /= totalWeight; + } + } + + private double effectiveSampleSize() { + double sumSq = 0.0; + for (Particle p : particles) { + sumSq += p.weight * p.weight; + } + return sumSq <= 0.0 ? 0.0 : 1.0 / sumSq; + } + + private void resampleSystematic() { + List resampled = new ArrayList<>(particleCount); + double step = 1.0 / particleCount; + double u0 = rng.nextDouble() * step; + double cumulative = particles.get(0).weight; + int i = 0; + + for (int m = 0; m < particleCount; m++) { + double threshold = u0 + m * step; + while (threshold > cumulative && i < particles.size() - 1) { + i++; + cumulative += particles.get(i).weight; + } + Particle picked = particles.get(i).copy(); + picked.weight = 1.0 / particleCount; + resampled.add(picked); + } + + particles.clear(); + particles.addAll(resampled); + } + + public double[] estimate() { + if (!initialised || particles.isEmpty()) return null; + + double east = 0.0; + double north = 0.0; + for (Particle p : particles) { + east += p.east * p.weight; + north += p.north * p.weight; + } + return new double[]{east, north}; + } + + public double getConfidence() { + if (!initialised || particles.isEmpty()) return 0.0; + double ess = effectiveSampleSize(); + return Math.max(0.0, Math.min(1.0, ess / particleCount)); + } +} From 38fbb0b78a9e86cec60215db057d17a8b8b2b6fb Mon Sep 17 00:00:00 2001 From: jagsnapuri Date: Sun, 29 Mar 2026 14:49:56 +0100 Subject: [PATCH 09/26] First draft - Particle filter algorithm --- .../main/java/positioning/FusionManager.java | 83 +++++++++++++++++++ 1 file changed, 83 insertions(+) create mode 100644 app/src/main/java/positioning/FusionManager.java diff --git a/app/src/main/java/positioning/FusionManager.java b/app/src/main/java/positioning/FusionManager.java new file mode 100644 index 00000000..94605070 --- /dev/null +++ b/app/src/main/java/positioning/FusionManager.java @@ -0,0 +1,83 @@ +package com.openpositioning.PositionMe.positioning; + +import android.util.Log; + +public class FusionManager { + + private static final String TAG = "FusionManager"; + private static final int PARTICLE_COUNT = 250; + + private static final FusionManager instance = new FusionManager(); + + private ParticleFilter particleFilter; + private CoordinateUtils coords; + private boolean ready = false; + + private FusionManager() {} + + public static FusionManager getInstance() { + return instance; + } + + public synchronized void reset() { + particleFilter = null; + coords = null; + ready = false; + } + + public synchronized void onGnss(double lat, double lon, float accuracyMeters) { + float sigma = accuracyMeters > 0 ? accuracyMeters : 8.0f; + if (!ready) { + coords = new CoordinateUtils(lat, lon); + particleFilter = new ParticleFilter(PARTICLE_COUNT); + particleFilter.initialise(0.0, 0.0, Math.max(sigma, 4.0f)); + ready = true; + Log.d(TAG, "Initialised from GNSS: " + lat + ", " + lon + " acc=" + sigma); + return; + } + + double[] en = coords.toLocal(lat, lon); + particleFilter.updateGnss(en[0], en[1], sigma); + } + + public synchronized void onWifi(double lat, double lon) { + if (!ready) { + coords = new CoordinateUtils(lat, lon); + particleFilter = new ParticleFilter(PARTICLE_COUNT); + particleFilter.initialise(0.0, 0.0, 6.0); + ready = true; + Log.d(TAG, "Initialised from WiFi: " + lat + ", " + lon); + return; + } + + double[] en = coords.toLocal(lat, lon); + particleFilter.updateWifi(en[0], en[1]); + } + + public synchronized void onStep(double stepLen, double headingRad) { + if (!ready || particleFilter == null) return; + if (Double.isNaN(stepLen) || Double.isNaN(headingRad) || stepLen <= 0.05) return; + particleFilter.predict(stepLen, headingRad); + } + + public synchronized double[] getBestPosition() { + if (!ready || particleFilter == null || coords == null) return null; + double[] en = particleFilter.estimate(); + if (en == null) return null; + return coords.toGlobal(en[0], en[1]); + } + + public synchronized double[] getBestPositionLocal() { + if (!ready || particleFilter == null) return null; + return particleFilter.estimate(); + } + + public synchronized double getConfidence() { + if (!ready || particleFilter == null) return 0.0; + return particleFilter.getConfidence(); + } + + public synchronized boolean isReady() { + return ready; + } +} \ No newline at end of file From da19654b52bee15666838288acfa69c028aeaf3a Mon Sep 17 00:00:00 2001 From: jagsnapuri Date: Mon, 30 Mar 2026 13:44:21 +0100 Subject: [PATCH 10/26] First draft - Particle filter algorithm --- README.md | 4 +- .../positioning/CoordinateUtils.java | 0 .../positioning/FusionManager.java | 66 +++++++++++++++++-- .../positioning/ParticleFilter.java | 11 ++-- .../fragment/TrajectoryMapFragment.java | 4 +- .../PositionMe/sensors/SensorFusion.java | 6 +- .../PositionMe/sensors/WiFiPositioning.java | 12 ++-- .../sensors/WifiPositionManager.java | 16 ++--- app/src/main/java/positioning/EKFFilter.java | 38 ----------- 9 files changed, 88 insertions(+), 69 deletions(-) rename app/src/main/java/{ => com/openpositioning/PositionMe}/positioning/CoordinateUtils.java (100%) rename app/src/main/java/{ => com/openpositioning/PositionMe}/positioning/FusionManager.java (55%) rename app/src/main/java/{ => com/openpositioning/PositionMe}/positioning/ParticleFilter.java (93%) delete mode 100644 app/src/main/java/positioning/EKFFilter.java diff --git a/README.md b/README.md index 659ffb48..2baf3074 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -**PositionMe** is an indoor positioning data collection application initially developed for the University of Edinburgh's Embedded Wireless course. The application now includes enhanced features, including **trajectory playback**, improved UI design, and comprehensive location tracking. +**PositionMe** is an indoor com.openpositioning.PositionMe.positioning data collection application initially developed for the University of Edinburgh's Embedded Wireless course. The application now includes enhanced features, including **trajectory playback**, improved UI design, and comprehensive location tracking. ///////// ## Features @@ -34,7 +34,7 @@ - Sensor access - Location services - Internet connectivity -4. **Collect real-time positioning data**: +4. **Collect real-time com.openpositioning.PositionMe.positioning data**: - Follow on-screen instructions to record sensor data. 5. **Replay previously recorded trajectories**: - Navigate to the **Files** section. diff --git a/app/src/main/java/positioning/CoordinateUtils.java b/app/src/main/java/com/openpositioning/PositionMe/positioning/CoordinateUtils.java similarity index 100% rename from app/src/main/java/positioning/CoordinateUtils.java rename to app/src/main/java/com/openpositioning/PositionMe/positioning/CoordinateUtils.java diff --git a/app/src/main/java/positioning/FusionManager.java b/app/src/main/java/com/openpositioning/PositionMe/positioning/FusionManager.java similarity index 55% rename from app/src/main/java/positioning/FusionManager.java rename to app/src/main/java/com/openpositioning/PositionMe/positioning/FusionManager.java index 94605070..5165f973 100644 --- a/app/src/main/java/positioning/FusionManager.java +++ b/app/src/main/java/com/openpositioning/PositionMe/positioning/FusionManager.java @@ -13,6 +13,9 @@ public class FusionManager { private CoordinateUtils coords; private boolean ready = false; + private double[] smoothedLatLon = null; + private static final double DISPLAY_SMOOTHING_ALPHA = 0.25; + private FusionManager() {} public static FusionManager getInstance() { @@ -23,20 +26,44 @@ public synchronized void reset() { particleFilter = null; coords = null; ready = false; + smoothedLatLon = null; } public synchronized void onGnss(double lat, double lon, float accuracyMeters) { - float sigma = accuracyMeters > 0 ? accuracyMeters : 8.0f; + float rawAccuracy = accuracyMeters > 0 ? accuracyMeters : 8.0f; + + // Ignore very poor GNSS fixes + if (rawAccuracy > 20.0f) { + Log.d(TAG, "Ignoring GNSS fix due to poor accuracy: " + rawAccuracy); + return; + } + + float sigma = Math.max(6.0f, rawAccuracy * 1.5f); + if (!ready) { coords = new CoordinateUtils(lat, lon); particleFilter = new ParticleFilter(PARTICLE_COUNT); - particleFilter.initialise(0.0, 0.0, Math.max(sigma, 4.0f)); + particleFilter.initialise(0.0, 0.0, Math.max(sigma, 6.0f)); ready = true; Log.d(TAG, "Initialised from GNSS: " + lat + ", " + lon + " acc=" + sigma); return; } double[] en = coords.toLocal(lat, lon); + + // Outlier gate: reject fixes far from current predicted estimate + double[] est = particleFilter.estimate(); + if (est != null) { + double de = en[0] - est[0]; + double dn = en[1] - est[1]; + double dist = Math.sqrt(de * de + dn * dn); + + if (dist > Math.max(18.0, sigma * 2.5)) { + Log.d(TAG, "Rejected GNSS outlier. Dist=" + dist + " sigma=" + sigma); + return; + } + } + particleFilter.updateGnss(en[0], en[1], sigma); } @@ -44,13 +71,27 @@ public synchronized void onWifi(double lat, double lon) { if (!ready) { coords = new CoordinateUtils(lat, lon); particleFilter = new ParticleFilter(PARTICLE_COUNT); - particleFilter.initialise(0.0, 0.0, 6.0); + particleFilter.initialise(0.0, 0.0, 8.0); ready = true; Log.d(TAG, "Initialised from WiFi: " + lat + ", " + lon); return; } double[] en = coords.toLocal(lat, lon); + + double[] est = particleFilter.estimate(); + if (est != null) { + double de = en[0] - est[0]; + double dn = en[1] - est[1]; + double dist = Math.sqrt(de * de + dn * dn); + + // WiFi is usually noisier than GNSS here, so reject big jumps + if (dist > 15.0) { + Log.d(TAG, "Rejected WiFi outlier. Dist=" + dist); + return; + } + } + particleFilter.updateWifi(en[0], en[1]); } @@ -62,9 +103,24 @@ public synchronized void onStep(double stepLen, double headingRad) { public synchronized double[] getBestPosition() { if (!ready || particleFilter == null || coords == null) return null; + double[] en = particleFilter.estimate(); if (en == null) return null; - return coords.toGlobal(en[0], en[1]); + + double[] latLon = coords.toGlobal(en[0], en[1]); + + if (smoothedLatLon == null) { + smoothedLatLon = latLon; + } else { + smoothedLatLon[0] = + DISPLAY_SMOOTHING_ALPHA * latLon[0] + + (1.0 - DISPLAY_SMOOTHING_ALPHA) * smoothedLatLon[0]; + smoothedLatLon[1] = + DISPLAY_SMOOTHING_ALPHA * latLon[1] + + (1.0 - DISPLAY_SMOOTHING_ALPHA) * smoothedLatLon[1]; + } + + return new double[]{smoothedLatLon[0], smoothedLatLon[1]}; } public synchronized double[] getBestPositionLocal() { @@ -80,4 +136,4 @@ public synchronized double getConfidence() { public synchronized boolean isReady() { return ready; } -} \ No newline at end of file +} diff --git a/app/src/main/java/positioning/ParticleFilter.java b/app/src/main/java/com/openpositioning/PositionMe/positioning/ParticleFilter.java similarity index 93% rename from app/src/main/java/positioning/ParticleFilter.java rename to app/src/main/java/com/openpositioning/PositionMe/positioning/ParticleFilter.java index 59064050..153e8bc4 100644 --- a/app/src/main/java/positioning/ParticleFilter.java +++ b/app/src/main/java/com/openpositioning/PositionMe/positioning/ParticleFilter.java @@ -23,12 +23,13 @@ Particle copy() { } private static final double DEFAULT_INIT_SIGMA = 4.0; - private static final double MOTION_NOISE_BASE = 0.20; - private static final double MOTION_NOISE_STEP_SCALE = 0.12; - private static final double STEP_NOISE_SIGMA = 0.10; - private static final double HEADING_NOISE_SIGMA = 0.08; + private static final double MOTION_NOISE_BASE = 0.12; + private static final double MOTION_NOISE_STEP_SCALE = 0.08; + private static final double STEP_NOISE_SIGMA = 0.05; + private static final double HEADING_NOISE_SIGMA = 0.05; private static final double GNSS_SIGMA_MIN = 3.0; - private static final double WIFI_SIGMA_DEFAULT = 6.0; + //private static final double WIFI_SIGMA_DEFAULT = 6.0; + private static final double WIFI_SIGMA_DEFAULT = 10.0; private final Random rng = new Random(42); private final List particles = new ArrayList<>(); 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..511afbfe 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 @@ -682,7 +682,7 @@ private void stopAutoFloor() { } /** - * Evaluates the current floor using WiFi positioning (priority) or + * Evaluates the current floor using WiFi com.openpositioning.PositionMe.positioning (priority) or * barometric elevation (fallback). Applies a 3-second debounce window * to prevent jittery floor switching. */ @@ -692,7 +692,7 @@ private void evaluateAutoFloor() { int candidateFloor; - // Priority 1: WiFi-based floor (only if WiFi positioning has returned data) + // Priority 1: WiFi-based floor (only if WiFi com.openpositioning.PositionMe.positioning has returned data) if (sensorFusion.getLatLngWifiPositioning() != null) { candidateFloor = sensorFusion.getWifiFloor(); } else { 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 3310076f..984221f5 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -42,7 +42,7 @@ *
  • {@link SensorState} – shared sensor data holder
    • *
  • {@link SensorEventHandler} – sensor event dispatch (switch logic)
    • *
  • {@link TrajectoryRecorder} – recording lifecycle & protobuf construction
    • - *
  • {@link WifiPositionManager} – WiFi scan processing & positioning
    • + *
  • {@link WifiPositionManager} – WiFi scan processing & com.openpositioning.PositionMe.positioning
    • * * *

    The public API is unchanged – all external callers continue to use @@ -608,7 +608,7 @@ public int getHoldMode() { } /** - * Returns the user position obtained using WiFi positioning. + * Returns the user position obtained using WiFi com.openpositioning.PositionMe.positioning. * * @return {@link LatLng} corresponding to user's position. */ @@ -617,7 +617,7 @@ public LatLng getLatLngWifiPositioning() { } /** - * Returns the current floor the user is on, obtained using WiFi positioning. + * Returns the current floor the user is on, obtained using WiFi com.openpositioning.PositionMe.positioning. * * @return current floor number. */ 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..6e70b68f 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/WiFiPositioning.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/WiFiPositioning.java @@ -12,7 +12,7 @@ import org.json.JSONObject; /** * Class for creating and handling POST requests for obtaining the current position using - * WiFi positioning API from https://openpositioning.org/api/position/fine + * WiFi com.openpositioning.PositionMe.positioning API from https://openpositioning.org/api/position/fine * * The class creates POST requests based on WiFi fingerprints and obtains the user's location * @@ -30,21 +30,21 @@ public class WiFiPositioning { // Queue for storing the POST requests made private RequestQueue requestQueue; - // URL for WiFi positioning API + // URL for WiFi com.openpositioning.PositionMe.positioning API private static final String url="https://openpositioning.org/api/position/fine"; /** - * Getter for the WiFi positioning coordinates obtained using openpositioning API + * Getter for the WiFi com.openpositioning.PositionMe.positioning coordinates obtained using openpositioning API * @return the user's coordinates based on openpositioning API */ public LatLng getWifiLocation() { return wifiLocation; } - // Store user's location obtained using WiFi positioning + // Store user's location obtained using WiFi com.openpositioning.PositionMe.positioning private LatLng wifiLocation; /** - * Getter for the WiFi positioning floor obtained using openpositioning API + * Getter for the WiFi com.openpositioning.PositionMe.positioning floor obtained using openpositioning API * @return the user's location based on openpositioning API */ public int getFloor() { @@ -56,7 +56,7 @@ public int getFloor() { /** - * Constructor to create the WiFi positioning object + * Constructor to create the WiFi com.openpositioning.PositionMe.positioning object * * Initialising a request queue to handle the POST requests asynchronously * 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 fe02ae4f..9d187989 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java @@ -13,13 +13,13 @@ import com.openpositioning.PositionMe.positioning.FusionManager; /** - * Manages WiFi scan result processing and WiFi-based positioning requests. + * Manages WiFi scan result processing and WiFi-based com.openpositioning.PositionMe.positioning requests. * *

    Implements {@link Observer} to receive updates from {@link WifiDataProcessor}, * replacing the role previously held by {@link SensorFusion}.

    * * @see WifiDataProcessor the observable that triggers WiFi scan updates - * @see WiFiPositioning the API client for WiFi-based positioning + * @see WiFiPositioning the API client for WiFi-based com.openpositioning.PositionMe.positioning */ public class WifiPositionManager implements Observer { @@ -32,7 +32,7 @@ public class WifiPositionManager implements Observer { /** * Creates a new WifiPositionManager. * - * @param wiFiPositioning WiFi positioning API client + * @param wiFiPositioning WiFi com.openpositioning.PositionMe.positioning API client * @param recorder trajectory recorder for writing WiFi fingerprints */ public WifiPositionManager(WiFiPositioning wiFiPositioning, @@ -46,7 +46,7 @@ public WifiPositionManager(WiFiPositioning wiFiPositioning, * *

    Receives updates from {@link WifiDataProcessor}. Converts the raw object array * to a typed list, delegates fingerprint recording to {@link TrajectoryRecorder}, - * and triggers a WiFi positioning request.

    + * and triggers a WiFi com.openpositioning.PositionMe.positioning request.

    */ @Override public void update(Object[] wifiList) { @@ -73,7 +73,7 @@ private void createWifiPositioningRequest() { } /** - * Creates a WiFi positioning request using the Volley callback pattern. + * Creates a WiFi com.openpositioning.PositionMe.positioning request using the Volley callback pattern. */ private void createWifiPositionRequestCallback() { if (this.wifiList == null || this.wifiList.isEmpty()) { @@ -99,7 +99,7 @@ public void onSuccess(LatLng wifiLocation, int floor) { @Override public void onError(String message) { - Log.e("WifiPositionManager", "WiFi positioning failed: " + message); + Log.e("WifiPositionManager", "WiFi com.openpositioning.PositionMe.positioning failed: " + message); } }); } catch (JSONException e) { @@ -108,7 +108,7 @@ public void onError(String message) { } /** - * Returns the user position obtained using WiFi positioning. + * Returns the user position obtained using WiFi com.openpositioning.PositionMe.positioning. * * @return {@link LatLng} corresponding to the user's position */ @@ -117,7 +117,7 @@ public LatLng getLatLngWifiPositioning() { } /** - * Returns the current floor the user is on, obtained using WiFi positioning. + * Returns the current floor the user is on, obtained using WiFi com.openpositioning.PositionMe.positioning. * * @return current floor number */ diff --git a/app/src/main/java/positioning/EKFFilter.java b/app/src/main/java/positioning/EKFFilter.java deleted file mode 100644 index 3e58518f..00000000 --- a/app/src/main/java/positioning/EKFFilter.java +++ /dev/null @@ -1,38 +0,0 @@ -package com.openpositioning.PositionMe.positioning; - -public class EKFFilter { - private double east, north; - private double pEast, pNorth; - - private static final double Q = 0.3; - private static final double R_GNSS = 15.0; - private static final double R_WIFI = 6.0; - - public EKFFilter(double initEast, double initNorth) { - this.east = initEast; - this.north = initNorth; - this.pEast = 100.0; - this.pNorth = 100.0; - } - - public void predict(double dE, double dN) { - east += dE; - north += dN; - pEast += Q; - pNorth += Q; - } - - public void update(double obsEast, double obsNorth, double R) { - double kE = pEast / (pEast + R); - double kN = pNorth / (pNorth + R); - east += kE * (obsEast - east); - north += kN * (obsNorth - north); - pEast *= (1 - kE); - pNorth *= (1 - kN); - } - - public double getEast() { return east; } - public double getNorth() { return north; } - public static double getGnssNoise() { return R_GNSS; } - public static double getWifiNoise() { return R_WIFI; } -} \ No newline at end of file From e209243763b46a3be8a59d540ca8f253c8c65dc1 Mon Sep 17 00:00:00 2001 From: Saleh-AlMulla Date: Mon, 30 Mar 2026 14:11:01 +0100 Subject: [PATCH 11/26] Update RecordingFragment.java --- .../PositionMe/presentation/fragment/RecordingFragment.java | 4 ++++ 1 file changed, 4 insertions(+) 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 537e2fd4..d64b8315 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 @@ -270,12 +270,14 @@ private void updateUIandPosition() { // Or simply pass relative data for the TrajectoryMapFragment to handle // For example: // Use fused position if available, otherwise fall back to PDR + // Use fused position if available, otherwise fall back to PDR double[] fusedPos = FusionManager.getInstance().getBestPosition(); if (fusedPos != null) { LatLng newLocation = new LatLng(fusedPos[0], fusedPos[1]); if (trajectoryMapFragment != null) { trajectoryMapFragment.updateUserLocation(newLocation, (float) Math.toDegrees(sensorFusion.passOrientation())); + trajectoryMapFragment.updatePdrObservation(newLocation); } } else { // Fallback to raw PDR until fusion is initialised @@ -289,9 +291,11 @@ private void updateUIandPosition() { if (trajectoryMapFragment != null) { trajectoryMapFragment.updateUserLocation(newLocation, (float) Math.toDegrees(sensorFusion.passOrientation())); + trajectoryMapFragment.updatePdrObservation(newLocation); } } } + // GNSS logic if you want to show GNSS error, etc. float[] gnss = sensorFusion.getSensorValueMap().get(SensorTypes.GNSSLATLONG); if (gnss != null && trajectoryMapFragment != null) { From e250e452b2cdcadf4b1856c9a3d53bf437edf791 Mon Sep 17 00:00:00 2001 From: Saleh-AlMulla Date: Mon, 30 Mar 2026 15:33:56 +0100 Subject: [PATCH 12/26] Data display updates and map matching integration --- .../mapmatching/MapMatchingEngine.java | 23 ++++++ .../PositionMe/positioning/FusionManager.java | 35 +++++++-- .../fragment/RecordingFragment.java | 77 +++++++++++-------- .../fragment/StartLocationFragment.java | 13 +--- .../fragment/TrajectoryMapFragment.java | 42 ++++++---- 5 files changed, 129 insertions(+), 61 deletions(-) diff --git a/app/src/main/java/com/openpositioning/PositionMe/mapmatching/MapMatchingEngine.java b/app/src/main/java/com/openpositioning/PositionMe/mapmatching/MapMatchingEngine.java index 4f2ac6ba..83c300ab 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/mapmatching/MapMatchingEngine.java +++ b/app/src/main/java/com/openpositioning/PositionMe/mapmatching/MapMatchingEngine.java @@ -136,6 +136,29 @@ public void initialise(double lat, double lng, int floor, + lat + ", " + lng + ") floor " + floor); } + public void logWallStats() { + if (floorShapes == null) { + Log.d(TAG, "No floor shapes loaded"); + return; + } + for (int f = 0; f < floorShapes.size(); f++) { + int wallSegments = 0; + int stairFeatures = 0; + int liftFeatures = 0; + for (FloorplanApiClient.MapShapeFeature feature : floorShapes.get(f).getFeatures()) { + String type = feature.getIndoorType(); + if ("wall".equals(type)) { + for (List part : feature.getParts()) { + wallSegments += part.size() - 1; + } + } else if ("stairs".equals(type)) stairFeatures++; + else if ("lift".equals(type)) liftFeatures++; + } + Log.d(TAG, "Floor " + f + ": " + wallSegments + " wall segments, " + + stairFeatures + " stair features, " + liftFeatures + " lift features"); + } + } + /** * Returns whether the engine has been initialised and is active. */ diff --git a/app/src/main/java/com/openpositioning/PositionMe/positioning/FusionManager.java b/app/src/main/java/com/openpositioning/PositionMe/positioning/FusionManager.java index 5165f973..f0233e40 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/positioning/FusionManager.java +++ b/app/src/main/java/com/openpositioning/PositionMe/positioning/FusionManager.java @@ -16,6 +16,9 @@ public class FusionManager { private double[] smoothedLatLon = null; private static final double DISPLAY_SMOOTHING_ALPHA = 0.25; + // Track initialisation source to prevent re-init from weaker source + private boolean initialisedFromWifi = false; + private FusionManager() {} public static FusionManager getInstance() { @@ -27,13 +30,14 @@ public synchronized void reset() { coords = null; ready = false; smoothedLatLon = null; + initialisedFromWifi = false; } public synchronized void onGnss(double lat, double lon, float accuracyMeters) { float rawAccuracy = accuracyMeters > 0 ? accuracyMeters : 8.0f; - // Ignore very poor GNSS fixes - if (rawAccuracy > 20.0f) { + // Stricter threshold — indoor GNSS below 15m is rare + if (rawAccuracy > 15.0f) { Log.d(TAG, "Ignoring GNSS fix due to poor accuracy: " + rawAccuracy); return; } @@ -51,7 +55,7 @@ public synchronized void onGnss(double lat, double lon, float accuracyMeters) { double[] en = coords.toLocal(lat, lon); - // Outlier gate: reject fixes far from current predicted estimate + // Outlier gate: reject fixes far from current estimate double[] est = particleFilter.estimate(); if (est != null) { double de = en[0] - est[0]; @@ -69,14 +73,36 @@ public synchronized void onGnss(double lat, double lon, float accuracyMeters) { public synchronized void onWifi(double lat, double lon) { if (!ready) { + // First fix — initialise from WiFi coords = new CoordinateUtils(lat, lon); particleFilter = new ParticleFilter(PARTICLE_COUNT); particleFilter.initialise(0.0, 0.0, 8.0); ready = true; + initialisedFromWifi = true; Log.d(TAG, "Initialised from WiFi: " + lat + ", " + lon); return; } + // If we initialised from GNSS and WiFi arrives soon after, + // re-centre on WiFi since it's more accurate indoors + if (!initialisedFromWifi && coords != null) { + double[] en = coords.toLocal(lat, lon); + double[] est = particleFilter.estimate(); + if (est != null) { + double dist = Math.sqrt(Math.pow(en[0] - est[0], 2) + Math.pow(en[1] - est[1], 2)); + // If WiFi says we're far from GNSS init, re-centre once + if (dist > 10.0) { + coords = new CoordinateUtils(lat, lon); + particleFilter = new ParticleFilter(PARTICLE_COUNT); + particleFilter.initialise(0.0, 0.0, 8.0); + initialisedFromWifi = true; + Log.d(TAG, "Re-centred from WiFi (GNSS was " + String.format("%.1f", dist) + "m off): " + lat + ", " + lon); + return; + } + } + initialisedFromWifi = true; // Don't try re-centring again + } + double[] en = coords.toLocal(lat, lon); double[] est = particleFilter.estimate(); @@ -85,7 +111,6 @@ public synchronized void onWifi(double lat, double lon) { double dn = en[1] - est[1]; double dist = Math.sqrt(de * de + dn * dn); - // WiFi is usually noisier than GNSS here, so reject big jumps if (dist > 15.0) { Log.d(TAG, "Rejected WiFi outlier. Dist=" + dist); return; @@ -136,4 +161,4 @@ public synchronized double getConfidence() { public synchronized boolean isReady() { return ready; } -} +} \ No newline at end of file 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 d64b8315..af47fc48 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 @@ -85,6 +85,8 @@ public class RecordingFragment extends Fragment { private float previousPosX = 0f; private float previousPosY = 0f; + private LatLng rawPdrPosition = null; + // References to the child map fragment private TrajectoryMapFragment trajectoryMapFragment; @@ -265,41 +267,56 @@ 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: - // Use fused position if available, otherwise fall back to PDR - // Use fused position if available, otherwise fall back to PDR - double[] fusedPos = FusionManager.getInstance().getBestPosition(); - if (fusedPos != null) { - LatLng newLocation = new LatLng(fusedPos[0], fusedPos[1]); - if (trajectoryMapFragment != null) { - trajectoryMapFragment.updateUserLocation(newLocation, - (float) Math.toDegrees(sensorFusion.passOrientation())); - trajectoryMapFragment.updatePdrObservation(newLocation); + // Compute raw PDR position independently for the red trace + float[] latLngArray = sensorFusion.getGNSSLatitude(true); + if (latLngArray != null) { + if (rawPdrPosition == null) { + rawPdrPosition = new LatLng(latLngArray[0], latLngArray[1]); } - } else { - // Fallback to raw PDR until fusion is initialised - 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())); - trajectoryMapFragment.updatePdrObservation(newLocation); - } + rawPdrPosition = UtilFunctions.calculateNewPos( + rawPdrPosition, + new float[]{ pdrValues[0] - previousPosX, pdrValues[1] - previousPosY } + ); + } + + // Draw raw PDR on the red polyline + if (rawPdrPosition != null && trajectoryMapFragment != null) { + trajectoryMapFragment.addRawPdrPoint(rawPdrPosition); + } + + // Best position: prefer MapMatchingEngine (wall-aware) > FusionManager > raw PDR + LatLng fusedLocation = null; + + // First choice: map-matched position (has wall constraints) + com.openpositioning.PositionMe.mapmatching.MapMatchingEngine mmEngine = + sensorFusion.getMapMatchingEngine(); + if (mmEngine != null && mmEngine.isActive()) { + fusedLocation = mmEngine.getEstimatedPosition(); + } + + // Second choice: FusionManager (WiFi/GNSS corrections, no walls) + if (fusedLocation == null) { + double[] fusedPos = FusionManager.getInstance().getBestPosition(); + if (fusedPos != null) { + fusedLocation = new LatLng(fusedPos[0], fusedPos[1]); } } - - // GNSS logic if you want to show GNSS error, etc. + + // Display the best available position + if (fusedLocation != null && trajectoryMapFragment != null) { + trajectoryMapFragment.updateUserLocation(fusedLocation, + (float) Math.toDegrees(sensorFusion.passOrientation())); + trajectoryMapFragment.updatePdrObservation(fusedLocation); + } else if (rawPdrPosition != null && trajectoryMapFragment != null) { + // Fallback: raw PDR before any fusion is ready + trajectoryMapFragment.updateUserLocation(rawPdrPosition, + (float) Math.toDegrees(sensorFusion.passOrientation())); + trajectoryMapFragment.updatePdrObservation(rawPdrPosition); + } + + // GNSS logic float[] gnss = sensorFusion.getSensorValueMap().get(SensorTypes.GNSSLATLONG); if (gnss != null && trajectoryMapFragment != null) { - // If user toggles showing GNSS in the map, call e.g. if (trajectoryMapFragment.isGnssEnabled()) { LatLng gnssLocation = new LatLng(gnss[0], gnss[1]); LatLng currentLoc = trajectoryMapFragment.getCurrentLocation(); 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 2842216e..262fad3f 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 @@ -299,17 +299,10 @@ private void onBuildingSelected(String buildingName, Polygon polygon) { // Store building selection selectedBuildingId = buildingName; - // Compute building centre from polygon points + // Compute building centre for camera zoom only LatLng center = computePolygonCenter(polygon); - // Move the marker to building centre - if (startMarker != null) { - startMarker.setPosition(center); - } - startPosition[0] = (float) center.latitude; - startPosition[1] = (float) center.longitude; - - // Zoom to the building + // Keep marker at user's actual position, don't move to building centre mMap.animateCamera(CameraUpdateFactory.newLatLngZoom(center, 20f)); // Show floor plan overlay for the selected building @@ -473,7 +466,6 @@ public void onViewCreated(@NonNull View view, @Nullable Bundle savedInstanceStat // Start sensor recording + set the start location sensorFusion.startRecording(); sensorFusion.setStartGNSSLatitude(startPosition); - // Write trajectory_id, initial_position and initial heading to protobuf sensorFusion.writeInitialMetadata(); // Initialise particle filter if inside a known building @@ -493,6 +485,7 @@ public void onViewCreated(@NonNull View view, @Nullable Bundle savedInstanceStat startFloor, floorHeight, building.getFloorShapesList() ); + sensorFusion.getMapMatchingEngine().logWallStats(); Log.d(TAG, "Map matching initialised for " + selectedBuildingId); } } 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 792223af..81532cf6 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 @@ -112,7 +112,7 @@ public class TrajectoryMapFragment extends Fragment { * The "last N observations" requirement in the assignment brief (section 3.3) is * satisfied by these caps: old markers are automatically removed as new ones appear. */ - private static final int MAX_PDR_OBSERVATIONS = 6; + private static final int MAX_PDR_OBSERVATIONS = 3; private static final int MAX_GNSS_OBSERVATIONS = 5; private static final int MAX_WIFI_OBSERVATIONS = 5; @@ -167,7 +167,7 @@ public class TrajectoryMapFragment extends Fragment { * noise is larger in scale than PDR noise. */ - private static final double PDR_OBSERVATION_MIN_DISTANCE_M = 1.0; + private static final double PDR_OBSERVATION_MIN_DISTANCE_M = 3.0; // ------------------------------------------------------------------------- @@ -478,20 +478,19 @@ 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); - } - } - +// 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); +// } +// } // Update indoor map overlay if (indoorMapManager != null) { @@ -694,6 +693,17 @@ public void updatePdrObservation(@NonNull LatLng pdrLocation) { } } + /** + * Adds a point to the raw PDR polyline (red) without moving the camera or marker. + * Shows the unfiltered PDR path for comparison against the fused trajectory. + */ + public void addRawPdrPoint(@NonNull LatLng point) { + if (polyline == null) return; + List points = new ArrayList<>(polyline.getPoints()); + points.add(point); + polyline.setPoints(points); + } + // ------------------------------------------------------------------------- // WIFI OBSERVATION UPDATE (NEW) // ------------------------------------------------------------------------- From 067945dcc62cb5eab049d3e86f387970068b77be Mon Sep 17 00:00:00 2001 From: Saleh-AlMulla Date: Mon, 30 Mar 2026 17:54:59 +0100 Subject: [PATCH 13/26] Enhance MapMatching engine, WiFi feed & UI fixes Refactor and harden the map-matching engine: reorganised tunable parameters and comments, lowered wall penalty, increased initial spread, added resample jitter and a lift horizontal threshold. Improved floor-transition logic to use barometric baseline deltas and accumulated horizontal movement to distinguish stairs vs lifts, with clamped floor updates and safer polygon/wall checks (guard against zero-length closing rings). Added systematic-resampling jitter, weight-collapse recovery, small performance/cleanup changes (loop/variable inlining), and various debug logging counters/TODOs. Integrations and bugfixes: WiFiPositionManager now feeds WiFi fixes into both FusionManager and the MapMatchingEngine (previously missing), and includes Volley callback handling and logging. SensorFusion and SensorEventHandler reset PDR baselines to avoid a large initial step spike after map-matching initialisation. RecordingFragment UI cleanup: minor layout/flow adjustments, dialog handling simplifications, periodic logging of position source, and other small UI/refresh fixes. --- .../mapmatching/MapMatchingEngine.java | 402 ++++++++++-------- .../fragment/RecordingFragment.java | 81 ++-- .../sensors/SensorEventHandler.java | 7 +- .../PositionMe/sensors/SensorFusion.java | 11 + .../sensors/WifiPositionManager.java | 84 ++-- 5 files changed, 326 insertions(+), 259 deletions(-) diff --git a/app/src/main/java/com/openpositioning/PositionMe/mapmatching/MapMatchingEngine.java b/app/src/main/java/com/openpositioning/PositionMe/mapmatching/MapMatchingEngine.java index 83c300ab..6c14d9f7 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/mapmatching/MapMatchingEngine.java +++ b/app/src/main/java/com/openpositioning/PositionMe/mapmatching/MapMatchingEngine.java @@ -19,9 +19,9 @@ * *

    Map constraint types (from floorplan API):

    *
      - *
    • wall — impassable barriers; particles crossing them are penalised
      • - *
    • stairs — floor transitions with height change
      • - *
    • lift — floor transitions without significant horizontal displacement
      • + *
    • wall — impassable barriers; particles crossing them are penalised
      • + *
    • stairs — floor transitions with horizontal movement
      • + *
    • lift — floor transitions without significant horizontal displacement
      • *
    * *

    Integration points:

    @@ -39,7 +39,9 @@ public class MapMatchingEngine { private static final String TAG = "MapMatchingEngine"; - // --- Tuneable parameters --- + // ------------------------------------------------------------------------- + // Tuneable parameters + // ------------------------------------------------------------------------- /** Number of particles in the filter. */ private static final int NUM_PARTICLES = 200; @@ -56,55 +58,82 @@ public class MapMatchingEngine { /** Standard deviation for GNSS observation likelihood (metres). */ private static final double GNSS_SIGMA = 12.0; - /** Weight penalty multiplier for particles that cross a wall. */ - private static final double WALL_PENALTY = 0.01; + /** + * Weight multiplier applied to particles that cross a wall. + * Lower value = harsher penalty. + */ + private static final double WALL_PENALTY = 0.001; - /** Distance threshold (metres) to consider a particle "near" stairs or lift. */ + /** Distance threshold (metres) to consider a particle near stairs or lift. */ private static final double TRANSITION_PROXIMITY = 4.0; - /** Minimum elevation change (metres) to trigger a floor change attempt. */ + /** + * Minimum elevation change from the last floor's baseline (metres) required + * to trigger a floor transition check. + */ private static final double FLOOR_CHANGE_ELEVATION_THRESHOLD = 2.0; /** Effective sample size ratio below which resampling is triggered. */ private static final double RESAMPLE_THRESHOLD = 0.5; - /** Spread radius (metres) for initial particle distribution. */ - private static final double INIT_SPREAD = 3.0; + /** + * Spread radius (metres) for initial particle distribution. + * Set to 6.0 to handle indoor GPS error which can easily be 5-10m. + */ + private static final double INIT_SPREAD = 6.0; + + /** + * Noise (metres) injected into each particle after resampling to maintain + * diversity and prevent the filter collapsing to a single point. + */ + private static final double RESAMPLE_JITTER = 0.5; + + /** + * Maximum total horizontal distance (metres) accumulated while elevation is + * changing that still classifies the transition as a lift (not stairs). + */ + private static final double LIFT_HORIZONTAL_THRESHOLD = 1.5; - // --- Coordinate conversion constants --- - // Approximate metres per degree at Edinburgh latitude (~55.92°N) + // ------------------------------------------------------------------------- + // Coordinate conversion — metres per degree at Edinburgh (~55.92 N) + // ------------------------------------------------------------------------- private static final double METRES_PER_DEG_LAT = 111_320.0; private static final double METRES_PER_DEG_LNG = 111_320.0 * Math.cos(Math.toRadians(55.92)); - // --- State --- + // ------------------------------------------------------------------------- + // State + // ------------------------------------------------------------------------- private final List particles = new ArrayList<>(); private final Random random = new Random(); - /** Reference point (latitude, longitude) for local coordinate conversion. */ private double refLat; private double refLng; private boolean initialised = false; - /** Current floor shapes from the floorplan API for wall/stair/lift checking. */ private List floorShapes; - /** Latest elevation relative to start (from barometer via PdrProcessing). */ private float currentElevation = 0f; - - /** Floor height of the current building (metres). */ + private float elevationAtLastFloorChange = 0f; private float floorHeight = 4.0f; - - /** Current estimated floor from the particle filter. */ private int estimatedFloor = 0; - - /** Whether the engine is actively running. */ private boolean enabled = false; - // ===================================================================== + // Stairs / lift detection + private float lastStepLength = 0f; + private float horizontalDuringElevationChange = 0f; + private boolean inElevationChange = false; + private float elevationChangeStartedAt = 0f; + + // Debug counters — TODO: remove before submission + private int wallHitCount = 0; + private int wallCheckCount = 0; + private int predictCallCount = 0; + + // ========================================================================= // Initialisation - // ===================================================================== + // ========================================================================= /** * Initialises the particle filter around a starting position. @@ -122,6 +151,13 @@ public void initialise(double lat, double lng, int floor, this.floorHeight = floorHeight; this.floorShapes = shapes; this.estimatedFloor = floor; + this.currentElevation = 0f; + this.elevationAtLastFloorChange = 0f; + this.horizontalDuringElevationChange = 0f; + this.inElevationChange = false; + this.wallHitCount = 0; + this.wallCheckCount = 0; + this.predictCallCount = 0; particles.clear(); for (int i = 0; i < NUM_PARTICLES; i++) { @@ -132,13 +168,18 @@ public void initialise(double lat, double lng, int floor, this.initialised = true; this.enabled = true; - Log.d(TAG, "Initialised with " + NUM_PARTICLES + " particles at (" - + lat + ", " + lng + ") floor " + floor); + + // TODO: remove before submission + Log.d(TAG, "initialise() called — lat=" + lat + " lng=" + lng + + " floor=" + floor + " floorHeight=" + floorHeight + + " shapes=" + (shapes == null ? "NULL" : shapes.size() + " floors")); + logWallStats(); } + // TODO: remove before submission public void logWallStats() { if (floorShapes == null) { - Log.d(TAG, "No floor shapes loaded"); + Log.d(TAG, "logWallStats: floorShapes is NULL — wall checking disabled"); return; } for (int f = 0; f < floorShapes.size(); f++) { @@ -151,86 +192,96 @@ public void logWallStats() { for (List part : feature.getParts()) { wallSegments += part.size() - 1; } - } else if ("stairs".equals(type)) stairFeatures++; - else if ("lift".equals(type)) liftFeatures++; + } else if ("stairs".equals(type)) { + stairFeatures++; + } else if ("lift".equals(type)) { + liftFeatures++; + } } - Log.d(TAG, "Floor " + f + ": " + wallSegments + " wall segments, " + Log.d(TAG, " Floor " + f + ": " + wallSegments + " wall segments, " + stairFeatures + " stair features, " + liftFeatures + " lift features"); } } - /** - * Returns whether the engine has been initialised and is active. - */ + /** Returns whether the engine has been initialised and is active. */ public boolean isActive() { return initialised && enabled; } - /** - * Enables or disables the engine without clearing particles. - */ + /** Enables or disables the engine without clearing particles. */ public void setEnabled(boolean enabled) { + // TODO: remove before submission + Log.d(TAG, "setEnabled(" + enabled + ") — was " + this.enabled); this.enabled = enabled; } - /** - * Resets the engine, clearing all particles and state. - */ + /** Resets the engine, clearing all particles and state. */ public void reset() { + // TODO: remove before submission + Log.d(TAG, "reset() called"); particles.clear(); initialised = false; enabled = false; currentElevation = 0f; + elevationAtLastFloorChange = 0f; estimatedFloor = 0; + horizontalDuringElevationChange = 0f; + inElevationChange = false; + wallHitCount = 0; + wallCheckCount = 0; + predictCallCount = 0; } - /** - * Updates the floor shapes used for wall/stair/lift constraints. - * Call this when the user enters a new building. - * - * @param shapes per-floor shape data - */ + /** Updates the floor shapes used for wall/stair/lift constraints. */ public void setFloorShapes(List shapes) { this.floorShapes = shapes; + // TODO: remove before submission + Log.d(TAG, "setFloorShapes() — " + + (shapes == null ? "NULL" : shapes.size() + " floors")); } - /** - * Updates the floor height for the current building. - */ + /** Updates the floor height for the current building. */ public void setFloorHeight(float height) { this.floorHeight = height; } - // ===================================================================== + // ========================================================================= // Prediction step — called on each PDR step - // ===================================================================== + // ========================================================================= /** * Propagates all particles using the PDR displacement and applies map constraints. * - *

    Called from {@code SensorEventHandler} on each detected step. The heading - * is the device orientation in radians (0 = north, clockwise positive).

    - * * @param stepLength step length in metres from PDR * @param headingRad heading in radians (north = 0, clockwise) * @param elevation current relative elevation from barometer (metres) */ public void predict(float stepLength, float headingRad, float elevation) { - if (!isActive()) return; + // TODO: remove before submission + predictCallCount++; + if (predictCallCount <= 3 || predictCallCount % 20 == 0) { + Log.d(TAG, "predict() #" + predictCallCount + + " — initialised=" + initialised + + " enabled=" + enabled + + " stepLen=" + String.format("%.2f", stepLength) + + " heading=" + String.format("%.1f", Math.toDegrees(headingRad)) + "deg" + + " elev=" + String.format("%.2f", elevation) + "m"); + } + + if (!isActive()) { + Log.w(TAG, "predict() bailed — initialised=" + initialised + + " enabled=" + enabled); + return; + } this.currentElevation = elevation; + this.lastStepLength = stepLength; for (Particle p : particles) { - // Add noise to step length and heading double noisyStep = stepLength + random.nextGaussian() * PDR_NOISE_STEP; double noisyHeading = headingRad + random.nextGaussian() * PDR_NOISE_HEADING; + noisyStep = Math.max(0, Math.min(noisyStep, 2.0)); - // Clamp step length to reasonable bounds - if (noisyStep < 0) noisyStep = 0; - if (noisyStep > 2.0) noisyStep = 2.0; - - // Convert heading to local XY displacement - // heading: 0=north, pi/2=east => dx = step*sin(h), dy = step*cos(h) double dx = noisyStep * Math.sin(noisyHeading); double dy = noisyStep * Math.cos(noisyHeading); @@ -239,42 +290,49 @@ public void predict(float stepLength, float headingRad, float elevation) { double newX = oldX + dx; double newY = oldY + dy; - // Wall collision check + wallCheckCount++; if (doesCrossWall(oldX, oldY, newX, newY, p.getFloor())) { - // Penalise but don't move — particle stays at old position p.setWeight(p.getWeight() * WALL_PENALTY); + wallHitCount++; } else { p.setX(newX); p.setY(newY); } + } -/* Log.d(TAG, "Predict: particles=" + particles.size() - + " pos=" + getEstimatedPosition() - + " floor=" + getEstimatedFloor());*/ - + // TODO: remove before submission — fires once per step (200 checks per step) + if (wallCheckCount > 0 && wallCheckCount % 200 == 0) { + Log.d(TAG, "Wall check #" + (wallCheckCount / 200) + + " — hits=" + wallHitCount + "/" + wallCheckCount + + " (" + String.format("%.1f", 100.0 * wallHitCount / wallCheckCount) + "%)" + + " floor=" + estimatedFloor + + " shapes=" + (floorShapes == null ? "NULL" : floorShapes.size() + " floors")); + if (!particles.isEmpty()) { + Particle p0 = particles.get(0); + Log.d(TAG, " Particle[0] xy=(" + + String.format("%.2f", p0.getX()) + ", " + + String.format("%.2f", p0.getY()) + ") floor=" + p0.getFloor()); + } } - // Check for floor transitions based on elevation change checkFloorTransition(); - - // Normalise weights and conditionally resample normaliseWeights(); if (effectiveSampleSize() < RESAMPLE_THRESHOLD * NUM_PARTICLES) { resample(); } } - // ===================================================================== + // ========================================================================= // Observation updates - // ===================================================================== + // ========================================================================= /** * Updates particle weights based on a WiFi position observation. * - * @param lat WiFi-estimated latitude - * @param lng WiFi-estimated longitude + * @param lat WiFi-estimated latitude + * @param lng WiFi-estimated longitude * @param wifiFloor WiFi-estimated floor number - * @param accuracy estimated accuracy in metres (used to scale sigma) + * @param accuracy estimated accuracy in metres */ public void updateWifi(double lat, double lng, int wifiFloor, float accuracy) { if (!isActive()) return; @@ -287,12 +345,7 @@ public void updateWifi(double lat, double lng, int wifiFloor, float accuracy) { double dist = Math.sqrt(Math.pow(p.getX() - obsX, 2) + Math.pow(p.getY() - obsY, 2)); double likelihood = gaussianLikelihood(dist, sigma); - - // Floor mismatch penalty - if (p.getFloor() != wifiFloor) { - likelihood *= 0.1; - } - + if (p.getFloor() != wifiFloor) likelihood *= 0.1; p.setWeight(p.getWeight() * likelihood); } @@ -301,7 +354,12 @@ public void updateWifi(double lat, double lng, int wifiFloor, float accuracy) { resample(); } - Log.d(TAG, "WiFi update at (" + lat + ", " + lng + ") floor " + wifiFloor); + // TODO: remove before submission + Log.d(TAG, "updateWifi() — obsXY=(" + + String.format("%.2f", obsX) + ", " + + String.format("%.2f", obsY) + ")" + + " floor=" + wifiFloor + + " sigma=" + String.format("%.1f", sigma)); } /** @@ -328,48 +386,74 @@ public void updateGnss(double lat, double lng, float accuracy) { normaliseWeights(); } - // ===================================================================== + // ========================================================================= // Floor transition logic - // ===================================================================== + // ========================================================================= /** - * Checks if the barometric elevation change warrants a floor transition. - * Particles near stairs/lifts are allowed to change floors; others are penalised. + * Checks if the barometric elevation change warrants a floor transition, + * and distinguishes between stairs and lifts based on accumulated horizontal + * movement during the elevation change. */ private void checkFloorTransition() { if (floorShapes == null || floorHeight <= 0) return; - // Calculate expected floor offset from elevation - int elevationFloorDelta = (int) Math.round(currentElevation / floorHeight); - int targetFloor = estimatedFloor + elevationFloorDelta; + float elevationDeltaFromBaseline = currentElevation - elevationAtLastFloorChange; + float elevationChangeMagnitude = Math.abs(elevationDeltaFromBaseline); + + if (elevationChangeMagnitude > 0.3f) { + if (!inElevationChange) { + inElevationChange = true; + elevationChangeStartedAt = currentElevation; + horizontalDuringElevationChange = 0f; + } + horizontalDuringElevationChange += lastStepLength; + } else { + inElevationChange = false; + horizontalDuringElevationChange = 0f; + } + + if (elevationChangeMagnitude < FLOOR_CHANGE_ELEVATION_THRESHOLD) return; - // Only act if elevation suggests we've moved at least one floor - if (Math.abs(currentElevation) < FLOOR_CHANGE_ELEVATION_THRESHOLD) return; + int elevationFloorDelta = (int) Math.round(elevationDeltaFromBaseline / floorHeight); + if (elevationFloorDelta == 0) return; + + int targetFloor = clampFloor(estimatedFloor + elevationFloorDelta); + boolean isLift = horizontalDuringElevationChange < LIFT_HORIZONTAL_THRESHOLD; for (Particle p : particles) { - if (p.getFloor() == targetFloor) continue; // already on target floor + if (p.getFloor() == targetFloor) continue; boolean nearTransition = isNearStairsOrLift(p.getX(), p.getY(), p.getFloor()); if (nearTransition) { - // Allow floor change - p.setFloor(clampFloor(targetFloor)); - // Slight bonus for particles that can transition - p.setWeight(p.getWeight() * 1.2); + p.setFloor(targetFloor); + if (isLift) { + p.setWeight(p.getWeight() * 1.2); + } else { + p.setX(p.getX() + random.nextGaussian() * 1.0); + p.setY(p.getY() + random.nextGaussian() * 1.0); + p.setWeight(p.getWeight() * 1.2); + } } else { - // Penalise particles that should change floor but aren't near stairs/lift p.setWeight(p.getWeight() * 0.5); } } + + // TODO: remove before submission + Log.d(TAG, "Floor transition → floor " + targetFloor + + " via " + (isLift ? "LIFT" : "STAIRS") + + " horiz=" + String.format("%.2f", horizontalDuringElevationChange) + "m" + + " elevDelta=" + String.format("%.2f", elevationDeltaFromBaseline) + "m"); + + this.estimatedFloor = targetFloor; + this.elevationAtLastFloorChange = currentElevation; + this.horizontalDuringElevationChange = 0f; + this.inElevationChange = false; } /** * Checks if a position is near stairs or lift features on the given floor. - * - * @param x easting in metres - * @param y northing in metres - * @param floor floor index - * @return true if within {@link #TRANSITION_PROXIMITY} of a stair or lift feature */ private boolean isNearStairsOrLift(double x, double y, int floor) { if (floorShapes == null || floor < 0 || floor >= floorShapes.size()) return false; @@ -383,9 +467,7 @@ private boolean isNearStairsOrLift(double x, double y, int floor) { double fx = (point.longitude - refLng) * METRES_PER_DEG_LNG; double fy = (point.latitude - refLat) * METRES_PER_DEG_LAT; double dist = Math.sqrt(Math.pow(x - fx, 2) + Math.pow(y - fy, 2)); - if (dist < TRANSITION_PROXIMITY) { - return true; - } + if (dist < TRANSITION_PROXIMITY) return true; } } } @@ -393,23 +475,19 @@ private boolean isNearStairsOrLift(double x, double y, int floor) { return false; } - /** - * Clamps a floor index to valid bounds. - */ + /** Clamps a floor index to valid bounds given the loaded floor shapes. */ private int clampFloor(int floor) { if (floorShapes == null || floorShapes.isEmpty()) return floor; return Math.max(0, Math.min(floor, floorShapes.size() - 1)); } - // ===================================================================== + // ========================================================================= // Wall collision detection - // ===================================================================== + // ========================================================================= /** * Tests whether moving from (x1,y1) to (x2,y2) crosses any wall segment - * on the given floor. - * - * @return true if the movement line intersects a wall + * on the given floor. Coordinates are in local ENU metres. */ private boolean doesCrossWall(double x1, double y1, double x2, double y2, int floor) { if (floorShapes == null || floor < 0 || floor >= floorShapes.size()) return false; @@ -419,7 +497,8 @@ private boolean doesCrossWall(double x1, double y1, double x2, double y2, int fl if (!"wall".equals(feature.getIndoorType())) continue; for (List part : feature.getParts()) { - for (int i = 0; i < part.size() - 1; i++) { + int size = part.size(); + for (int i = 0; i < size - 1; i++) { LatLng a = part.get(i); LatLng b = part.get(i + 1); @@ -433,12 +512,16 @@ private boolean doesCrossWall(double x1, double y1, double x2, double y2, int fl } } - // For polygons, also check the closing edge - if (part.size() >= 3) { + // Check closing edge for polygon rings, guarding against zero-length + // segments (standard GeoJSON closed rings repeat first point as last) + if (size >= 3) { String geoType = feature.getGeometryType(); if ("MultiPolygon".equals(geoType) || "Polygon".equals(geoType)) { LatLng first = part.get(0); - LatLng last = part.get(part.size() - 1); + LatLng last = part.get(size - 1); + if (first.latitude == last.latitude + && first.longitude == last.longitude) continue; + double fx = (first.longitude - refLng) * METRES_PER_DEG_LNG; double fy = (first.latitude - refLat) * METRES_PER_DEG_LAT; double lx = (last.longitude - refLng) * METRES_PER_DEG_LNG; @@ -455,10 +538,8 @@ private boolean doesCrossWall(double x1, double y1, double x2, double y2, int fl } /** - * Tests if two line segments (p1-p2) and (p3-p4) intersect using the + * Tests if two line segments (p1 to p2) and (p3 to p4) intersect using the * cross-product orientation method. - * - * @return true if segments intersect */ private static boolean segmentsIntersect(double p1x, double p1y, double p2x, double p2y, double p3x, double p3y, double p4x, double p4y) { @@ -472,7 +553,6 @@ private static boolean segmentsIntersect(double p1x, double p1y, double p2x, dou return true; } - // Collinear cases if (d1 == 0 && onSegment(p3x, p3y, p4x, p4y, p1x, p1y)) return true; if (d2 == 0 && onSegment(p3x, p3y, p4x, p4y, p2x, p2y)) return true; if (d3 == 0 && onSegment(p1x, p1y, p2x, p2y, p3x, p3y)) return true; @@ -481,48 +561,43 @@ private static boolean segmentsIntersect(double p1x, double p1y, double p2x, dou return false; } - /** - * Cross product of vectors (bx-ax, by-ay) and (cx-ax, cy-ay). - */ + /** Cross product of vectors (b-a) x (c-a). */ private static double crossProduct(double ax, double ay, double bx, double by, double cx, double cy) { return (bx - ax) * (cy - ay) - (by - ay) * (cx - ax); } - /** - * Checks if point (px,py) lies on segment (ax,ay)-(bx,by), assuming collinearity. - */ + /** Checks if point (px,py) lies on segment (ax,ay) to (bx,by), assuming collinearity. */ private static boolean onSegment(double ax, double ay, double bx, double by, double px, double py) { return Math.min(ax, bx) <= px && px <= Math.max(ax, bx) && Math.min(ay, by) <= py && py <= Math.max(ay, by); } - // ===================================================================== + // ========================================================================= // Resampling - // ===================================================================== + // ========================================================================= /** * Calculates the effective sample size (ESS) of the particle population. - * ESS = 1 / sum(w_i^2). Low ESS indicates weight degeneracy. + * ESS = 1 / sum(wi squared). Low ESS indicates weight degeneracy. */ private double effectiveSampleSize() { double sumSq = 0; - for (Particle p : particles) { - sumSq += p.getWeight() * p.getWeight(); - } + for (Particle p : particles) sumSq += p.getWeight() * p.getWeight(); return (sumSq > 0) ? 1.0 / sumSq : 0; } /** - * Systematic resampling: selects particles proportional to their weights - * and replaces the population with copies of selected particles. + * Systematic resampling: selects particles proportional to their weights, + * replaces the population with copies reset to uniform weight, then injects + * small Gaussian jitter ({@link #RESAMPLE_JITTER}) to maintain diversity + * and prevent the filter collapsing to a single point. */ private void resample() { int n = particles.size(); if (n == 0) return; - // Build cumulative weight array double[] cumulative = new double[n]; cumulative[0] = particles.get(0).getWeight(); for (int i = 1; i < n; i++) { @@ -536,42 +611,43 @@ private void resample() { int idx = 0; for (int i = 0; i < n; i++) { double target = start + i * step; - while (idx < n - 1 && cumulative[idx] < target) { - idx++; - } + while (idx < n - 1 && cumulative[idx] < target) idx++; Particle copy = particles.get(idx).copy(); copy.setWeight(1.0 / n); newParticles.add(copy); } + // Inject diversity jitter to prevent full collapse after aggressive resampling. + // Without this, all surviving particles occupy the same XY and the filter + // behaves like a single point tracker. + for (Particle p : newParticles) { + p.setX(p.getX() + random.nextGaussian() * RESAMPLE_JITTER); + p.setY(p.getY() + random.nextGaussian() * RESAMPLE_JITTER); + } + particles.clear(); particles.addAll(newParticles); } /** * Normalises all particle weights to sum to 1. + * Resets to uniform if weights have collapsed entirely. */ private void normaliseWeights() { double sum = 0; - for (Particle p : particles) { - sum += p.getWeight(); - } + for (Particle p : particles) sum += p.getWeight(); if (sum <= 0) { - // Catastrophic weight collapse — reset to uniform + Log.w(TAG, "Weight collapse — resetting to uniform weights"); double uniform = 1.0 / particles.size(); - for (Particle p : particles) { - p.setWeight(uniform); - } + for (Particle p : particles) p.setWeight(uniform); return; } - for (Particle p : particles) { - p.setWeight(p.getWeight() / sum); - } + for (Particle p : particles) p.setWeight(p.getWeight() / sum); } - // ===================================================================== - // Output — estimated position - // ===================================================================== + // ========================================================================= + // Output + // ========================================================================= /** * Returns the weighted mean position of all particles as a LatLng. @@ -593,20 +669,18 @@ public LatLng getEstimatedPosition() { } /** - * Returns the weighted-majority floor estimate from the particles. + * Returns the weighted-majority floor estimate from the particle distribution. * * @return estimated floor index */ public int getEstimatedFloor() { if (!isActive() || particles.isEmpty()) return estimatedFloor; - // Accumulate weight per floor java.util.Map floorWeights = new java.util.HashMap<>(); for (Particle p : particles) { floorWeights.merge(p.getFloor(), p.getWeight(), Double::sum); } - // Find floor with highest accumulated weight int bestFloor = estimatedFloor; double bestWeight = -1; for (java.util.Map.Entry entry : floorWeights.entrySet()) { @@ -637,27 +711,21 @@ public float[] getEstimatedXY() { } /** - * Returns the current particle list (read-only snapshot). + * Returns the current particle list as a snapshot (copies). * Useful for debug visualisation. - * - * @return list of particle copies */ public List getParticles() { List snapshot = new ArrayList<>(particles.size()); - for (Particle p : particles) { - snapshot.add(p.copy()); - } + for (Particle p : particles) snapshot.add(p.copy()); return snapshot; } - // ===================================================================== + // ========================================================================= // Utility - // ===================================================================== + // ========================================================================= - /** - * Gaussian likelihood function: exp(-d^2 / (2 * sigma^2)). - */ + /** Gaussian likelihood: exp(-d squared / (2 * sigma squared)). */ private static double gaussianLikelihood(double distance, double sigma) { return Math.exp(-(distance * distance) / (2.0 * sigma * sigma)); } -} +} \ No newline at end of file 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 af47fc48..51710dc1 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 @@ -7,6 +7,7 @@ import android.os.Bundle; import android.os.CountDownTimer; import android.os.Handler; +import android.util.Log; import android.view.LayoutInflater; import android.view.View; import android.view.ViewGroup; @@ -38,9 +39,6 @@ import android.widget.Toast; import com.openpositioning.PositionMe.positioning.FusionManager; - - - /** * Fragment responsible for managing the recording process of trajectory data. *

    @@ -60,12 +58,12 @@ * @see RecordingActivity The activity managing the recording workflow. * @see SensorFusion Handles sensor data collection. * @see SensorTypes Enumeration of available sensor types. - * - * @author Shu Gu */ - public class RecordingFragment extends Fragment { + // TODO: remove before submission + private static final String TAG = "RecordingFragment"; + // UI elements private MaterialButton completeButton, cancelButton; private ImageView recIcon; @@ -90,11 +88,13 @@ public class RecordingFragment extends Fragment { // References to the child map fragment private TrajectoryMapFragment trajectoryMapFragment; + // TODO: remove before submission + private int uiUpdateCount = 0; + private final Runnable refreshDataTask = new Runnable() { @Override public void run() { updateUIandPosition(); - // Loop again refreshDataHandler.postDelayed(refreshDataTask, 200); } }; @@ -117,7 +117,6 @@ public void onCreate(Bundle savedInstanceState) { public View onCreateView(@NonNull LayoutInflater inflater, @Nullable ViewGroup container, @Nullable Bundle savedInstanceState) { - // Inflate only the "recording" UI parts (no map) return inflater.inflate(R.layout.fragment_recording, container, false); } @@ -126,12 +125,9 @@ public void onViewCreated(@NonNull View view, @Nullable Bundle savedInstanceState) { super.onViewCreated(view, savedInstanceState); - // Child Fragment: the container in fragment_recording.xml - // where TrajectoryMapFragment is placed trajectoryMapFragment = (TrajectoryMapFragment) getChildFragmentManager().findFragmentById(R.id.trajectoryMapFragmentContainer); - // If not present, create it if (trajectoryMapFragment == null) { trajectoryMapFragment = new TrajectoryMapFragment(); getChildFragmentManager() @@ -140,7 +136,6 @@ public void onViewCreated(@NonNull View view, .commit(); } - // Initialize UI references elevation = view.findViewById(R.id.currentElevation); distanceTravelled = view.findViewById(R.id.currentDistanceTraveled); gnssError = view.findViewById(R.id.gnssError); @@ -151,54 +146,39 @@ public void onViewCreated(@NonNull View view, timeRemaining = view.findViewById(R.id.timeRemainingBar); view.findViewById(R.id.btn_test_point).setOnClickListener(v -> onAddTestPoint()); - - // Hide or initialize default values gnssError.setVisibility(View.GONE); elevation.setText(getString(R.string.elevation, "0")); distanceTravelled.setText(getString(R.string.meter, "0")); - // Buttons completeButton.setOnClickListener(v -> { - // Stop recording & go to correction if (autoStop != null) autoStop.cancel(); sensorFusion.stopRecording(); - // Show Correction screen ((RecordingActivity) requireActivity()).showCorrectionScreen(); }); - - // Cancel button with confirmation dialog cancelButton.setOnClickListener(v -> { AlertDialog dialog = new AlertDialog.Builder(requireActivity()) .setTitle("Confirm Cancel") .setMessage("Are you sure you want to cancel the recording? Your progress will be lost permanently!") .setNegativeButton("Yes", (dialogInterface, which) -> { - // User confirmed cancellation sensorFusion.stopRecording(); if (autoStop != null) autoStop.cancel(); requireActivity().onBackPressed(); }) - .setPositiveButton("No", (dialogInterface, which) -> { - // User cancelled the dialog. Do nothing. - dialogInterface.dismiss(); - }) - .create(); // Create the dialog but do not show it yet + .setPositiveButton("No", (dialogInterface, which) -> dialogInterface.dismiss()) + .create(); - // Show the dialog and change the button color dialog.setOnShowListener(dialogInterface -> { Button negativeButton = dialog.getButton(AlertDialog.BUTTON_NEGATIVE); - negativeButton.setTextColor(Color.RED); // Set "Yes" button color to red + negativeButton.setTextColor(Color.RED); }); - dialog.show(); // Finally, show the dialog + dialog.show(); }); - // The blinking effect for recIcon blinkingRecordingIcon(); - // Start the timed or indefinite UI refresh if (this.settings.getBoolean("split_trajectory", false)) { - // A maximum recording time is set long limit = this.settings.getInt("split_duration", 30) * 60000L; timeRemaining.setMax((int) (limit / 1000)); timeRemaining.setProgress(0); @@ -218,39 +198,28 @@ public void onFinish() { } }.start(); } else { - // No set time limit, just keep refreshing refreshDataHandler.post(refreshDataTask); } } private void onAddTestPoint() { - // 1) Ensure the map fragment is ready if (trajectoryMapFragment == null) return; - // 2) Read current track position (must lie on the current path) LatLng cur = trajectoryMapFragment.getCurrentLocation(); if (cur == null) { - Toast.makeText(requireContext(), "" + + Toast.makeText(requireContext(), "I haven't gotten my current location yet, let me take a couple of steps/wait for the map to load.", Toast.LENGTH_SHORT).show(); return; } - // 3) Generate index + timestamp (satisfies "save timestamp") int idx = ++testPointIndex; long ts = System.currentTimeMillis(); - - // 4) Keep a local copy for in-session tracking testPoints.add(new TestPoint(idx, ts, cur.latitude, cur.longitude)); - - // Write test point into protobuf payload sensorFusion.addTestPointToProto(ts, cur.latitude, cur.longitude); - - // 5) Draw numbered marker on map (satisfies "leave numbered marker") trajectoryMapFragment.addTestPointMarker(idx, ts, cur); } - /** * Update the UI with sensor data and pass map updates to TrajectoryMapFragment. */ @@ -258,6 +227,8 @@ private void updateUIandPosition() { float[] pdrValues = sensorFusion.getSensorValueMap().get(SensorTypes.PDR); if (pdrValues == null) return; + uiUpdateCount++; + // Distance distance += Math.sqrt(Math.pow(pdrValues[0] - previousPosX, 2) + Math.pow(pdrValues[1] - previousPosY, 2)); @@ -286,12 +257,15 @@ private void updateUIandPosition() { // Best position: prefer MapMatchingEngine (wall-aware) > FusionManager > raw PDR LatLng fusedLocation = null; + String positionSource = "none"; // First choice: map-matched position (has wall constraints) com.openpositioning.PositionMe.mapmatching.MapMatchingEngine mmEngine = sensorFusion.getMapMatchingEngine(); + if (mmEngine != null && mmEngine.isActive()) { fusedLocation = mmEngine.getEstimatedPosition(); + positionSource = "MapMatchingEngine"; } // Second choice: FusionManager (WiFi/GNSS corrections, no walls) @@ -299,6 +273,21 @@ private void updateUIandPosition() { double[] fusedPos = FusionManager.getInstance().getBestPosition(); if (fusedPos != null) { fusedLocation = new LatLng(fusedPos[0], fusedPos[1]); + positionSource = "FusionManager"; + } + } + + // TODO: remove before submission — log position source every 5 UI updates (~1s) + if (uiUpdateCount % 5 == 0) { + if (mmEngine == null) { + Log.d(TAG, "Position source=" + positionSource + + " | mmEngine=NULL"); + } else { + Log.d(TAG, "Position source=" + positionSource + + " | mmEngine.isActive=" + mmEngine.isActive() + + " | fusedLoc=" + (fusedLocation == null ? "null" + : String.format("(%.5f, %.5f)", + fusedLocation.latitude, fusedLocation.longitude))); } } @@ -358,7 +347,7 @@ public void onPause() { @Override public void onResume() { super.onResume(); - if(!this.settings.getBoolean("split_trajectory", false)) { + if (!this.settings.getBoolean("split_trajectory", false)) { refreshDataHandler.postDelayed(refreshDataTask, 500); } } @@ -380,6 +369,4 @@ private static class TestPoint { } private final List testPoints = new ArrayList<>(); - - -} +} \ No newline at end of file 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 ced72e0f..96f15046 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java @@ -188,9 +188,12 @@ public void handleSensorEvent(SensorEvent sensorEvent) { state.orientation[0], state.elevation ); - state.lastPdrX = newCords[0]; - state.lastPdrY = newCords[1]; + } + // Always update regardless of engine state — prevents a large stepLen + // spike on the first predict() call after the engine initialises + state.lastPdrX = newCords[0]; + state.lastPdrY = newCords[1]; // Feed step into FusionManager float avgStepLength = pdrProcessing.getAverageStepLength(); 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 dbbd2333..aecc9a09 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -503,6 +503,17 @@ public void initialiseMapMatching(double lat, double lng, int floor, float floorHeight, java.util.List shapes) { mapMatchingEngine.initialise(lat, lng, floor, floorHeight, shapes); + + // Reset PDR baseline so the first predict() after init doesn't + // compute a giant step from accumulated PDR history. + float[] currentPdr = pdrProcessing.getPDRMovement(); + if (currentPdr != null) { + state.lastPdrX = currentPdr[0]; + state.lastPdrY = currentPdr[1]; + } else { + state.lastPdrX = 0; + state.lastPdrY = 0; + } } /** 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 5273d3e0..012b1868 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java @@ -10,21 +10,23 @@ import java.util.List; import java.util.stream.Collectors; import java.util.stream.Stream; -import com.openpositioning.PositionMe.positioning.FusionManager; import com.openpositioning.PositionMe.mapmatching.MapMatchingEngine; +import com.openpositioning.PositionMe.positioning.FusionManager; /** - * Manages WiFi scan result processing and WiFi-based com.openpositioning.PositionMe.positioning requests. + * Manages WiFi scan result processing and WiFi-based positioning requests. * - *

    Implements {@link Observer} to receive updates from {@link WifiDataProcessor}, - * replacing the role previously held by {@link SensorFusion}.

    + *

    Implements {@link Observer} to receive updates from {@link WifiDataProcessor}. + * On each WiFi scan result, it fires a positioning request and feeds the result + * into both {@link FusionManager} and {@link MapMatchingEngine}.

    * * @see WifiDataProcessor the observable that triggers WiFi scan updates - * @see WiFiPositioning the API client for WiFi-based com.openpositioning.PositionMe.positioning + * @see WiFiPositioning the API client for WiFi-based positioning */ public class WifiPositionManager implements Observer { + private static final String TAG = "WifiPositionManager"; private static final String WIFI_FINGERPRINT = "wf"; private final WiFiPositioning wiFiPositioning; @@ -34,7 +36,7 @@ public class WifiPositionManager implements Observer { /** * Creates a new WifiPositionManager. * - * @param wiFiPositioning WiFi com.openpositioning.PositionMe.positioning API client + * @param wiFiPositioning WiFi positioning API client * @param recorder trajectory recorder for writing WiFi fingerprints */ public WifiPositionManager(WiFiPositioning wiFiPositioning, @@ -48,7 +50,7 @@ public WifiPositionManager(WiFiPositioning wiFiPositioning, * *

    Receives updates from {@link WifiDataProcessor}. Converts the raw object array * to a typed list, delegates fingerprint recording to {@link TrajectoryRecorder}, - * and triggers a WiFi com.openpositioning.PositionMe.positioning request.

    + * and triggers a WiFi positioning request.

    */ @Override public void update(Object[] wifiList) { @@ -58,34 +60,8 @@ public void update(Object[] wifiList) { } /** - * Creates a request to obtain a WiFi location for the obtained WiFi fingerprint. - */ - private void createWifiPositioningRequest() { - try { - JSONObject wifiAccessPoints = new JSONObject(); - for (Wifi data : this.wifiList) { - wifiAccessPoints.put(String.valueOf(data.getBssid()), data.getLevel()); - } - JSONObject wifiFingerPrint = new JSONObject(); - wifiFingerPrint.put(WIFI_FINGERPRINT, wifiAccessPoints); - this.wiFiPositioning.request(wifiFingerPrint); - - // Feed WiFi position to map matching engine - LatLng wifiPos = this.wiFiPositioning.getWifiLocation(); - int wifiFloor = this.wiFiPositioning.getFloor(); - if (wifiPos != null) { - MapMatchingEngine engine = SensorFusion.getInstance().getMapMatchingEngine(); - if (engine != null && engine.isActive()) { - engine.updateWifi(wifiPos.latitude, wifiPos.longitude, wifiFloor, 10.0f); - } - } - } catch (JSONException e) { - Log.e("jsonErrors", "Error creating json object" + e.toString()); - } - } - - /** - * Creates a WiFi com.openpositioning.PositionMe.positioning request using the Volley callback pattern. + * Creates a WiFi positioning request using the Volley callback pattern. + * On success, feeds the result into both FusionManager and MapMatchingEngine. */ private void createWifiPositionRequestCallback() { if (this.wifiList == null || this.wifiList.isEmpty()) { @@ -98,29 +74,51 @@ private void createWifiPositionRequestCallback() { } JSONObject wifiFingerPrint = new JSONObject(); wifiFingerPrint.put(WIFI_FINGERPRINT, wifiAccessPoints); + this.wiFiPositioning.request(wifiFingerPrint, new WiFiPositioning.VolleyCallback() { @Override public void onSuccess(LatLng wifiLocation, int floor) { - if (wifiLocation != null) { - FusionManager.getInstance().onWifi( + if (wifiLocation == null) return; + + // Feed into FusionManager (map-unaware, auto-initialising filter) + FusionManager.getInstance().onWifi( + wifiLocation.latitude, + wifiLocation.longitude + ); + + // Feed into MapMatchingEngine (wall-aware filter) + // This was previously missing — WiFi corrections were never + // reaching the map matching engine. + MapMatchingEngine engine = SensorFusion.getInstance().getMapMatchingEngine(); + if (engine != null && engine.isActive()) { + engine.updateWifi( wifiLocation.latitude, - wifiLocation.longitude + wifiLocation.longitude, + floor, + 10.0f ); } + + // TODO: remove before submission + Log.d(TAG, "WiFi fix → lat=" + wifiLocation.latitude + + " lng=" + wifiLocation.longitude + + " floor=" + floor + + " mmEngine=" + (engine != null && engine.isActive() + ? "updated" : "not active")); } @Override public void onError(String message) { - Log.e("WifiPositionManager", "WiFi com.openpositioning.PositionMe.positioning failed: " + message); + Log.e(TAG, "WiFi positioning failed: " + message); } }); } catch (JSONException e) { - Log.e("jsonErrors", "Error creating json object" + e.toString()); + Log.e(TAG, "Error creating WiFi JSON: " + e); } } /** - * Returns the user position obtained using WiFi com.openpositioning.PositionMe.positioning. + * Returns the user position obtained using WiFi positioning. * * @return {@link LatLng} corresponding to the user's position */ @@ -129,7 +127,7 @@ public LatLng getLatLngWifiPositioning() { } /** - * Returns the current floor the user is on, obtained using WiFi com.openpositioning.PositionMe.positioning. + * Returns the current floor the user is on, obtained using WiFi positioning. * * @return current floor number */ @@ -145,4 +143,4 @@ public int getWifiFloor() { public List getWifiList() { return this.wifiList; } -} +} \ No newline at end of file From 5c85a2e9c55b937506ee052b6bdb1b695bf0f54e Mon Sep 17 00:00:00 2001 From: Saleh-AlMulla Date: Mon, 30 Mar 2026 19:17:26 +0100 Subject: [PATCH 14/26] Tighten WiFi & floor-transition logic; add smoothing Adjust map-matching tuning and robustness: reduce WIFI_SIGMA (8->5) and resample jitter (0.5->0.2); add floor-transition guards (raised floor change threshold 2->4m, warmup period, sustained-steps requirement, min steps between changes) to avoid barometric false positives; classify lift vs stairs by horizontal motion and always apply barometric floor assignment while penalising particles not near transitions. Improve resampling and particle handling: wall-aware jitter (discard jitter that would cross walls), harsher wall-crossing penalty in predict, trigger resample on GNSS/low ESS, and reduce WiFi floor-mismatch penalty (0.1->0.4) so WiFi corrections pull particles. Add position output improvements: wall-aware fallback to best particle if mean lies across a wall, plus EMA smoothing with snap threshold for large corrections. Also update WifiPositionManager to use the tighter 5.0f WiFi parameter. Changes affect MapMatchingEngine.java and WifiPositionManager.java. --- .../mapmatching/MapMatchingEngine.java | 253 +++++++++++++++--- .../sensors/WifiPositionManager.java | 8 +- 2 files changed, 216 insertions(+), 45 deletions(-) diff --git a/app/src/main/java/com/openpositioning/PositionMe/mapmatching/MapMatchingEngine.java b/app/src/main/java/com/openpositioning/PositionMe/mapmatching/MapMatchingEngine.java index 6c14d9f7..e8220276 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/mapmatching/MapMatchingEngine.java +++ b/app/src/main/java/com/openpositioning/PositionMe/mapmatching/MapMatchingEngine.java @@ -53,7 +53,7 @@ public class MapMatchingEngine { private static final double PDR_NOISE_HEADING = Math.toRadians(8); /** Standard deviation for WiFi observation likelihood (metres). */ - private static final double WIFI_SIGMA = 8.0; + private static final double WIFI_SIGMA = 5.0; /** Standard deviation for GNSS observation likelihood (metres). */ private static final double GNSS_SIGMA = 12.0; @@ -68,32 +68,77 @@ public class MapMatchingEngine { private static final double TRANSITION_PROXIMITY = 4.0; /** - * Minimum elevation change from the last floor's baseline (metres) required - * to trigger a floor transition check. + * Minimum elevation change from the last floor baseline (metres) required + * to trigger a floor transition. Set to one full floor height so barometric + * noise from doors/HVAC cannot trigger false transitions. */ - private static final double FLOOR_CHANGE_ELEVATION_THRESHOLD = 2.0; + private static final double FLOOR_CHANGE_ELEVATION_THRESHOLD = 4.0; + + /** + * Number of consecutive steps the elevation must exceed + * {@link #FLOOR_CHANGE_ELEVATION_THRESHOLD} before a floor change commits. + * Prevents single-step barometric spikes from triggering false transitions. + */ + private static final int ELEVATION_SUSTAIN_STEPS = 5; + + /** + * Minimum steps between consecutive floor changes to prevent rapid + * oscillation from barometric noise. + */ + private static final int MIN_STEPS_BETWEEN_FLOOR_CHANGES = 10; + + /** + * Time in milliseconds after initialisation during which floor transitions + * are suppressed. During this period the elevation baseline continuously + * tracks the barometer, absorbing startup drift from HVAC, doors, and + * sensor stabilisation before any transition can fire. + */ + private static final long FLOOR_TRANSITION_WARMUP_MS = 30_000; /** Effective sample size ratio below which resampling is triggered. */ private static final double RESAMPLE_THRESHOLD = 0.5; /** * Spread radius (metres) for initial particle distribution. - * Set to 6.0 to handle indoor GPS error which can easily be 5-10m. + * 6.0m accounts for indoor GPS error which can easily be 5-10m. */ private static final double INIT_SPREAD = 6.0; /** - * Noise (metres) injected into each particle after resampling to maintain - * diversity and prevent the filter collapsing to a single point. + * Noise (metres) injected into particles after resampling to maintain + * diversity. Small value (0.2m) prevents particles teleporting through + * thin wall segments while still avoiding filter collapse. */ - private static final double RESAMPLE_JITTER = 0.5; + private static final double RESAMPLE_JITTER = 0.2; /** - * Maximum total horizontal distance (metres) accumulated while elevation is - * changing that still classifies the transition as a lift (not stairs). + * Maximum total horizontal distance (metres) during an elevation episode + * that classifies the transition as a lift rather than stairs. */ private static final double LIFT_HORIZONTAL_THRESHOLD = 1.5; + /** + * EMA smoothing factor for the output position displayed on screen. + * Range 0-1: lower = smoother but more lag, higher = more responsive. + * Large position jumps (WiFi corrections > 8m) bypass smoothing to snap + * immediately to the corrected position. + */ + private static final double POSITION_SMOOTH_ALPHA = 0.3; + + /** + * Distance (metres) beyond which a position change is treated as a + * deliberate WiFi/GNSS correction and bypasses EMA smoothing. + */ + private static final double POSITION_SMOOTH_SNAP_THRESHOLD = 8.0; + + /** + * Distance threshold (metres) between weighted mean and best-weight particle. + * If exceeded, the best particle is used as the position estimate instead of + * the mean, preventing the output from landing inside a wall when the particle + * cloud is split across a wall boundary. + */ + private static final double WALL_MEAN_FALLBACK_THRESHOLD = 3.0; + // ------------------------------------------------------------------------- // Coordinate conversion — metres per degree at Edinburgh (~55.92 N) // ------------------------------------------------------------------------- @@ -120,11 +165,21 @@ public class MapMatchingEngine { private int estimatedFloor = 0; private boolean enabled = false; + /** Wall clock time (ms) when initialise() was called. Used for warmup guard. */ + private long initialiseTimeMs = 0; + // Stairs / lift detection private float lastStepLength = 0f; private float horizontalDuringElevationChange = 0f; private boolean inElevationChange = false; - private float elevationChangeStartedAt = 0f; + + // Floor transition guards + private int stepsAboveElevationThreshold = 0; + private int stepsSinceLastFloorChange = 0; + + // Output smoothing (EMA applied to getEstimatedPosition output) + private double smoothedOutputX = Double.NaN; + private double smoothedOutputY = Double.NaN; // Debug counters — TODO: remove before submission private int wallHitCount = 0; @@ -155,6 +210,11 @@ public void initialise(double lat, double lng, int floor, this.elevationAtLastFloorChange = 0f; this.horizontalDuringElevationChange = 0f; this.inElevationChange = false; + this.stepsAboveElevationThreshold = 0; + this.stepsSinceLastFloorChange = 0; + this.initialiseTimeMs = System.currentTimeMillis(); + this.smoothedOutputX = Double.NaN; + this.smoothedOutputY = Double.NaN; this.wallHitCount = 0; this.wallCheckCount = 0; this.predictCallCount = 0; @@ -227,6 +287,11 @@ public void reset() { estimatedFloor = 0; horizontalDuringElevationChange = 0f; inElevationChange = false; + stepsAboveElevationThreshold = 0; + stepsSinceLastFloorChange = 0; + initialiseTimeMs = 0; + smoothedOutputX = Double.NaN; + smoothedOutputY = Double.NaN; wallHitCount = 0; wallCheckCount = 0; predictCallCount = 0; @@ -276,6 +341,7 @@ public void predict(float stepLength, float headingRad, float elevation) { this.currentElevation = elevation; this.lastStepLength = stepLength; + this.stepsSinceLastFloorChange++; for (Particle p : particles) { double noisyStep = stepLength + random.nextGaussian() * PDR_NOISE_STEP; @@ -292,6 +358,8 @@ public void predict(float stepLength, float headingRad, float elevation) { wallCheckCount++; if (doesCrossWall(oldX, oldY, newX, newY, p.getFloor())) { + // Penalise wall-crossing particles and leave them in place. + // The harsh penalty ensures they are eliminated at next resample. p.setWeight(p.getWeight() * WALL_PENALTY); wallHitCount++; } else { @@ -300,7 +368,7 @@ public void predict(float stepLength, float headingRad, float elevation) { } } - // TODO: remove before submission — fires once per step (200 checks per step) + // TODO: remove before submission — fires once per step (200 checks/step) if (wallCheckCount > 0 && wallCheckCount % 200 == 0) { Log.d(TAG, "Wall check #" + (wallCheckCount / 200) + " — hits=" + wallHitCount + "/" + wallCheckCount @@ -345,7 +413,9 @@ public void updateWifi(double lat, double lng, int wifiFloor, float accuracy) { double dist = Math.sqrt(Math.pow(p.getX() - obsX, 2) + Math.pow(p.getY() - obsY, 2)); double likelihood = gaussianLikelihood(dist, sigma); - if (p.getFloor() != wifiFloor) likelihood *= 0.1; + // Reduced floor mismatch penalty (0.4 not 0.1) so WiFi corrections + // still pull particles even when the floor estimate is temporarily wrong. + if (p.getFloor() != wifiFloor) likelihood *= 0.4; p.setWeight(p.getWeight() * likelihood); } @@ -384,6 +454,11 @@ public void updateGnss(double lat, double lng, float accuracy) { } normaliseWeights(); + // Trigger resample on GNSS updates as well as WiFi — keeps particle + // population healthy after sharp corrections. + if (effectiveSampleSize() < RESAMPLE_THRESHOLD * NUM_PARTICLES) { + resample(); + } } // ========================================================================= @@ -391,29 +466,62 @@ public void updateGnss(double lat, double lng, float accuracy) { // ========================================================================= /** - * Checks if the barometric elevation change warrants a floor transition, - * and distinguishes between stairs and lifts based on accumulated horizontal - * movement during the elevation change. + * Checks if the barometric elevation change warrants a floor transition. + * + *

    Four guards prevent false transitions from barometric noise:

    + *
      + *
    1. Warmup: for {@link #FLOOR_TRANSITION_WARMUP_MS} ms after init, + * the elevation baseline continuously tracks the barometer absorbing startup + * drift. No transitions can fire during this period.
      2. + *
    2. Full-floor threshold: delta must exceed + * {@link #FLOOR_CHANGE_ELEVATION_THRESHOLD} (one floor height).
      3. + *
    3. Sustained elevation: threshold must hold for + * {@link #ELEVATION_SUSTAIN_STEPS} consecutive steps.
      4. + *
    4. Minimum gap: at least {@link #MIN_STEPS_BETWEEN_FLOOR_CHANGES} + * steps since the last floor change.
      5. + *
    + * + *

    Stairs vs lift is classified by accumulated horizontal distance during + * the elevation change episode. Lifts show negligible horizontal movement.

    */ private void checkFloorTransition() { if (floorShapes == null || floorHeight <= 0) return; + // Guard 1: warmup — absorb barometric startup drift before allowing transitions + if (System.currentTimeMillis() - initialiseTimeMs < FLOOR_TRANSITION_WARMUP_MS) { + elevationAtLastFloorChange = currentElevation; + stepsAboveElevationThreshold = 0; + return; + } + + // Guard 2: minimum steps between floor changes + if (stepsSinceLastFloorChange < MIN_STEPS_BETWEEN_FLOOR_CHANGES) return; + float elevationDeltaFromBaseline = currentElevation - elevationAtLastFloorChange; float elevationChangeMagnitude = Math.abs(elevationDeltaFromBaseline); - if (elevationChangeMagnitude > 0.3f) { + // Track horizontal movement during elevation change episodes + if (elevationChangeMagnitude > 1.0f) { if (!inElevationChange) { inElevationChange = true; - elevationChangeStartedAt = currentElevation; horizontalDuringElevationChange = 0f; } horizontalDuringElevationChange += lastStepLength; } else { inElevationChange = false; horizontalDuringElevationChange = 0f; + stepsAboveElevationThreshold = 0; + } + + // Guard 3: full-floor threshold + if (elevationChangeMagnitude < FLOOR_CHANGE_ELEVATION_THRESHOLD) { + stepsAboveElevationThreshold = 0; + return; } - if (elevationChangeMagnitude < FLOOR_CHANGE_ELEVATION_THRESHOLD) return; + // Guard 4: sustained elevation over multiple steps + stepsAboveElevationThreshold++; + if (stepsAboveElevationThreshold < ELEVATION_SUSTAIN_STEPS) return; int elevationFloorDelta = (int) Math.round(elevationDeltaFromBaseline / floorHeight); if (elevationFloorDelta == 0) return; @@ -422,21 +530,20 @@ private void checkFloorTransition() { boolean isLift = horizontalDuringElevationChange < LIFT_HORIZONTAL_THRESHOLD; for (Particle p : particles) { - if (p.getFloor() == targetFloor) continue; - boolean nearTransition = isNearStairsOrLift(p.getX(), p.getY(), p.getFloor()); - + // Always move floor — barometer overrides per assignment spec §3.2. + // Penalise particles not near any transition feature. + p.setFloor(targetFloor); if (nearTransition) { - p.setFloor(targetFloor); - if (isLift) { - p.setWeight(p.getWeight() * 1.2); - } else { + if (!isLift) { + // Stairs: small horizontal spread to account for staircase exit position p.setX(p.getX() + random.nextGaussian() * 1.0); p.setY(p.getY() + random.nextGaussian() * 1.0); - p.setWeight(p.getWeight() * 1.2); } + // Lift: keep X/Y — shaft is vertical, no horizontal displacement + p.setWeight(p.getWeight() * 1.2); } else { - p.setWeight(p.getWeight() * 0.5); + p.setWeight(p.getWeight() * 0.3); } } @@ -444,12 +551,15 @@ private void checkFloorTransition() { Log.d(TAG, "Floor transition → floor " + targetFloor + " via " + (isLift ? "LIFT" : "STAIRS") + " horiz=" + String.format("%.2f", horizontalDuringElevationChange) + "m" - + " elevDelta=" + String.format("%.2f", elevationDeltaFromBaseline) + "m"); + + " elevDelta=" + String.format("%.2f", elevationDeltaFromBaseline) + "m" + + " sustainedSteps=" + stepsAboveElevationThreshold); this.estimatedFloor = targetFloor; this.elevationAtLastFloorChange = currentElevation; this.horizontalDuringElevationChange = 0f; this.inElevationChange = false; + this.stepsAboveElevationThreshold = 0; + this.stepsSinceLastFloorChange = 0; } /** @@ -591,8 +701,11 @@ private double effectiveSampleSize() { /** * Systematic resampling: selects particles proportional to their weights, * replaces the population with copies reset to uniform weight, then injects - * small Gaussian jitter ({@link #RESAMPLE_JITTER}) to maintain diversity - * and prevent the filter collapsing to a single point. + * small wall-aware Gaussian jitter to maintain diversity. + * + *

    Jitter is wall-checked before being applied — if the jitter displacement + * would cross a wall segment it is discarded, preventing particles from being + * teleported through thin walls during resampling.

    */ private void resample() { int n = particles.size(); @@ -617,12 +730,16 @@ private void resample() { newParticles.add(copy); } - // Inject diversity jitter to prevent full collapse after aggressive resampling. - // Without this, all surviving particles occupy the same XY and the filter - // behaves like a single point tracker. + // Inject diversity jitter only if it does not cross a wall. + // This prevents particles from teleporting through thin wall segments + // while still maintaining population diversity. for (Particle p : newParticles) { - p.setX(p.getX() + random.nextGaussian() * RESAMPLE_JITTER); - p.setY(p.getY() + random.nextGaussian() * RESAMPLE_JITTER); + double jx = p.getX() + random.nextGaussian() * RESAMPLE_JITTER; + double jy = p.getY() + random.nextGaussian() * RESAMPLE_JITTER; + if (!doesCrossWall(p.getX(), p.getY(), jx, jy, p.getFloor())) { + p.setX(jx); + p.setY(jy); + } } particles.clear(); @@ -650,21 +767,77 @@ private void normaliseWeights() { // ========================================================================= /** - * Returns the weighted mean position of all particles as a LatLng. + * Returns the best-estimate position as a LatLng, with two improvements + * over a naive weighted mean: * - * @return best-estimate position, or null if not initialised + *
      + *
    1. Wall-aware fallback: if the weighted mean is more than + * {@link #WALL_MEAN_FALLBACK_THRESHOLD} metres from the highest-weight + * particle, the particle cloud has likely split across a wall boundary. + * In this case the highest-weight particle position is used instead of + * the mean, keeping the output on the correct side of the wall.
      2. + *
    2. EMA smoothing: an exponential moving average + * (alpha = {@link #POSITION_SMOOTH_ALPHA}) is applied to the output + * coordinates before returning, reducing jitter from step-to-step PDR + * noise. Large position changes (WiFi/GNSS corrections greater than + * {@link #POSITION_SMOOTH_SNAP_THRESHOLD}) bypass smoothing and snap + * immediately to the corrected position.
      3. + *
    + * + * @return smoothed best-estimate position, or null if not initialised */ public LatLng getEstimatedPosition() { if (!isActive() || particles.isEmpty()) return null; + // Compute weighted mean and find highest-weight particle simultaneously double weightedX = 0, weightedY = 0; + Particle bestParticle = null; + double bestWeight = -1; + for (Particle p : particles) { weightedX += p.getX() * p.getWeight(); weightedY += p.getY() * p.getWeight(); + if (p.getWeight() > bestWeight) { + bestWeight = p.getWeight(); + bestParticle = p; + } + } + + // Wall-aware fallback: if the mean is far from the best particle the cloud + // has split across a wall — use the best particle instead + double rawX = weightedX; + double rawY = weightedY; + if (bestParticle != null) { + double dx = weightedX - bestParticle.getX(); + double dy = weightedY - bestParticle.getY(); + if (Math.sqrt(dx * dx + dy * dy) > WALL_MEAN_FALLBACK_THRESHOLD) { + rawX = bestParticle.getX(); + rawY = bestParticle.getY(); + } + } + + // EMA smoothing — snap on large corrections, smooth small movements + if (Double.isNaN(smoothedOutputX)) { + smoothedOutputX = rawX; + smoothedOutputY = rawY; + } else { + double jumpDist = Math.sqrt( + Math.pow(rawX - smoothedOutputX, 2) + + Math.pow(rawY - smoothedOutputY, 2)); + if (jumpDist > POSITION_SMOOTH_SNAP_THRESHOLD) { + // Large jump (WiFi/GNSS correction) — snap immediately + smoothedOutputX = rawX; + smoothedOutputY = rawY; + } else { + smoothedOutputX = POSITION_SMOOTH_ALPHA * rawX + + (1.0 - POSITION_SMOOTH_ALPHA) * smoothedOutputX; + smoothedOutputY = POSITION_SMOOTH_ALPHA * rawY + + (1.0 - POSITION_SMOOTH_ALPHA) * smoothedOutputY; + } } - double lat = refLat + weightedY / METRES_PER_DEG_LAT; - double lng = refLng + weightedX / METRES_PER_DEG_LNG; + double lat = refLat + smoothedOutputY / METRES_PER_DEG_LAT; + double lng = refLng + smoothedOutputX / METRES_PER_DEG_LNG; return new LatLng(lat, lng); } 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 012b1868..fbfabd83 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java @@ -18,8 +18,8 @@ * Manages WiFi scan result processing and WiFi-based positioning requests. * *

    Implements {@link Observer} to receive updates from {@link WifiDataProcessor}. - * On each WiFi scan result, it fires a positioning request and feeds the result - * into both {@link FusionManager} and {@link MapMatchingEngine}.

    + * On each WiFi scan result, fires a positioning request and feeds the result into + * both {@link FusionManager} and {@link MapMatchingEngine}.

    * * @see WifiDataProcessor the observable that triggers WiFi scan updates * @see WiFiPositioning the API client for WiFi-based positioning @@ -87,15 +87,13 @@ public void onSuccess(LatLng wifiLocation, int floor) { ); // Feed into MapMatchingEngine (wall-aware filter) - // This was previously missing — WiFi corrections were never - // reaching the map matching engine. MapMatchingEngine engine = SensorFusion.getInstance().getMapMatchingEngine(); if (engine != null && engine.isActive()) { engine.updateWifi( wifiLocation.latitude, wifiLocation.longitude, floor, - 10.0f + 5.0f ); } From 086390fb90426c6b989dcd36ca9d4e7124ae3bb3 Mon Sep 17 00:00:00 2001 From: jagsnapuri Date: Tue, 31 Mar 2026 15:48:32 +0100 Subject: [PATCH 15/26] Redone sensor fusion and display stability improvements --- .../fragment/CorrectionFragment.java | 28 ++++-- .../fragment/RecordingFragment.java | 86 ++++++++++++++++--- .../fragment/TrajectoryMapFragment.java | 12 +++ .../PositionMe/utils/PdrProcessing.java | 9 +- .../main/res/layout-small/fragment_home.xml | 2 +- app/src/main/res/layout/fragment_home.xml | 2 +- 6 files changed, 115 insertions(+), 24 deletions(-) diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/CorrectionFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/CorrectionFragment.java index 165b9e16..1f6fd3ef 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/CorrectionFragment.java +++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/CorrectionFragment.java @@ -108,21 +108,35 @@ public void onViewCreated(@NonNull View view, @Nullable Bundle savedInstanceStat this.pathView = view.findViewById(R.id.pathView1); averageStepLength = sensorFusion.passAverageStepLength(); - averageStepLengthText.setText(getString(R.string.averageStepLgn) + ": " - + String.format("%.2f", averageStepLength)); + if (Float.isNaN(averageStepLength) || averageStepLength <= 0.05f) { + averageStepLengthText.setText(getString(R.string.averageStepLgn) + ": --"); + } else { + averageStepLengthText.setText(getString(R.string.averageStepLgn) + ": " + + String.format("%.2f", averageStepLength)); + } // Listen for ENTER key this.stepLengthInput.setOnKeyListener((v, keyCode, event) -> { if (keyCode == KeyEvent.KEYCODE_ENTER) { - newStepLength = Float.parseFloat(changedText.toString()); - // Rescale path - sensorFusion.redrawPath(newStepLength / averageStepLength); + String text = changedText == null ? "" : changedText.toString().trim(); + if (text.isEmpty()) return false; + + try { + newStepLength = Float.parseFloat(text); + } catch (NumberFormatException e) { + return false; + } + + if (!Float.isNaN(averageStepLength) && averageStepLength > 0.05f) { + sensorFusion.redrawPath(newStepLength / averageStepLength); + pathView.invalidate(); + } + averageStepLengthText.setText(getString(R.string.averageStepLgn) + ": " + String.format("%.2f", newStepLength)); - pathView.invalidate(); secondPass++; - if (secondPass == 2) { + if (secondPass == 2 || Float.isNaN(averageStepLength) || averageStepLength <= 0.05f) { averageStepLength = newStepLength; secondPass = 0; } 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 af47fc48..2b5fb753 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 @@ -90,6 +90,19 @@ public class RecordingFragment extends Fragment { // References to the child map fragment private TrajectoryMapFragment trajectoryMapFragment; + private LatLng lastDisplayedGnssObservation = null; + private long lastDisplayedGnssTimeMs = 0L; + private static final long GNSS_DISPLAY_MIN_INTERVAL_MS = 1500L; + private static final double GNSS_DISPLAY_MIN_DISTANCE_M = 3.0; + + private LatLng lastDisplayedWifiObservation = null; + private long lastDisplayedWifiTimeMs = 0L; + private static final long WIFI_DISPLAY_MIN_INTERVAL_MS = 1500L; + private static final double WIFI_DISPLAY_MIN_DISTANCE_M = 2.0; + + private Float smoothedHeadingDeg = null; + private static final float HEADING_SMOOTHING_ALPHA = 0.20f; + private final Runnable refreshDataTask = new Runnable() { @Override public void run() { @@ -287,14 +300,12 @@ private void updateUIandPosition() { // Best position: prefer MapMatchingEngine (wall-aware) > FusionManager > raw PDR LatLng fusedLocation = null; - // First choice: map-matched position (has wall constraints) com.openpositioning.PositionMe.mapmatching.MapMatchingEngine mmEngine = sensorFusion.getMapMatchingEngine(); if (mmEngine != null && mmEngine.isActive()) { fusedLocation = mmEngine.getEstimatedPosition(); } - // Second choice: FusionManager (WiFi/GNSS corrections, no walls) if (fusedLocation == null) { double[] fusedPos = FusionManager.getInstance().getBestPosition(); if (fusedPos != null) { @@ -302,19 +313,19 @@ private void updateUIandPosition() { } } - // Display the best available position + float headingDeg = smoothHeadingDeg((float) Math.toDegrees(sensorFusion.passOrientation())); + if (fusedLocation != null && trajectoryMapFragment != null) { - trajectoryMapFragment.updateUserLocation(fusedLocation, - (float) Math.toDegrees(sensorFusion.passOrientation())); + trajectoryMapFragment.updateUserLocation(fusedLocation, headingDeg); trajectoryMapFragment.updatePdrObservation(fusedLocation); } else if (rawPdrPosition != null && trajectoryMapFragment != null) { - // Fallback: raw PDR before any fusion is ready - trajectoryMapFragment.updateUserLocation(rawPdrPosition, - (float) Math.toDegrees(sensorFusion.passOrientation())); + trajectoryMapFragment.updateUserLocation(rawPdrPosition, headingDeg); trajectoryMapFragment.updatePdrObservation(rawPdrPosition); } - // GNSS logic + long now = System.currentTimeMillis(); + + // GNSS logic: only draw when a genuinely new GNSS observation arrives float[] gnss = sensorFusion.getSensorValueMap().get(SensorTypes.GNSSLATLONG); if (gnss != null && trajectoryMapFragment != null) { if (trajectoryMapFragment.isGnssEnabled()) { @@ -325,18 +336,71 @@ private void updateUIandPosition() { gnssError.setVisibility(View.VISIBLE); gnssError.setText(String.format(getString(R.string.gnss_error) + "%.2fm", errorDist)); } - trajectoryMapFragment.updateGNSS(gnssLocation); + + if (shouldDisplayObservation( + gnssLocation, + lastDisplayedGnssObservation, + now - lastDisplayedGnssTimeMs, + GNSS_DISPLAY_MIN_INTERVAL_MS, + GNSS_DISPLAY_MIN_DISTANCE_M)) { + trajectoryMapFragment.updateGNSS(gnssLocation); + lastDisplayedGnssObservation = gnssLocation; + lastDisplayedGnssTimeMs = now; + } } else { gnssError.setVisibility(View.GONE); trajectoryMapFragment.clearGNSS(); } } - // Update previous + // WiFi logic: draw green observation markers only when a new accepted WiFi fix appears + if (trajectoryMapFragment != null) { + LatLng wifiLocation = sensorFusion.getLatLngWifiPositioning(); + if (wifiLocation != null && shouldDisplayObservation( + wifiLocation, + lastDisplayedWifiObservation, + now - lastDisplayedWifiTimeMs, + WIFI_DISPLAY_MIN_INTERVAL_MS, + WIFI_DISPLAY_MIN_DISTANCE_M)) { + trajectoryMapFragment.updateWifiObservation(wifiLocation); + lastDisplayedWifiObservation = wifiLocation; + lastDisplayedWifiTimeMs = now; + } + } + previousPosX = pdrValues[0]; previousPosY = pdrValues[1]; } + private boolean shouldDisplayObservation(LatLng candidate, + LatLng previous, + long ageMs, + long minIntervalMs, + double minDistanceMeters) { + if (candidate == null) return false; + if (previous == null) return true; + if (ageMs < minIntervalMs) return false; + return UtilFunctions.distanceBetweenPoints(previous, candidate) >= minDistanceMeters; + } + + private float smoothHeadingDeg(float newHeadingDeg) { + if (Float.isNaN(newHeadingDeg) || Float.isInfinite(newHeadingDeg)) { + return smoothedHeadingDeg != null ? smoothedHeadingDeg : 0f; + } + + if (smoothedHeadingDeg == null) { + smoothedHeadingDeg = newHeadingDeg; + return newHeadingDeg; + } + + float delta = newHeadingDeg - smoothedHeadingDeg; + while (delta > 180f) delta -= 360f; + while (delta < -180f) delta += 360f; + + smoothedHeadingDeg = smoothedHeadingDeg + HEADING_SMOOTHING_ALPHA * delta; + return smoothedHeadingDeg; + } + /** * Start the blinking effect for the recording icon. */ 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 81532cf6..296557cb 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 @@ -134,6 +134,8 @@ public class TrajectoryMapFragment extends Fragment { private final List wifiObservationMarkers = new ArrayList<>(); private final List pdrObservationMarkers = new ArrayList<>(); + private LatLng lastWifiObservationLocation = null; + /** * Orange polyline connecting every accepted PDR position (filtered by distance threshold). @@ -731,6 +733,15 @@ public void updateWifiObservation(@NonNull LatLng wifiLocation) { return; } + if (lastWifiObservationLocation != null) { + double dist = UtilFunctions.distanceBetweenPoints(lastWifiObservationLocation, wifiLocation); + if (dist < 2.0) { + Log.d("DisplayDebug", "updateWifiObservation skipped: too close (" + + String.format("%.2f", dist) + "m < 2.0m)"); + return; + } + } + addObservationMarker( wifiLocation, "WiFi", @@ -738,6 +749,7 @@ public void updateWifiObservation(@NonNull LatLng wifiLocation) { wifiObservationMarkers, MAX_WIFI_OBSERVATIONS ); + lastWifiObservationLocation = wifiLocation; } 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..64a0609c 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java @@ -403,14 +403,15 @@ public void resetPDR() { * @return average step length in meters. */ public float getAverageStepLength(){ - //Calculate average step length - float averageStepLength = sumStepLength/(float) stepCount; + if (stepCount <= 0) { + return 0f; + } + + float averageStepLength = sumStepLength / (float) stepCount; - //Reset sum and number of steps stepCount = 0; sumStepLength = 0; - //Return average step length return averageStepLength; } diff --git a/app/src/main/res/layout-small/fragment_home.xml b/app/src/main/res/layout-small/fragment_home.xml index bd713b67..b3af6a36 100644 --- a/app/src/main/res/layout-small/fragment_home.xml +++ b/app/src/main/res/layout-small/fragment_home.xml @@ -155,7 +155,7 @@ android:textColor="@color/md_theme_onSecondary" /> - + - + Date: Tue, 31 Mar 2026 16:01:47 +0100 Subject: [PATCH 16/26] Update RecordingFragment.java --- .../PositionMe/presentation/fragment/RecordingFragment.java | 1 + 1 file changed, 1 insertion(+) 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 499586f7..7ba17b14 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 @@ -100,6 +100,7 @@ public class RecordingFragment extends Fragment { private static final double WIFI_DISPLAY_MIN_DISTANCE_M = 2.0; private Float smoothedHeadingDeg = null; + private static final float HEADING_SMOOTHING_ALPHA = 0.20f; private final Runnable refreshDataTask = new Runnable() { @Override public void run() { From f62a0c70f2d4d8209efdbfaf737527b693b5e979 Mon Sep 17 00:00:00 2001 From: Lane2048 <145272991+Lane2048@users.noreply.github.com> Date: Tue, 31 Mar 2026 17:21:30 +0100 Subject: [PATCH 17/26] xml added path smoothing switch --- app/src/main/res/layout/fragment_trajectory_map.xml | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/app/src/main/res/layout/fragment_trajectory_map.xml b/app/src/main/res/layout/fragment_trajectory_map.xml index a72425bf..a5b71a5d 100644 --- a/app/src/main/res/layout/fragment_trajectory_map.xml +++ b/app/src/main/res/layout/fragment_trajectory_map.xml @@ -43,6 +43,13 @@ android:layout_height="wrap_content" android:text="@string/auto_floor" /> + + Date: Tue, 31 Mar 2026 18:58:38 +0100 Subject: [PATCH 18/26] Fix Wifi Observations --- .../presentation/fragment/RecordingFragment.java | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 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 af47fc48..19f5bfea 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 @@ -231,7 +231,7 @@ private void onAddTestPoint() { LatLng cur = trajectoryMapFragment.getCurrentLocation(); if (cur == null) { Toast.makeText(requireContext(), "" + - "I haven't gotten my current location yet, let me take a couple of steps/wait for the map to load.", + "I haven't gotten my current location yet, let me take a couple of steps/wait for the map to load.", Toast.LENGTH_SHORT).show(); return; } @@ -332,6 +332,12 @@ private void updateUIandPosition() { } } + // WiFi observation marker + LatLng wifiLocation = sensorFusion.getLatLngWifiPositioning(); + if (wifiLocation != null && trajectoryMapFragment != null) { + trajectoryMapFragment.updateWifiObservation(wifiLocation); + } + // Update previous previousPosX = pdrValues[0]; previousPosY = pdrValues[1]; @@ -382,4 +388,4 @@ private static class TestPoint { private final List testPoints = new ArrayList<>(); -} +} \ No newline at end of file From 4507ded451201e9226a8ab828326f26c66169660 Mon Sep 17 00:00:00 2001 From: Saleh-AlMulla Date: Tue, 31 Mar 2026 19:07:58 +0100 Subject: [PATCH 19/26] Improve map-matching recovery, smoothing & UI Adjust MapMatchingEngine parameters and behaviour to improve stability and UX: reduce EMA alpha to 0.25, increase snap threshold to 20m and wall-fallback threshold to 5m. Add lost-filter recovery on WiFi updates (reinitialise particles around WiFi fix and reset EMA to prevent trace smearing). Simplify/rescue wall-cross handling and make resampling wall-aware. Update RecordingFragment to prioritise MapMatchingEngine as the primary position source, wait for the engine to be active before drawing traces, initialise raw PDR from the engine start to keep traces consistent, and streamline GNSS/WiFi observation display logic and logging. Miscellaneous comment cleanups and small formatting/refactor changes across both files. --- .../mapmatching/MapMatchingEngine.java | 161 ++++++++---------- .../fragment/RecordingFragment.java | 141 ++++++++------- 2 files changed, 145 insertions(+), 157 deletions(-) diff --git a/app/src/main/java/com/openpositioning/PositionMe/mapmatching/MapMatchingEngine.java b/app/src/main/java/com/openpositioning/PositionMe/mapmatching/MapMatchingEngine.java index e8220276..e02837cc 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/mapmatching/MapMatchingEngine.java +++ b/app/src/main/java/com/openpositioning/PositionMe/mapmatching/MapMatchingEngine.java @@ -106,8 +106,8 @@ public class MapMatchingEngine { /** * Noise (metres) injected into particles after resampling to maintain - * diversity. Small value (0.2m) prevents particles teleporting through - * thin wall segments while still avoiding filter collapse. + * diversity. Wall-checked before applying to prevent particles teleporting + * through thin wall segments. */ private static final double RESAMPLE_JITTER = 0.2; @@ -118,26 +118,24 @@ public class MapMatchingEngine { private static final double LIFT_HORIZONTAL_THRESHOLD = 1.5; /** - * EMA smoothing factor for the output position displayed on screen. - * Range 0-1: lower = smoother but more lag, higher = more responsive. - * Large position jumps (WiFi corrections > 8m) bypass smoothing to snap - * immediately to the corrected position. + * EMA smoothing factor for the output position. + * 0.25 balances responsiveness with smoothness — higher than 0.15 so the + * marker visibly follows movement, lower than 0.3 to reduce step jitter. */ - private static final double POSITION_SMOOTH_ALPHA = 0.3; + private static final double POSITION_SMOOTH_ALPHA = 0.25; /** - * Distance (metres) beyond which a position change is treated as a - * deliberate WiFi/GNSS correction and bypasses EMA smoothing. + * Distance (metres) beyond which a position change bypasses EMA smoothing. + * Only used for genuine large corrections, not lost-filter reinitialisation + * (which resets smoothedOutputX/Y directly to avoid trace smearing). */ - private static final double POSITION_SMOOTH_SNAP_THRESHOLD = 8.0; + private static final double POSITION_SMOOTH_SNAP_THRESHOLD = 20.0; /** * Distance threshold (metres) between weighted mean and best-weight particle. - * If exceeded, the best particle is used as the position estimate instead of - * the mean, preventing the output from landing inside a wall when the particle - * cloud is split across a wall boundary. + * If exceeded, best particle position is used (mean likely inside a wall). */ - private static final double WALL_MEAN_FALLBACK_THRESHOLD = 3.0; + private static final double WALL_MEAN_FALLBACK_THRESHOLD = 5.0; // ------------------------------------------------------------------------- // Coordinate conversion — metres per degree at Edinburgh (~55.92 N) @@ -358,8 +356,6 @@ public void predict(float stepLength, float headingRad, float elevation) { wallCheckCount++; if (doesCrossWall(oldX, oldY, newX, newY, p.getFloor())) { - // Penalise wall-crossing particles and leave them in place. - // The harsh penalty ensures they are eliminated at next resample. p.setWeight(p.getWeight() * WALL_PENALTY); wallHitCount++; } else { @@ -368,7 +364,7 @@ public void predict(float stepLength, float headingRad, float elevation) { } } - // TODO: remove before submission — fires once per step (200 checks/step) + // TODO: remove before submission if (wallCheckCount > 0 && wallCheckCount % 200 == 0) { Log.d(TAG, "Wall check #" + (wallCheckCount / 200) + " — hits=" + wallHitCount + "/" + wallCheckCount @@ -397,6 +393,12 @@ public void predict(float stepLength, float headingRad, float elevation) { /** * Updates particle weights based on a WiFi position observation. * + *

    Also handles lost-filter recovery: if the weighted mean has drifted + * more than 15m from the WiFi fix, the filter has diverged — particles are + * reinitialised around the WiFi fix. The EMA smoothed output is reset to the + * new position immediately to prevent trace smearing between old and new + * positions while the EMA catches up.

    + * * @param lat WiFi-estimated latitude * @param lng WiFi-estimated longitude * @param wifiFloor WiFi-estimated floor number @@ -413,13 +415,44 @@ public void updateWifi(double lat, double lng, int wifiFloor, float accuracy) { double dist = Math.sqrt(Math.pow(p.getX() - obsX, 2) + Math.pow(p.getY() - obsY, 2)); double likelihood = gaussianLikelihood(dist, sigma); - // Reduced floor mismatch penalty (0.4 not 0.1) so WiFi corrections - // still pull particles even when the floor estimate is temporarily wrong. if (p.getFloor() != wifiFloor) likelihood *= 0.4; p.setWeight(p.getWeight() * likelihood); } normaliseWeights(); + + // ── Lost-filter recovery ────────────────────────────────────────────── + double weightedX = 0, weightedY = 0; + for (Particle p : particles) { + weightedX += p.getX() * p.getWeight(); + weightedY += p.getY() * p.getWeight(); + } + double meanDistFromWifi = Math.sqrt( + Math.pow(weightedX - obsX, 2) + Math.pow(weightedY - obsY, 2)); + + if (meanDistFromWifi > 8.0) { + Log.w(TAG, "Lost filter detected — reinitialising around WiFi fix" + + " dist=" + String.format("%.1f", meanDistFromWifi) + "m"); + particles.clear(); + for (int i = 0; i < NUM_PARTICLES; i++) { + double px = obsX + random.nextGaussian() * WIFI_SIGMA; + double py = obsY + random.nextGaussian() * WIFI_SIGMA; + particles.add(new Particle(px, py, wifiFloor, 1.0 / NUM_PARTICLES)); + } + // Reset EMA to particle weighted mean after reinit, not raw WiFi fix, + // keeping displayed position closer to the building interior. + normaliseWeights(); + double reinitMeanX = 0, reinitMeanY = 0; + for (Particle p : particles) { + reinitMeanX += p.getX() * p.getWeight(); + reinitMeanY += p.getY() * p.getWeight(); + } + smoothedOutputX = reinitMeanX; + smoothedOutputY = reinitMeanY; + return; + } + // ───────────────────────────────────────────────────────────────────── + if (effectiveSampleSize() < RESAMPLE_THRESHOLD * NUM_PARTICLES) { resample(); } @@ -454,8 +487,6 @@ public void updateGnss(double lat, double lng, float accuracy) { } normaliseWeights(); - // Trigger resample on GNSS updates as well as WiFi — keeps particle - // population healthy after sharp corrections. if (effectiveSampleSize() < RESAMPLE_THRESHOLD * NUM_PARTICLES) { resample(); } @@ -468,39 +499,28 @@ public void updateGnss(double lat, double lng, float accuracy) { /** * Checks if the barometric elevation change warrants a floor transition. * - *

    Four guards prevent false transitions from barometric noise:

    + *

    Four guards prevent false transitions:

    *
      - *
    1. Warmup: for {@link #FLOOR_TRANSITION_WARMUP_MS} ms after init, - * the elevation baseline continuously tracks the barometer absorbing startup - * drift. No transitions can fire during this period.
      2. - *
    2. Full-floor threshold: delta must exceed - * {@link #FLOOR_CHANGE_ELEVATION_THRESHOLD} (one floor height).
      3. - *
    3. Sustained elevation: threshold must hold for - * {@link #ELEVATION_SUSTAIN_STEPS} consecutive steps.
      4. - *
    4. Minimum gap: at least {@link #MIN_STEPS_BETWEEN_FLOOR_CHANGES} - * steps since the last floor change.
      5. + *
    5. Warmup: baseline tracks barometer for 30s absorbing startup drift.
      6. + *
    6. Full-floor threshold: delta must exceed one floor height (4.0m).
      7. + *
    7. Sustained elevation: must hold for 5 consecutive steps.
      8. + *
    8. Minimum gap: at least 10 steps since last floor change.
      9. *
    - * - *

    Stairs vs lift is classified by accumulated horizontal distance during - * the elevation change episode. Lifts show negligible horizontal movement.

    */ private void checkFloorTransition() { if (floorShapes == null || floorHeight <= 0) return; - // Guard 1: warmup — absorb barometric startup drift before allowing transitions if (System.currentTimeMillis() - initialiseTimeMs < FLOOR_TRANSITION_WARMUP_MS) { elevationAtLastFloorChange = currentElevation; stepsAboveElevationThreshold = 0; return; } - // Guard 2: minimum steps between floor changes if (stepsSinceLastFloorChange < MIN_STEPS_BETWEEN_FLOOR_CHANGES) return; float elevationDeltaFromBaseline = currentElevation - elevationAtLastFloorChange; float elevationChangeMagnitude = Math.abs(elevationDeltaFromBaseline); - // Track horizontal movement during elevation change episodes if (elevationChangeMagnitude > 1.0f) { if (!inElevationChange) { inElevationChange = true; @@ -513,13 +533,11 @@ private void checkFloorTransition() { stepsAboveElevationThreshold = 0; } - // Guard 3: full-floor threshold if (elevationChangeMagnitude < FLOOR_CHANGE_ELEVATION_THRESHOLD) { stepsAboveElevationThreshold = 0; return; } - // Guard 4: sustained elevation over multiple steps stepsAboveElevationThreshold++; if (stepsAboveElevationThreshold < ELEVATION_SUSTAIN_STEPS) return; @@ -531,16 +549,12 @@ private void checkFloorTransition() { for (Particle p : particles) { boolean nearTransition = isNearStairsOrLift(p.getX(), p.getY(), p.getFloor()); - // Always move floor — barometer overrides per assignment spec §3.2. - // Penalise particles not near any transition feature. p.setFloor(targetFloor); if (nearTransition) { if (!isLift) { - // Stairs: small horizontal spread to account for staircase exit position p.setX(p.getX() + random.nextGaussian() * 1.0); p.setY(p.getY() + random.nextGaussian() * 1.0); } - // Lift: keep X/Y — shaft is vertical, no horizontal displacement p.setWeight(p.getWeight() * 1.2); } else { p.setWeight(p.getWeight() * 0.3); @@ -562,9 +576,7 @@ private void checkFloorTransition() { this.stepsSinceLastFloorChange = 0; } - /** - * Checks if a position is near stairs or lift features on the given floor. - */ + /** Checks if a position is near stairs or lift features on the given floor. */ private boolean isNearStairsOrLift(double x, double y, int floor) { if (floorShapes == null || floor < 0 || floor >= floorShapes.size()) return false; @@ -622,8 +634,6 @@ private boolean doesCrossWall(double x1, double y1, double x2, double y2, int fl } } - // Check closing edge for polygon rings, guarding against zero-length - // segments (standard GeoJSON closed rings repeat first point as last) if (size >= 3) { String geoType = feature.getGeometryType(); if ("MultiPolygon".equals(geoType) || "Polygon".equals(geoType)) { @@ -647,10 +657,6 @@ private boolean doesCrossWall(double x1, double y1, double x2, double y2, int fl return false; } - /** - * Tests if two line segments (p1 to p2) and (p3 to p4) intersect using the - * cross-product orientation method. - */ private static boolean segmentsIntersect(double p1x, double p1y, double p2x, double p2y, double p3x, double p3y, double p4x, double p4y) { double d1 = crossProduct(p3x, p3y, p4x, p4y, p1x, p1y); @@ -671,13 +677,11 @@ private static boolean segmentsIntersect(double p1x, double p1y, double p2x, dou return false; } - /** Cross product of vectors (b-a) x (c-a). */ private static double crossProduct(double ax, double ay, double bx, double by, double cx, double cy) { return (bx - ax) * (cy - ay) - (by - ay) * (cx - ax); } - /** Checks if point (px,py) lies on segment (ax,ay) to (bx,by), assuming collinearity. */ private static boolean onSegment(double ax, double ay, double bx, double by, double px, double py) { return Math.min(ax, bx) <= px && px <= Math.max(ax, bx) @@ -688,10 +692,6 @@ private static boolean onSegment(double ax, double ay, double bx, double by, // Resampling // ========================================================================= - /** - * Calculates the effective sample size (ESS) of the particle population. - * ESS = 1 / sum(wi squared). Low ESS indicates weight degeneracy. - */ private double effectiveSampleSize() { double sumSq = 0; for (Particle p : particles) sumSq += p.getWeight() * p.getWeight(); @@ -699,13 +699,7 @@ private double effectiveSampleSize() { } /** - * Systematic resampling: selects particles proportional to their weights, - * replaces the population with copies reset to uniform weight, then injects - * small wall-aware Gaussian jitter to maintain diversity. - * - *

    Jitter is wall-checked before being applied — if the jitter displacement - * would cross a wall segment it is discarded, preventing particles from being - * teleported through thin walls during resampling.

    + * Systematic resampling with wall-aware jitter. */ private void resample() { int n = particles.size(); @@ -730,9 +724,6 @@ private void resample() { newParticles.add(copy); } - // Inject diversity jitter only if it does not cross a wall. - // This prevents particles from teleporting through thin wall segments - // while still maintaining population diversity. for (Particle p : newParticles) { double jx = p.getX() + random.nextGaussian() * RESAMPLE_JITTER; double jy = p.getY() + random.nextGaussian() * RESAMPLE_JITTER; @@ -746,10 +737,6 @@ private void resample() { particles.addAll(newParticles); } - /** - * Normalises all particle weights to sum to 1. - * Resets to uniform if weights have collapsed entirely. - */ private void normaliseWeights() { double sum = 0; for (Particle p : particles) sum += p.getWeight(); @@ -767,29 +754,19 @@ private void normaliseWeights() { // ========================================================================= /** - * Returns the best-estimate position as a LatLng, with two improvements - * over a naive weighted mean: + * Returns the best-estimate position as a LatLng. * - *
      - *
    1. Wall-aware fallback: if the weighted mean is more than - * {@link #WALL_MEAN_FALLBACK_THRESHOLD} metres from the highest-weight - * particle, the particle cloud has likely split across a wall boundary. - * In this case the highest-weight particle position is used instead of - * the mean, keeping the output on the correct side of the wall.
      2. - *
    2. EMA smoothing: an exponential moving average - * (alpha = {@link #POSITION_SMOOTH_ALPHA}) is applied to the output - * coordinates before returning, reducing jitter from step-to-step PDR - * noise. Large position changes (WiFi/GNSS corrections greater than - * {@link #POSITION_SMOOTH_SNAP_THRESHOLD}) bypass smoothing and snap - * immediately to the corrected position.
      3. - *
    + *

    Uses weighted mean with wall-aware fallback (if mean is more than + * {@link #WALL_MEAN_FALLBACK_THRESHOLD} metres from the best particle, uses + * best particle instead). EMA smoothing with alpha={@link #POSITION_SMOOTH_ALPHA} + * is applied, with snap for corrections larger than + * {@link #POSITION_SMOOTH_SNAP_THRESHOLD}.

    * - * @return smoothed best-estimate position, or null if not initialised + * @return smoothed wall-constrained position, or null if not initialised */ public LatLng getEstimatedPosition() { if (!isActive() || particles.isEmpty()) return null; - // Compute weighted mean and find highest-weight particle simultaneously double weightedX = 0, weightedY = 0; Particle bestParticle = null; double bestWeight = -1; @@ -803,8 +780,6 @@ public LatLng getEstimatedPosition() { } } - // Wall-aware fallback: if the mean is far from the best particle the cloud - // has split across a wall — use the best particle instead double rawX = weightedX; double rawY = weightedY; if (bestParticle != null) { @@ -816,7 +791,6 @@ public LatLng getEstimatedPosition() { } } - // EMA smoothing — snap on large corrections, smooth small movements if (Double.isNaN(smoothedOutputX)) { smoothedOutputX = rawX; smoothedOutputY = rawY; @@ -825,7 +799,6 @@ public LatLng getEstimatedPosition() { Math.pow(rawX - smoothedOutputX, 2) + Math.pow(rawY - smoothedOutputY, 2)); if (jumpDist > POSITION_SMOOTH_SNAP_THRESHOLD) { - // Large jump (WiFi/GNSS correction) — snap immediately smoothedOutputX = rawX; smoothedOutputY = rawY; } else { @@ -897,7 +870,7 @@ public List getParticles() { // Utility // ========================================================================= - /** Gaussian likelihood: exp(-d squared / (2 * sigma squared)). */ + /** Gaussian likelihood: exp(-d² / (2σ²)). */ private static double gaussianLikelihood(double distance, double sigma) { return Math.exp(-(distance * distance) / (2.0 * sigma * sigma)); } 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 7ba17b14..854e9266 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 @@ -44,20 +44,23 @@ *

    * The RecordingFragment serves as the interface for users to initiate, monitor, and * complete trajectory recording. It integrates sensor fusion data to track user movement - * and updates a map view in real time. Additionally, it provides UI controls to cancel, - * stop, and monitor recording progress. + * and updates a map view in real time. *

    - * Features: - * - Starts and stops trajectory recording. - * - Displays real-time sensor data such as elevation and distance traveled. - * - Provides UI controls to cancel or complete recording. - * - Uses {@link TrajectoryMapFragment} to visualize recorded paths. - * - Manages GNSS tracking and error display. + * Position display strategy: + *

      + *
    1. Primary: MapMatchingEngine — wall-constrained particle filter with floor detection. + * EMA-smoothed output with lost-filter recovery that resets the EMA on reinitialisation + * to prevent trace smearing outside the building.
      2. + *
    2. Fallback: FusionManager — smooth GPS+WiFi+PDR Kalman filter, used when + * MapMatchingEngine has not yet been initialised.
      3. + *
    3. Last resort: raw PDR position.
      4. + *
    + * The orange trace (marker + polyline) always reflects one consistent source per cycle. + * The red trace shows raw step-counting only, allowing visual comparison. * * @see TrajectoryMapFragment The map fragment displaying the recorded trajectory. * @see RecordingActivity The activity managing the recording workflow. * @see SensorFusion Handles sensor data collection. - * @see SensorTypes Enumeration of available sensor types. */ public class RecordingFragment extends Fragment { @@ -101,6 +104,7 @@ public class RecordingFragment extends Fragment { private Float smoothedHeadingDeg = null; private static final float HEADING_SMOOTHING_ALPHA = 0.20f; + private final Runnable refreshDataTask = new Runnable() { @Override public void run() { @@ -231,7 +235,21 @@ private void onAddTestPoint() { } /** - * Update the UI with sensor data and pass map updates to TrajectoryMapFragment. + * Updates the UI with sensor data and passes map updates to TrajectoryMapFragment. + * + *

    Position source priority each cycle (~200ms):

    + *
      + *
    1. MapMatchingEngine: wall-constrained particle filter — primary source. + * EMA-smoothed, resets cleanly on lost-filter recovery.
      2. + *
    2. FusionManager: Kalman-filtered GPS+WiFi+PDR — fallback before + * MapMatchingEngine initialises.
      3. + *
    3. Raw PDR: fallback of last resort.
      4. + *
    + * + *

    The same {@code fusedLocation} variable is used for both + * {@link TrajectoryMapFragment#updateUserLocation} (marker) and + * {@link TrajectoryMapFragment#updatePdrObservation} (observation dot), + * ensuring marker and polyline are always in sync.

    */ private void updateUIandPosition() { float[] pdrValues = sensorFusion.getSensorValueMap().get(SensorTypes.PDR); @@ -248,34 +266,45 @@ private void updateUIandPosition() { float elevationVal = sensorFusion.getElevation(); elevation.setText(getString(R.string.elevation, String.format("%.1f", elevationVal))); - // Compute raw PDR position independently for the red trace - float[] latLngArray = sensorFusion.getGNSSLatitude(true); - if (latLngArray != null) { - if (rawPdrPosition == null) { - rawPdrPosition = new LatLng(latLngArray[0], latLngArray[1]); + com.openpositioning.PositionMe.mapmatching.MapMatchingEngine mmEngine = + sensorFusion.getMapMatchingEngine(); + + // ── Wait for MapMatchingEngine before drawing anything ──────────────── + // This ensures both traces start at the manually dragged position, + // not the GPS auto-fix. Before the engine is active, we still track + // PDR accumulation so the first step delta is correct when we do start. + if (mmEngine == null || !mmEngine.isActive()) { + previousPosX = pdrValues[0]; + previousPosY = pdrValues[1]; + return; + } + // ───────────────────────────────────────────────────────────────────── + + // Initialise raw PDR position from engine's start (manually dragged point). + // Only done once — first time engine is active. + if (rawPdrPosition == null) { + LatLng mmStart = mmEngine.getEstimatedPosition(); + if (mmStart != null) { + rawPdrPosition = mmStart; } + } + + // Advance raw PDR by step delta + if (rawPdrPosition != null) { rawPdrPosition = UtilFunctions.calculateNewPos( rawPdrPosition, new float[]{ pdrValues[0] - previousPosX, pdrValues[1] - previousPosY } ); } - // Draw raw PDR on the red polyline + // Red polyline — raw step-counting only, no corrections if (rawPdrPosition != null && trajectoryMapFragment != null) { trajectoryMapFragment.addRawPdrPoint(rawPdrPosition); } - // Best position: prefer MapMatchingEngine (wall-aware) > FusionManager > raw PDR - LatLng fusedLocation = null; - String positionSource = "none"; - - com.openpositioning.PositionMe.mapmatching.MapMatchingEngine mmEngine = - sensorFusion.getMapMatchingEngine(); - - if (mmEngine != null && mmEngine.isActive()) { - fusedLocation = mmEngine.getEstimatedPosition(); - positionSource = "MapMatchingEngine"; - } + // ── Position source selection ────────────────────────────────────────── + LatLng fusedLocation = mmEngine.getEstimatedPosition(); + String positionSource = "MapMatchingEngine"; if (fusedLocation == null) { double[] fusedPos = FusionManager.getInstance().getBestPosition(); @@ -284,22 +313,19 @@ private void updateUIandPosition() { positionSource = "FusionManager"; } } + // ───────────────────────────────────────────────────────────────────── - // TODO: remove before submission — log position source every 5 UI updates (~1s) + // TODO: remove before submission if (uiUpdateCount % 5 == 0) { - if (mmEngine == null) { - Log.d(TAG, "Position source=" + positionSource - + " | mmEngine=NULL"); - } else { - Log.d(TAG, "Position source=" + positionSource - + " | mmEngine.isActive=" + mmEngine.isActive() - + " | fusedLoc=" + (fusedLocation == null ? "null" - : String.format("(%.5f, %.5f)", - fusedLocation.latitude, fusedLocation.longitude))); - } + Log.d(TAG, "Position source=" + positionSource + + " | mmEngine.isActive=" + mmEngine.isActive() + + " | fusedLoc=" + (fusedLocation == null ? "null" + : String.format("(%.5f, %.5f)", + fusedLocation.latitude, fusedLocation.longitude))); } - float headingDeg = smoothHeadingDeg((float) Math.toDegrees(sensorFusion.passOrientation())); + float headingDeg = smoothHeadingDeg( + (float) Math.toDegrees(sensorFusion.passOrientation())); if (fusedLocation != null && trajectoryMapFragment != null) { trajectoryMapFragment.updateUserLocation(fusedLocation, headingDeg); @@ -311,24 +337,23 @@ private void updateUIandPosition() { long now = System.currentTimeMillis(); - // GNSS logic: only draw when a genuinely new GNSS observation arrives + // GNSS markers float[] gnss = sensorFusion.getSensorValueMap().get(SensorTypes.GNSSLATLONG); if (gnss != null && trajectoryMapFragment != null) { if (trajectoryMapFragment.isGnssEnabled()) { LatLng gnssLocation = new LatLng(gnss[0], gnss[1]); LatLng currentLoc = trajectoryMapFragment.getCurrentLocation(); if (currentLoc != null) { - double errorDist = UtilFunctions.distanceBetweenPoints(currentLoc, gnssLocation); + double errorDist = UtilFunctions.distanceBetweenPoints( + currentLoc, gnssLocation); gnssError.setVisibility(View.VISIBLE); - gnssError.setText(String.format(getString(R.string.gnss_error) + "%.2fm", errorDist)); + gnssError.setText(String.format( + getString(R.string.gnss_error) + "%.2fm", errorDist)); } - if (shouldDisplayObservation( - gnssLocation, - lastDisplayedGnssObservation, + gnssLocation, lastDisplayedGnssObservation, now - lastDisplayedGnssTimeMs, - GNSS_DISPLAY_MIN_INTERVAL_MS, - GNSS_DISPLAY_MIN_DISTANCE_M)) { + GNSS_DISPLAY_MIN_INTERVAL_MS, GNSS_DISPLAY_MIN_DISTANCE_M)) { trajectoryMapFragment.updateGNSS(gnssLocation); lastDisplayedGnssObservation = gnssLocation; lastDisplayedGnssTimeMs = now; @@ -339,15 +364,13 @@ private void updateUIandPosition() { } } - // WiFi logic: draw green observation markers only when a new accepted WiFi fix appears + // WiFi markers if (trajectoryMapFragment != null) { LatLng wifiLocation = sensorFusion.getLatLngWifiPositioning(); if (wifiLocation != null && shouldDisplayObservation( - wifiLocation, - lastDisplayedWifiObservation, + wifiLocation, lastDisplayedWifiObservation, now - lastDisplayedWifiTimeMs, - WIFI_DISPLAY_MIN_INTERVAL_MS, - WIFI_DISPLAY_MIN_DISTANCE_M)) { + WIFI_DISPLAY_MIN_INTERVAL_MS, WIFI_DISPLAY_MIN_DISTANCE_M)) { trajectoryMapFragment.updateWifiObservation(wifiLocation); lastDisplayedWifiObservation = wifiLocation; lastDisplayedWifiTimeMs = now; @@ -358,10 +381,8 @@ private void updateUIandPosition() { previousPosY = pdrValues[1]; } - private boolean shouldDisplayObservation(LatLng candidate, - LatLng previous, - long ageMs, - long minIntervalMs, + private boolean shouldDisplayObservation(LatLng candidate, LatLng previous, + long ageMs, long minIntervalMs, double minDistanceMeters) { if (candidate == null) return false; if (previous == null) return true; @@ -373,23 +394,17 @@ private float smoothHeadingDeg(float newHeadingDeg) { if (Float.isNaN(newHeadingDeg) || Float.isInfinite(newHeadingDeg)) { return smoothedHeadingDeg != null ? smoothedHeadingDeg : 0f; } - if (smoothedHeadingDeg == null) { smoothedHeadingDeg = newHeadingDeg; return newHeadingDeg; } - float delta = newHeadingDeg - smoothedHeadingDeg; while (delta > 180f) delta -= 360f; while (delta < -180f) delta += 360f; - smoothedHeadingDeg = smoothedHeadingDeg + HEADING_SMOOTHING_ALPHA * delta; return smoothedHeadingDeg; } - /** - * Start the blinking effect for the recording icon. - */ private void blinkingRecordingIcon() { Animation blinking = new AlphaAnimation(1, 0); blinking.setDuration(800); @@ -430,4 +445,4 @@ private static class TestPoint { } private final List testPoints = new ArrayList<>(); -} \ No newline at end of file +} From 5a28425246899f4eb91f61ab337ed4f53f458a98 Mon Sep 17 00:00:00 2001 From: Lane2048 <145272991+Lane2048@users.noreply.github.com> Date: Wed, 1 Apr 2026 15:14:49 +0100 Subject: [PATCH 20/26] add Catmull-Rom spline smoothing to PDR trajectory polylines --- .../fragment/TrajectoryMapFragment.java | 66 +++++++++++++++++-- 1 file changed, 60 insertions(+), 6 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 81532cf6..89c87cfb 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 @@ -65,6 +65,10 @@ public class TrajectoryMapFragment extends Fragment { private final List testPointMarkers = new ArrayList<>(); private Polyline polyline; // Polyline representing user's movement path + + // Rolling 4-point buffers for Catmull-Rom spline — one per polyline + private final List rawPdrControlPoints = new ArrayList<>(); + private final List fusedPdrControlPoints = new ArrayList<>(); private boolean isRed = true; // Tracks whether the polyline color is red private boolean isGnssOn = false; // Tracks if GNSS tracking is enabled @@ -687,21 +691,67 @@ public void updatePdrObservation(@NonNull LatLng pdrLocation) { // Extend the orange trajectory polyline with this accepted position. // This builds the continuous walking path shown on the map. if (pdrTrajectoryPolyline != null) { - List points = new ArrayList<>(pdrTrajectoryPolyline.getPoints()); - points.add(pdrLocation); - pdrTrajectoryPolyline.setPoints(points); +// List points = new ArrayList<>(pdrTrajectoryPolyline.getPoints()); +// points.add(pdrLocation); +// pdrTrajectoryPolyline.setPoints(points); + // Use Catmull-Rom smoothing instead of raw point append + appendSmoothedPoint(pdrLocation, fusedPdrControlPoints, pdrTrajectoryPolyline); } } + /** + * Catmull-Rom spline: generates smooth interpolated points between p1→p2, + * using p0 and p3 as tangent guides. Curve always passes through p1 and p2. + */ + private static List catmullRomSegment( + LatLng p0, LatLng p1, LatLng p2, LatLng p3, int numSteps) { + List result = new ArrayList<>(); + for (int i = 1; i <= numSteps; i++) { + double t = (double) i / numSteps, t2 = t*t, t3 = t2*t; + double lat = 0.5 * ((-t3+2*t2-t)*p0.latitude + (3*t3-5*t2+2)*p1.latitude + + (-3*t3+4*t2+t)*p2.latitude + (t3-t2)*p3.latitude); + double lng = 0.5 * ((-t3+2*t2-t)*p0.longitude + (3*t3-5*t2+2)*p1.longitude + + (-3*t3+4*t2+t)*p2.longitude + (t3-t2)*p3.longitude); + result.add(new LatLng(lat, lng)); + } + return result; + } + + /** + * Adds newPoint to a 4-point sliding window, then extends targetLine with a + * smooth Catmull-Rom segment. Falls back to straight line until 4 points arrive. + */ + private void appendSmoothedPoint( + @NonNull LatLng newPoint, + @NonNull List controlBuf, + @NonNull Polyline targetLine) { + controlBuf.add(newPoint); + if (controlBuf.size() > 4) controlBuf.remove(0); + + List pts = new ArrayList<>(targetLine.getPoints()); + if (controlBuf.size() < 4) { + // Not enough points yet — draw straight line to keep path visible early on + pts.add(newPoint); + } else { + // Enough points — draw smooth curve from p1 to p2 + pts.addAll(catmullRomSegment( + controlBuf.get(0), controlBuf.get(1), + controlBuf.get(2), controlBuf.get(3), 10)); + } + targetLine.setPoints(pts); + } + /** * Adds a point to the raw PDR polyline (red) without moving the camera or marker. * Shows the unfiltered PDR path for comparison against the fused trajectory. */ public void addRawPdrPoint(@NonNull LatLng point) { if (polyline == null) return; - List points = new ArrayList<>(polyline.getPoints()); - points.add(point); - polyline.setPoints(points); +// List points = new ArrayList<>(polyline.getPoints()); +// points.add(point); +// polyline.setPoints(points); + // Use Catmull-Rom smoothing instead of raw point append + appendSmoothedPoint(point, rawPdrControlPoints, polyline); } // ------------------------------------------------------------------------- @@ -844,6 +894,10 @@ public void clearMapAndReset() { } // Reset the distance filter anchor so the first new point is always accepted lastPdrObservationLocation = null; + // Clear spline buffers so new session starts fresh + rawPdrControlPoints.clear(); + fusedPdrControlPoints.clear(); + pdrTrajectoryPolyline = gMap.addPolyline(new PolylineOptions() .color(Color.rgb(255, 165, 0)) .width(5f) From 79adf44f18900b47154a1bc678b40330b32eb9ea Mon Sep 17 00:00:00 2001 From: Saleh-AlMulla Date: Wed, 1 Apr 2026 16:21:51 +0100 Subject: [PATCH 21/26] Improve map-matching and fusion robustness MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Refactor and harden map-matching and fusion logic. MapMatchingEngine: - Large overhaul: clarify Javadoc, reorganise constants/state and add section comments (§3.2). - Tuned particle filter parameters (NUM_PARTICLES, PDR noise, WIFI/GNSS sigma, WALL_PENALTY, etc.). - Add building-outline support (setBuildingOutline + constrainPosition) with ray-casting point-in-polygon to hard-snap FM output when outside the building. - Implement wall-aware particle propagation: penalise particles that cross walls, improve wall-segment intersection checks, and add post-resample jitter guarded by wall checks. - Improve lost-filter recovery on WiFi fixes (higher outlier threshold, reinit with WiFi and reset smoothed output/outline tracker). - Revise floor transition logic: warmup, sustained-step guard, min-gap between transitions, stair vs lift classification by horizontal displacement, and proximity bonuses/penalties for transition features. - Simplify and optimise utility functions (segment intersection, cross/on-seg helpers), smoothing (EMA) and effective sample size/resampling. FusionManager: - Documented and added §3.1 summary for the fusion component. - Adjust display smoothing alpha and add WiFi/GNSS handling improvements. - Add WiFi death-spiral recovery counters and WiFi outlier gate logic to force re-centre after consecutive rejections. - Minor API and reset/state hygiene changes. Overall: these changes implement map- and barometer-based floor/fence guards, improve robustness to drift and weight collapse, and tighten GNSS/WiFi integration for indoor scenarios. --- .../mapmatching/MapMatchingEngine.java | 783 ++++++++---------- .../PositionMe/positioning/FusionManager.java | 125 ++- .../positioning/ParticleFilter.java | 98 ++- .../fragment/RecordingFragment.java | 140 ++-- .../fragment/StartLocationFragment.java | 11 + .../fragment/TrajectoryMapFragment.java | 24 +- .../PositionMe/sensors/SensorFusion.java | 11 + 7 files changed, 674 insertions(+), 518 deletions(-) diff --git a/app/src/main/java/com/openpositioning/PositionMe/mapmatching/MapMatchingEngine.java b/app/src/main/java/com/openpositioning/PositionMe/mapmatching/MapMatchingEngine.java index e02837cc..69fc1956 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/mapmatching/MapMatchingEngine.java +++ b/app/src/main/java/com/openpositioning/PositionMe/mapmatching/MapMatchingEngine.java @@ -10,150 +10,87 @@ import java.util.Random; /** - * Particle filter engine for indoor map matching. + * Map-matching engine for indoor positioning (§3.2). * - *

    Uses a set of weighted particles to estimate the user's position and floor. - * Each particle is propagated using PDR displacement, then weighted against map - * constraints (walls, stairs, lifts) and positioning observations (WiFi, GNSS). - * Systematic resampling keeps the particle population healthy.

    + *

    Provides three levels of map-based correction on top of the + * {@link com.openpositioning.PositionMe.positioning.FusionManager} estimate:

    * - *

    Map constraint types (from floorplan API):

    - *
      - *
    • wall — impassable barriers; particles crossing them are penalised
      • - *
    • stairs — floor transitions with horizontal movement
      • - *
    • lift — floor transitions without significant horizontal displacement
      • - *
    + *
      + *
    1. Wall correction (particle filter): 200 particles propagated by + * PDR steps; particles that cross wall segments receive a 0.1× weight + * penalty, causing the filter to favour wall-respecting trajectories. + * Satisfies: "correct estimations that pass through walls".
      2. + *
    2. Building outline (constrainPosition): ray-casting point-in-polygon + * test against the building outline; if FM drifts outside the polygon, + * the displayed position is held at the last valid inside position. + * Satisfies: "wall — areas that cannot be crossed".
      3. + *
    3. Floor detection (barometric): barometric elevation tracked across + * steps with 30s warmup, 5-step sustained threshold, and proximity + * bonus/penalty for stairs/lift features. Horizontal displacement during + * elevation episodes distinguishes stairs from lifts. + * Satisfies: "floor changes only near elevators/lifts" and + * "distinguish between lift and stairs".
      4. + *
    * - *

    Integration points:

    - *
      - *
    • {@link com.openpositioning.PositionMe.sensors.SensorFusion} — owns the engine instance
      • - *
    • {@link com.openpositioning.PositionMe.sensors.SensorEventHandler} — calls - * {@link #predict(float, float, float)} on each PDR step
      • - *
    • {@link com.openpositioning.PositionMe.sensors.WifiPositionManager} — calls - * {@link #updateWifi(double, double, int, float)} on WiFi position updates
      • - *
    - * - * @see Particle individual particle state + * @see com.openpositioning.PositionMe.positioning.FusionManager for the primary position source */ public class MapMatchingEngine { private static final String TAG = "MapMatchingEngine"; - // ------------------------------------------------------------------------- - // Tuneable parameters - // ------------------------------------------------------------------------- - - /** Number of particles in the filter. */ + // ── Particle filter parameters ─────────────────────────────────────────── private static final int NUM_PARTICLES = 200; - - /** Standard deviation of noise added to PDR step length (metres). */ private static final double PDR_NOISE_STEP = 0.15; - - /** Standard deviation of noise added to PDR heading (radians). */ - private static final double PDR_NOISE_HEADING = Math.toRadians(8); - - /** Standard deviation for WiFi observation likelihood (metres). */ - private static final double WIFI_SIGMA = 5.0; - - /** Standard deviation for GNSS observation likelihood (metres). */ + private static final double PDR_NOISE_HEADING = Math.toRadians(5); + private static final double WIFI_SIGMA = 8.0; private static final double GNSS_SIGMA = 12.0; - /** - * Weight multiplier applied to particles that cross a wall. - * Lower value = harsher penalty. - */ - private static final double WALL_PENALTY = 0.001; + /** Weight multiplier for particles crossing a wall segment (§3.2 wall correction). */ + private static final double WALL_PENALTY = 0.1; - /** Distance threshold (metres) to consider a particle near stairs or lift. */ + /** Radius (metres) within which a particle is considered near a stairs/lift feature. */ private static final double TRANSITION_PROXIMITY = 4.0; - /** - * Minimum elevation change from the last floor baseline (metres) required - * to trigger a floor transition. Set to one full floor height so barometric - * noise from doors/HVAC cannot trigger false transitions. - */ + private static final double INIT_SPREAD = 3.0; + private static final double RESAMPLE_JITTER = 0.2; + private static final double RESAMPLE_THRESHOLD = 0.5; + private static final double POSITION_SMOOTH_ALPHA = 0.5; + private static final double POSITION_SMOOTH_SNAP_THRESHOLD = 20.0; + private static final double WALL_MEAN_FALLBACK_THRESHOLD = 8.0; + + // ── Floor transition parameters (§3.2 floor detection) ─────────────────── + + /** Minimum barometric elevation change (metres) to trigger a floor transition. */ private static final double FLOOR_CHANGE_ELEVATION_THRESHOLD = 4.0; - /** - * Number of consecutive steps the elevation must exceed - * {@link #FLOOR_CHANGE_ELEVATION_THRESHOLD} before a floor change commits. - * Prevents single-step barometric spikes from triggering false transitions. - */ + /** Steps the elevation must remain above threshold before confirming transition. */ private static final int ELEVATION_SUSTAIN_STEPS = 5; - /** - * Minimum steps between consecutive floor changes to prevent rapid - * oscillation from barometric noise. - */ + /** Minimum steps between consecutive floor changes to prevent oscillation. */ private static final int MIN_STEPS_BETWEEN_FLOOR_CHANGES = 10; - /** - * Time in milliseconds after initialisation during which floor transitions - * are suppressed. During this period the elevation baseline continuously - * tracks the barometer, absorbing startup drift from HVAC, doors, and - * sensor stabilisation before any transition can fire. - */ + /** Warmup period (ms) after init during which floor transitions are suppressed. */ private static final long FLOOR_TRANSITION_WARMUP_MS = 30_000; - /** Effective sample size ratio below which resampling is triggered. */ - private static final double RESAMPLE_THRESHOLD = 0.5; - - /** - * Spread radius (metres) for initial particle distribution. - * 6.0m accounts for indoor GPS error which can easily be 5-10m. - */ - private static final double INIT_SPREAD = 6.0; - /** - * Noise (metres) injected into particles after resampling to maintain - * diversity. Wall-checked before applying to prevent particles teleporting - * through thin wall segments. - */ - private static final double RESAMPLE_JITTER = 0.2; - - /** - * Maximum total horizontal distance (metres) during an elevation episode - * that classifies the transition as a lift rather than stairs. + * Maximum horizontal displacement (metres) during an elevation episode + * to classify the transition as a lift rather than stairs (§3.2). */ private static final double LIFT_HORIZONTAL_THRESHOLD = 1.5; - /** - * EMA smoothing factor for the output position. - * 0.25 balances responsiveness with smoothness — higher than 0.15 so the - * marker visibly follows movement, lower than 0.3 to reduce step jitter. - */ - private static final double POSITION_SMOOTH_ALPHA = 0.25; - - /** - * Distance (metres) beyond which a position change bypasses EMA smoothing. - * Only used for genuine large corrections, not lost-filter reinitialisation - * (which resets smoothedOutputX/Y directly to avoid trace smearing). - */ - private static final double POSITION_SMOOTH_SNAP_THRESHOLD = 20.0; - - /** - * Distance threshold (metres) between weighted mean and best-weight particle. - * If exceeded, best particle position is used (mean likely inside a wall). - */ - private static final double WALL_MEAN_FALLBACK_THRESHOLD = 5.0; - - // ------------------------------------------------------------------------- - // Coordinate conversion — metres per degree at Edinburgh (~55.92 N) - // ------------------------------------------------------------------------- + // ── Coordinate conversion ──────────────────────────────────────────────── private static final double METRES_PER_DEG_LAT = 111_320.0; private static final double METRES_PER_DEG_LNG = 111_320.0 * Math.cos(Math.toRadians(55.92)); - // ------------------------------------------------------------------------- - // State - // ------------------------------------------------------------------------- - + // ── State ──────────────────────────────────────────────────────────────── private final List particles = new ArrayList<>(); private final Random random = new Random(); private double refLat; private double refLng; private boolean initialised = false; + private boolean enabled = false; private List floorShapes; @@ -161,12 +98,9 @@ public class MapMatchingEngine { private float elevationAtLastFloorChange = 0f; private float floorHeight = 4.0f; private int estimatedFloor = 0; - private boolean enabled = false; - - /** Wall clock time (ms) when initialise() was called. Used for warmup guard. */ private long initialiseTimeMs = 0; - // Stairs / lift detection + // Stairs / lift classification state private float lastStepLength = 0f; private float horizontalDuringElevationChange = 0f; private boolean inElevationChange = false; @@ -175,11 +109,19 @@ public class MapMatchingEngine { private int stepsAboveElevationThreshold = 0; private int stepsSinceLastFloorChange = 0; - // Output smoothing (EMA applied to getEstimatedPosition output) + // Output smoothing private double smoothedOutputX = Double.NaN; private double smoothedOutputY = Double.NaN; - // Debug counters — TODO: remove before submission + // Building outline boundary tracking + private double lastValidX = Double.NaN; + private double lastValidY = Double.NaN; + + // Building outline polygon in ENU coordinates + private double[] outlineX = null; + private double[] outlineY = null; + + // Debug counters private int wallHitCount = 0; private int wallCheckCount = 0; private int predictCallCount = 0; @@ -189,13 +131,14 @@ public class MapMatchingEngine { // ========================================================================= /** - * Initialises the particle filter around a starting position. + * Initialises the map-matching particle filter at the given position. + * Called from StartLocationFragment when the user places their start pin. * - * @param lat starting latitude (WGS84) - * @param lng starting longitude (WGS84) - * @param floor starting floor index - * @param floorHeight floor height of the current building (metres) - * @param shapes per-floor vector shape data from floorplan API (may be null) + * @param lat WGS84 latitude of start position + * @param lng WGS84 longitude of start position + * @param floor initial floor number + * @param floorHeight estimated height per floor (metres) + * @param shapes floor shape data from the floorplan API */ public void initialise(double lat, double lng, int floor, float floorHeight, List shapes) { @@ -213,6 +156,8 @@ public void initialise(double lat, double lng, int floor, this.initialiseTimeMs = System.currentTimeMillis(); this.smoothedOutputX = Double.NaN; this.smoothedOutputY = Double.NaN; + this.lastValidX = Double.NaN; + this.lastValidY = Double.NaN; this.wallHitCount = 0; this.wallCheckCount = 0; this.predictCallCount = 0; @@ -227,56 +172,66 @@ public void initialise(double lat, double lng, int floor, this.initialised = true; this.enabled = true; - // TODO: remove before submission - Log.d(TAG, "initialise() called — lat=" + lat + " lng=" + lng + Log.d(TAG, "initialise() — lat=" + lat + " lng=" + lng + " floor=" + floor + " floorHeight=" + floorHeight - + " shapes=" + (shapes == null ? "NULL" : shapes.size() + " floors")); + + " shapes=" + (shapes == null ? "NULL" : shapes.size() + " floors") + + " outline=" + (outlineX == null ? "NONE" : outlineX.length + " pts")); logWallStats(); } - // TODO: remove before submission + /** + * Sets the building outline polygon for boundary checking in + * {@link #constrainPosition}. Converts WGS84 vertices to local ENU + * coordinates for fast ray-casting tests. + * + * @param outline building outline vertices from the floorplan API + * @param refLat reference latitude (same as initialise) + * @param refLng reference longitude (same as initialise) + */ + public void setBuildingOutline(List outline, double refLat, double refLng) { + if (outline == null || outline.size() < 3) { + this.outlineX = null; + this.outlineY = null; + Log.d(TAG, "setBuildingOutline: null or too few points"); + return; + } + int n = outline.size(); + outlineX = new double[n]; + outlineY = new double[n]; + for (int i = 0; i < n; i++) { + LatLng p = outline.get(i); + outlineX[i] = (p.longitude - refLng) * METRES_PER_DEG_LNG; + outlineY[i] = (p.latitude - refLat) * METRES_PER_DEG_LAT; + } + Log.d(TAG, "setBuildingOutline: " + n + " points loaded"); + } + + /** Logs wall, stairs, and lift feature counts per floor for debugging. */ public void logWallStats() { if (floorShapes == null) { - Log.d(TAG, "logWallStats: floorShapes is NULL — wall checking disabled"); + Log.d(TAG, "logWallStats: floorShapes is NULL"); return; } for (int f = 0; f < floorShapes.size(); f++) { - int wallSegments = 0; - int stairFeatures = 0; - int liftFeatures = 0; + int wallSegments = 0, stairFeatures = 0, liftFeatures = 0; for (FloorplanApiClient.MapShapeFeature feature : floorShapes.get(f).getFeatures()) { String type = feature.getIndoorType(); if ("wall".equals(type)) { - for (List part : feature.getParts()) { + for (List part : feature.getParts()) wallSegments += part.size() - 1; - } - } else if ("stairs".equals(type)) { - stairFeatures++; - } else if ("lift".equals(type)) { - liftFeatures++; - } + } else if ("stairs".equals(type)) stairFeatures++; + else if ("lift".equals(type)) liftFeatures++; } Log.d(TAG, " Floor " + f + ": " + wallSegments + " wall segments, " + stairFeatures + " stair features, " + liftFeatures + " lift features"); } } - /** Returns whether the engine has been initialised and is active. */ - public boolean isActive() { - return initialised && enabled; - } - - /** Enables or disables the engine without clearing particles. */ - public void setEnabled(boolean enabled) { - // TODO: remove before submission - Log.d(TAG, "setEnabled(" + enabled + ") — was " + this.enabled); - this.enabled = enabled; - } + public boolean isActive() { return initialised && enabled; } + public void setEnabled(boolean enabled) { this.enabled = enabled; } - /** Resets the engine, clearing all particles and state. */ + /** Resets all state. Called between recording sessions. */ public void reset() { - // TODO: remove before submission - Log.d(TAG, "reset() called"); particles.clear(); initialised = false; enabled = false; @@ -290,52 +245,40 @@ public void reset() { initialiseTimeMs = 0; smoothedOutputX = Double.NaN; smoothedOutputY = Double.NaN; + lastValidX = Double.NaN; + lastValidY = Double.NaN; wallHitCount = 0; wallCheckCount = 0; predictCallCount = 0; } - /** Updates the floor shapes used for wall/stair/lift constraints. */ - public void setFloorShapes(List shapes) { - this.floorShapes = shapes; - // TODO: remove before submission - Log.d(TAG, "setFloorShapes() — " - + (shapes == null ? "NULL" : shapes.size() + " floors")); - } - - /** Updates the floor height for the current building. */ - public void setFloorHeight(float height) { - this.floorHeight = height; - } + public void setFloorShapes(List shapes) { this.floorShapes = shapes; } + public void setFloorHeight(float height) { this.floorHeight = height; } // ========================================================================= - // Prediction step — called on each PDR step + // Prediction step — wall-aware particle propagation (§3.2) // ========================================================================= /** - * Propagates all particles using the PDR displacement and applies map constraints. + * Propagates all particles forward by one PDR step. Particles that cross + * a wall segment receive {@code WALL_PENALTY} (0.1×) weight, causing + * wall-respecting trajectories to dominate after resampling. * - * @param stepLength step length in metres from PDR - * @param headingRad heading in radians (north = 0, clockwise) - * @param elevation current relative elevation from barometer (metres) + *

    Also tracks barometric elevation for floor transition detection.

    + * + * @param stepLength PDR step length in metres + * @param headingRad compass heading in radians + * @param elevation barometric elevation in metres */ public void predict(float stepLength, float headingRad, float elevation) { - // TODO: remove before submission predictCallCount++; if (predictCallCount <= 3 || predictCallCount % 20 == 0) { Log.d(TAG, "predict() #" + predictCallCount - + " — initialised=" + initialised - + " enabled=" + enabled + " stepLen=" + String.format("%.2f", stepLength) + " heading=" + String.format("%.1f", Math.toDegrees(headingRad)) + "deg" + " elev=" + String.format("%.2f", elevation) + "m"); } - - if (!isActive()) { - Log.w(TAG, "predict() bailed — initialised=" + initialised - + " enabled=" + enabled); - return; - } + if (!isActive()) return; this.currentElevation = elevation; this.lastStepLength = stepLength; @@ -348,35 +291,25 @@ public void predict(float stepLength, float headingRad, float elevation) { double dx = noisyStep * Math.sin(noisyHeading); double dy = noisyStep * Math.cos(noisyHeading); - double oldX = p.getX(); double oldY = p.getY(); double newX = oldX + dx; double newY = oldY + dy; + p.setX(newX); + p.setY(newY); + + // §3.2: penalise particles that cross wall segments wallCheckCount++; if (doesCrossWall(oldX, oldY, newX, newY, p.getFloor())) { p.setWeight(p.getWeight() * WALL_PENALTY); wallHitCount++; - } else { - p.setX(newX); - p.setY(newY); } } - // TODO: remove before submission if (wallCheckCount > 0 && wallCheckCount % 200 == 0) { - Log.d(TAG, "Wall check #" + (wallCheckCount / 200) - + " — hits=" + wallHitCount + "/" + wallCheckCount - + " (" + String.format("%.1f", 100.0 * wallHitCount / wallCheckCount) + "%)" - + " floor=" + estimatedFloor - + " shapes=" + (floorShapes == null ? "NULL" : floorShapes.size() + " floors")); - if (!particles.isEmpty()) { - Particle p0 = particles.get(0); - Log.d(TAG, " Particle[0] xy=(" - + String.format("%.2f", p0.getX()) + ", " - + String.format("%.2f", p0.getY()) + ") floor=" + p0.getFloor()); - } + Log.d(TAG, "Wall stats — hits=" + wallHitCount + "/" + wallCheckCount + + " (" + String.format("%.1f", 100.0 * wallHitCount / wallCheckCount) + "%)"); } checkFloorTransition(); @@ -391,18 +324,14 @@ public void predict(float stepLength, float headingRad, float elevation) { // ========================================================================= /** - * Updates particle weights based on a WiFi position observation. + * Updates particle weights from a WiFi observation. Includes lost-filter + * recovery: if the weighted mean drifts more than 12m from WiFi, the + * filter reinitialises around the WiFi fix. * - *

    Also handles lost-filter recovery: if the weighted mean has drifted - * more than 15m from the WiFi fix, the filter has diverged — particles are - * reinitialised around the WiFi fix. The EMA smoothed output is reset to the - * new position immediately to prevent trace smearing between old and new - * positions while the EMA catches up.

    - * - * @param lat WiFi-estimated latitude - * @param lng WiFi-estimated longitude - * @param wifiFloor WiFi-estimated floor number - * @param accuracy estimated accuracy in metres + * @param lat WiFi latitude + * @param lng WiFi longitude + * @param wifiFloor WiFi-reported floor number + * @param accuracy WiFi accuracy estimate */ public void updateWifi(double lat, double lng, int wifiFloor, float accuracy) { if (!isActive()) return; @@ -412,16 +341,15 @@ public void updateWifi(double lat, double lng, int wifiFloor, float accuracy) { double sigma = Math.max(WIFI_SIGMA, accuracy); for (Particle p : particles) { - double dist = Math.sqrt(Math.pow(p.getX() - obsX, 2) - + Math.pow(p.getY() - obsY, 2)); + double dist = Math.sqrt(Math.pow(p.getX() - obsX, 2) + Math.pow(p.getY() - obsY, 2)); double likelihood = gaussianLikelihood(dist, sigma); + // Floor mismatch penalty if (p.getFloor() != wifiFloor) likelihood *= 0.4; p.setWeight(p.getWeight() * likelihood); } - normaliseWeights(); - // ── Lost-filter recovery ────────────────────────────────────────────── + // Lost-filter recovery double weightedX = 0, weightedY = 0; for (Particle p : particles) { weightedX += p.getX() * p.getWeight(); @@ -430,98 +358,82 @@ public void updateWifi(double lat, double lng, int wifiFloor, float accuracy) { double meanDistFromWifi = Math.sqrt( Math.pow(weightedX - obsX, 2) + Math.pow(weightedY - obsY, 2)); - if (meanDistFromWifi > 8.0) { - Log.w(TAG, "Lost filter detected — reinitialising around WiFi fix" - + " dist=" + String.format("%.1f", meanDistFromWifi) + "m"); + if (meanDistFromWifi > 12.0) { + Log.w(TAG, "Lost filter — reinitialising dist=" + String.format("%.1f", meanDistFromWifi) + "m"); particles.clear(); for (int i = 0; i < NUM_PARTICLES; i++) { - double px = obsX + random.nextGaussian() * WIFI_SIGMA; - double py = obsY + random.nextGaussian() * WIFI_SIGMA; - particles.add(new Particle(px, py, wifiFloor, 1.0 / NUM_PARTICLES)); + particles.add(new Particle( + obsX + random.nextGaussian() * WIFI_SIGMA, + obsY + random.nextGaussian() * WIFI_SIGMA, + wifiFloor, 1.0 / NUM_PARTICLES)); } - // Reset EMA to particle weighted mean after reinit, not raw WiFi fix, - // keeping displayed position closer to the building interior. normaliseWeights(); - double reinitMeanX = 0, reinitMeanY = 0; + double rx = 0, ry = 0; for (Particle p : particles) { - reinitMeanX += p.getX() * p.getWeight(); - reinitMeanY += p.getY() * p.getWeight(); + rx += p.getX() * p.getWeight(); + ry += p.getY() * p.getWeight(); } - smoothedOutputX = reinitMeanX; - smoothedOutputY = reinitMeanY; + smoothedOutputX = rx; + smoothedOutputY = ry; + this.lastValidX = rx; + this.lastValidY = ry; return; } - // ───────────────────────────────────────────────────────────────────── - if (effectiveSampleSize() < RESAMPLE_THRESHOLD * NUM_PARTICLES) { - resample(); - } + // WiFi fixes reset the building-outline constraint tracker + this.lastValidX = obsX; + this.lastValidY = obsY; - // TODO: remove before submission - Log.d(TAG, "updateWifi() — obsXY=(" - + String.format("%.2f", obsX) + ", " - + String.format("%.2f", obsY) + ")" - + " floor=" + wifiFloor - + " sigma=" + String.format("%.1f", sigma)); + if (effectiveSampleSize() < RESAMPLE_THRESHOLD * NUM_PARTICLES) resample(); + Log.d(TAG, "updateWifi() — floor=" + wifiFloor); } - /** - * Updates particle weights based on a GNSS position observation. - * - * @param lat GNSS latitude - * @param lng GNSS longitude - * @param accuracy GNSS accuracy in metres - */ + /** Updates particle weights from a GNSS observation. */ public void updateGnss(double lat, double lng, float accuracy) { if (!isActive()) return; - double obsX = (lng - refLng) * METRES_PER_DEG_LNG; double obsY = (lat - refLat) * METRES_PER_DEG_LAT; double sigma = Math.max(GNSS_SIGMA, accuracy); - for (Particle p : particles) { - double dist = Math.sqrt(Math.pow(p.getX() - obsX, 2) - + Math.pow(p.getY() - obsY, 2)); - double likelihood = gaussianLikelihood(dist, sigma); - p.setWeight(p.getWeight() * likelihood); + double dist = Math.sqrt(Math.pow(p.getX() - obsX, 2) + Math.pow(p.getY() - obsY, 2)); + p.setWeight(p.getWeight() * gaussianLikelihood(dist, sigma)); } - normaliseWeights(); - if (effectiveSampleSize() < RESAMPLE_THRESHOLD * NUM_PARTICLES) { - resample(); - } + if (effectiveSampleSize() < RESAMPLE_THRESHOLD * NUM_PARTICLES) resample(); } // ========================================================================= - // Floor transition logic + // Floor transition logic (§3.2) // ========================================================================= /** - * Checks if the barometric elevation change warrants a floor transition. + * Checks whether the accumulated barometric elevation change indicates a + * floor transition. Implements all §3.2 floor requirements: * - *

    Four guards prevent false transitions:

    - *
      - *
    1. Warmup: baseline tracks barometer for 30s absorbing startup drift.
      2. - *
    2. Full-floor threshold: delta must exceed one floor height (4.0m).
      3. - *
    3. Sustained elevation: must hold for 5 consecutive steps.
      4. - *
    4. Minimum gap: at least 10 steps since last floor change.
      5. - *
    + *
      + *
    • 30s warmup after init to let the barometer stabilise
      • + *
    • 5-step sustained elevation threshold to reject transients
      • + *
    • Minimum 10 steps between transitions to prevent oscillation
      • + *
    • Particles near stairs/lift features get 1.2× bonus; others 0.3×
      • + *
    • Horizontal displacement during episode classifies stairs vs lift
      • + *
    */ private void checkFloorTransition() { if (floorShapes == null || floorHeight <= 0) return; + // Suppress during warmup — barometer needs time to stabilise if (System.currentTimeMillis() - initialiseTimeMs < FLOOR_TRANSITION_WARMUP_MS) { elevationAtLastFloorChange = currentElevation; stepsAboveElevationThreshold = 0; return; } - if (stepsSinceLastFloorChange < MIN_STEPS_BETWEEN_FLOOR_CHANGES) return; - float elevationDeltaFromBaseline = currentElevation - elevationAtLastFloorChange; - float elevationChangeMagnitude = Math.abs(elevationDeltaFromBaseline); + float delta = currentElevation - elevationAtLastFloorChange; + float mag = Math.abs(delta); - if (elevationChangeMagnitude > 1.0f) { + // Track horizontal displacement during elevation episodes + if (mag > 1.0f) { if (!inElevationChange) { inElevationChange = true; horizontalDuringElevationChange = 0f; @@ -533,27 +445,32 @@ private void checkFloorTransition() { stepsAboveElevationThreshold = 0; } - if (elevationChangeMagnitude < FLOOR_CHANGE_ELEVATION_THRESHOLD) { + if (mag < FLOOR_CHANGE_ELEVATION_THRESHOLD) { stepsAboveElevationThreshold = 0; return; } + // Require sustained elevation change to confirm transition stepsAboveElevationThreshold++; if (stepsAboveElevationThreshold < ELEVATION_SUSTAIN_STEPS) return; - int elevationFloorDelta = (int) Math.round(elevationDeltaFromBaseline / floorHeight); - if (elevationFloorDelta == 0) return; + int floorDelta = (int) Math.round(delta / floorHeight); + if (floorDelta == 0) return; + + int targetFloor = clampFloor(estimatedFloor + floorDelta); - int targetFloor = clampFloor(estimatedFloor + elevationFloorDelta); + // §3.2: distinguish lift vs stairs by horizontal displacement boolean isLift = horizontalDuringElevationChange < LIFT_HORIZONTAL_THRESHOLD; + // §3.2: bonus for particles near stairs/lift, penalty for others for (Particle p : particles) { - boolean nearTransition = isNearStairsOrLift(p.getX(), p.getY(), p.getFloor()); + boolean near = isNearStairsOrLift(p.getX(), p.getY(), p.getFloor()); p.setFloor(targetFloor); - if (nearTransition) { + if (near) { + // Stairs: add horizontal jitter; lift: no displacement if (!isLift) { - p.setX(p.getX() + random.nextGaussian() * 1.0); - p.setY(p.getY() + random.nextGaussian() * 1.0); + p.setX(p.getX() + random.nextGaussian()); + p.setY(p.getY() + random.nextGaussian()); } p.setWeight(p.getWeight() * 1.2); } else { @@ -561,12 +478,8 @@ private void checkFloorTransition() { } } - // TODO: remove before submission - Log.d(TAG, "Floor transition → floor " + targetFloor - + " via " + (isLift ? "LIFT" : "STAIRS") - + " horiz=" + String.format("%.2f", horizontalDuringElevationChange) + "m" - + " elevDelta=" + String.format("%.2f", elevationDeltaFromBaseline) + "m" - + " sustainedSteps=" + stepsAboveElevationThreshold); + Log.d(TAG, "Floor transition → floor " + targetFloor + " via " + (isLift ? "LIFT" : "STAIRS") + + " elevDelta=" + String.format("%.2f", delta) + "m"); this.estimatedFloor = targetFloor; this.elevationAtLastFloorChange = currentElevation; @@ -576,20 +489,21 @@ private void checkFloorTransition() { this.stepsSinceLastFloorChange = 0; } - /** Checks if a position is near stairs or lift features on the given floor. */ + /** + * Tests whether a particle position is within {@code TRANSITION_PROXIMITY} + * metres of any stairs or lift feature on the given floor. + */ private boolean isNearStairsOrLift(double x, double y, int floor) { if (floorShapes == null || floor < 0 || floor >= floorShapes.size()) return false; - - FloorplanApiClient.FloorShapes floorData = floorShapes.get(floor); - for (FloorplanApiClient.MapShapeFeature feature : floorData.getFeatures()) { + for (FloorplanApiClient.MapShapeFeature feature : floorShapes.get(floor).getFeatures()) { String type = feature.getIndoorType(); if ("stairs".equals(type) || "lift".equals(type)) { for (List part : feature.getParts()) { for (LatLng point : part) { double fx = (point.longitude - refLng) * METRES_PER_DEG_LNG; double fy = (point.latitude - refLat) * METRES_PER_DEG_LAT; - double dist = Math.sqrt(Math.pow(x - fx, 2) + Math.pow(y - fy, 2)); - if (dist < TRANSITION_PROXIMITY) return true; + if (Math.sqrt(Math.pow(x - fx, 2) + Math.pow(y - fy, 2)) < TRANSITION_PROXIMITY) + return true; } } } @@ -597,59 +511,114 @@ private boolean isNearStairsOrLift(double x, double y, int floor) { return false; } - /** Clamps a floor index to valid bounds given the loaded floor shapes. */ private int clampFloor(int floor) { if (floorShapes == null || floorShapes.isEmpty()) return floor; return Math.max(0, Math.min(floor, floorShapes.size() - 1)); } // ========================================================================= - // Wall collision detection + // constrainPosition — building outline boundary (§3.2) + // ========================================================================= + + /** + * Applies building outline constraint to FusionManager's position output. + * + *

    Uses ray-casting point-in-polygon to test whether the position is + * inside the building. If outside, returns the last known valid inside + * position (hard snap). When FM re-centres via WiFi, the constraint + * tracker is reset through {@link #updateWifi}.

    + * + *

    Interior wall corrections are handled by the particle filter's + * {@code WALL_PENALTY} in {@link #predict}, not here — the building's + * 594 wall segments on F1 cause excessive false positives for any + * direct line-crossing check on the displayed position.

    + * + * @param lat FusionManager latitude + * @param lng FusionManager longitude + * @return constrained position (unchanged if inside, last-valid if outside) + */ + public LatLng constrainPosition(double lat, double lng) { + if (!isActive()) return new LatLng(lat, lng); + + double posX = (lng - refLng) * METRES_PER_DEG_LNG; + double posY = (lat - refLat) * METRES_PER_DEG_LAT; + + // First call — accept unconditionally + if (Double.isNaN(lastValidX)) { + lastValidX = posX; + lastValidY = posY; + return new LatLng(lat, lng); + } + + // Building outline — hard snap (position cannot leave the building) + if (outlineX != null && !isInsidePolygon(posX, posY, outlineX, outlineY)) { + Log.d(TAG, "constrainPosition: OUTSIDE BUILDING — hard snap"); + return new LatLng(refLat + lastValidY / METRES_PER_DEG_LAT, + refLng + lastValidX / METRES_PER_DEG_LNG); + } + + // Inside building — accept fully + lastValidX = posX; + lastValidY = posY; + return new LatLng(lat, lng); + } + + // ========================================================================= + // Point-in-polygon (ray casting algorithm) // ========================================================================= /** - * Tests whether moving from (x1,y1) to (x2,y2) crosses any wall segment - * on the given floor. Coordinates are in local ENU metres. + * Ray-casting algorithm for point-in-polygon testing. Counts the number + * of times a horizontal ray from (px, py) crosses polygon edges. + * Odd crossings = inside, even = outside. Works for convex and concave. + */ + private static boolean isInsidePolygon(double px, double py, double[] polyX, double[] polyY) { + int n = polyX.length; + boolean inside = false; + for (int i = 0, j = n - 1; i < n; j = i++) { + if ((polyY[i] > py) != (polyY[j] > py) + && px < (polyX[j] - polyX[i]) * (py - polyY[i]) / (polyY[j] - polyY[i]) + polyX[i]) { + inside = !inside; + } + } + return inside; + } + + // ========================================================================= + // Wall collision detection (§3.2) + // ========================================================================= + + /** + * Tests whether the line segment from (x1,y1) to (x2,y2) intersects + * any wall feature on the given floor. Used by {@link #predict} to + * penalise wall-crossing particles. */ private boolean doesCrossWall(double x1, double y1, double x2, double y2, int floor) { if (floorShapes == null || floor < 0 || floor >= floorShapes.size()) return false; - FloorplanApiClient.FloorShapes floorData = floorShapes.get(floor); for (FloorplanApiClient.MapShapeFeature feature : floorData.getFeatures()) { if (!"wall".equals(feature.getIndoorType())) continue; - for (List part : feature.getParts()) { int size = part.size(); for (int i = 0; i < size - 1; i++) { - LatLng a = part.get(i); - LatLng b = part.get(i + 1); - + LatLng a = part.get(i), b = part.get(i + 1); double ax = (a.longitude - refLng) * METRES_PER_DEG_LNG; double ay = (a.latitude - refLat) * METRES_PER_DEG_LAT; double bx = (b.longitude - refLng) * METRES_PER_DEG_LNG; double by = (b.latitude - refLat) * METRES_PER_DEG_LAT; - - if (segmentsIntersect(x1, y1, x2, y2, ax, ay, bx, by)) { - return true; - } + if (segmentsIntersect(x1, y1, x2, y2, ax, ay, bx, by)) return true; } - + // Close polygon rings if not already closed if (size >= 3) { String geoType = feature.getGeometryType(); if ("MultiPolygon".equals(geoType) || "Polygon".equals(geoType)) { - LatLng first = part.get(0); - LatLng last = part.get(size - 1); - if (first.latitude == last.latitude - && first.longitude == last.longitude) continue; - + LatLng first = part.get(0), last = part.get(size - 1); + if (first.latitude == last.latitude && first.longitude == last.longitude) continue; double fx = (first.longitude - refLng) * METRES_PER_DEG_LNG; double fy = (first.latitude - refLat) * METRES_PER_DEG_LAT; double lx = (last.longitude - refLng) * METRES_PER_DEG_LNG; double ly = (last.latitude - refLat) * METRES_PER_DEG_LAT; - - if (segmentsIntersect(x1, y1, x2, y2, lx, ly, fx, fy)) { - return true; - } + if (segmentsIntersect(x1, y1, x2, y2, lx, ly, fx, fy)) return true; } } } @@ -657,33 +626,27 @@ private boolean doesCrossWall(double x1, double y1, double x2, double y2, int fl return false; } + /** Tests whether two line segments intersect using the cross-product method. */ private static boolean segmentsIntersect(double p1x, double p1y, double p2x, double p2y, double p3x, double p3y, double p4x, double p4y) { - double d1 = crossProduct(p3x, p3y, p4x, p4y, p1x, p1y); - double d2 = crossProduct(p3x, p3y, p4x, p4y, p2x, p2y); - double d3 = crossProduct(p1x, p1y, p2x, p2y, p3x, p3y); - double d4 = crossProduct(p1x, p1y, p2x, p2y, p4x, p4y); - + double d1 = cross(p3x, p3y, p4x, p4y, p1x, p1y); + double d2 = cross(p3x, p3y, p4x, p4y, p2x, p2y); + double d3 = cross(p1x, p1y, p2x, p2y, p3x, p3y); + double d4 = cross(p1x, p1y, p2x, p2y, p4x, p4y); if (((d1 > 0 && d2 < 0) || (d1 < 0 && d2 > 0)) - && ((d3 > 0 && d4 < 0) || (d3 < 0 && d4 > 0))) { - return true; - } - - if (d1 == 0 && onSegment(p3x, p3y, p4x, p4y, p1x, p1y)) return true; - if (d2 == 0 && onSegment(p3x, p3y, p4x, p4y, p2x, p2y)) return true; - if (d3 == 0 && onSegment(p1x, p1y, p2x, p2y, p3x, p3y)) return true; - if (d4 == 0 && onSegment(p1x, p1y, p2x, p2y, p4x, p4y)) return true; - + && ((d3 > 0 && d4 < 0) || (d3 < 0 && d4 > 0))) return true; + if (d1 == 0 && onSeg(p3x, p3y, p4x, p4y, p1x, p1y)) return true; + if (d2 == 0 && onSeg(p3x, p3y, p4x, p4y, p2x, p2y)) return true; + if (d3 == 0 && onSeg(p1x, p1y, p2x, p2y, p3x, p3y)) return true; + if (d4 == 0 && onSeg(p1x, p1y, p2x, p2y, p4x, p4y)) return true; return false; } - private static double crossProduct(double ax, double ay, double bx, double by, - double cx, double cy) { + private static double cross(double ax, double ay, double bx, double by, double cx, double cy) { return (bx - ax) * (cy - ay) - (by - ay) * (cx - ax); } - private static boolean onSegment(double ax, double ay, double bx, double by, - double px, double py) { + private static boolean onSeg(double ax, double ay, double bx, double by, double px, double py) { return Math.min(ax, bx) <= px && px <= Math.max(ax, bx) && Math.min(ay, by) <= py && py <= Math.max(ay, by); } @@ -693,38 +656,33 @@ private static boolean onSegment(double ax, double ay, double bx, double by, // ========================================================================= private double effectiveSampleSize() { - double sumSq = 0; - for (Particle p : particles) sumSq += p.getWeight() * p.getWeight(); - return (sumSq > 0) ? 1.0 / sumSq : 0; + double s = 0; + for (Particle p : particles) s += p.getWeight() * p.getWeight(); + return s > 0 ? 1.0 / s : 0; } - /** - * Systematic resampling with wall-aware jitter. - */ + /** Low-variance systematic resampling with post-resample jitter. */ private void resample() { int n = particles.size(); if (n == 0) return; + double[] cum = new double[n]; + cum[0] = particles.get(0).getWeight(); + for (int i = 1; i < n; i++) cum[i] = cum[i - 1] + particles.get(i).getWeight(); - double[] cumulative = new double[n]; - cumulative[0] = particles.get(0).getWeight(); - for (int i = 1; i < n; i++) { - cumulative[i] = cumulative[i - 1] + particles.get(i).getWeight(); - } - - List newParticles = new ArrayList<>(n); + List np = new ArrayList<>(n); double step = 1.0 / n; double start = random.nextDouble() * step; - int idx = 0; for (int i = 0; i < n; i++) { - double target = start + i * step; - while (idx < n - 1 && cumulative[idx] < target) idx++; - Particle copy = particles.get(idx).copy(); - copy.setWeight(1.0 / n); - newParticles.add(copy); + double t = start + i * step; + while (idx < n - 1 && cum[idx] < t) idx++; + Particle c = particles.get(idx).copy(); + c.setWeight(1.0 / n); + np.add(c); } - for (Particle p : newParticles) { + // Post-resample jitter — only if it doesn't cross a wall + for (Particle p : np) { double jx = p.getX() + random.nextGaussian() * RESAMPLE_JITTER; double jy = p.getY() + random.nextGaussian() * RESAMPLE_JITTER; if (!doesCrossWall(p.getX(), p.getY(), jx, jy, p.getFloor())) { @@ -734,16 +692,15 @@ private void resample() { } particles.clear(); - particles.addAll(newParticles); + particles.addAll(np); } private void normaliseWeights() { double sum = 0; for (Particle p : particles) sum += p.getWeight(); if (sum <= 0) { - Log.w(TAG, "Weight collapse — resetting to uniform weights"); - double uniform = 1.0 / particles.size(); - for (Particle p : particles) p.setWeight(uniform); + double u = 1.0 / particles.size(); + for (Particle p : particles) p.setWeight(u); return; } for (Particle p : particles) p.setWeight(p.getWeight() / sum); @@ -753,125 +710,97 @@ private void normaliseWeights() { // Output // ========================================================================= + /** Returns the manual start pin position. */ + public LatLng getStartLatLng() { + return initialised ? new LatLng(refLat, refLng) : null; + } + /** - * Returns the best-estimate position as a LatLng. - * - *

    Uses weighted mean with wall-aware fallback (if mean is more than - * {@link #WALL_MEAN_FALLBACK_THRESHOLD} metres from the best particle, uses - * best particle instead). EMA smoothing with alpha={@link #POSITION_SMOOTH_ALPHA} - * is applied, with snap for corrections larger than - * {@link #POSITION_SMOOTH_SNAP_THRESHOLD}.

    - * - * @return smoothed wall-constrained position, or null if not initialised + * Returns the smoothed particle filter position estimate. + * Used as fallback before FusionManager initialises. */ public LatLng getEstimatedPosition() { if (!isActive() || particles.isEmpty()) return null; - double weightedX = 0, weightedY = 0; - Particle bestParticle = null; - double bestWeight = -1; - + double wx = 0, wy = 0; + Particle best = null; + double bw = -1; for (Particle p : particles) { - weightedX += p.getX() * p.getWeight(); - weightedY += p.getY() * p.getWeight(); - if (p.getWeight() > bestWeight) { - bestWeight = p.getWeight(); - bestParticle = p; + wx += p.getX() * p.getWeight(); + wy += p.getY() * p.getWeight(); + if (p.getWeight() > bw) { + bw = p.getWeight(); + best = p; } } - double rawX = weightedX; - double rawY = weightedY; - if (bestParticle != null) { - double dx = weightedX - bestParticle.getX(); - double dy = weightedY - bestParticle.getY(); - if (Math.sqrt(dx * dx + dy * dy) > WALL_MEAN_FALLBACK_THRESHOLD) { - rawX = bestParticle.getX(); - rawY = bestParticle.getY(); - } + // If weighted mean is far from best particle, use best particle + // (prevents wall-split mean from landing inside a wall) + double rx = wx, ry = wy; + if (best != null && Math.sqrt(Math.pow(wx - best.getX(), 2) + Math.pow(wy - best.getY(), 2)) + > WALL_MEAN_FALLBACK_THRESHOLD) { + rx = best.getX(); + ry = best.getY(); } + // Exponential moving average for smooth output if (Double.isNaN(smoothedOutputX)) { - smoothedOutputX = rawX; - smoothedOutputY = rawY; + smoothedOutputX = rx; + smoothedOutputY = ry; } else { - double jumpDist = Math.sqrt( - Math.pow(rawX - smoothedOutputX, 2) + - Math.pow(rawY - smoothedOutputY, 2)); - if (jumpDist > POSITION_SMOOTH_SNAP_THRESHOLD) { - smoothedOutputX = rawX; - smoothedOutputY = rawY; + double j = Math.sqrt(Math.pow(rx - smoothedOutputX, 2) + Math.pow(ry - smoothedOutputY, 2)); + if (j > POSITION_SMOOTH_SNAP_THRESHOLD) { + smoothedOutputX = rx; + smoothedOutputY = ry; } else { - smoothedOutputX = POSITION_SMOOTH_ALPHA * rawX - + (1.0 - POSITION_SMOOTH_ALPHA) * smoothedOutputX; - smoothedOutputY = POSITION_SMOOTH_ALPHA * rawY - + (1.0 - POSITION_SMOOTH_ALPHA) * smoothedOutputY; + smoothedOutputX = POSITION_SMOOTH_ALPHA * rx + (1.0 - POSITION_SMOOTH_ALPHA) * smoothedOutputX; + smoothedOutputY = POSITION_SMOOTH_ALPHA * ry + (1.0 - POSITION_SMOOTH_ALPHA) * smoothedOutputY; } } - double lat = refLat + smoothedOutputY / METRES_PER_DEG_LAT; - double lng = refLng + smoothedOutputX / METRES_PER_DEG_LNG; - return new LatLng(lat, lng); + return new LatLng(refLat + smoothedOutputY / METRES_PER_DEG_LAT, + refLng + smoothedOutputX / METRES_PER_DEG_LNG); } /** - * Returns the weighted-majority floor estimate from the particle distribution. - * - * @return estimated floor index + * Returns the most likely floor from the particle distribution. + * Uses the barometric particle filter with all §3.2 floor guards. */ public int getEstimatedFloor() { if (!isActive() || particles.isEmpty()) return estimatedFloor; - - java.util.Map floorWeights = new java.util.HashMap<>(); - for (Particle p : particles) { - floorWeights.merge(p.getFloor(), p.getWeight(), Double::sum); - } - - int bestFloor = estimatedFloor; - double bestWeight = -1; - for (java.util.Map.Entry entry : floorWeights.entrySet()) { - if (entry.getValue() > bestWeight) { - bestWeight = entry.getValue(); - bestFloor = entry.getKey(); + java.util.Map fw = new java.util.HashMap<>(); + for (Particle p : particles) fw.merge(p.getFloor(), p.getWeight(), Double::sum); + int bf = estimatedFloor; + double bw = -1; + for (java.util.Map.Entry e : fw.entrySet()) { + if (e.getValue() > bw) { + bw = e.getValue(); + bf = e.getKey(); } } - - this.estimatedFloor = bestFloor; - return bestFloor; + this.estimatedFloor = bf; + return bf; } - /** - * Returns the estimated position as local XY coordinates (metres from reference). - * - * @return float[2] with {x, y}, or null if not initialised - */ + /** Returns the raw weighted mean position in local ENU. */ public float[] getEstimatedXY() { if (!isActive() || particles.isEmpty()) return null; - - double weightedX = 0, weightedY = 0; + double wx = 0, wy = 0; for (Particle p : particles) { - weightedX += p.getX() * p.getWeight(); - weightedY += p.getY() * p.getWeight(); + wx += p.getX() * p.getWeight(); + wy += p.getY() * p.getWeight(); } - return new float[]{(float) weightedX, (float) weightedY}; + return new float[]{(float) wx, (float) wy}; } - /** - * Returns the current particle list as a snapshot (copies). - * Useful for debug visualisation. - */ + /** Returns a snapshot of the particle cloud for visualisation. */ public List getParticles() { - List snapshot = new ArrayList<>(particles.size()); - for (Particle p : particles) snapshot.add(p.copy()); - return snapshot; + List s = new ArrayList<>(particles.size()); + for (Particle p : particles) s.add(p.copy()); + return s; } - // ========================================================================= - // Utility - // ========================================================================= - - /** Gaussian likelihood: exp(-d² / (2σ²)). */ - private static double gaussianLikelihood(double distance, double sigma) { - return Math.exp(-(distance * distance) / (2.0 * sigma * sigma)); + private static double gaussianLikelihood(double d, double s) { + return Math.exp(-(d * d) / (2.0 * s * s)); } } \ No newline at end of file diff --git a/app/src/main/java/com/openpositioning/PositionMe/positioning/FusionManager.java b/app/src/main/java/com/openpositioning/PositionMe/positioning/FusionManager.java index f0233e40..69984bfc 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/positioning/FusionManager.java +++ b/app/src/main/java/com/openpositioning/PositionMe/positioning/FusionManager.java @@ -2,6 +2,26 @@ import android.util.Log; +/** + * Sensor fusion engine using a particle filter to combine PDR, GNSS, and WiFi + * observations into a single position estimate. + * + *

    Implements §3.1 (Positioning Fusion):

    + *
      + *
    • Particle filter with 250 particles in easting/northing space
      • + *
    • Automatic initialisation from GNSS or WiFi (no user selection)
      • + *
    • PDR movement model via {@link #onStep}
      • + *
    • GNSS positioning updates via {@link #onGnss}
      • + *
    • WiFi positioning updates via {@link #onWifi}
      • + *
    + * + *

    Includes a WiFi death-spiral recovery mechanism: if the compass drifts + * the estimate beyond the WiFi outlier gate and consecutive fixes are rejected, + * the filter re-centres on WiFi to prevent permanent divergence.

    + * + * @see ParticleFilter for the underlying filter implementation + * @see com.openpositioning.PositionMe.mapmatching.MapMatchingEngine for map-based corrections + */ public class FusionManager { private static final String TAG = "FusionManager"; @@ -13,30 +33,57 @@ public class FusionManager { private CoordinateUtils coords; private boolean ready = false; + /** Exponential moving average for display smoothing (higher = faster response). */ private double[] smoothedLatLon = null; - private static final double DISPLAY_SMOOTHING_ALPHA = 0.25; + private static final double DISPLAY_SMOOTHING_ALPHA = 0.5; - // Track initialisation source to prevent re-init from weaker source + /** Tracks whether FM was initialised from WiFi to avoid re-init from weaker GNSS. */ private boolean initialisedFromWifi = false; + /** + * WiFi death-spiral recovery counter. When compass error causes FM to drift + * past the outlier gate, every subsequent WiFi fix is also rejected, causing + * permanent divergence. After {@code MAX_WIFI_REJECTIONS_BEFORE_RECENTRE} + * consecutive rejections, FM force re-centres on the WiFi position. + */ + private int consecutiveWifiRejections = 0; + private static final int MAX_WIFI_REJECTIONS_BEFORE_RECENTRE = 2; + + /** WiFi outlier gate distance in metres — fixes beyond this are rejected. */ + private static final double WIFI_OUTLIER_GATE_M = 10.0; + private FusionManager() {} public static FusionManager getInstance() { return instance; } + /** Resets all state for a new recording session. */ public synchronized void reset() { particleFilter = null; coords = null; ready = false; smoothedLatLon = null; initialisedFromWifi = false; + consecutiveWifiRejections = 0; } + // ========================================================================= + // GNSS observation update (§3.1) + // ========================================================================= + + /** + * Processes a GNSS position fix. Initialises the filter on first valid fix, + * or updates the particle weights for subsequent fixes. + * + * @param lat WGS84 latitude + * @param lon WGS84 longitude + * @param accuracyMeters reported horizontal accuracy from the location provider + */ public synchronized void onGnss(double lat, double lon, float accuracyMeters) { float rawAccuracy = accuracyMeters > 0 ? accuracyMeters : 8.0f; - // Stricter threshold — indoor GNSS below 15m is rare + // Indoor GNSS below 15m is rare — reject poor fixes early if (rawAccuracy > 15.0f) { Log.d(TAG, "Ignoring GNSS fix due to poor accuracy: " + rawAccuracy); return; @@ -71,6 +118,21 @@ public synchronized void onGnss(double lat, double lon, float accuracyMeters) { particleFilter.updateGnss(en[0], en[1], sigma); } + // ========================================================================= + // WiFi observation update (§3.1) + // ========================================================================= + + /** + * Processes a WiFi position fix from the openpositioning API. + * + *

    On first fix, initialises the filter. If GNSS-initialised and WiFi + * disagrees significantly, re-centres on WiFi (more reliable indoors). + * Includes death-spiral recovery: after consecutive rejections, force + * re-centres to prevent permanent compass-driven divergence.

    + * + * @param lat WGS84 latitude from WiFi positioning + * @param lon WGS84 longitude from WiFi positioning + */ public synchronized void onWifi(double lat, double lon) { if (!ready) { // First fix — initialise from WiFi @@ -79,53 +141,95 @@ public synchronized void onWifi(double lat, double lon) { particleFilter.initialise(0.0, 0.0, 8.0); ready = true; initialisedFromWifi = true; + consecutiveWifiRejections = 0; Log.d(TAG, "Initialised from WiFi: " + lat + ", " + lon); return; } - // If we initialised from GNSS and WiFi arrives soon after, - // re-centre on WiFi since it's more accurate indoors + // If GNSS-initialised and WiFi is far away, re-centre once on WiFi + // since WiFi is more accurate indoors than GNSS if (!initialisedFromWifi && coords != null) { double[] en = coords.toLocal(lat, lon); double[] est = particleFilter.estimate(); if (est != null) { double dist = Math.sqrt(Math.pow(en[0] - est[0], 2) + Math.pow(en[1] - est[1], 2)); - // If WiFi says we're far from GNSS init, re-centre once if (dist > 10.0) { coords = new CoordinateUtils(lat, lon); particleFilter = new ParticleFilter(PARTICLE_COUNT); particleFilter.initialise(0.0, 0.0, 8.0); initialisedFromWifi = true; + consecutiveWifiRejections = 0; Log.d(TAG, "Re-centred from WiFi (GNSS was " + String.format("%.1f", dist) + "m off): " + lat + ", " + lon); return; } } - initialisedFromWifi = true; // Don't try re-centring again + initialisedFromWifi = true; } double[] en = coords.toLocal(lat, lon); + // Outlier gate with death-spiral recovery double[] est = particleFilter.estimate(); if (est != null) { double de = en[0] - est[0]; double dn = en[1] - est[1]; double dist = Math.sqrt(de * de + dn * dn); - if (dist > 15.0) { - Log.d(TAG, "Rejected WiFi outlier. Dist=" + dist); + if (dist > WIFI_OUTLIER_GATE_M) { + consecutiveWifiRejections++; + + if (consecutiveWifiRejections >= MAX_WIFI_REJECTIONS_BEFORE_RECENTRE) { + // Death-spiral detected: compass drift has pushed FM so far + // that every WiFi fix gets rejected. WiFi is the most reliable + // indoor source — re-centre unconditionally. + coords = new CoordinateUtils(lat, lon); + particleFilter = new ParticleFilter(PARTICLE_COUNT); + particleFilter.initialise(0.0, 0.0, 8.0); + smoothedLatLon = null; + consecutiveWifiRejections = 0; + Log.w(TAG, "Re-centred from WiFi after " + MAX_WIFI_REJECTIONS_BEFORE_RECENTRE + + " consecutive rejections. Dist was " + String.format("%.1f", dist) + "m"); + return; + } + + Log.d(TAG, "Rejected WiFi outlier. Dist=" + dist + + " (rejection " + consecutiveWifiRejections + + "/" + MAX_WIFI_REJECTIONS_BEFORE_RECENTRE + ")"); return; } } + // WiFi accepted — reset rejection counter and update particles + consecutiveWifiRejections = 0; particleFilter.updateWifi(en[0], en[1]); } + // ========================================================================= + // PDR movement model (§3.1) + // ========================================================================= + + /** + * Propagates all particles forward using the PDR step detection. + * + * @param stepLen step length in metres from the step detector + * @param headingRad compass heading in radians from the rotation vector sensor + */ public synchronized void onStep(double stepLen, double headingRad) { if (!ready || particleFilter == null) return; if (Double.isNaN(stepLen) || Double.isNaN(headingRad) || stepLen <= 0.05) return; particleFilter.predict(stepLen, headingRad); } + // ========================================================================= + // Position output + // ========================================================================= + + /** + * Returns the best fused position estimate in WGS84, with exponential + * moving average display smoothing applied. + * + * @return [latitude, longitude] or null if not yet initialised + */ public synchronized double[] getBestPosition() { if (!ready || particleFilter == null || coords == null) return null; @@ -148,16 +252,19 @@ public synchronized double[] getBestPosition() { return new double[]{smoothedLatLon[0], smoothedLatLon[1]}; } + /** Returns the raw particle filter estimate in local ENU coordinates. */ public synchronized double[] getBestPositionLocal() { if (!ready || particleFilter == null) return null; return particleFilter.estimate(); } + /** Returns the effective sample size ratio as a confidence metric (0–1). */ public synchronized double getConfidence() { if (!ready || particleFilter == null) return 0.0; return particleFilter.getConfidence(); } + /** Returns true once the filter has been initialised from any source. */ public synchronized boolean isReady() { return ready; } diff --git a/app/src/main/java/com/openpositioning/PositionMe/positioning/ParticleFilter.java b/app/src/main/java/com/openpositioning/PositionMe/positioning/ParticleFilter.java index 153e8bc4..6997fb23 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/positioning/ParticleFilter.java +++ b/app/src/main/java/com/openpositioning/PositionMe/positioning/ParticleFilter.java @@ -4,8 +4,27 @@ import java.util.List; import java.util.Random; +/** + * Bootstrap particle filter for sensor fusion in easting/northing (ENU) space. + * + *

    Implements the predict–update–resample cycle used by {@link FusionManager} + * to fuse PDR step detections with absolute GNSS and WiFi observations. + * Operates entirely in local metres around a WGS84 reference point managed + * by {@link CoordinateUtils}.

    + * + *

    Key design choices:

    + *
      + *
    • Systematic resampling with ESS threshold at 50% for particle diversity
      • + *
    • Gaussian likelihood weighting for absolute observations
      • + *
    • Additive motion noise proportional to step length for realistic spread
      • + *
    • WiFi sigma tuned to 10m to avoid over-correcting on noisy indoor fixes
      • + *
    + * + * @see FusionManager which owns and drives this filter + */ public class ParticleFilter { + /** Single hypothesis in the particle cloud. */ private static class Particle { double east; double north; @@ -22,20 +41,34 @@ Particle copy() { } } + // ── Tuning constants ───────────────────────────────────────────────────── private static final double DEFAULT_INIT_SIGMA = 4.0; + + /** Base random walk noise added per step (metres). */ private static final double MOTION_NOISE_BASE = 0.12; + /** Additional noise scaled by step length (metres per metre). */ private static final double MOTION_NOISE_STEP_SCALE = 0.08; + /** Gaussian noise on the step length itself (metres). */ private static final double STEP_NOISE_SIGMA = 0.05; + /** Gaussian noise on the compass heading (radians). */ private static final double HEADING_NOISE_SIGMA = 0.05; + + /** Minimum sigma for GNSS observations (metres). */ private static final double GNSS_SIGMA_MIN = 3.0; - //private static final double WIFI_SIGMA_DEFAULT = 6.0; + /** Sigma for WiFi observations — tuned higher to avoid over-correction. */ private static final double WIFI_SIGMA_DEFAULT = 10.0; + // ── State ──────────────────────────────────────────────────────────────── private final Random rng = new Random(42); private final List particles = new ArrayList<>(); private final int particleCount; private boolean initialised = false; + /** + * Creates a new particle filter with the specified number of particles. + * + * @param particleCount number of particles to maintain + */ public ParticleFilter(int particleCount) { this.particleCount = particleCount; } @@ -44,6 +77,13 @@ public boolean isInitialised() { return initialised; } + /** + * Initialises the particle cloud as a Gaussian distribution around (east, north). + * + * @param east centre easting in local metres + * @param north centre northing in local metres + * @param sigmaMeters spread of the initial distribution + */ public void initialise(double east, double north, double sigmaMeters) { particles.clear(); double sigma = Math.max(1.0, sigmaMeters > 0 ? sigmaMeters : DEFAULT_INIT_SIGMA); @@ -57,6 +97,18 @@ public void initialise(double east, double north, double sigmaMeters) { initialised = true; } + // ========================================================================= + // Predict step — PDR movement model (§3.1) + // ========================================================================= + + /** + * Propagates all particles forward by one PDR step. Each particle receives + * independent noise on step length, heading, and position to maintain + * diversity in the cloud. + * + * @param stepLength estimated step length in metres + * @param headingRad compass heading in radians (0 = north, clockwise) + */ public void predict(double stepLength, double headingRad) { if (!initialised || stepLength <= 0.05 || Double.isNaN(stepLength) || Double.isNaN(headingRad)) { return; @@ -76,15 +128,37 @@ public void predict(double stepLength, double headingRad) { } } + // ========================================================================= + // Update steps — absolute observations (§3.1) + // ========================================================================= + + /** + * Updates particle weights from a GNSS observation. + * + * @param obsEast observed easting in local metres + * @param obsNorth observed northing in local metres + * @param accuracyMeters reported GNSS accuracy + */ public void updateGnss(double obsEast, double obsNorth, float accuracyMeters) { double sigma = Math.max(GNSS_SIGMA_MIN, accuracyMeters); updateAbsolute(obsEast, obsNorth, sigma); } + /** + * Updates particle weights from a WiFi observation. + * + * @param obsEast observed easting in local metres + * @param obsNorth observed northing in local metres + */ public void updateWifi(double obsEast, double obsNorth) { updateAbsolute(obsEast, obsNorth, WIFI_SIGMA_DEFAULT); } + /** + * Core observation update: re-weights particles by Gaussian likelihood + * relative to the observed position, normalises, and resamples if ESS + * drops below 50%. + */ private void updateAbsolute(double obsEast, double obsNorth, double sigma) { if (!initialised) return; @@ -100,6 +174,7 @@ private void updateAbsolute(double obsEast, double obsNorth, double sigma) { totalWeight += p.weight; } + // Weight collapse — reinitialise around observation if (totalWeight < 1e-20) { initialise(obsEast, obsNorth, sigma); return; @@ -112,12 +187,17 @@ private void updateAbsolute(double obsEast, double obsNorth, double sigma) { } } + // ========================================================================= + // Resampling + // ========================================================================= + private void normalise(double totalWeight) { for (Particle p : particles) { p.weight /= totalWeight; } } + /** Effective sample size: 1/sum(w_i^2). Measures particle diversity. */ private double effectiveSampleSize() { double sumSq = 0.0; for (Particle p : particles) { @@ -126,6 +206,10 @@ private double effectiveSampleSize() { return sumSq <= 0.0 ? 0.0 : 1.0 / sumSq; } + /** + * Low-variance systematic resampling. Produces a new set of equally-weighted + * particles drawn proportionally to the current weight distribution. + */ private void resampleSystematic() { List resampled = new ArrayList<>(particleCount); double step = 1.0 / particleCount; @@ -148,6 +232,15 @@ private void resampleSystematic() { particles.addAll(resampled); } + // ========================================================================= + // Output + // ========================================================================= + + /** + * Returns the weighted mean position of all particles. + * + * @return [easting, northing] in local metres, or null if uninitialised + */ public double[] estimate() { if (!initialised || particles.isEmpty()) return null; @@ -160,9 +253,10 @@ public double[] estimate() { return new double[]{east, north}; } + /** Returns ESS/N as a confidence metric (0–1). */ public double getConfidence() { if (!initialised || particles.isEmpty()) return 0.0; double ess = effectiveSampleSize(); return Math.max(0.0, Math.min(1.0, ess / particleCount)); } -} +} \ No newline at end of file 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 854e9266..8137ebf7 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 @@ -40,58 +40,51 @@ import com.openpositioning.PositionMe.positioning.FusionManager; /** - * Fragment responsible for managing the recording process of trajectory data. - *

    - * The RecordingFragment serves as the interface for users to initiate, monitor, and - * complete trajectory recording. It integrates sensor fusion data to track user movement - * and updates a map view in real time. - *

    - * Position display strategy: + * Fragment responsible for the recording screen during a live positioning session. + * + *

    Position display architecture (§3.1 + §3.2 + §3.3):

    *
      - *
    1. Primary: MapMatchingEngine — wall-constrained particle filter with floor detection. - * EMA-smoothed output with lost-filter recovery that resets the EMA on reinitialisation - * to prevent trace smearing outside the building.
      2. - *
    2. Fallback: FusionManager — smooth GPS+WiFi+PDR Kalman filter, used when - * MapMatchingEngine has not yet been initialised.
      3. - *
    3. Last resort: raw PDR position.
      4. + *
    4. Primary: FusionManager particle filter output, passed through + * MapMatchingEngine's {@code constrainPosition()} for building outline + * boundary enforcement.
      5. + *
    5. Fallback: MapMatchingEngine particle filter estimate (before + * FusionManager initialises from WiFi/GNSS).
      6. + *
    6. Last resort: raw PDR position from step detection.
      7. *
    - * The orange trace (marker + polyline) always reflects one consistent source per cycle. - * The red trace shows raw step-counting only, allowing visual comparison. * - * @see TrajectoryMapFragment The map fragment displaying the recorded trajectory. - * @see RecordingActivity The activity managing the recording workflow. - * @see SensorFusion Handles sensor data collection. + *

    Floor detection uses MapMatchingEngine's barometric particle filter + * with 30s warmup, sustained-step guards, and stairs/lift classification.

    + * + *

    Display updates at 5 Hz (200ms interval) with colour-coded observation + * markers for GNSS, WiFi, and PDR (§3.3).

    */ public class RecordingFragment extends Fragment { - // TODO: remove before submission private static final String TAG = "RecordingFragment"; - // UI elements private MaterialButton completeButton, cancelButton; private ImageView recIcon; private ProgressBar timeRemaining; private TextView elevation, distanceTravelled, gnssError; - // App settings private SharedPreferences settings; - - // Sensor & data logic private SensorFusion sensorFusion; private Handler refreshDataHandler; private CountDownTimer autoStop; - // Distance tracking private float distance = 0f; private float previousPosX = 0f; private float previousPosY = 0f; private LatLng rawPdrPosition = null; - // References to the child map fragment private TrajectoryMapFragment trajectoryMapFragment; private int uiUpdateCount = 0; + /** Tracks last MME floor to detect and log floor changes. */ + private int lastMmFloor = -1; + + // Observation display throttling — prevents marker spam on the map private LatLng lastDisplayedGnssObservation = null; private long lastDisplayedGnssTimeMs = 0L; private static final long GNSS_DISPLAY_MIN_INTERVAL_MS = 1500L; @@ -102,9 +95,11 @@ public class RecordingFragment extends Fragment { private static final long WIFI_DISPLAY_MIN_INTERVAL_MS = 1500L; private static final double WIFI_DISPLAY_MIN_DISTANCE_M = 2.0; + /** Heading smoothing to prevent arrow jitter on the map (§3.3). */ private Float smoothedHeadingDeg = null; private static final float HEADING_SMOOTHING_ALPHA = 0.20f; + /** UI refresh task at 5 Hz. */ private final Runnable refreshDataTask = new Runnable() { @Override public void run() { @@ -113,9 +108,7 @@ public void run() { } }; - public RecordingFragment() { - // Required empty public constructor - } + public RecordingFragment() {} @Override public void onCreate(Bundle savedInstanceState) { @@ -216,6 +209,7 @@ public void onFinish() { } } + /** Adds a user ground-truth test point marker at the current position. */ private void onAddTestPoint() { if (trajectoryMapFragment == null) return; @@ -235,21 +229,10 @@ private void onAddTestPoint() { } /** - * Updates the UI with sensor data and passes map updates to TrajectoryMapFragment. - * - *

    Position source priority each cycle (~200ms):

    - *
      - *
    1. MapMatchingEngine: wall-constrained particle filter — primary source. - * EMA-smoothed, resets cleanly on lost-filter recovery.
      2. - *
    2. FusionManager: Kalman-filtered GPS+WiFi+PDR — fallback before - * MapMatchingEngine initialises.
      3. - *
    3. Raw PDR: fallback of last resort.
      4. - *
    + * Core UI update cycle. Called at 5 Hz. * - *

    The same {@code fusedLocation} variable is used for both - * {@link TrajectoryMapFragment#updateUserLocation} (marker) and - * {@link TrajectoryMapFragment#updatePdrObservation} (observation dot), - * ensuring marker and polyline are always in sync.

    + *

    Retrieves the best position from FusionManager, applies MapMatchingEngine's + * building outline constraint, updates the map, and refreshes sensor readouts.

    */ private void updateUIandPosition() { float[] pdrValues = sensorFusion.getSensorValueMap().get(SensorTypes.PDR); @@ -257,76 +240,83 @@ private void updateUIandPosition() { uiUpdateCount++; - // Distance + // Update distance counter distance += Math.sqrt(Math.pow(pdrValues[0] - previousPosX, 2) + Math.pow(pdrValues[1] - previousPosY, 2)); distanceTravelled.setText(getString(R.string.meter, String.format("%.2f", distance))); - // Elevation + // Update elevation readout float elevationVal = sensorFusion.getElevation(); elevation.setText(getString(R.string.elevation, String.format("%.1f", elevationVal))); com.openpositioning.PositionMe.mapmatching.MapMatchingEngine mmEngine = sensorFusion.getMapMatchingEngine(); - // ── Wait for MapMatchingEngine before drawing anything ──────────────── - // This ensures both traces start at the manually dragged position, - // not the GPS auto-fix. Before the engine is active, we still track - // PDR accumulation so the first step delta is correct when we do start. + // Wait for MME initialisation before drawing — ensures traces + // start from the manual pin position, not from (0,0) if (mmEngine == null || !mmEngine.isActive()) { previousPosX = pdrValues[0]; previousPosY = pdrValues[1]; return; } - // ───────────────────────────────────────────────────────────────────── - // Initialise raw PDR position from engine's start (manually dragged point). - // Only done once — first time engine is active. + // Initialise raw PDR trace from the manual start pin (once) if (rawPdrPosition == null) { - LatLng mmStart = mmEngine.getEstimatedPosition(); - if (mmStart != null) { - rawPdrPosition = mmStart; - } + LatLng mmStart = mmEngine.getStartLatLng(); + if (mmStart == null) mmStart = mmEngine.getEstimatedPosition(); + if (mmStart != null) rawPdrPosition = mmStart; } - // Advance raw PDR by step delta + // Advance raw PDR by step delta (red polyline, §3.3) if (rawPdrPosition != null) { rawPdrPosition = UtilFunctions.calculateNewPos( rawPdrPosition, - new float[]{ pdrValues[0] - previousPosX, pdrValues[1] - previousPosY } - ); + new float[]{ pdrValues[0] - previousPosX, pdrValues[1] - previousPosY }); } - // Red polyline — raw step-counting only, no corrections if (rawPdrPosition != null && trajectoryMapFragment != null) { trajectoryMapFragment.addRawPdrPoint(rawPdrPosition); } - // ── Position source selection ────────────────────────────────────────── - LatLng fusedLocation = mmEngine.getEstimatedPosition(); - String positionSource = "MapMatchingEngine"; + // ── Position source selection (§3.1 + §3.2) ───────────────────────── + LatLng fusedLocation = null; + String positionSource = "none"; + // Primary: FusionManager → building outline constraint + double[] fusedPos = FusionManager.getInstance().getBestPosition(); + if (fusedPos != null) { + fusedLocation = mmEngine.constrainPosition(fusedPos[0], fusedPos[1]); + positionSource = "FusionManager+MMConstraint"; + } + + // Fallback: MME particle filter (before FM initialises) if (fusedLocation == null) { - double[] fusedPos = FusionManager.getInstance().getBestPosition(); - if (fusedPos != null) { - fusedLocation = new LatLng(fusedPos[0], fusedPos[1]); - positionSource = "FusionManager"; - } + fusedLocation = mmEngine.getEstimatedPosition(); + if (fusedLocation != null) positionSource = "MapMatchingEngine"; + } + // ───────────────────────────────────────────────────────────────────── + + // ── Floor detection from MME (§3.2) ────────────────────────────────── + int mmFloor = mmEngine.getEstimatedFloor(); + if (mmFloor != lastMmFloor && lastMmFloor != -1) { + Log.d(TAG, "MME floor change: " + lastMmFloor + " → " + mmFloor); } + lastMmFloor = mmFloor; // ───────────────────────────────────────────────────────────────────── - // TODO: remove before submission if (uiUpdateCount % 5 == 0) { Log.d(TAG, "Position source=" + positionSource - + " | mmEngine.isActive=" + mmEngine.isActive() + + " | floor=" + mmFloor + " | fusedLoc=" + (fusedLocation == null ? "null" : String.format("(%.5f, %.5f)", fusedLocation.latitude, fusedLocation.longitude))); } + // Smooth heading for display arrow (§3.3) float headingDeg = smoothHeadingDeg( (float) Math.toDegrees(sensorFusion.passOrientation())); + // Update map with best position if (fusedLocation != null && trajectoryMapFragment != null) { trajectoryMapFragment.updateUserLocation(fusedLocation, headingDeg); trajectoryMapFragment.updatePdrObservation(fusedLocation); @@ -337,7 +327,7 @@ private void updateUIandPosition() { long now = System.currentTimeMillis(); - // GNSS markers + // ── GNSS observation markers (§3.3) ────────────────────────────────── float[] gnss = sensorFusion.getSensorValueMap().get(SensorTypes.GNSSLATLONG); if (gnss != null && trajectoryMapFragment != null) { if (trajectoryMapFragment.isGnssEnabled()) { @@ -364,7 +354,7 @@ private void updateUIandPosition() { } } - // WiFi markers + // ── WiFi observation markers (§3.3) ────────────────────────────────── if (trajectoryMapFragment != null) { LatLng wifiLocation = sensorFusion.getLatLngWifiPositioning(); if (wifiLocation != null && shouldDisplayObservation( @@ -381,6 +371,10 @@ private void updateUIandPosition() { previousPosY = pdrValues[1]; } + /** + * Throttles observation marker display to prevent map clutter. + * Only shows a new marker if enough time and distance have passed. + */ private boolean shouldDisplayObservation(LatLng candidate, LatLng previous, long ageMs, long minIntervalMs, double minDistanceMeters) { @@ -390,6 +384,7 @@ private boolean shouldDisplayObservation(LatLng candidate, LatLng previous, return UtilFunctions.distanceBetweenPoints(previous, candidate) >= minDistanceMeters; } + /** Exponential heading smoother to prevent display arrow jitter. */ private float smoothHeadingDeg(float newHeadingDeg) { if (Float.isNaN(newHeadingDeg) || Float.isInfinite(newHeadingDeg)) { return smoothedHeadingDeg != null ? smoothedHeadingDeg : 0f; @@ -428,6 +423,7 @@ public void onResume() { } } + // ── Test point tracking ────────────────────────────────────────────────── private int testPointIndex = 0; private static class TestPoint { @@ -445,4 +441,4 @@ private static class TestPoint { } private final List testPoints = new ArrayList<>(); -} +} \ No newline at end of file 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 262fad3f..d363eb02 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 @@ -485,6 +485,17 @@ public void onViewCreated(@NonNull View view, @Nullable Bundle savedInstanceStat startFloor, floorHeight, building.getFloorShapesList() ); + + // Pass building outline for boundary checking + List outline = building.getOutlinePolygon(); + if (outline != null) { + sensorFusion.getMapMatchingEngine().setBuildingOutline( + outline, + startPosition[0], + startPosition[1] + ); + } + sensorFusion.getMapMatchingEngine().logWallStats(); Log.d(TAG, "Map matching initialised for " + selectedBuildingId); } 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 296557cb..e0b65809 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 @@ -993,7 +993,11 @@ private void applyImmediateFloor() { if (!indoorMapManager.getIsIndoorMapSet()) return; int candidateFloor; - if (sensorFusion.getLatLngWifiPositioning() != null) { + + int mmFloor = sensorFusion.getMapMatchingFloor(); + if (mmFloor >= 0) { + candidateFloor = mmFloor; + } else if (sensorFusion.getLatLngWifiPositioning() != null) { candidateFloor = sensorFusion.getWifiFloor(); } else { float elevation = sensorFusion.getElevation(); @@ -1004,7 +1008,6 @@ private void applyImmediateFloor() { indoorMapManager.setCurrentFloor(candidateFloor, true); updateFloorLabel(); - // Seed the debounce state so subsequent checks don't re-trigger immediately lastCandidateFloor = candidateFloor; lastCandidateTime = SystemClock.elapsedRealtime(); } @@ -1032,18 +1035,24 @@ private void evaluateAutoFloor() { int candidateFloor; - // Priority 1: WiFi-based floor (only if WiFi com.openpositioning.PositionMe.positioning has returned data) - if (sensorFusion.getLatLngWifiPositioning() != null) { + // Priority 1: MapMatchingEngine floor (barometric with guards) + int mmFloor = sensorFusion.getMapMatchingFloor(); + if (mmFloor >= 0) { + candidateFloor = mmFloor; + } + // Priority 2: WiFi floor + else if (sensorFusion.getLatLngWifiPositioning() != null) { candidateFloor = sensorFusion.getWifiFloor(); - } else { - // Fallback: barometric elevation estimate + } + // Priority 3: Raw barometric elevation + else { float elevation = sensorFusion.getElevation(); float floorHeight = indoorMapManager.getFloorHeight(); if (floorHeight <= 0) return; candidateFloor = Math.round(elevation / floorHeight); } - // Debounce: require the same floor reading for AUTO_FLOOR_DEBOUNCE_MS + // Debounce (unchanged from original) long now = SystemClock.elapsedRealtime(); if (candidateFloor != lastCandidateFloor) { lastCandidateFloor = candidateFloor; @@ -1054,7 +1063,6 @@ private void evaluateAutoFloor() { if (now - lastCandidateTime >= AUTO_FLOOR_DEBOUNCE_MS) { indoorMapManager.setCurrentFloor(candidateFloor, true); updateFloorLabel(); - // Reset timer so we don't keep re-applying the same floor lastCandidateTime = now; } } 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 aecc9a09..edf50577 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -488,6 +488,17 @@ public float[] getGNSSLatitude(boolean start) { return latLong; } + /** + * Returns the floor estimated by the MapMatchingEngine's barometric + * particle filter, or -1 if not available. + */ + public int getMapMatchingFloor() { + if (mapMatchingEngine != null && mapMatchingEngine.isActive()) { + return mapMatchingEngine.getEstimatedFloor(); + } + return -1; + } + /** * Returns the map matching particle filter engine. */ From 9676548ff3658d098ae422377dad6a8a0d1766a3 Mon Sep 17 00:00:00 2001 From: Saleh-AlMulla Date: Wed, 1 Apr 2026 18:25:52 +0100 Subject: [PATCH 22/26] Tune positioning and map-matching parameters Adjust various heuristics and smoothing values to make floor detection and pose updates more responsive and stable. Changes: - MapMatchingEngine: lower FLOOR_CHANGE_ELEVATION_THRESHOLD 4.0->2.0m, ELEVATION_SUSTAIN_STEPS 5->2, MIN_STEPS_BETWEEN_FLOOR_CHANGES 10->3, FLOOR_TRANSITION_WARMUP_MS 30000->5000ms to detect floor transitions faster. - ParticleFilter: increase WIFI_SIGMA_DEFAULT 10.0->14.0 to reduce over-correction from WiFi observations. - RecordingFragment: increase HEADING_SMOOTHING_ALPHA 0.20->0.40 for stronger heading smoothing. - TrajectoryMapFragment: reduce AUTO_FLOOR_DEBOUNCE_MS 3000->500ms and cap observation lists (PDR/GNSS/WIFI) from 3/5/5 to 2/2/2 to limit displayed markers. These are tuning changes to improve responsiveness; be aware they may increase sensitivity to noisy measurements and should be validated on-device. --- .../PositionMe/mapmatching/MapMatchingEngine.java | 8 ++++---- .../PositionMe/positioning/ParticleFilter.java | 2 +- .../presentation/fragment/RecordingFragment.java | 2 +- .../presentation/fragment/TrajectoryMapFragment.java | 8 ++++---- 4 files changed, 10 insertions(+), 10 deletions(-) diff --git a/app/src/main/java/com/openpositioning/PositionMe/mapmatching/MapMatchingEngine.java b/app/src/main/java/com/openpositioning/PositionMe/mapmatching/MapMatchingEngine.java index 69fc1956..71a8b944 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/mapmatching/MapMatchingEngine.java +++ b/app/src/main/java/com/openpositioning/PositionMe/mapmatching/MapMatchingEngine.java @@ -61,16 +61,16 @@ public class MapMatchingEngine { // ── Floor transition parameters (§3.2 floor detection) ─────────────────── /** Minimum barometric elevation change (metres) to trigger a floor transition. */ - private static final double FLOOR_CHANGE_ELEVATION_THRESHOLD = 4.0; + private static final double FLOOR_CHANGE_ELEVATION_THRESHOLD = 2.0; /** Steps the elevation must remain above threshold before confirming transition. */ - private static final int ELEVATION_SUSTAIN_STEPS = 5; + private static final int ELEVATION_SUSTAIN_STEPS = 2; /** Minimum steps between consecutive floor changes to prevent oscillation. */ - private static final int MIN_STEPS_BETWEEN_FLOOR_CHANGES = 10; + private static final int MIN_STEPS_BETWEEN_FLOOR_CHANGES = 3; /** Warmup period (ms) after init during which floor transitions are suppressed. */ - private static final long FLOOR_TRANSITION_WARMUP_MS = 30_000; + private static final long FLOOR_TRANSITION_WARMUP_MS = 5_000; /** * Maximum horizontal displacement (metres) during an elevation episode diff --git a/app/src/main/java/com/openpositioning/PositionMe/positioning/ParticleFilter.java b/app/src/main/java/com/openpositioning/PositionMe/positioning/ParticleFilter.java index 6997fb23..3624f2ae 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/positioning/ParticleFilter.java +++ b/app/src/main/java/com/openpositioning/PositionMe/positioning/ParticleFilter.java @@ -56,7 +56,7 @@ Particle copy() { /** Minimum sigma for GNSS observations (metres). */ private static final double GNSS_SIGMA_MIN = 3.0; /** Sigma for WiFi observations — tuned higher to avoid over-correction. */ - private static final double WIFI_SIGMA_DEFAULT = 10.0; + private static final double WIFI_SIGMA_DEFAULT = 14.0; // ── State ──────────────────────────────────────────────────────────────── private final Random rng = new Random(42); 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 8137ebf7..697f17b0 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 @@ -97,7 +97,7 @@ public class RecordingFragment extends Fragment { /** Heading smoothing to prevent arrow jitter on the map (§3.3). */ private Float smoothedHeadingDeg = null; - private static final float HEADING_SMOOTHING_ALPHA = 0.20f; + private static final float HEADING_SMOOTHING_ALPHA = 0.4f; /** UI refresh task at 5 Hz. */ private final Runnable refreshDataTask = new Runnable() { 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 e0b65809..cdeb0855 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 @@ -79,7 +79,7 @@ public class TrajectoryMapFragment extends Fragment { // Auto-floor state private static final String TAG = "TrajectoryMapFragment"; - private static final long AUTO_FLOOR_DEBOUNCE_MS = 3000; + private static final long AUTO_FLOOR_DEBOUNCE_MS = 500; private static final long AUTO_FLOOR_CHECK_INTERVAL_MS = 1000; private Handler autoFloorHandler; private Runnable autoFloorTask; @@ -112,9 +112,9 @@ public class TrajectoryMapFragment extends Fragment { * The "last N observations" requirement in the assignment brief (section 3.3) is * satisfied by these caps: old markers are automatically removed as new ones appear. */ - private static final int MAX_PDR_OBSERVATIONS = 3; - private static final int MAX_GNSS_OBSERVATIONS = 5; - private static final int MAX_WIFI_OBSERVATIONS = 5; + private static final int MAX_PDR_OBSERVATIONS = 2; + private static final int MAX_GNSS_OBSERVATIONS = 2; + private static final int MAX_WIFI_OBSERVATIONS = 2; /** * Separate lists for each positioning source's observation markers. From 5370fcc303f1d00bf2b02df3f48e22ad815fbd28 Mon Sep 17 00:00:00 2001 From: Lane2048 <145272991+Lane2048@users.noreply.github.com> Date: Wed, 1 Apr 2026 18:40:27 +0100 Subject: [PATCH 23/26] add Catmull-Rom path smoothing toggle to orange trajectory line Added SwitchMaterial toggle (Smooth Path) to fragment_trajectory_map.xml Catmull-Rom smoothing applies only to orange (fused) polyline, never red (raw PDR) Mid-recording toggle only smooths future points via catmullStartIndex, history stays raw rawPdrPoints stores accepted orange-line waypoints as smoothing source Segments >5m apart drawn as straight lines to prevent spikes on sharp turns/GNSS dropouts clearMapAndReset() clears rawPdrPoints and resets catmullStartIndex for new sessions --- .../fragment/TrajectoryMapFragment.java | 141 +++++++++++++++++- .../res/layout/fragment_trajectory_map.xml | 7 + app/src/main/res/values/strings.xml | 1 + 3 files changed, 143 insertions(+), 6 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 81532cf6..73063e0c 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 @@ -65,6 +65,10 @@ public class TrajectoryMapFragment extends Fragment { private final List testPointMarkers = new ArrayList<>(); private Polyline polyline; // Polyline representing user's movement path + // Catmull-Rom smoothing toggle + private boolean isCatmullRomEnabled = false; + private SwitchMaterial catmullSwitch; + private boolean isRed = true; // Tracks whether the polyline color is red private boolean isGnssOn = false; // Tracks if GNSS tracking is enabled @@ -134,6 +138,13 @@ public class TrajectoryMapFragment extends Fragment { private final List wifiObservationMarkers = new ArrayList<>(); private final List pdrObservationMarkers = new ArrayList<>(); + // Raw accepted PDR positions, used as source for Catmull-Rom redraw + private final List rawPdrPoints = new ArrayList<>(); + + // Index into rawPdrPoints from which Catmull-Rom smoothing applies. + // Points before this index are always drawn raw. + private int catmullStartIndex = 0; + /** * Orange polyline connecting every accepted PDR position (filtered by distance threshold). @@ -167,7 +178,7 @@ public class TrajectoryMapFragment extends Fragment { * noise is larger in scale than PDR noise. */ - private static final double PDR_OBSERVATION_MIN_DISTANCE_M = 3.0; + private static final double PDR_OBSERVATION_MIN_DISTANCE_M = 3; // ------------------------------------------------------------------------- @@ -253,6 +264,13 @@ public void onViewCreated(@NonNull View view, floorLabel = view.findViewById(R.id.floorLabel); switchColorButton = view.findViewById(R.id.lineColorButton); + catmullSwitch = view.findViewById(R.id.catmullSwitch); + catmullSwitch.setOnCheckedChangeListener((buttonView, isChecked) -> { + isCatmullRomEnabled = isChecked; + // Redraw BOTH polylines with current points using new mode + redrawPdrPolyline(); + }); + // Setup floor up/down UI hidden initially until we know there's an indoor map setFloorControlsVisibility(View.GONE); @@ -684,13 +702,121 @@ public void updatePdrObservation(@NonNull LatLng pdrLocation) { MAX_PDR_OBSERVATIONS ); - // Extend the orange trajectory polyline with this accepted position. - // This builds the continuous walking path shown on the map. + // Store raw point for Catmull-Rom redraw source. + rawPdrPoints.add(pdrLocation); + + // Build the displayed path: + // - points before catmullStartIndex are always drawn raw (history) + // - points from catmullStartIndex onward are smoothed if switch is on if (pdrTrajectoryPolyline != null) { - List points = new ArrayList<>(pdrTrajectoryPolyline.getPoints()); - points.add(pdrLocation); - pdrTrajectoryPolyline.setPoints(points); + List displayPoints; + if (isCatmullRomEnabled && catmullStartIndex < rawPdrPoints.size()) { + // Raw history segment + List history = rawPdrPoints.subList(0, catmullStartIndex); + // Smoothed future segment (need at least 4 points for Catmull-Rom) + List future = new ArrayList<>(rawPdrPoints.subList(catmullStartIndex, rawPdrPoints.size())); + List smoothFuture = applyCatmullRom(future); + displayPoints = new ArrayList<>(history); + displayPoints.addAll(smoothFuture); + } else { + displayPoints = new ArrayList<>(rawPdrPoints); + } + pdrTrajectoryPolyline.setPoints(displayPoints); + } + } + + // ------------------------------------------------------------------------- + // CATMULL-ROM SMOOTHING (NEW) + // ------------------------------------------------------------------------- + + /** + * Computes one segment of a Catmull-Rom spline between p1 and p2, + * using p0 and p3 as the surrounding control points. + * + * WHY CATMULL-ROM: + * Unlike Bezier curves, Catmull-Rom splines pass through every control point, + * which means the smoothed path still visits every accepted PDR observation. + * This gives a natural-looking walking curve without drifting away from real data. + * + * @param p0 Control point before p1 (or p1 itself if i==0) + * @param p1 Start of this segment + * @param p2 End of this segment + * @param p3 Control point after p2 (or p2 itself if at end) + * @param steps Number of interpolated sub-points between p1 and p2 (higher = smoother) + * @return Interpolated LatLng points for this segment + */ + private List catmullRomSegment(LatLng p0, LatLng p1, LatLng p2, LatLng p3, int steps) { + List result = new ArrayList<>(); + for (int i = 0; i <= steps; i++) { + float t = i / (float) steps; + float t2 = t * t, t3 = t2 * t; + double lat = 0.5 * ((2 * p1.latitude) + + (-p0.latitude + p2.latitude) * t + + (2*p0.latitude - 5*p1.latitude + 4*p2.latitude - p3.latitude) * t2 + + (-p0.latitude + 3*p1.latitude - 3*p2.latitude + p3.latitude) * t3); + double lng = 0.5 * ((2 * p1.longitude) + + (-p0.longitude + p2.longitude) * t + + (2*p0.longitude - 5*p1.longitude + 4*p2.longitude - p3.longitude) * t2 + + (-p0.longitude + 3*p1.longitude - 3*p2.longitude + p3.longitude) * t3); + result.add(new LatLng(lat, lng)); } + return result; + } + + /** + * Applies Catmull-Rom smoothing to a full list of LatLng points. + * + * WHY 4-POINT MINIMUM: + * Catmull-Rom needs 4 points to define even one segment (p0 as lead-in, + * p1-p2 as the actual segment, p3 as look-ahead). With fewer than 4 points + * the spline is undefined, so we fall back to returning the raw points. + * + * @param pts Raw list of accepted PDR positions + * @return Smoothed list ready to be set on the polyline + */ + private List applyCatmullRom(List pts) { + if (pts.size() < 4) return new ArrayList<>(pts); + List smooth = new ArrayList<>(); + for (int i = 0; i < pts.size() - 1; i++) { + LatLng p1 = pts.get(i); + LatLng p2 = pts.get(i + 1); + // Skip smoothing for large jumps (e.g. GNSS dropout, sudden position jump) + // — draw straight line instead to avoid Catmull-Rom overshoot spikes + if (UtilFunctions.distanceBetweenPoints(p1, p2) > 5.0) { + smooth.add(p1); + smooth.add(p2); + continue; + } + LatLng p0 = pts.get(Math.max(i - 1, 0)); + LatLng p3 = pts.get(Math.min(i + 2, pts.size() - 1)); + smooth.addAll(catmullRomSegment(p0, p1, p2, p3, 10)); + } + return smooth; + } + + /** + * Redraws the orange PDR trajectory polyline. + * + * WHY WE SPLIT AT catmullStartIndex: + * When the switch is toggled mid-recording, we don't want to retroactively + * smooth the path the user already walked. catmullStartIndex marks the point + * in rawPdrPoints where smoothing was turned on, so history stays raw and + * only future points get the Catmull-Rom curve applied. + */ + private void redrawPdrPolyline() { + if (pdrTrajectoryPolyline == null) return; + + if (!isCatmullRomEnabled) { + // Switch turned off — restore full raw path + pdrTrajectoryPolyline.setPoints(new ArrayList<>(rawPdrPoints)); + return; + } + + // Switch just turned on — record where smoothing begins + catmullStartIndex = rawPdrPoints.size(); + + // History stays raw, nothing to smooth yet — no redraw needed + pdrTrajectoryPolyline.setPoints(new ArrayList<>(rawPdrPoints)); } /** @@ -844,6 +970,9 @@ public void clearMapAndReset() { } // Reset the distance filter anchor so the first new point is always accepted lastPdrObservationLocation = null; + // Clear the raw point store so Catmull-Rom starts fresh for the new session + rawPdrPoints.clear(); + catmullStartIndex = 0; pdrTrajectoryPolyline = gMap.addPolyline(new PolylineOptions() .color(Color.rgb(255, 165, 0)) .width(5f) diff --git a/app/src/main/res/layout/fragment_trajectory_map.xml b/app/src/main/res/layout/fragment_trajectory_map.xml index a72425bf..a42806e4 100644 --- a/app/src/main/res/layout/fragment_trajectory_map.xml +++ b/app/src/main/res/layout/fragment_trajectory_map.xml @@ -37,6 +37,13 @@ android:layout_height="wrap_content" android:text="@string/gnssSwitch" /> + + Show locally Set Cancel + Smooth Path Accelerometer From e7a228654405b60fe0a92f93d1aaa4baaa3468a7 Mon Sep 17 00:00:00 2001 From: Saleh-AlMulla Date: Wed, 1 Apr 2026 18:41:39 +0100 Subject: [PATCH 24/26] Tune WiFi sigma and heading smoothing Adjust particle filter and UI smoothing constants: lower WIFI_SIGMA_DEFAULT from 14.0 to 10.0 in ParticleFilter to make WiFi observations slightly more confident, and reduce HEADING_SMOOTHING_ALPHA from 0.4f to 0.20f in RecordingFragment to increase heading smoothing and further reduce arrow jitter on the map. These are small tuning changes to balance responsiveness and stability. --- .../openpositioning/PositionMe/positioning/ParticleFilter.java | 2 +- .../PositionMe/presentation/fragment/RecordingFragment.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/app/src/main/java/com/openpositioning/PositionMe/positioning/ParticleFilter.java b/app/src/main/java/com/openpositioning/PositionMe/positioning/ParticleFilter.java index 3624f2ae..6997fb23 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/positioning/ParticleFilter.java +++ b/app/src/main/java/com/openpositioning/PositionMe/positioning/ParticleFilter.java @@ -56,7 +56,7 @@ Particle copy() { /** Minimum sigma for GNSS observations (metres). */ private static final double GNSS_SIGMA_MIN = 3.0; /** Sigma for WiFi observations — tuned higher to avoid over-correction. */ - private static final double WIFI_SIGMA_DEFAULT = 14.0; + private static final double WIFI_SIGMA_DEFAULT = 10.0; // ── State ──────────────────────────────────────────────────────────────── private final Random rng = new Random(42); 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 697f17b0..8137ebf7 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 @@ -97,7 +97,7 @@ public class RecordingFragment extends Fragment { /** Heading smoothing to prevent arrow jitter on the map (§3.3). */ private Float smoothedHeadingDeg = null; - private static final float HEADING_SMOOTHING_ALPHA = 0.4f; + private static final float HEADING_SMOOTHING_ALPHA = 0.20f; /** UI refresh task at 5 Hz. */ private final Runnable refreshDataTask = new Runnable() { From 9a27a4fdf5d3093e11d822b118765e2aaa04910a Mon Sep 17 00:00:00 2001 From: Lane2048 <145272991+Lane2048@users.noreply.github.com> Date: Wed, 1 Apr 2026 18:58:01 +0100 Subject: [PATCH 25/26] deleted extra smooth path switch --- app/src/main/res/layout/fragment_trajectory_map.xml | 6 ------ 1 file changed, 6 deletions(-) diff --git a/app/src/main/res/layout/fragment_trajectory_map.xml b/app/src/main/res/layout/fragment_trajectory_map.xml index cd4bf941..10d34a68 100644 --- a/app/src/main/res/layout/fragment_trajectory_map.xml +++ b/app/src/main/res/layout/fragment_trajectory_map.xml @@ -50,12 +50,6 @@ android:layout_height="wrap_content" android:text="@string/auto_floor" /> - Date: Wed, 1 Apr 2026 22:00:59 +0100 Subject: [PATCH 26/26] Update RecordingFragment.java --- .../fragment/RecordingFragment.java | 43 ++++++++++++++++--- 1 file changed, 37 insertions(+), 6 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 8137ebf7..55d344c8 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 @@ -90,6 +90,12 @@ public class RecordingFragment extends Fragment { private static final long GNSS_DISPLAY_MIN_INTERVAL_MS = 1500L; private static final double GNSS_DISPLAY_MIN_DISTANCE_M = 3.0; + // Display-only GNSS sanity gates: +// 1) If GNSS is >25 m away from the current displayed path, don't draw it. +// 2) If a new GNSS point jumps >20 m from the last drawn GNSS point, don't draw it. + private static final double GNSS_MAX_DISPLAY_ERROR_M = 25.0; + private static final double GNSS_MAX_JUMP_FROM_LAST_M = 20.0; + private LatLng lastDisplayedWifiObservation = null; private long lastDisplayedWifiTimeMs = 0L; private static final long WIFI_DISPLAY_MIN_INTERVAL_MS = 1500L; @@ -333,21 +339,46 @@ private void updateUIandPosition() { if (trajectoryMapFragment.isGnssEnabled()) { LatLng gnssLocation = new LatLng(gnss[0], gnss[1]); LatLng currentLoc = trajectoryMapFragment.getCurrentLocation(); + + double errorDist = -1.0; if (currentLoc != null) { - double errorDist = UtilFunctions.distanceBetweenPoints( - currentLoc, gnssLocation); + errorDist = UtilFunctions.distanceBetweenPoints(currentLoc, gnssLocation); gnssError.setVisibility(View.VISIBLE); gnssError.setText(String.format( getString(R.string.gnss_error) + "%.2fm", errorDist)); + } else { + gnssError.setVisibility(View.GONE); } - if (shouldDisplayObservation( - gnssLocation, lastDisplayedGnssObservation, - now - lastDisplayedGnssTimeMs, - GNSS_DISPLAY_MIN_INTERVAL_MS, GNSS_DISPLAY_MIN_DISTANCE_M)) { + + boolean tooFarFromDisplayedPath = + (currentLoc != null && errorDist > GNSS_MAX_DISPLAY_ERROR_M); + + boolean hugeJumpFromLastDisplayed = + (lastDisplayedGnssObservation != null && + UtilFunctions.distanceBetweenPoints(lastDisplayedGnssObservation, gnssLocation) + > GNSS_MAX_JUMP_FROM_LAST_M); + + if (!tooFarFromDisplayedPath && !hugeJumpFromLastDisplayed && + shouldDisplayObservation( + gnssLocation, + lastDisplayedGnssObservation, + now - lastDisplayedGnssTimeMs, + GNSS_DISPLAY_MIN_INTERVAL_MS, + GNSS_DISPLAY_MIN_DISTANCE_M)) { + trajectoryMapFragment.updateGNSS(gnssLocation); lastDisplayedGnssObservation = gnssLocation; lastDisplayedGnssTimeMs = now; + + } else { + if (tooFarFromDisplayedPath) { + Log.d(TAG, "Skipping GNSS draw: too far from displayed path (" + + String.format("%.2f", errorDist) + "m)"); + } else if (hugeJumpFromLastDisplayed) { + Log.d(TAG, "Skipping GNSS draw: huge jump from last GNSS point"); + } } + } else { gnssError.setVisibility(View.GONE); trajectoryMapFragment.clearGNSS();