From 4f70181b38893c0b94b9cf2950c448632e989ab3 Mon Sep 17 00:00:00 2001 From: JP10JOT <97289379+JapjotS@users.noreply.github.com> Date: Tue, 31 Mar 2026 13:25:24 +0100 Subject: [PATCH 01/36] update secrets --- secrets.properties | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/secrets.properties b/secrets.properties index f0dc54fd..649224a0 100644 --- a/secrets.properties +++ b/secrets.properties @@ -1,6 +1,6 @@ # # Modify the variables to set your keys # -MAPS_API_KEY= -OPENPOSITIONING_API_KEY= -OPENPOSITIONING_MASTER_KEY= +MAPS_API_KEY=AIzaSyBOCzNnbeqjbVS9DIIAnJWmn1ovlNoZwiY +OPENPOSITIONING_API_KEY=1v8fHGQ-Eu1q42j_3D_2nw +OPENPOSITIONING_MASTER_KEY=ewireless From f87f0cabfbdd1509a9b88fba16b405c1e750e328 Mon Sep 17 00:00:00 2001 From: JP10JOT <97289379+JapjotS@users.noreply.github.com> Date: Tue, 31 Mar 2026 14:42:25 +0100 Subject: [PATCH 02/36] GNSS positioning updates (WGS84, transform to the easting/northing space) --- .../PositionMe/utils/UtilFunctions.java | 36 +++++++++++++++++++ 1 file changed, 36 insertions(+) diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/UtilFunctions.java b/app/src/main/java/com/openpositioning/PositionMe/utils/UtilFunctions.java index 56a88aa3..0ee1d45d 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/UtilFunctions.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/UtilFunctions.java @@ -16,6 +16,42 @@ */ public class UtilFunctions { // Constant 1degree of latitiude/longitude (in m) + private static final double EARTH_RADIUS = 6371000.0; // in meters + + public static float[] convertWGS84ToENU(LatLng origin, LatLng point){ + //convert to radians + + double lat1 = Math.toRadians(origin.latitude); + double lon1 = Math.toRadians(origin.longitude); + double lat2 = Math.toRadians(point.latitude); + double lon2 = Math.toRadians(point.longitude); + + //calculate easting + double easting = EARTH_RADIUS * (lon2 - lon1) * Math.cos(lat1); + //calculate northing + double northing = EARTH_RADIUS * (lat2 - lat1); + //calculate up + double up = 0; // Assuming flat surface + + return new float[]{(float) easting, (float) northing, (float) up}; + } + + public static LatLng convertENUToWGS84(LatLng origin, float[] enu){ + //convert to radians + double lat1 = Math.toRadians(origin.latitude); + double lon1 = Math.toRadians(origin.longitude); + + //calculate latitude + double lat2 = lat1 + (enu[1] / EARTH_RADIUS); + //calculate longitude + double lon2 = lon1 + (enu[0] / (EARTH_RADIUS * Math.cos(lat1))); + //calculate up (not used in this case) + double up = 0; // Assuming flat surface + + return new LatLng(Math.toDegrees(lat2), Math.toDegrees(lon2)); + } + + private static final int DEGREE_IN_M=111111; /** * Simple function to calculate the angle between two close points From a39fce9a72b1a08e08ddfeb38baace741098f8f9 Mon Sep 17 00:00:00 2001 From: JP10JOT <97289379+JapjotS@users.noreply.github.com> Date: Tue, 31 Mar 2026 15:08:42 +0100 Subject: [PATCH 03/36] Initialise estimation (from GNSS, WiFi, others, no user selection) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit On app start, wait for either a GNSS fix or a WiFi positioning result — whichever comes first Set that as the origin of the easting/northing frame Initialise the particle filter particles spread around that origin (Gaussian spread based on the accuracy of the source — GNSS accuracy field, or ~20–30m for WiFi) No button press or user selection needed --- .../PositionMe/sensors/ParticleFilter.java | 48 +++++++++++++++++++ .../PositionMe/sensors/SensorFusion.java | 26 +++++++++- .../PositionMe/utils/UtilFunctions.java | 2 +- 3 files changed, 74 insertions(+), 2 deletions(-) create mode 100644 app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java new file mode 100644 index 00000000..8e8cf549 --- /dev/null +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java @@ -0,0 +1,48 @@ + +package com.openpositioning.PositionMe.sensors; +import android.util.Log; +import com.google.android.gms.maps.model.LatLng; +import com.openpositioning.PositionMe.utils.UtilFunctions; +import java.util.ArrayList; +import java.util.List; + + +import java.util.Random; //FOR PARTICLE FILTER + +public class ParticleFilter { + private static final String TAG = "ParticleFilter"; + private static final int NUM_PARTICLES = 1000; // Number of particles + private float [] [] particles; + + //[num particles][2] is {easting, northing} + private float [] weights; // Weights for each particle + private LatLng origin; // Origin point for ENU conversion + + private boolean initialized = false; + private final Random random = new Random(); + + public void initialise(LatLng firstFix, float accuracyMeters) { + if (initialized) return; // ignore subsequent calls, DONE + this.origin = firstFix; + this.particles = new float[NUM_PARTICLES][2]; + + + this.weights = new float[NUM_PARTICLES]; + float spread = accuracyMeters; + + for (int i = 0; i < NUM_PARTICLES; i++) { + particles[i][0] = (float) (random.nextGaussian() * spread); // east + particles[i][1] = (float) (random.nextGaussian() * spread); // north + weights[i] = 1.0f / NUM_PARTICLES; + } + initialized = true; + Log.d(TAG, "Initialized at " + firstFix + " accuraccy is =" + accuracyMeters + "m"); + } + + public boolean isInitialized() { + return initialized ; + } + public LatLng getOrigin() { + return origin; + } +} 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..f21ef990 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -87,6 +87,8 @@ public class SensorFusion implements SensorEventListener { // PDR and path private PdrProcessing pdrProcessing; private PathView pathView; + // particle filter + private ParticleFilter particleFilter; // Sensor registration latency setting long maxReportLatencyNs = 0; @@ -166,7 +168,16 @@ public void setContext(Context context) { // Register WiFi observer on WifiPositionManager (not on SensorFusion) this.wifiProcessor = new WifiDataProcessor(context); - wifiProcessor.registerObserver(wifiPositionManager); + wifiProcessor.registerObserver(objects -> { + new Handler(Looper.getMainLooper()).postDelayed(() -> { + if (!particleFilter.isInitialised()) { + LatLng wifiPosition = wifiPositionManager.getLatLngWifiPositioning();// Get WiFi position for particle filter initialization + if (wifiPosition != null) { // Check if WiFi position is available before initializing particle filter + particleFilter.initialise(wifiPosition, 20f); + } + } + }, 1000); //1second delay to allow wifi position manager to update with new scan data + }); // Initialise BLE scanner and register observer for trajectory recording this.bleProcessor = new BleDataProcessor(context); @@ -177,8 +188,11 @@ public void update(Object[] objList) { .map(o -> (BleDevice) o).collect(Collectors.toList()); recorder.addBleFingerprint(bleList); } + }); + + // Initialise WiFi RTT manager and register as WiFi scan observer this.rttManager = new RttManager(appContext, recorder, wifiProcessor); wifiProcessor.registerObserver(rttManager); @@ -187,6 +201,9 @@ public void update(Object[] objList) { this.bleRttManager = new BleRttManager(recorder); bleProcessor.registerObserver(bleRttManager); + //added particle filter + this.particleFilter = new ParticleFilter(); + if (!rttManager.isRttSupported()) { new Handler(Looper.getMainLooper()).post(() -> Toast.makeText(appContext, @@ -645,6 +662,13 @@ public void onLocationChanged(@NonNull Location location) { state.latitude = (float) location.getLatitude(); state.longitude = (float) location.getLongitude(); recorder.addGnssData(location); + + //initialize particle filter automatically on first GNSS fix with accuracy as spread + if (!particleFilter.isInitialised()) { + float accuracy = location.hasAccuracy() ? location.getAccuracy() : 20f; + particleFilter.initialise( + new LatLng(location.getLatitude(), location.getLongitude()),accuracy); + } } } diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/UtilFunctions.java b/app/src/main/java/com/openpositioning/PositionMe/utils/UtilFunctions.java index 0ee1d45d..7aab0596 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/UtilFunctions.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/UtilFunctions.java @@ -52,7 +52,7 @@ public static LatLng convertENUToWGS84(LatLng origin, float[] enu){ } - private static final int DEGREE_IN_M=111111; + private static final int DEGREE_IN_M=111111; //NOT NEEDED FOR CONVERSION, JUST USED AS A CONSTANT TO CALCULATE NEW COORDINATES /** * Simple function to calculate the angle between two close points * @param pointA Starting point From 98f48bc59c5f6b532d96d008a1c404f2220ac4be Mon Sep 17 00:00:00 2001 From: JP10JOT <97289379+JapjotS@users.noreply.github.com> Date: Tue, 31 Mar 2026 15:10:37 +0100 Subject: [PATCH 04/36] Auto-Initialisation (No User Input) bug fixes MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit On app start, wait for either a GNSS fix or a WiFi positioning result — whichever comes first Set that as the origin of the easting/northing frame Initialise the particle filter particles spread around that origin (Gaussian spread based on the accuracy of the source — GNSS accuracy field, or ~20–30m for WiFi) No button press or user selection needed --- .../com/openpositioning/PositionMe/sensors/SensorFusion.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 f21ef990..36049474 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -170,7 +170,7 @@ public void setContext(Context context) { this.wifiProcessor = new WifiDataProcessor(context); wifiProcessor.registerObserver(objects -> { new Handler(Looper.getMainLooper()).postDelayed(() -> { - if (!particleFilter.isInitialised()) { + if (!particleFilter.isInitialized()) { LatLng wifiPosition = wifiPositionManager.getLatLngWifiPositioning();// Get WiFi position for particle filter initialization if (wifiPosition != null) { // Check if WiFi position is available before initializing particle filter particleFilter.initialise(wifiPosition, 20f); @@ -664,7 +664,7 @@ public void onLocationChanged(@NonNull Location location) { recorder.addGnssData(location); //initialize particle filter automatically on first GNSS fix with accuracy as spread - if (!particleFilter.isInitialised()) { + if (!particleFilter.isInitialized()) { float accuracy = location.hasAccuracy() ? location.getAccuracy() : 20f; particleFilter.initialise( new LatLng(location.getLatitude(), location.getLongitude()),accuracy); From ec76f5d0a6a0888c8c1996d6433ba1734cd8d508 Mon Sep 17 00:00:00 2001 From: JP10JOT <97289379+JapjotS@users.noreply.github.com> Date: Tue, 31 Mar 2026 15:32:21 +0100 Subject: [PATCH 05/36] Add particle filter prediction & sensor hooks Reduce particle count and add step-based prediction for the particle filter. ParticleFilter: NUM_PARTICLES lowered from 1000 to 200 for performance, added HEADING_NOISE_STD and STRIDE_LENGTH_NOISE_STD, and a predict(deltaEasting, deltaNorthing) method that updates particles using noisy heading/stride. SensorEventHandler: introduced a StepListener, tracks last easting/northing and emits delta steps to the listener when new PDR coordinates arrive. SensorFusion: instantiate WifiDataProcessor before registering observers and register wifiPositionManager on it; wire eventHandler.setStepListener to call particleFilter.predict when initialized. These changes connect PDR step outputs to the particle filter and reorder WiFi observer registration. --- .../PositionMe/sensors/ParticleFilter.java | 27 ++++++++++++++++- .../sensors/SensorEventHandler.java | 29 +++++++++++++++++++ .../PositionMe/sensors/SensorFusion.java | 9 +++++- 3 files changed, 63 insertions(+), 2 deletions(-) diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java index 8e8cf549..7e9fd13c 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java @@ -11,7 +11,7 @@ public class ParticleFilter { private static final String TAG = "ParticleFilter"; - private static final int NUM_PARTICLES = 1000; // Number of particles + private static final int NUM_PARTICLES = 200; // Number of particles private float [] [] particles; //[num particles][2] is {easting, northing} @@ -45,4 +45,29 @@ public boolean isInitialized() { public LatLng getOrigin() { return origin; } + + + public static final float HEADING_NOISE_STD = 0.01f; //TODO CHANGE LATER?? + public static final float STRIDE_LENGTH_NOISE_STD = 0.01f; // WE CAN TUNE THESE NOISE PARAMETERS BASED ON EXPECTED SENSOR ACCURACY AND ENVIRONMENTAL CONDITIONS + + public void predict(float deltaEasting, float deltaNorthing) { + if (!initialized) return; // ignore if not initialized + + + float stride = (float) Math.sqrt(deltaEasting * deltaEasting + deltaNorthing * deltaNorthing); + float heading = (float) Math.atan2(deltaNorthing, deltaEasting); // calculate heading from deltas + + for (int i = 0; i < NUM_PARTICLES; i++) { + // Add noise to heading and stride + float noisyHeading = heading + (float) (random.nextGaussian() * HEADING_NOISE_STD); + float noisyStride = stride + (float) (random.nextGaussian() * STRIDE_LENGTH_NOISE_STD); + + // Update particle position based on noisy heading and stride + particles[i][0] += noisyStride * Math.cos(noisyHeading); // update east + particles[i][1] += noisyStride * Math.sin(noisyHeading); // update north + } + + } + + } 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..7068775e 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java @@ -22,6 +22,23 @@ */ public class SensorEventHandler { + // FOR PARTICLE FILTER + public interface StepListener { + void onStep(float deltaEasting, float deltaNorthing); + } + + private StepListener stepListener; // Listener for step events to update the particle filter + private float lastEasting = 0f; // Track last easting for calculating deltas + private float lastNorthing = 0f;// Track last northing for calculating deltas + + public void setStepListener(StepListener listener) { + this.stepListener = listener; // Set the step listener for particle filter updates + } + + // END OF PARTICLE FILTER + + + private static final float ALPHA = 0.8f; private static final long LARGE_GAP_THRESHOLD_MS = 500; @@ -180,6 +197,18 @@ public void handleSensorEvent(SensorEvent sensorEvent) { SystemClock.uptimeMillis() - bootTime, newCords[0], newCords[1]); } + + // Update the particle filter with the new step data + if (stepListener != null) { + float deltaEasting = newCords[0] - lastEasting; + float deltaNorthing = newCords[1] - lastNorthing; + stepListener.onStep(deltaEasting, deltaNorthing); + } + + lastEasting = newCords[0]; + lastNorthing = newCords[1]; + + break; } } 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 36049474..8fc18d31 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -166,8 +166,9 @@ public void setContext(Context context) { this.eventHandler = new SensorEventHandler( state, pdrProcessing, pathView, recorder, bootTime); - // Register WiFi observer on WifiPositionManager (not on SensorFusion) + // Create wifiProcessor first, then register all observers on it this.wifiProcessor = new WifiDataProcessor(context); + wifiProcessor.registerObserver(wifiPositionManager); wifiProcessor.registerObserver(objects -> { new Handler(Looper.getMainLooper()).postDelayed(() -> { if (!particleFilter.isInitialized()) { @@ -210,6 +211,12 @@ public void update(Object[] objList) { "WiFi RTT is not supported on this device", Toast.LENGTH_LONG).show()); } + + eventHandler.setStepListener((deltaEasting, deltaNorthing) -> { + if (particleFilter.isInitialized()) { + particleFilter.predict(deltaEasting, deltaNorthing); + } + }); } //endregion From 52152b285e9c72b59159981716bdefceeaeaec9e Mon Sep 17 00:00:00 2001 From: JP10JOT <97289379+JapjotS@users.noreply.github.com> Date: Tue, 31 Mar 2026 15:49:33 +0100 Subject: [PATCH 06/36] Add GNSS update and init for particle filter Implement updateGNSS in ParticleFilter: convert WGS84 GNSS fix to ENU, compute Gaussian likelihood weights using provided accuracy (variance = sigma^2), normalize and perform systematic resampling, then reset weights and log the update. Integrate into SensorFusion: use GNSS fix and accuracy to initialize the particle filter on first fix or call updateGNSS on subsequent fixes. Small cleanup of commented initialization code. --- .../PositionMe/sensors/ParticleFilter.java | 67 +++++++++++++++++++ .../PositionMe/sensors/SensorFusion.java | 15 +++-- 2 files changed, 78 insertions(+), 4 deletions(-) diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java index 7e9fd13c..0b17ca47 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java @@ -69,5 +69,72 @@ public void predict(float deltaEasting, float deltaNorthing) { } + public void updateGNSS(LatLng gnssPosition, float gnssAccuracy) { + if (!initialized) return; // ignore if not initialized + + // Convert GNSS position to ENU coordinates + float[] mesurementENU = UtilFunctions.convertWGS84ToENU(origin, gnssPosition); + + float mx = mesurementENU[0]; //easting value of the measurement + float my = mesurementENU[1]; //northing value of the measurement + + float variance = gnssAccuracy * gnssAccuracy; // Convert accuracy to variance sigma^2 + + + + //gaussian likelihood function + + float weightSum = 0f; + for (int i = 0; i < NUM_PARTICLES; i++) { + float dx = particles[i][0] - mx; + float dy = particles[i][1] - my; + float distanceSquared = dx * dx + dy * dy; + + // Calculate weight using Gaussian likelihood + weights[i] = (float) Math.exp(-distanceSquared / (2 * variance)); + weightSum += weights[i]; + } + + if (weightSum < 1e-6) { + // Avoid division by zero, reinitialize weights uniformly + for (int i = 0; i < NUM_PARTICLES; i++) { + weights[i] = 1.0f / NUM_PARTICLES; + } + } else { + // Normalize weights + for (int i = 0; i < NUM_PARTICLES; i++) { + weights[i] /= weightSum; + } + } + + //resample particles + + float[][] newParticles = new float[NUM_PARTICLES][2]; // New array for resampled particles + float step = 1.0f / NUM_PARTICLES; // Step size for resampling + float cumulativeWeight = weights[0]; // Cumulative weight for resampling + + float random1 = random.nextFloat() * step; //random start point for resampling + + int j = 0; // Index for particles + for (int i = 0; i < NUM_PARTICLES; i++) { + float threshold = random1 + i * step; // Threshold for selecting particle + while (threshold > cumulativeWeight && j < NUM_PARTICLES - 1) { // Move to the next particle + j++; + + cumulativeWeight += weights[j]; // Move to next particle + } + newParticles[i][0] = particles[j][0]; // Resample east + newParticles[i][1] = particles[j][1]; // Resample north + } + particles = newParticles; // Replace old particles with resampled particles + + //reset + for (int i = 0; i < NUM_PARTICLES; i++) { + weights[i] = 1.0f / NUM_PARTICLES; // Reset weights to uniform after resampling + } + Log.d(TAG, "GNSS UPDATE" + " GNSS position: " + gnssPosition + " accuracy: " + gnssAccuracy + "m" + (mx + ", " + my)); + + } + } 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 8fc18d31..71768ae6 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -670,14 +670,21 @@ public void onLocationChanged(@NonNull Location location) { state.longitude = (float) location.getLongitude(); recorder.addGnssData(location); + LatLng GNSSPosition = new LatLng(location.getLatitude(), location.getLongitude()); + float acc = location.hasAccuracy() ? location.getAccuracy() : 20f; + //initialize particle filter automatically on first GNSS fix with accuracy as spread if (!particleFilter.isInitialized()) { - float accuracy = location.hasAccuracy() ? location.getAccuracy() : 20f; - particleFilter.initialise( - new LatLng(location.getLatitude(), location.getLongitude()),accuracy); + // float accuracy = location.hasAccuracy() ? location.getAccuracy() : 20f; + particleFilter.initialise(GNSSPosition, acc); + } else { + particleFilter.updateGNSS(GNSSPosition, acc); + } + // particleFilter.initialise( + // new LatLng(location.getLatitude(), location.getLongitude()),accuracy); } } } //endregion -} + From d962f252fd414809876172e4fce7225bb9c07d19 Mon Sep 17 00:00:00 2001 From: JP10JOT <97289379+JapjotS@users.noreply.github.com> Date: Tue, 31 Mar 2026 16:46:25 +0100 Subject: [PATCH 07/36] Integrate particle filter + WiFi/GNSS fusion Wire up a ParticleFilter-based fusion pipeline and use its fused position in the UI. Key changes: - RecordingFragment: replace commented PDR/GNSS sample code with use of sensorFusion.getFusedPosition() to update the map with the fused location and orientation. - ParticleFilter: format/cleanup, add updateWiFi(...) to weight/resample particles using a WiFi measurement, add getFusedPosition() to compute a weighted mean and convert ENU->WGS84. Implements weight normalization and resampling. - SensorFusion: instantiate a single ParticleFilter instance, hook the step listener to call predict(...), register a WiFi observer that initializes the particle filter on first WiFi fix (20m spread) and calls updateWiFi(...) afterwards, and initialize/update the particle filter on GNSS fixes (using reported accuracy or 20m default). Add a getFusedPosition() wrapper. These changes enable fused-position output from particle filtering (GNSS + WiFi + PDR predict) and simplify map updates to use the fused estimate. --- .../fragment/RecordingFragment.java | 43 +++++--- .../PositionMe/sensors/ParticleFilter.java | 101 ++++++++++++++++-- .../PositionMe/sensors/SensorFusion.java | 76 +++++++------ 3 files changed, 159 insertions(+), 61 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..0ae7a6e7 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/RecordingFragment.java +++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/RecordingFragment.java @@ -264,25 +264,34 @@ private void updateUIandPosition() { float elevationVal = sensorFusion.getElevation(); elevation.setText(getString(R.string.elevation, String.format("%.1f", elevationVal))); - // Current location - // Convert PDR coordinates to actual LatLng if you have a known starting lat/lon - // Or simply pass relative data for the TrajectoryMapFragment to handle - // For example: - float[] latLngArray = sensorFusion.getGNSSLatitude(true); - if (latLngArray != null) { - LatLng oldLocation = trajectoryMapFragment.getCurrentLocation(); // or store locally - LatLng newLocation = UtilFunctions.calculateNewPos( - oldLocation == null ? new LatLng(latLngArray[0], latLngArray[1]) : oldLocation, - new float[]{ pdrValues[0] - previousPosX, pdrValues[1] - previousPosY } - ); - - // Pass the location + orientation to the map - if (trajectoryMapFragment != null) { - trajectoryMapFragment.updateUserLocation(newLocation, - (float) Math.toDegrees(sensorFusion.passOrientation())); - } + // // Current location + // // Convert PDR coordinates to actual LatLng if you have a known starting lat/lon + // // Or simply pass relative data for the TrajectoryMapFragment to handle + // // For example: + // float[] latLngArray = sensorFusion.getGNSSLatitude(true); + // if (latLngArray != null) { + // LatLng oldLocation = trajectoryMapFragment.getCurrentLocation(); // or store locally + // LatLng newLocation = UtilFunctions.calculateNewPos( + // oldLocation == null ? new LatLng(latLngArray[0], latLngArray[1]) : oldLocation, + // new float[]{ pdrValues[0] - previousPosX, pdrValues[1] - previousPosY } + // ); + + // // Pass the location + orientation to the map + // if (trajectoryMapFragment != null) { + // trajectoryMapFragment.updateUserLocation(newLocation, + // (float) Math.toDegrees(sensorFusion.passOrientation())); + // } + // } + + // Use fused position + LatLng fusedPosition = sensorFusion.getFusedPosition(); + if (fusedPosition != null && trajectoryMapFragment != null) { + trajectoryMapFragment.updateUserLocation(fusedPosition, + (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/ParticleFilter.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java index 0b17ca47..80916189 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java @@ -12,10 +12,10 @@ public class ParticleFilter { private static final String TAG = "ParticleFilter"; private static final int NUM_PARTICLES = 200; // Number of particles - private float [] [] particles; + private float[][] particles; //[num particles][2] is {easting, northing} - private float [] weights; // Weights for each particle + private float[] weights; // Weights for each particle private LatLng origin; // Origin point for ENU conversion private boolean initialized = false; @@ -23,25 +23,26 @@ public class ParticleFilter { public void initialise(LatLng firstFix, float accuracyMeters) { if (initialized) return; // ignore subsequent calls, DONE - this.origin = firstFix; + this.origin = firstFix; this.particles = new float[NUM_PARTICLES][2]; - this.weights = new float[NUM_PARTICLES]; - float spread = accuracyMeters; + this.weights = new float[NUM_PARTICLES]; + float spread = accuracyMeters; for (int i = 0; i < NUM_PARTICLES; i++) { particles[i][0] = (float) (random.nextGaussian() * spread); // east particles[i][1] = (float) (random.nextGaussian() * spread); // north - weights[i] = 1.0f / NUM_PARTICLES; + weights[i] = 1.0f / NUM_PARTICLES; } initialized = true; Log.d(TAG, "Initialized at " + firstFix + " accuraccy is =" + accuracyMeters + "m"); } public boolean isInitialized() { - return initialized ; + return initialized; } + public LatLng getOrigin() { return origin; } @@ -81,7 +82,6 @@ public void updateGNSS(LatLng gnssPosition, float gnssAccuracy) { float variance = gnssAccuracy * gnssAccuracy; // Convert accuracy to variance sigma^2 - //gaussian likelihood function float weightSum = 0f; @@ -120,7 +120,7 @@ public void updateGNSS(LatLng gnssPosition, float gnssAccuracy) { float threshold = random1 + i * step; // Threshold for selecting particle while (threshold > cumulativeWeight && j < NUM_PARTICLES - 1) { // Move to the next particle j++; - + cumulativeWeight += weights[j]; // Move to next particle } newParticles[i][0] = particles[j][0]; // Resample east @@ -134,7 +134,88 @@ public void updateGNSS(LatLng gnssPosition, float gnssAccuracy) { } Log.d(TAG, "GNSS UPDATE" + " GNSS position: " + gnssPosition + " accuracy: " + gnssAccuracy + "m" + (mx + ", " + my)); + + } + + public void updateWiFi(LatLng wifiPosition, float wifiAccuracy) { + if (!initialized) return; // ignore if not initialized + float[] mesurementENU = UtilFunctions.convertWGS84ToENU(origin, wifiPosition); + float mx = mesurementENU[0]; //easting value of the measurement + float my = mesurementENU[1]; //northing value of the measurement + float variance = wifiAccuracy * wifiAccuracy; // Convert accuracy to variance sigma^2 + + + //gaussian likelihood function + + float weightSum = 0f; + for (int i = 0; i < NUM_PARTICLES; i++) { + float dx = particles[i][0] - mx; + float dy = particles[i][1] - my; + float distanceSquared = dx * dx + dy * dy; + + // Calculate weight using Gaussian likelihood + weights[i] = (float) Math.exp(-distanceSquared / (2 * variance)); + weightSum += weights[i]; + } + + if (weightSum < 1e-6) { + // Avoid division by zero, reinitialize weights uniformly + for (int i = 0; i < NUM_PARTICLES; i++) { + weights[i] = 1.0f / NUM_PARTICLES; + } + } else { + // Normalize weights + for (int i = 0; i < NUM_PARTICLES; i++) { + weights[i] /= weightSum; + } + } + + //resample particles + + float[][] newParticles = new float[NUM_PARTICLES][2]; // New array for resampled particles + float step = 1.0f / NUM_PARTICLES; // Step size for resampling + float cumulativeWeight = weights[0]; // Cumulative weight for resampling + + float random1 = random.nextFloat() * step; //random start point for resampling + + int j = 0; // Index for particles + for (int i = 0; i < NUM_PARTICLES; i++) { + float threshold = random1 + i * step; // Threshold for selecting particle + while (threshold > cumulativeWeight && j < NUM_PARTICLES - 1) { // Move to the next particle + j++; + + cumulativeWeight += weights[j]; // Move to next particle + } + newParticles[i][0] = particles[j][0]; // Resample east + newParticles[i][1] = particles[j][1]; // Resample north + } + particles = newParticles; // Replace old particles with resampled particles + + //reset + for (int i = 0; i < NUM_PARTICLES; i++) { + weights[i] = 1.0f / NUM_PARTICLES; // Reset weights to uniform after resampling + } + Log.d(TAG, "wifi UPDATE" + " WiFi position: " + wifiPosition + " accuracy: " + wifiAccuracy + "m" + (mx + ", " + my)); + + + } + + public LatLng getFusedPosition() { + if (!initialized) return null; // Return null if not initialized + + // Calculate weighted average of particles + float meanEasting = 0f; + float meanNorthing = 0f; + for (int i = 0; i < NUM_PARTICLES; i++) { + meanEasting += particles[i][0] * weights[i]; + meanNorthing += particles[i][1] * weights[i]; + } + + // Convert mean ENU back to WGS84 coordinates + return UtilFunctions.convertENUToWGS84(origin, new float[]{meanEasting, meanNorthing, 0f}); + + } - } + diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java index 71768ae6..743dd5a2 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -87,12 +87,15 @@ public class SensorFusion implements SensorEventListener { // PDR and path private PdrProcessing pdrProcessing; private PathView pathView; - // particle filter - private ParticleFilter particleFilter; // Sensor registration latency setting long maxReportLatencyNs = 0; + //particle filter + private final ParticleFilter particleFilter = new ParticleFilter(); + + + // Floorplan API cache (latest result from start-location step) private final Map floorplanBuildingCache = new HashMap<>(); @@ -166,19 +169,19 @@ public void setContext(Context context) { this.eventHandler = new SensorEventHandler( state, pdrProcessing, pathView, recorder, bootTime); - // Create wifiProcessor first, then register all observers on it + + //particle filter + eventHandler.setStepListener((deltaEasting, deltaNorthing) -> { + if (particleFilter.isInitialized()) { + particleFilter.predict(deltaEasting, deltaNorthing); + } + }); + + + + // Register WiFi observer on WifiPositionManager (not on SensorFusion) this.wifiProcessor = new WifiDataProcessor(context); wifiProcessor.registerObserver(wifiPositionManager); - wifiProcessor.registerObserver(objects -> { - new Handler(Looper.getMainLooper()).postDelayed(() -> { - if (!particleFilter.isInitialized()) { - LatLng wifiPosition = wifiPositionManager.getLatLngWifiPositioning();// Get WiFi position for particle filter initialization - if (wifiPosition != null) { // Check if WiFi position is available before initializing particle filter - particleFilter.initialise(wifiPosition, 20f); - } - } - }, 1000); //1second delay to allow wifi position manager to update with new scan data - }); // Initialise BLE scanner and register observer for trajectory recording this.bleProcessor = new BleDataProcessor(context); @@ -189,11 +192,8 @@ public void update(Object[] objList) { .map(o -> (BleDevice) o).collect(Collectors.toList()); recorder.addBleFingerprint(bleList); } - }); - - // Initialise WiFi RTT manager and register as WiFi scan observer this.rttManager = new RttManager(appContext, recorder, wifiProcessor); wifiProcessor.registerObserver(rttManager); @@ -202,9 +202,7 @@ public void update(Object[] objList) { this.bleRttManager = new BleRttManager(recorder); bleProcessor.registerObserver(bleRttManager); - //added particle filter - this.particleFilter = new ParticleFilter(); - + if (!rttManager.isRttSupported()) { new Handler(Looper.getMainLooper()).post(() -> Toast.makeText(appContext, @@ -212,11 +210,18 @@ public void update(Object[] objList) { Toast.LENGTH_LONG).show()); } - eventHandler.setStepListener((deltaEasting, deltaNorthing) -> { - if (particleFilter.isInitialized()) { - particleFilter.predict(deltaEasting, deltaNorthing); + wifiProcessor.registerObserver(objects -> { + new Handler(Looper.getMainLooper()).postDelayed(() -> { + LatLng wifiPosition = wifiPositionManager.getLatLngWifiPositioning(); + if (wifiPosition != null) { // Check for null to avoid errors when no WiFi position is available + if (!particleFilter.isInitialized()) { + particleFilter.initialise(wifiPosition, 20f); // Initial spread of 20 meters, can be tuned based on expected WiFi accuracy + } else { + particleFilter.updateWiFi(wifiPosition, 20f); + } } - }); + }, 1000); + }); } //endregion @@ -506,6 +511,15 @@ public float[] getGNSSLatitude(boolean start) { return latLong; } + /** + * Getter function for fused position from particle filter. + * + * @return LatLng of the current estimated position. JAPJOT + */ + public LatLng getFusedPosition(){ + return particleFilter.getFusedPosition(); + } + /** * Setter function for core location data. * @@ -670,21 +684,15 @@ public void onLocationChanged(@NonNull Location location) { state.longitude = (float) location.getLongitude(); recorder.addGnssData(location); - LatLng GNSSPosition = new LatLng(location.getLatitude(), location.getLongitude()); - float acc = location.hasAccuracy() ? location.getAccuracy() : 20f; - - //initialize particle filter automatically on first GNSS fix with accuracy as spread + LatLng gnssPos = new LatLng(location.getLatitude(), location.getLongitude()); + float accuracy = location.hasAccuracy() ? location.getAccuracy() : 20f; // Default to 20m if accuracy is unavailable if (!particleFilter.isInitialized()) { - // float accuracy = location.hasAccuracy() ? location.getAccuracy() : 20f; - particleFilter.initialise(GNSSPosition, acc); + particleFilter.initialise(gnssPos, accuracy); } else { - particleFilter.updateGNSS(GNSSPosition, acc); + particleFilter.updateGNSS(gnssPos, accuracy); } - // particleFilter.initialise( - // new LatLng(location.getLatitude(), location.getLongitude()),accuracy); - } } } //endregion - +} From cd545984eb9c35962ef0aa0b27660c1e0c4ff963 Mon Sep 17 00:00:00 2001 From: Yue Date: Tue, 31 Mar 2026 17:38:25 +0100 Subject: [PATCH 08/36] map_data_guards --- MAP_MATCHING_CONSTRAINTS.md | 56 +++++++++++++++ .../fragment/TrajectoryMapFragment.java | 34 +++++++++ .../utils/CrossFloorClassifier.java | 51 ++++++++++++++ .../PositionMe/utils/IndoorMapManager.java | 43 ++++++++++++ .../PositionMe/utils/MapMatchingConfig.java | 41 +++++++++++ .../PositionMe/utils/PdrProcessing.java | 18 +++-- .../utils/WallCollisionCorrector.java | 69 +++++++++++++++++++ .../utils/CrossFloorClassifierTest.java | 34 +++++++++ .../utils/MapMatchingConfigTest.java | 30 ++++++++ .../utils/WallCollisionCorrectorTest.java | 38 ++++++++++ gradlew | 0 11 files changed, 409 insertions(+), 5 deletions(-) create mode 100644 MAP_MATCHING_CONSTRAINTS.md create mode 100644 app/src/main/java/com/openpositioning/PositionMe/utils/CrossFloorClassifier.java create mode 100644 app/src/main/java/com/openpositioning/PositionMe/utils/MapMatchingConfig.java create mode 100644 app/src/main/java/com/openpositioning/PositionMe/utils/WallCollisionCorrector.java create mode 100644 app/src/test/java/com/openpositioning/PositionMe/utils/CrossFloorClassifierTest.java create mode 100644 app/src/test/java/com/openpositioning/PositionMe/utils/MapMatchingConfigTest.java create mode 100644 app/src/test/java/com/openpositioning/PositionMe/utils/WallCollisionCorrectorTest.java mode change 100644 => 100755 gradlew diff --git a/MAP_MATCHING_CONSTRAINTS.md b/MAP_MATCHING_CONSTRAINTS.md new file mode 100644 index 00000000..37ebad6b --- /dev/null +++ b/MAP_MATCHING_CONSTRAINTS.md @@ -0,0 +1,56 @@ +# Map Matching Constraints (Java project) + +Rules to follow when implementing or modifying indoor map matching and floor estimation. Review this file before any change touching positioning, motion model, or map data. + +## Scope +- New or changed logic for position estimation, floor estimation, trajectory correction, or sensor fusion (IMU, barometer). +- Reading, storing, or updating building map data with `wall` / `stairs` / `lift` features. + +## Core data assumptions +- Map is layered by floor; each floor has its own feature set. Feature types are limited to `wall`, `stairs`, `lift`. +- Coordinate system and units must stay consistent with existing map data (typically meters). Always state units and reference frame. +- `wall`: impassable area; corrected paths must not intersect wall geometry. +- `stairs`: combine horizontal displacement with height change; horizontal step length must be respected. +- `lift`: height change with near‑zero horizontal displacement or constrained inside the elevator shaft footprint. + +## Motion model and wall constraints +- Predict position with the motion model first; if the prediction intersects a `wall`, apply collision correction (projection/slide/constraint inside walkable polygon). +- No teleport through walls: if a step crosses a wall, truncate or slide along the wall instead of jumping past it. +- Wall handling must work with polygons/segments; do not assume rectangles. + +## Floor switching with barometer +- Allow floor changes only when the current position is near a `stairs` or `lift` feature **and** the barometric height delta exceeds the configured threshold. +- Thresholds must come from project configuration; if hardcoded, place them in a single config file and document the source. +- If no nearby cross‑floor feature exists, ignore height changes and mark the reading as low confidence instead of switching floors. + +## Distinguish lift vs stairs +- Using motion cues: + - Horizontal displacement < `liftHorizontalMax` with significant height change → lift event. + - Horizontal displacement ≥ `stairsHorizontalMin` with stair‑height accumulation → stairs event. +- Record every event with start/end time, height delta, horizontal distance, and candidate feature ID. +- When both fit, pick the type whose feature is geometrically closer. + +## Error handling and fallback +- On sensor anomalies (pressure spikes, IMU saturation), keep the last trusted state and mark the current estimate as low confidence; do not write permanent tracks. +- If map lookup fails or is missing, do not disable wall checks; provide a degraded but explainable path (motion model only, flagged unreliable). + +## Implementation requirements (Java) +- New classes/methods must have unit tests covering: wall crossing correction, floor change rejection, and lift vs stairs boundary cases. +- Use existing geometry libraries where possible; new libs require license/size review and must be recorded in dependency notes. +- Centralize parameters (e.g., `config/map_matching.properties` or DI config); no scattered magic numbers. +- Public methods must declare input/output units and coordinate frame; state whether height is relative or absolute (sea level vs floor datum). +- **Do not modify unrelated code when adding/updating map-matching logic. Keep diffs minimal and targeted.** + +## Debugging and logging +- Log at controllable levels for key decisions: wall collision handling, accepted/rejected floor switches, lift vs stairs classification. +- Logs should include floor index, position, feature ID, and summarized sensor values; avoid dumping full raw streams. + +## Change checklist (self‑review before commit) +- [ ] No path passes through walls; add regression/simulation if needed. +- [ ] Floor changes happen only near `stairs`/`lift` and when height thresholds are met. +- [ ] Lift/stairs decision is explainable via horizontal displacement or feature distance. +- [ ] All new parameters are centralized with defaults; no magic numbers. +- [ ] Unit tests cover core rules and pass. +- [ ] New or edited comments are concise, in English, and style‑consistent. + +Exceptions to these constraints must be documented in design notes and approved before merging. 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..2801ddc9 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 @@ -23,7 +23,9 @@ import com.google.android.gms.maps.OnMapReadyCallback; import com.openpositioning.PositionMe.R; import com.openpositioning.PositionMe.sensors.SensorFusion; +import com.openpositioning.PositionMe.utils.MapMatchingConfig; import com.openpositioning.PositionMe.utils.IndoorMapManager; +import com.openpositioning.PositionMe.utils.CrossFloorClassifier; import com.openpositioning.PositionMe.utils.UtilFunctions; import com.google.android.gms.maps.CameraUpdateFactory; import com.google.android.gms.maps.GoogleMap; @@ -86,6 +88,8 @@ public class TrajectoryMapFragment extends Fragment { private int lastCandidateFloor = Integer.MIN_VALUE; private long lastCandidateTime = 0; + private final MapMatchingConfig mapMatchingConfig = new MapMatchingConfig(); + // UI private Spinner switchMapSpinner; @@ -616,6 +620,8 @@ private void drawBuildingPolygon() { } //region Auto-floor logic + // Uses WiFi floor when available; otherwise barometric elevation divided by floor height (meters). + // Behavior unchanged; proximity to stairs/lift will be added in later steps. /** * Starts the periodic auto-floor evaluation task. Checks every second @@ -658,8 +664,21 @@ private void applyImmediateFloor() { } else { float elevation = sensorFusion.getElevation(); float floorHeight = indoorMapManager.getFloorHeight(); + if (floorHeight <= 0) { + // Fallback to config default if building metadata is missing + floorHeight = mapMatchingConfig.baroHeightThreshold; + } + if (Math.abs(elevation) < mapMatchingConfig.baroHeightThreshold) { + return; // Ignore small height changes + } if (floorHeight <= 0) return; candidateFloor = Math.round(elevation / floorHeight); + + // Require proximity to stairs/lift when using barometer path + boolean nearFeature = indoorMapManager.isNearCrossFloorFeature(mapMatchingConfig.crossFeatureProximity); + if (!nearFeature) { + return; + } } indoorMapManager.setCurrentFloor(candidateFloor, true); @@ -699,10 +718,25 @@ private void evaluateAutoFloor() { // Fallback: barometric elevation estimate float elevation = sensorFusion.getElevation(); float floorHeight = indoorMapManager.getFloorHeight(); + if (floorHeight <= 0) { + // Fallback to config default if building metadata is missing + floorHeight = mapMatchingConfig.baroHeightThreshold; + } + if (Math.abs(elevation) < mapMatchingConfig.baroHeightThreshold) { + return; // Ignore small height changes + } if (floorHeight <= 0) return; candidateFloor = Math.round(elevation / floorHeight); } + // Require proximity to stairs/lift when using barometer path + if (sensorFusion.getLatLngWifiPositioning() == null) { + boolean nearFeature = indoorMapManager.isNearCrossFloorFeature(mapMatchingConfig.crossFeatureProximity); + if (!nearFeature) { + return; + } + } + // Debounce: require the same floor reading for AUTO_FLOOR_DEBOUNCE_MS long now = SystemClock.elapsedRealtime(); if (candidateFloor != lastCandidateFloor) { diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/CrossFloorClassifier.java b/app/src/main/java/com/openpositioning/PositionMe/utils/CrossFloorClassifier.java new file mode 100644 index 00000000..c1d40304 --- /dev/null +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/CrossFloorClassifier.java @@ -0,0 +1,51 @@ +package com.openpositioning.PositionMe.utils; + +/** + * Classifies cross-floor movements into lift or stairs based on displacement and height change. + * Pure function: no side effects, safe for unit testing and future integration. + */ +public final class CrossFloorClassifier { + + public enum Mode { + LIFT, + STAIRS, + UNKNOWN + } + + private CrossFloorClassifier() { + // Utility class + } + + /** + * Decide whether a cross-floor movement is a lift or stairs event. + * + * @param horizontalDelta horizontal displacement in meters. + * @param heightDelta vertical displacement in meters (absolute value is considered). + * @param featureDistance distance to the nearest cross-floor feature in meters (unused for now, + * reserved for tie-breaks). + * @param config thresholds used for classification. + * @return mode describing the movement. + */ + public static Mode classify(double horizontalDelta, + double heightDelta, + double featureDistance, + MapMatchingConfig config) { + double absHeight = Math.abs(heightDelta); + double absHorizontal = Math.abs(horizontalDelta); + + boolean heightSignificant = absHeight >= config.baroHeightThreshold; + if (!heightSignificant) { + return Mode.UNKNOWN; + } + + if (absHorizontal < config.liftHorizontalMax) { + return Mode.LIFT; + } + + if (absHorizontal >= config.stairsHorizontalMin) { + return Mode.STAIRS; + } + + return Mode.UNKNOWN; + } +} diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java b/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java index f8058603..7b2383b0 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java @@ -40,6 +40,7 @@ public class IndoorMapManager { private boolean isIndoorMapSet = false; private int currentFloor; private int currentBuilding = BUILDING_NONE; + // Floor height in meters derived from building metadata private float floorHeight; // Vector shapes currently drawn on the map (cleared on floor switch or exit) @@ -60,6 +61,9 @@ public class IndoorMapManager { private static final int ROOM_FILL = Color.argb(40, 33, 150, 243); private static final int DEFAULT_STROKE = Color.argb(150, 100, 100, 100); + // Last known user location (lat/lng on map) + private LatLng lastLocation; + /** * Constructor to set the map instance. * @@ -77,9 +81,15 @@ public IndoorMapManager(GoogleMap map) { */ public void setCurrentLocation(LatLng currentLocation) { this.currentLocation = currentLocation; + this.lastLocation = currentLocation; setBuildingOverlay(); } + /** Returns last location stored via setCurrentLocation (may be null). */ + public LatLng getLastLocation() { + return lastLocation; + } + /** * Returns the current building's floor height. * @@ -117,6 +127,39 @@ public int getCurrentFloor() { return currentFloor; } + /** + * Returns true if the last known location is within the given radius (meters) + * of any stairs or lift feature on the current floor. + * + * @param radiusMeters proximity threshold in meters + * @return true when near stairs/lift; false otherwise or when data missing + */ + public boolean isNearCrossFloorFeature(float radiusMeters) { + if (lastLocation == null || currentFloorShapes == null) return false; + if (currentFloor < 0 || currentFloor >= currentFloorShapes.size()) return false; + + FloorplanApiClient.FloorShapes floor = currentFloorShapes.get(currentFloor); + if (floor == null || floor.getFeatures() == null) return false; + + double minDistance = Double.MAX_VALUE; + for (FloorplanApiClient.MapShapeFeature feature : floor.getFeatures()) { + String type = feature.getIndoorType(); + if (!"stairs".equals(type) && !"lift".equals(type)) continue; + for (List part : feature.getParts()) { + for (LatLng point : part) { + double d = UtilFunctions.distanceBetweenPoints(lastLocation, point); + if (d < minDistance) { + minDistance = d; + } + if (minDistance <= radiusMeters) { + return true; + } + } + } + } + return false; + } + /** * Returns the display name for the current floor (e.g. "LG", "G", "1"). * Falls back to the numeric index if no display name is available. diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/MapMatchingConfig.java b/app/src/main/java/com/openpositioning/PositionMe/utils/MapMatchingConfig.java new file mode 100644 index 00000000..b8ff4dc0 --- /dev/null +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/MapMatchingConfig.java @@ -0,0 +1,41 @@ +package com.openpositioning.PositionMe.utils; + +/** + * Centralized defaults for map-matching parameters. + * Values are in meters unless stated otherwise. + * No behavior change is made by instantiating this class; it is a holder for later tuning. + */ +public class MapMatchingConfig { + + /** Minimum vertical delta from barometer to consider a floor change (meters). */ + public final float baroHeightThreshold; + + /** Maximum horizontal displacement to still classify a cross-floor move as lift (meters). */ + public final float liftHorizontalMax; + + /** Minimum horizontal displacement to classify a cross-floor move as stairs (meters). */ + public final float stairsHorizontalMin; + + /** Padding distance to keep corrected paths away from walls (meters). */ + public final float wallPadding; + + /** Proximity radius to accept cross-floor features (meters). */ + public final float crossFeatureProximity; + + public MapMatchingConfig() { + // Defaults chosen to align with current app behavior (floor height ~4m, small wall padding) + this(4.0f, 1.5f, 2.0f, 0.2f, 5.0f); + } + + public MapMatchingConfig(float baroHeightThreshold, + float liftHorizontalMax, + float stairsHorizontalMin, + float wallPadding, + float crossFeatureProximity) { + this.baroHeightThreshold = baroHeightThreshold; + this.liftHorizontalMax = liftHorizontalMax; + this.stairsHorizontalMin = stairsHorizontalMin; + this.wallPadding = wallPadding; + this.crossFeatureProximity = crossFeatureProximity; + } +} 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..093ceaa8 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java @@ -17,9 +17,8 @@ /** * Processes data recorded in the {@link SensorFusion} class and calculates live PDR estimates. - * It calculates the position from the steps and directions detected, using either estimated values - * (eg. stride length from the Weiberg algorithm) or provided constants, calculates the elevation - * and attempts to estimate the current floor as well as elevators. + * Coordinates are relative to the session start, in meters (x east, y north). Elevation is + * relative to the initial barometer median. Attempts to estimate current floor and elevator use. * * @author Mate Stodulka * @author Michal Dvorak @@ -44,6 +43,9 @@ public class PdrProcessing { // Settings for accessing shared variables private SharedPreferences settings; + // Centralized map-matching thresholds (currently informational only) + private final MapMatchingConfig mapMatchingConfig; + // Step length private float stepLength; // Using manually input constants instead of estimated values @@ -58,6 +60,7 @@ public class PdrProcessing { private float startElevation; private int setupIndex = 0; private float elevation; + // Floor-to-floor height in meters (manual setting) private int floorHeight; private int currentFloor; @@ -80,8 +83,13 @@ public class PdrProcessing { * @param context Application context for variable access. */ public PdrProcessing(Context context) { + this(context, new MapMatchingConfig()); + } + + public PdrProcessing(Context context, MapMatchingConfig mapMatchingConfig) { // Initialise settings this.settings = PreferenceManager.getDefaultSharedPreferences(context); + this.mapMatchingConfig = mapMatchingConfig; // Check if estimate or manual values should be used this.useManualStep = this.settings.getBoolean("manual_step_values", false); if(useManualStep) { @@ -123,7 +131,7 @@ public PdrProcessing(Context context) { } // Distance between floors is building dependent, use manual value - this.floorHeight = settings.getInt("floor_height", 4); + this.floorHeight = settings.getInt("floor_height", (int) mapMatchingConfig.baroHeightThreshold); // Array for holding initial values this.startElevationBuffer = new Float[3]; // Start floor - assumed to be zero @@ -390,7 +398,7 @@ public void resetPDR() { } // Distance between floors is building dependent, use manual value - this.floorHeight = settings.getInt("floor_height", 4); + this.floorHeight = settings.getInt("floor_height", (int) mapMatchingConfig.baroHeightThreshold); // Array for holding initial values this.startElevationBuffer = new Float[3]; // Start floor - assumed to be zero diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/WallCollisionCorrector.java b/app/src/main/java/com/openpositioning/PositionMe/utils/WallCollisionCorrector.java new file mode 100644 index 00000000..cd4662ae --- /dev/null +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/WallCollisionCorrector.java @@ -0,0 +1,69 @@ +package com.openpositioning.PositionMe.utils; + +import android.graphics.PointF; + +import java.util.List; + +/** + * Adjusts proposed 2D steps to avoid crossing walls. + * Uses simple segment intersection: if the step crosses any wall segment, movement is cancelled. + */ +public final class WallCollisionCorrector { + + private WallCollisionCorrector() { + // Utility class + } + + /** + * Returns a corrected point that avoids crossing wall segments. + * + * @param previous previous position (meters). + * @param candidate proposed next position (meters). + * @param walls list of wall polylines; each polyline is a list of points in meters. + * @return corrected point (candidate or previous when blocked). + */ + public static PointF correct(PointF previous, + PointF candidate, + List> walls) { + if (walls == null || walls.isEmpty()) { + return candidate; + } + for (List wall : walls) { + if (wall == null || wall.size() < 2) continue; + for (int i = 0; i < wall.size() - 1; i++) { + PointF a = wall.get(i); + PointF b = wall.get(i + 1); + if (segmentsIntersect(previous, candidate, a, b)) { + // Block movement; return previous position + return previous; + } + } + } + return candidate; + } + + private static boolean segmentsIntersect(PointF p1, PointF p2, PointF q1, PointF q2) { + float d1 = direction(q1, q2, p1); + float d2 = direction(q1, q2, p2); + float d3 = direction(p1, p2, q1); + float d4 = direction(p1, p2, q2); + + if (((d1 > 0 && d2 < 0) || (d1 < 0 && d2 > 0)) && + ((d3 > 0 && d4 < 0) || (d3 < 0 && d4 > 0))) { + return true; + } + return d1 == 0 && onSegment(q1, q2, p1) + || d2 == 0 && onSegment(q1, q2, p2) + || d3 == 0 && onSegment(p1, p2, q1) + || d4 == 0 && onSegment(p1, p2, q2); + } + + private static float direction(PointF a, PointF b, PointF c) { + return (b.x - a.x) * (c.y - a.y) - (b.y - a.y) * (c.x - a.x); + } + + private static boolean onSegment(PointF a, PointF b, PointF c) { + return Math.min(a.x, b.x) <= c.x && c.x <= Math.max(a.x, b.x) + && Math.min(a.y, b.y) <= c.y && c.y <= Math.max(a.y, b.y); + } +} diff --git a/app/src/test/java/com/openpositioning/PositionMe/utils/CrossFloorClassifierTest.java b/app/src/test/java/com/openpositioning/PositionMe/utils/CrossFloorClassifierTest.java new file mode 100644 index 00000000..a7f9c042 --- /dev/null +++ b/app/src/test/java/com/openpositioning/PositionMe/utils/CrossFloorClassifierTest.java @@ -0,0 +1,34 @@ +package com.openpositioning.PositionMe.utils; + +import org.junit.Test; + +import static org.junit.Assert.assertEquals; + +public class CrossFloorClassifierTest { + + private final MapMatchingConfig config = new MapMatchingConfig(4.0f, 1.5f, 2.0f, 0.2f, 5.0f); + + @Test + public void classifyReturnsLiftWhenHorizontalIsSmallAndHeightBig() { + CrossFloorClassifier.Mode mode = CrossFloorClassifier.classify(0.5, 4.5, 0.0, config); + assertEquals(CrossFloorClassifier.Mode.LIFT, mode); + } + + @Test + public void classifyReturnsStairsWhenHorizontalIsLargeAndHeightBig() { + CrossFloorClassifier.Mode mode = CrossFloorClassifier.classify(3.0, 4.5, 0.0, config); + assertEquals(CrossFloorClassifier.Mode.STAIRS, mode); + } + + @Test + public void classifyReturnsUnknownWhenHeightIsTooSmall() { + CrossFloorClassifier.Mode mode = CrossFloorClassifier.classify(0.5, 1.0, 0.0, config); + assertEquals(CrossFloorClassifier.Mode.UNKNOWN, mode); + } + + @Test + public void classifyReturnsUnknownWhenBetweenLiftAndStairsThresholds() { + CrossFloorClassifier.Mode mode = CrossFloorClassifier.classify(1.6, 5.0, 0.0, config); + assertEquals(CrossFloorClassifier.Mode.UNKNOWN, mode); + } +} diff --git a/app/src/test/java/com/openpositioning/PositionMe/utils/MapMatchingConfigTest.java b/app/src/test/java/com/openpositioning/PositionMe/utils/MapMatchingConfigTest.java new file mode 100644 index 00000000..cf5f3190 --- /dev/null +++ b/app/src/test/java/com/openpositioning/PositionMe/utils/MapMatchingConfigTest.java @@ -0,0 +1,30 @@ +package com.openpositioning.PositionMe.utils; + +import org.junit.Test; + +import static org.junit.Assert.assertEquals; + +public class MapMatchingConfigTest { + + @Test + public void defaultsAreStable() { + MapMatchingConfig config = new MapMatchingConfig(); + + assertEquals(4.0f, config.baroHeightThreshold, 0.0001f); + assertEquals(1.5f, config.liftHorizontalMax, 0.0001f); + assertEquals(2.0f, config.stairsHorizontalMin, 0.0001f); + assertEquals(0.2f, config.wallPadding, 0.0001f); + assertEquals(5.0f, config.crossFeatureProximity, 0.0001f); + } + + @Test + public void customValuesAreApplied() { + MapMatchingConfig config = new MapMatchingConfig(3.0f, 1.0f, 2.5f, 0.1f, 7.0f); + + assertEquals(3.0f, config.baroHeightThreshold, 0.0001f); + assertEquals(1.0f, config.liftHorizontalMax, 0.0001f); + assertEquals(2.5f, config.stairsHorizontalMin, 0.0001f); + assertEquals(0.1f, config.wallPadding, 0.0001f); + assertEquals(7.0f, config.crossFeatureProximity, 0.0001f); + } +} diff --git a/app/src/test/java/com/openpositioning/PositionMe/utils/WallCollisionCorrectorTest.java b/app/src/test/java/com/openpositioning/PositionMe/utils/WallCollisionCorrectorTest.java new file mode 100644 index 00000000..5f71a94c --- /dev/null +++ b/app/src/test/java/com/openpositioning/PositionMe/utils/WallCollisionCorrectorTest.java @@ -0,0 +1,38 @@ +package com.openpositioning.PositionMe.utils; + +import android.graphics.PointF; + +import org.junit.Test; + +import java.util.Arrays; +import java.util.List; + +import static org.junit.Assert.assertEquals; +import static org.junit.Assert.assertSame; + +public class WallCollisionCorrectorTest { + + @Test + public void stubCorrectionReturnsCandidate() { + PointF prev = new PointF(0f, 0f); + PointF cand = new PointF(5f, 5f); + PointF w1 = new PointF(10f, 0f); + PointF w2 = new PointF(10f, 10f); + + PointF corrected = WallCollisionCorrector.correct(prev, cand, Arrays.asList(Arrays.asList(w1, w2))); + + assertEquals(cand.x, corrected.x, 0.0001f); + assertEquals(cand.y, corrected.y, 0.0001f); + } + + @Test + public void crossingWallReturnsPrevious() { + PointF prev = new PointF(0f, 0f); + PointF cand = new PointF(10f, 0f); + List wall = Arrays.asList(new PointF(5f, -5f), new PointF(5f, 5f)); + + PointF corrected = WallCollisionCorrector.correct(prev, cand, Arrays.asList(wall)); + + assertSame(prev, corrected); + } +} diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 From 37d4a8da4b6f326c44d186a0d181365131c5fb91 Mon Sep 17 00:00:00 2001 From: Yue Date: Tue, 31 Mar 2026 18:13:02 +0100 Subject: [PATCH 09/36] map_matching_1 --- MAP_MATCHING_CONSTRAINTS.md | 1 + .../fragment/TrajectoryMapFragment.java | 39 ++++++++++++++++--- .../PositionMe/sensors/SensorFusion.java | 11 ++++++ .../PositionMe/utils/IndoorMapManager.java | 9 +++++ .../PositionMe/utils/PdrProcessing.java | 24 ++++++++++-- 5 files changed, 75 insertions(+), 9 deletions(-) diff --git a/MAP_MATCHING_CONSTRAINTS.md b/MAP_MATCHING_CONSTRAINTS.md index 37ebad6b..fc0be0be 100644 --- a/MAP_MATCHING_CONSTRAINTS.md +++ b/MAP_MATCHING_CONSTRAINTS.md @@ -52,5 +52,6 @@ Rules to follow when implementing or modifying indoor map matching and floor est - [ ] All new parameters are centralized with defaults; no magic numbers. - [ ] Unit tests cover core rules and pass. - [ ] New or edited comments are concise, in English, and style‑consistent. +- [ ] Update `MAP_MATCHING_CHANGELOG.md` with added/modified files and open tasks whenever map-matching code changes. Exceptions to these constraints must be documented in design notes and approved before merging. 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 2801ddc9..7feede72 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 @@ -22,11 +22,15 @@ import com.google.android.gms.maps.OnMapReadyCallback; import com.openpositioning.PositionMe.R; +import com.openpositioning.PositionMe.data.remote.FloorplanApiClient; import com.openpositioning.PositionMe.sensors.SensorFusion; import com.openpositioning.PositionMe.utils.MapMatchingConfig; import com.openpositioning.PositionMe.utils.IndoorMapManager; import com.openpositioning.PositionMe.utils.CrossFloorClassifier; +import com.openpositioning.PositionMe.utils.WallGeometryBuilder; import com.openpositioning.PositionMe.utils.UtilFunctions; +import android.graphics.PointF; +import java.util.List; import com.google.android.gms.maps.CameraUpdateFactory; import com.google.android.gms.maps.GoogleMap; import com.google.android.gms.maps.SupportMapFragment; @@ -658,6 +662,8 @@ private void applyImmediateFloor() { if (sensorFusion == null || indoorMapManager == null) return; if (!indoorMapManager.getIsIndoorMapSet()) return; + updateWallsForPdr(); + int candidateFloor; if (sensorFusion.getLatLngWifiPositioning() != null) { candidateFloor = sensorFusion.getWifiFloor(); @@ -679,6 +685,11 @@ private void applyImmediateFloor() { if (!nearFeature) { return; } + + CrossFloorClassifier.Mode mode = + CrossFloorClassifier.classify(0.0, elevation, 0.0, mapMatchingConfig); + Log.d(TAG, "Auto-floor (baro) mode=" + mode + " elevation=" + elevation + + " floorHeight=" + floorHeight); } indoorMapManager.setCurrentFloor(candidateFloor, true); @@ -709,6 +720,8 @@ private void evaluateAutoFloor() { if (sensorFusion == null || indoorMapManager == null) return; if (!indoorMapManager.getIsIndoorMapSet()) return; + updateWallsForPdr(); + int candidateFloor; // Priority 1: WiFi-based floor (only if WiFi positioning has returned data) @@ -725,16 +738,17 @@ private void evaluateAutoFloor() { if (Math.abs(elevation) < mapMatchingConfig.baroHeightThreshold) { return; // Ignore small height changes } - if (floorHeight <= 0) return; - candidateFloor = Math.round(elevation / floorHeight); - } - - // Require proximity to stairs/lift when using barometer path - if (sensorFusion.getLatLngWifiPositioning() == null) { boolean nearFeature = indoorMapManager.isNearCrossFloorFeature(mapMatchingConfig.crossFeatureProximity); if (!nearFeature) { return; } + if (floorHeight <= 0) return; + candidateFloor = Math.round(elevation / floorHeight); + + CrossFloorClassifier.Mode mode = + CrossFloorClassifier.classify(0.0, elevation, 0.0, mapMatchingConfig); + Log.d(TAG, "Auto-floor (baro) mode=" + mode + " elevation=" + elevation + + " floorHeight=" + floorHeight); } // Debounce: require the same floor reading for AUTO_FLOOR_DEBOUNCE_MS @@ -754,4 +768,17 @@ private void evaluateAutoFloor() { } //endregion + + private void updateWallsForPdr() { + if (sensorFusion == null || indoorMapManager == null) return; + if (!indoorMapManager.getIsIndoorMapSet()) return; + if (indoorMapManager.getLastLocation() == null) return; + + FloorplanApiClient.FloorShapes floor = indoorMapManager.getCurrentFloorShape(); + if (floor == null) return; + + List> walls = WallGeometryBuilder.buildWalls( + floor, indoorMapManager.getLastLocation()); + sensorFusion.setPdrWalls(walls); + } } 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 743dd5a2..0db85113 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -24,6 +24,8 @@ import com.openpositioning.PositionMe.utils.PdrProcessing; import com.openpositioning.PositionMe.utils.TrajectoryValidator; import com.openpositioning.PositionMe.data.remote.ServerCommunications; +import com.openpositioning.PositionMe.utils.WallGeometryBuilder; +import android.graphics.PointF; import java.util.ArrayList; import java.util.HashMap; @@ -427,6 +429,15 @@ public void setSelectedBuildingId(String buildingId) { recorder.setSelectedBuildingId(buildingId); } + /** + * Inject wall polylines (meters) into PDR for collision correction. + */ + public void setPdrWalls(List> walls) { + if (pdrProcessing != null) { + pdrProcessing.setWalls(walls); + } + } + /** * Gets the selected building identifier for the current recording session. * diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java b/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java index 7b2383b0..262e2546 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java @@ -127,6 +127,15 @@ public int getCurrentFloor() { return currentFloor; } + /** + * Returns the current floor shapes object, or null if unavailable. + */ + public FloorplanApiClient.FloorShapes getCurrentFloorShape() { + if (currentFloorShapes == null) return null; + if (currentFloor < 0 || currentFloor >= currentFloorShapes.size()) return null; + return currentFloorShapes.get(currentFloor); + } + /** * Returns true if the last known location is within the given radius (meters) * of any stairs or lift feature on the current floor. 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 093ceaa8..87510104 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java @@ -2,6 +2,7 @@ import android.content.Context; import android.content.SharedPreferences; +import android.graphics.PointF; import android.hardware.SensorManager; import androidx.preference.PreferenceManager; @@ -10,10 +11,10 @@ import java.util.Arrays; import java.util.Collections; -import java.util.List; import java.util.OptionalDouble; import java.util.Objects; import java.util.stream.Collectors; +import java.util.List; /** * Processes data recorded in the {@link SensorFusion} class and calculates live PDR estimates. @@ -55,6 +56,9 @@ public class PdrProcessing { private float positionX; private float positionY; + // Optional wall geometry in meters (polylines) + private List> walls; + // Vertical movement calculation private Float[] startElevationBuffer; private float startElevation; @@ -178,9 +182,16 @@ public float[] updatePdr(long currentStepEnd, List accelMagnitudeOvertim float x = (float) (stepLength * Math.cos(adaptedHeading)); float y = (float) (stepLength * Math.sin(adaptedHeading)); + // Apply wall collision correction if walls are provided + PointF previous = new PointF(this.positionX, this.positionY); + PointF candidate = new PointF(this.positionX + x, this.positionY + y); + if (walls != null && !walls.isEmpty()) { + candidate = WallCollisionCorrector.correct(previous, candidate, walls); + } + // Update position values - this.positionX += x; - this.positionY += y; + this.positionX = candidate.x; + this.positionY = candidate.y; // return current position return new float[]{this.positionX, this.positionY}; @@ -422,4 +433,11 @@ public float getAverageStepLength(){ return averageStepLength; } + /** + * Inject wall polylines (meters) for collision correction. Optional. + */ + public void setWalls(List> walls) { + this.walls = walls; + } + } From 00436a5041cf6fdfa6586d3a69b363d9f760ffb8 Mon Sep 17 00:00:00 2001 From: gayathrikjp Date: Wed, 1 Apr 2026 08:13:25 +0100 Subject: [PATCH 10/36] Data_Display_wifi_not_working All working except wifi. and will change the UI a little bit to make it neater --- .../fragment/RecordingFragment.java | 104 +++++- .../fragment/TrajectoryMapFragment.java | 328 +++++++++++++++++- .../sensors/WifiPositionManager.java | 2 + .../res/layout/fragment_trajectory_map.xml | 24 +- app/src/main/res/values/strings.xml | 4 +- 5 files changed, 443 insertions(+), 19 deletions(-) diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/RecordingFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/RecordingFragment.java index 0ae7a6e7..06b5d5b9 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; @@ -87,6 +88,18 @@ public class RecordingFragment extends Fragment { // References to the child map fragment private TrajectoryMapFragment trajectoryMapFragment; + // Update fused trajectory at least every 1 second + private static final long FUSED_UPDATE_INTERVAL_MS = 1000; + + // Also update immediately when movement exceeds this threshold + private static final double MOVEMENT_THRESHOLD_METERS = 0.75; + + // Last time a fused point was sent to the map + private long lastFusedUpdateTimeMs = 0L; + + // Last fused point that was actually rendered + private LatLng lastSentFusedPosition = null; + private final Runnable refreshDataTask = new Runnable() { @Override public void run() { @@ -228,7 +241,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; } @@ -283,11 +296,85 @@ private void updateUIandPosition() { // } // } - // Use fused position + // Get the latest fused position from SensorFusion (best estimate of user location) LatLng fusedPosition = sensorFusion.getFusedPosition(); + + // Only proceed if: + // 1. We have a valid fused position + // 2. The map fragment is ready to receive updates if (fusedPosition != null && trajectoryMapFragment != null) { - trajectoryMapFragment.updateUserLocation(fusedPosition, - (float) Math.toDegrees(sensorFusion.passOrientation())); + + // Get the current system time in milliseconds + long now = System.currentTimeMillis(); + + // --- CONDITION 1: First point --- + // If no previous fused point has been sent to the map, + // this is the very first update → must display it + boolean isFirstPoint = (lastSentFusedPosition == null); + + // --- CONDITION 2: Time-based update --- + // Check if at least 1 second has passed since the last update + boolean oneSecondElapsed = + (now - lastFusedUpdateTimeMs) >= FUSED_UPDATE_INTERVAL_MS; + + // --- CONDITION 3: Movement-based update --- + // Check if the user has moved a meaningful distance + boolean movementDetected = false; + + // Store how far the user moved (for debugging/logging) + double movedDistance = 0.0; + + // Only compute movement if we have a previous point + if (lastSentFusedPosition != null) { + + // Calculate distance between last displayed point and current fused position + movedDistance = UtilFunctions.distanceBetweenPoints( + lastSentFusedPosition, + fusedPosition + ); + + // If movement exceeds threshold (e.g., 0.75m), trigger update + movementDetected = movedDistance >= MOVEMENT_THRESHOLD_METERS; + } + + // --- FINAL DECISION --- + // Update the map if ANY of the following is true: + // - first point + // - 1 second passed + // - significant movement detected + if (isFirstPoint || oneSecondElapsed || movementDetected) { + + // Log that we are updating the map (useful for debugging) + Log.d("FUSED_TEST", "MAP UPDATE -> " + + "first=" + isFirstPoint + + ", oneSecondElapsed=" + oneSecondElapsed + + ", movementDetected=" + movementDetected + + ", movedDistance=" + movedDistance + + ", lat=" + fusedPosition.latitude + + ", lng=" + fusedPosition.longitude); + + // Send the fused position to the map: + // - updates marker position + // - rotates marker (orientation) + // - extends trajectory polyline + trajectoryMapFragment.updateUserLocation( + fusedPosition, + (float) Math.toDegrees(sensorFusion.passOrientation()) + ); + + // Save this position as the last displayed one + lastSentFusedPosition = fusedPosition; + + // Save the time of this update + lastFusedUpdateTimeMs = now; + + } else { + + // Log that this update was skipped (for debugging performance) + Log.d("FUSED_TEST", "SKIPPED -> " + + "movedDistance=" + movedDistance + + ", elapsedMs=" + (now - lastFusedUpdateTimeMs)); + } } @@ -311,6 +398,15 @@ private void updateUIandPosition() { } } + // WiFi observation logic for colour-coded last N updates + if (trajectoryMapFragment != null) { + LatLng wifiLocation = sensorFusion.getLatLngWifiPositioning(); + Log.d("WiFiDebug", "RecordingFragment wifiLocation = " + wifiLocation); + if (wifiLocation != null) { + trajectoryMapFragment.updateWiFiObservation(wifiLocation); + } + } + // Update previous previousPosX = pdrValues[0]; previousPosY = pdrValues[1]; diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java index 479ea51b..dea74ff3 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 @@ -33,6 +33,9 @@ import java.util.ArrayList; import java.util.List; +import com.google.android.gms.maps.model.Circle; +import com.google.android.gms.maps.model.CircleOptions; + /** * A fragment responsible for displaying a trajectory map using Google Maps. @@ -92,11 +95,74 @@ public class TrajectoryMapFragment extends Fragment { private SwitchMaterial gnssSwitch; private SwitchMaterial autoFloorSwitch; + private SwitchMaterial smoothingSwitch; + private com.google.android.material.floatingactionbutton.FloatingActionButton floorUpButton, floorDownButton; private TextView floorLabel; private Button switchColorButton; private Polygon buildingPolygon; + // --- Last N observation display state --- + private static final int MAX_OBSERVATIONS = 20; + + // Rolling histories of absolute position updates + private final List gnssHistory = new ArrayList<>(); + private final List wifiHistory = new ArrayList<>(); + private final List pdrHistory = new ArrayList<>(); + + // Rendered map circles for each source, so they can be removed/redrawn cleanly + private final List gnssCircles = new ArrayList<>(); + private final List wifiCircles = new ArrayList<>(); + private final List pdrCircles = new ArrayList<>(); + + // Optional UI switches for visibility control + private SwitchMaterial wifiSwitch; + private SwitchMaterial pdrSwitch; + + // --- Display smoothing state --- + + // Types of smoothing filters available for display + private enum SmoothingType { + RAW, // No smoothing + MOVING_AVERAGE, // Average over last N points + EXPONENTIAL // Exponential smoothing + } + private Spinner smoothingSpinner; + + // Current selected smoothing mode (default = RAW) + private SmoothingType smoothingType = SmoothingType.RAW; + + // For exponential smoothing + private LatLng smoothedDisplayLocation = null; + + // For moving average + private static final int SMOOTHING_WINDOW = 5; + private final List smoothingBuffer = new ArrayList<>(); + + // Exponential smoothing strength + private static final double ALPHA = 0.25; + + /** + * Adds a new position observation to a rolling history list. + * Maintains only the most recent MAX_OBSERVATIONS points. + * + * @param history The list storing past observations (GNSS, WiFi, or PDR) + * @param point The new LatLng position to add + */ + private void addObservation(List history, LatLng point) { + + // Ignore null points (e.g., when a sensor has no valid reading) + if (point == null) return; + + // Add the new observation to the history + history.add(point); + + // If we exceed the maximum allowed observations, + // remove the oldest point (FIFO behaviour) + if (history.size() > MAX_OBSERVATIONS) { + history.remove(0); + } + } public TrajectoryMapFragment() { // Required empty public constructor @@ -119,6 +185,9 @@ public void onViewCreated(@NonNull View view, // Grab references to UI controls switchMapSpinner = view.findViewById(R.id.mapSwitchSpinner); gnssSwitch = view.findViewById(R.id.gnssSwitch); + wifiSwitch = view.findViewById(R.id.wifiSwitch); + pdrSwitch = view.findViewById(R.id.pdrSwitch); + smoothingSpinner = view.findViewById(R.id.smoothingSpinner); autoFloorSwitch = view.findViewById(R.id.autoFloor); floorUpButton = view.findViewById(R.id.floorUpButton); floorDownButton = view.findViewById(R.id.floorDownButton); @@ -155,7 +224,8 @@ public void onMapReady(@NonNull GoogleMap googleMap) { } }); } - + // Smoothing type spinner setup + initSmoothingSpinner(); // Map type spinner setup initMapTypeSpinner(); @@ -166,8 +236,18 @@ public void onMapReady(@NonNull GoogleMap googleMap) { gnssMarker.remove(); gnssMarker = null; } + redrawObservationOverlays(); + }); + + wifiSwitch.setOnCheckedChangeListener((buttonView, isChecked) -> { + redrawObservationOverlays(); + }); + + pdrSwitch.setOnCheckedChangeListener((buttonView, isChecked) -> { + redrawObservationOverlays(); }); + // Color switch switchColorButton.setOnClickListener(v -> { if (polyline != null) { @@ -211,6 +291,77 @@ public void onMapReady(@NonNull GoogleMap googleMap) { }); } + /** + * Redraws the colour-coded observation circles for GNSS, WiFi, and PDR. + * Only sources enabled by their switches are displayed. + */ + private void redrawObservationOverlays() { + if (gMap == null) return; + + clearObservationCircles(); + + if (gnssSwitch != null && gnssSwitch.isChecked()) { + drawHistory(gnssHistory, gnssCircles, Color.BLUE); + } + + if (wifiSwitch != null && wifiSwitch.isChecked()) { + drawHistory(wifiHistory, wifiCircles, Color.GREEN); + } + + if (pdrSwitch != null && pdrSwitch.isChecked()) { + drawHistory(pdrHistory, pdrCircles, Color.RED); + } + } + + /** + * Draws one rolling history of observations on the map as circles. + * Older observations are faded, newer ones are more visible. + * + * @param history The observation points to render + * @param rendered The list of Circle references currently on the map + * @param color The base colour for this data source + */ + private void drawHistory(List history, List rendered, int color) { + for (int i = 0; i < history.size(); i++) { + LatLng point = history.get(i); + + int alpha = (int) (255f * (i + 1) / history.size()); + int fadedColor = Color.argb(alpha, Color.red(color), Color.green(color), Color.blue(color)); + + Circle circle = gMap.addCircle(new CircleOptions() + .center(point) + .radius(1.5) + .strokeWidth(2f) + .strokeColor(fadedColor) + .fillColor(fadedColor)); + + rendered.add(circle); + } + } + + /** + * Removes all currently displayed observation circles from the map. + */ + private void clearObservationCircles() { + removeAll(gnssCircles); + removeAll(wifiCircles); + removeAll(pdrCircles); + } + + /** + * Removes every circle in the provided list from the map and clears the list. + * + * @param circles The rendered circles to remove + */ + private void removeAll(List circles) { + for (Circle circle : circles) { + if (circle != null) { + circle.remove(); + } + } + circles.clear(); + } + /** * Initialize the map settings with the provided GoogleMap instance. *

@@ -300,6 +451,135 @@ public void onNothingSelected(AdapterView parent) {} }); } + /** + * Initializes the smoothing filter spinner. + * Allows user to select how trajectory is smoothed. + */ + private void initSmoothingSpinner() { + + String[] options = new String[]{ + "Raw", + "Moving Average", + "Exponential" + }; + + ArrayAdapter adapter = new ArrayAdapter<>( + requireContext(), + android.R.layout.simple_spinner_dropdown_item, + options + ); + + smoothingSpinner.setAdapter(adapter); + + smoothingSpinner.setOnItemSelectedListener(new AdapterView.OnItemSelectedListener() { + @Override + public void onItemSelected(AdapterView parent, View view, int position, long id) { + + // Update selected smoothing mode + switch (position) { + case 0: + smoothingType = SmoothingType.RAW; + break; + case 1: + smoothingType = SmoothingType.MOVING_AVERAGE; + break; + case 2: + smoothingType = SmoothingType.EXPONENTIAL; + break; + } + + // Reset smoothing state when switching modes + smoothedDisplayLocation = null; + smoothingBuffer.clear(); + } + + @Override + public void onNothingSelected(AdapterView parent) {} + }); + } + + + /** + * Returns the display location after applying the selected smoothing filter. + */ + private LatLng applySmoothing(@NonNull LatLng newLocation) { + + switch (smoothingType) { + + case RAW: + return newLocation; + + case MOVING_AVERAGE: + smoothingBuffer.add(newLocation); + + if (smoothingBuffer.size() > SMOOTHING_WINDOW) { + smoothingBuffer.remove(0); + } + + double sumLat = 0; + double sumLng = 0; + + for (LatLng p : smoothingBuffer) { + sumLat += p.latitude; + sumLng += p.longitude; + } + + return new LatLng( + sumLat / smoothingBuffer.size(), + sumLng / smoothingBuffer.size() + ); + + case EXPONENTIAL: + + if (smoothedDisplayLocation == null) { + smoothedDisplayLocation = newLocation; + return newLocation; + } + + double lat = ALPHA * newLocation.latitude + + (1 - ALPHA) * smoothedDisplayLocation.latitude; + + double lng = ALPHA * newLocation.longitude + + (1 - ALPHA) * smoothedDisplayLocation.longitude; + + smoothedDisplayLocation = new LatLng(lat, lng); + return smoothedDisplayLocation; + } + + return newLocation; + } + + /** + * Adds a GNSS observation to the rolling history and redraws the overlays. + * + * @param point New GNSS position + */ + public void addGnssObservation(@NonNull LatLng point) { + addObservation(gnssHistory, point); + redrawObservationOverlays(); + } + + /** + * Adds a WiFi observation to the rolling history and redraws the overlays. + * + * @param point New WiFi-derived position + */ + public void addWifiObservation(@NonNull LatLng point) { + addObservation(wifiHistory, point); + redrawObservationOverlays(); + } + + /** + * Adds a PDR observation to the rolling history and redraws the overlays. + * + * @param point New PDR-derived position + */ + public void addPdrObservation(@NonNull LatLng point) { + addObservation(pdrHistory, point); + redrawObservationOverlays(); + } + + /** * Update the user's current location on the map, create or move orientation marker, * and append to polyline if the user actually moved. @@ -310,27 +590,33 @@ public void onNothingSelected(AdapterView parent) {} public void updateUserLocation(@NonNull LatLng newLocation, float orientation) { if (gMap == null) return; - // Keep track of current location + // Apply selected smoothing filter before rendering + LatLng displayLocation = applySmoothing(newLocation); + + addObservation(pdrHistory, displayLocation); + redrawObservationOverlays(); + + // Keep track of current location using the displayed point LatLng oldLocation = this.currentLocation; - this.currentLocation = newLocation; + this.currentLocation = displayLocation; // If no marker, create it if (orientationMarker == null) { orientationMarker = gMap.addMarker(new MarkerOptions() - .position(newLocation) + .position(displayLocation) .flat(true) .title("Current Position") .icon(BitmapDescriptorFactory.fromBitmap( UtilFunctions.getBitmapFromVector(requireContext(), R.drawable.ic_baseline_navigation_24))) ); - gMap.moveCamera(CameraUpdateFactory.newLatLngZoom(newLocation, 19f)); + gMap.moveCamera(CameraUpdateFactory.newLatLngZoom(displayLocation, 19f)); } else { // Update marker position + orientation - orientationMarker.setPosition(newLocation); + orientationMarker.setPosition(displayLocation); orientationMarker.setRotation(orientation); // Move camera a bit - gMap.moveCamera(CameraUpdateFactory.newLatLng(newLocation)); + gMap.moveCamera(CameraUpdateFactory.newLatLng(displayLocation)); } // Extend polyline if movement occurred @@ -345,19 +631,18 @@ public void updateUserLocation(@NonNull LatLng newLocation, float orientation) { // First position fix: add the first polyline point if (oldLocation == null) { - points.add(newLocation); + points.add(displayLocation); polyline.setPoints(points); - } else if (!oldLocation.equals(newLocation)) { + } else if (!oldLocation.equals(displayLocation)) { // Subsequent movement: append a new polyline point - points.add(newLocation); + points.add(displayLocation); polyline.setPoints(points); } } - // Update indoor map overlay if (indoorMapManager != null) { - indoorMapManager.setCurrentLocation(newLocation); + indoorMapManager.setCurrentLocation(displayLocation); setFloorControlsVisibility(indoorMapManager.getIsIndoorMapSet() ? View.VISIBLE : View.GONE); } } @@ -418,6 +703,8 @@ public void addTestPointMarker(int index, long timestampMs, @NonNull LatLng posi */ public void updateGNSS(@NonNull LatLng gnssLocation) { if (gMap == null) return; + addObservation(gnssHistory, gnssLocation); + redrawObservationOverlays(); if (!isGnssOn) return; if (gnssMarker == null) { @@ -442,6 +729,14 @@ public void updateGNSS(@NonNull LatLng gnssLocation) { } } + /** + * Updates the WiFi history with a new location and refreshes the map display. + * @param wifiLocation The new coordinates to add to the observation history. + */ + public void updateWiFiObservation(@NonNull LatLng wifiLocation) { + addObservation(wifiHistory, wifiLocation); + redrawObservationOverlays(); + } /** * Remove GNSS marker if user toggles it off @@ -509,6 +804,13 @@ public void clearMapAndReset() { } testPointMarkers.clear(); + // remove coloured observation circles from map + gnssHistory.clear(); + wifiHistory.clear(); + pdrHistory.clear(); + clearObservationCircles(); + + smoothedDisplayLocation = null; // Re-create empty polylines with your chosen colors if (gMap != null) { @@ -584,7 +886,7 @@ private void drawBuildingPolygon() { .add(nkml1, nkml2, nkml3, nkml4, nkml1) .strokeColor(Color.BLUE) // Blue border .strokeWidth(10f) // Border width - // .fillColor(Color.argb(50, 0, 0, 255)) // Semi-transparent blue fill + // .fillColor(Color.argb(50, 0, 0, 255)) // Semi-transparent blue fill .zIndex(1); // Set a higher zIndex to ensure it appears above other overlays PolygonOptions buildingPolygonOptions3 = new PolygonOptions() 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..ee7c398e 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java @@ -65,6 +65,7 @@ private void createWifiPositioningRequest() { } JSONObject wifiFingerPrint = new JSONObject(); wifiFingerPrint.put(WIFI_FINGERPRINT, wifiAccessPoints); + Log.d("WiFiDebug", "Sending fingerprint: " + wifiFingerPrint.toString()); this.wiFiPositioning.request(wifiFingerPrint); } catch (JSONException e) { Log.e("jsonErrors", "Error creating json object" + e.toString()); @@ -82,6 +83,7 @@ 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) { diff --git a/app/src/main/res/layout/fragment_trajectory_map.xml b/app/src/main/res/layout/fragment_trajectory_map.xml index a72425bf..b88c35f4 100644 --- a/app/src/main/res/layout/fragment_trajectory_map.xml +++ b/app/src/main/res/layout/fragment_trajectory_map.xml @@ -35,7 +35,20 @@ android:id="@+id/gnssSwitch" android:layout_width="wrap_content" android:layout_height="wrap_content" - android:text="@string/gnssSwitch" /> + android:text="@string/gnssSwitch"/> + + + + + + + Default building assumptions Floor height in meters Color - 🛰 Show GNSS + 🛰🔵 Show GNSS + 📶🟢 WiFi + 🚶🔴 PDR Floor Down button Floor Up button Choose Map From c9b32366d682fa410beba1fa265f545c0081702f Mon Sep 17 00:00:00 2001 From: Yue Date: Wed, 1 Apr 2026 09:14:59 +0100 Subject: [PATCH 11/36] map_matching_2 --- MAP_MATCHING_CHANGELOG.md | 22 +++++++++ .../fragment/TrajectoryMapFragment.java | 10 +++- .../PositionMe/utils/WallGeometryBuilder.java | 47 +++++++++++++++++++ 3 files changed, 77 insertions(+), 2 deletions(-) create mode 100644 MAP_MATCHING_CHANGELOG.md create mode 100644 app/src/main/java/com/openpositioning/PositionMe/utils/WallGeometryBuilder.java diff --git a/MAP_MATCHING_CHANGELOG.md b/MAP_MATCHING_CHANGELOG.md new file mode 100644 index 00000000..d547d256 --- /dev/null +++ b/MAP_MATCHING_CHANGELOG.md @@ -0,0 +1,22 @@ +y# Map Matching Change Log (branch snapshot) + +This file tracks code additions and modifications versus `main` for the map-matching/floor logic. Update it whenever related code changes. + +## Added files +- `app/src/main/java/com/openpositioning/PositionMe/utils/MapMatchingConfig.java` — centralizes thresholds (baro, lift/stairs displacement, wall padding, feature proximity). +- `app/src/main/java/com/openpositioning/PositionMe/utils/CrossFloorClassifier.java` — pure function to label cross-floor moves as LIFT/STAIRS/UNKNOWN. +- `app/src/main/java/com/openpositioning/PositionMe/utils/WallCollisionCorrector.java` — segment-intersection check; blocks steps that cross wall segments. +- `app/src/test/java/com/openpositioning/PositionMe/utils/MapMatchingConfigTest.java` — verifies default/custom config values. +- `app/src/test/java/com/openpositioning/PositionMe/utils/CrossFloorClassifierTest.java` — tests lift/stairs/unknown boundaries. +- `app/src/test/java/com/openpositioning/PositionMe/utils/WallCollisionCorrectorTest.java` — tests wall-crossing block and pass-through. + +## Modified files +- `MAP_MATCHING_CONSTRAINTS.md` — translated to English; added rules for minimal diffs, English comments, config centralization, and checklists. +- `app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java` — accepts `MapMatchingConfig`; aligns default floor height with config; clarified units; applies `WallCollisionCorrector` when walls are provided via `setWalls(...)`. +- `app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java` — baro floor switch now requires height threshold and proximity to stairs/lift; fallback to config floor height when metadata missing. +- `app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java` — tracks last location; adds `isNearCrossFloorFeature` proximity helper and `getCurrentFloorShape`; clarifies floor height units. + +## Open tasks (to keep in sync with code) +- [x] Wire `WallCollisionCorrector` using actual wall geometry from floorplan data (convert GeoJSON to meter-based polylines and align frames). +- [x] Log `CrossFloorClassifier` results for floor switches (lift vs stairs) to aid device validation. +- [ ] Device-test barometric floor switching with current thresholds; tune `crossFeatureProximity`/`baroHeightThreshold` if needed. 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 7feede72..d1178b05 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 @@ -93,6 +93,7 @@ public class TrajectoryMapFragment extends Fragment { private long lastCandidateTime = 0; private final MapMatchingConfig mapMatchingConfig = new MapMatchingConfig(); + private LatLng wallOrigin; // UI private Spinner switchMapSpinner; @@ -772,13 +773,18 @@ private void evaluateAutoFloor() { private void updateWallsForPdr() { if (sensorFusion == null || indoorMapManager == null) return; if (!indoorMapManager.getIsIndoorMapSet()) return; - if (indoorMapManager.getLastLocation() == null) return; + LatLng current = indoorMapManager.getLastLocation(); + if (current == null) return; + + if (wallOrigin == null) { + wallOrigin = current; + } FloorplanApiClient.FloorShapes floor = indoorMapManager.getCurrentFloorShape(); if (floor == null) return; List> walls = WallGeometryBuilder.buildWalls( - floor, indoorMapManager.getLastLocation()); + floor, wallOrigin); sensorFusion.setPdrWalls(walls); } } diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/WallGeometryBuilder.java b/app/src/main/java/com/openpositioning/PositionMe/utils/WallGeometryBuilder.java new file mode 100644 index 00000000..dc5c00b3 --- /dev/null +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/WallGeometryBuilder.java @@ -0,0 +1,47 @@ +package com.openpositioning.PositionMe.utils; + +import android.graphics.PointF; + +import com.google.android.gms.maps.model.LatLng; +import com.openpositioning.PositionMe.data.remote.FloorplanApiClient; + +import java.util.ArrayList; +import java.util.List; + +/** + * Builds meter-based wall polylines from floorplan features, relative to an origin. + */ +public final class WallGeometryBuilder { + + private WallGeometryBuilder() {} + + /** + * Converts wall features (LineString/MultiLineString/Polygon rings) to local meter polylines. + * + * @param floor floor data containing features. + * @param origin reference lat/lng (e.g., initial PDR position) for local frame. + * @return list of wall polylines; empty if none. + */ + public static List> buildWalls(FloorplanApiClient.FloorShapes floor, LatLng origin) { + List> walls = new ArrayList<>(); + if (floor == null || origin == null) return walls; + for (FloorplanApiClient.MapShapeFeature feature : floor.getFeatures()) { + if (!"wall".equals(feature.getIndoorType())) continue; + for (List part : feature.getParts()) { + if (part == null || part.size() < 2) continue; + List line = new ArrayList<>(part.size()); + for (LatLng p : part) { + float dx = (float) UtilFunctions.degreesToMetersLng(p.longitude - origin.longitude, origin.latitude); + float dy = (float) UtilFunctions.degreesToMetersLat(p.latitude - origin.latitude); + line.add(new PointF(dx, dy)); + } + // Close ring for polygons so collision sees full wall loop + if (!line.isEmpty() && !line.get(0).equals(line.get(line.size() - 1))) { + line.add(new PointF(line.get(0).x, line.get(0).y)); + } + walls.add(line); + } + } + return walls; + } +} From 7168af2acb0ad89d2ec00c5672fa9bd59cc2979a Mon Sep 17 00:00:00 2001 From: JP10JOT <97289379+JapjotS@users.noreply.github.com> Date: Wed, 1 Apr 2026 11:09:03 +0100 Subject: [PATCH 12/36] basic as --- .gitignore | 1 + app/src/main/res/layout/fragment_home.xml | 20 ++++++++++---------- 2 files changed, 11 insertions(+), 10 deletions(-) diff --git a/.gitignore b/.gitignore index d4c3a57e..134bb5cd 100644 --- a/.gitignore +++ b/.gitignore @@ -14,3 +14,4 @@ .cxx local.properties /.idea/ +.claude/settings.json diff --git a/app/src/main/res/layout/fragment_home.xml b/app/src/main/res/layout/fragment_home.xml index 661afbb3..3e6fcffa 100644 --- a/app/src/main/res/layout/fragment_home.xml +++ b/app/src/main/res/layout/fragment_home.xml @@ -11,7 +11,7 @@ android:layout_width="match_parent" android:layout_height="match_parent" android:padding="24dp" - android:background="@color/md_theme_background"> + android:background="#504CAF50"> @@ -36,7 +36,7 @@ android:textSize="14sp" android:textStyle="italic" android:textAlignment="center" - android:textColor="@color/md_theme_primary" + android:textColor="#086108" app:layout_constraintTop_toBottomOf="@id/pageTitle" app:layout_constraintStart_toStartOf="parent" app:layout_constraintEnd_toEndOf="parent" @@ -90,7 +90,7 @@ android:layout_width="0dp" android:layout_height="0dp" android:layout_margin="8dp" - android:backgroundTint="@color/md_theme_primary" + android:backgroundTint="#52D557" android:text="@string/start_recording" android:textColor="@color/md_theme_onPrimary" app:cornerRadius="16dp" @@ -107,9 +107,9 @@ android:layout_width="0dp" android:layout_height="0dp" android:layout_margin="8dp" - android:backgroundTint="@color/md_theme_light_primary" + android:backgroundTint="#8BC34A" android:text="@string/files" - android:textColor="@color/md_theme_light_onPrimary" + android:textColor="@color/md_theme_onSecondary" app:cornerRadius="16dp" app:icon="@drawable/ic_baseline_folder_24" app:iconGravity="textTop" @@ -124,9 +124,9 @@ android:layout_width="0dp" android:layout_height="0dp" android:layout_margin="8dp" - android:backgroundTint="@color/md_theme_tertiary" + android:backgroundTint="#1E5420" android:text="@string/info" - android:textColor="@color/md_theme_onTertiary" + android:textColor="@color/md_theme_onSecondary" app:cornerRadius="16dp" app:icon="@drawable/ic_baseline_info_24" app:iconGravity="textTop" @@ -141,9 +141,9 @@ android:layout_width="0dp" android:layout_height="0dp" android:layout_margin="8dp" - android:backgroundTint="@color/md_theme_tertiary" + android:backgroundTint="#464D07" android:text="@string/measurements" - android:textColor="@color/md_theme_onTertiary" + android:textColor="@color/md_theme_onSecondary" app:cornerRadius="16dp" app:icon="@drawable/ic_baseline_data_array_24" app:iconGravity="textTop" From 17d5575aa3a03559da59734210ee0f842941fd3b Mon Sep 17 00:00:00 2001 From: JP10JOT <97289379+JapjotS@users.noreply.github.com> Date: Wed, 1 Apr 2026 12:04:55 +0100 Subject: [PATCH 13/36] Add wall collisions & floor-change handling Introduce wall-aware particle filtering and automatic floor-change detection. - ParticleFilter: add walls list and setWalls(), increase heading/stride noise, detect segment-wall intersections during predict(), reflect particles on collision, normalize weights and centralize resampling logic, and change GNSS/WiFi weight updates to multiply likelihoods (keep prior weight). Adds helper functions doIntersect, intersectsWall, normalizeWeightsAndResample, and resample. - SensorFusion: hook pressure sensor updates to new floor logic; add updateFloorLogic() that inspects barometer buffer, compares against configured floor height and building floor shapes, and classifies transitions (lift vs stairs) to update PDR floor. Add getParticleFilter() and setPdrWalls() to convert polyline PointF lists into float[] wall segments for the particle filter. - PdrProcessing: remove in-class wall collision correction and related setWalls(), simplify constructor and map-matching dependency, expose start elevation, elevation buffer, floor getters/setters, and use a fixed default floor height fallback (4m). Rationale: move wall collision handling into the particle filter for centralized collision correction and resampling, and add floor-change detection using pressure/barometer data and floorplan geometry so the fusion can update PDR floor state automatically. Also cleans up resampling and weight handling. --- .../PositionMe/sensors/ParticleFilter.java | 160 +++++++++++------- .../PositionMe/sensors/SensorFusion.java | 102 +++++++++-- .../PositionMe/utils/PdrProcessing.java | 71 ++++---- 3 files changed, 223 insertions(+), 110 deletions(-) diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java index 80916189..f4840360 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java @@ -21,6 +21,9 @@ public class ParticleFilter { private boolean initialized = false; private final Random random = new Random(); + // Wall segments: {x1, y1, x2, y2} in ENU + private List walls = new ArrayList<>(); + public void initialise(LatLng firstFix, float accuracyMeters) { if (initialized) return; // ignore subsequent calls, DONE this.origin = firstFix; @@ -47,9 +50,13 @@ public LatLng getOrigin() { return origin; } + public void setWalls(List wallSegments) { + this.walls = wallSegments; + } + - public static final float HEADING_NOISE_STD = 0.01f; //TODO CHANGE LATER?? - public static final float STRIDE_LENGTH_NOISE_STD = 0.01f; // WE CAN TUNE THESE NOISE PARAMETERS BASED ON EXPECTED SENSOR ACCURACY AND ENVIRONMENTAL CONDITIONS + public static final float HEADING_NOISE_STD = 0.05f; //TODO CHANGE LATER? -- JAPJOT + public static final float STRIDE_LENGTH_NOISE_STD = 0.05f; // WE CAN TUNE THESE NOISE PARAMETERS BASED ON EXPECTED SENSOR ACCURACY AND ENVIRONMENTAL CONDITIONS public void predict(float deltaEasting, float deltaNorthing) { if (!initialized) return; // ignore if not initialized @@ -64,10 +71,98 @@ public void predict(float deltaEasting, float deltaNorthing) { float noisyStride = stride + (float) (random.nextGaussian() * STRIDE_LENGTH_NOISE_STD); // Update particle position based on noisy heading and stride - particles[i][0] += noisyStride * Math.cos(noisyHeading); // update east - particles[i][1] += noisyStride * Math.sin(noisyHeading); // update north + + //JAPJOT: ive commented this out to add wall collision checking, we need to check if the line from old position to new position intersects any walls, if it does, we can either discard the particle (set weight to 0) + // or reflect it off the wall (more complex). and particles that hit walls will be reflected in opposite direction + //like dvd logo + + // particles[i][0] += noisyStride * Math.cos(noisyHeading); // update east + // particles[i][1] += noisyStride * Math.sin(noisyHeading); // update north + + + float oldX = particles[i][0]; + float oldY = particles[i][1]; + float newX = oldX + noisyStride * (float) Math.cos(noisyHeading); + float newY = oldY + noisyStride * (float) Math.sin(noisyHeading); + + if (intersectsWall(oldX, oldY, newX, newY)) { + // Simple reflection: reverse direction and reduce stride (simulate energy loss) + float reflectedHeading = (float) (noisyHeading + Math.PI); // reverse direction + float reducedStride = noisyStride * 0.5f; // reduce stride to simulate energy loss + + particles[i][0] = oldX + reducedStride * (float) Math.cos(reflectedHeading); + particles[i][1] = oldY + reducedStride * (float) Math.sin(reflectedHeading); + + //weights[i] = 0f; could also delete the particle + } else { + particles[i][0] = newX; + particles[i][1] = newY; + } + } + normalizeWeightsAndResample(); + } + + private boolean intersectsWall(float x1, float y1, float x2, float y2) { + // Check if the line segment from (x1, y1) to (x2, y2) intersects any wall segment + if (walls == null || walls.isEmpty()) return false; + for (float[] wall : walls) { + if (doIntersect(x1, y1, x2, y2, wall[0], wall[1], wall[2], wall[3])) { + return true; + } + } + return false; + } + + + + + private boolean doIntersect(float x1, float y1, float x2, float y2, float x3, float y3, float x4, float y4) { + float den = (y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1); //calculate the denominator of the intersection formula + if (den == 0) return false; + // Calculate the intersection point using the parametric form of the line segments + float ua = ((x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3)) / den; + float ub = ((x2 - x1) * (y1 - y3) - (y2 - y1) * (x1 - x3)) / den; + return ua >= 0 && ua <= 1 && ub >= 0 && ub <= 1; + } + + private void normalizeWeightsAndResample() { + float weightSum = 0f; + for (float w : weights) weightSum += w; + + if (weightSum < 1e-6) { + // All particles hit a wall or died, re-initialize around current best or uniform + for (int i = 0; i < NUM_PARTICLES; i++) { + weights[i] = 1.0f / NUM_PARTICLES; + // Add some jitter to avoid collapse if we just reset positions + } + return; + } + + for (int i = 0; i < NUM_PARTICLES; i++) { + weights[i] /= weightSum; } + // Resample only if effective sample size is low or every step (simple version: every step) + resample(); + } + + private void resample() { + float[][] newParticles = new float[NUM_PARTICLES][2]; + float step = 1.0f / NUM_PARTICLES; + float cumulativeWeight = weights[0]; + float randomStart = random.nextFloat() * step; + int j = 0; + for (int i = 0; i < NUM_PARTICLES; i++) { + float threshold = randomStart + i * step; + while (threshold > cumulativeWeight && j < NUM_PARTICLES - 1) { + j++; + cumulativeWeight += weights[j]; + } + newParticles[i][0] = particles[j][0]; + newParticles[i][1] = particles[j][1]; + } + particles = newParticles; + for (int i = 0; i < NUM_PARTICLES; i++) weights[i] = 1.0f / NUM_PARTICLES; } public void updateGNSS(LatLng gnssPosition, float gnssAccuracy) { @@ -91,7 +186,7 @@ public void updateGNSS(LatLng gnssPosition, float gnssAccuracy) { float distanceSquared = dx * dx + dy * dy; // Calculate weight using Gaussian likelihood - weights[i] = (float) Math.exp(-distanceSquared / (2 * variance)); + weights[i] *= (float) Math.exp(-distanceSquared / (2 * variance)); weightSum += weights[i]; } @@ -107,31 +202,7 @@ public void updateGNSS(LatLng gnssPosition, float gnssAccuracy) { } } - //resample particles - - float[][] newParticles = new float[NUM_PARTICLES][2]; // New array for resampled particles - float step = 1.0f / NUM_PARTICLES; // Step size for resampling - float cumulativeWeight = weights[0]; // Cumulative weight for resampling - - float random1 = random.nextFloat() * step; //random start point for resampling - - int j = 0; // Index for particles - for (int i = 0; i < NUM_PARTICLES; i++) { - float threshold = random1 + i * step; // Threshold for selecting particle - while (threshold > cumulativeWeight && j < NUM_PARTICLES - 1) { // Move to the next particle - j++; - - cumulativeWeight += weights[j]; // Move to next particle - } - newParticles[i][0] = particles[j][0]; // Resample east - newParticles[i][1] = particles[j][1]; // Resample north - } - particles = newParticles; // Replace old particles with resampled particles - - //reset - for (int i = 0; i < NUM_PARTICLES; i++) { - weights[i] = 1.0f / NUM_PARTICLES; // Reset weights to uniform after resampling - } + resample(); Log.d(TAG, "GNSS UPDATE" + " GNSS position: " + gnssPosition + " accuracy: " + gnssAccuracy + "m" + (mx + ", " + my)); @@ -154,7 +225,7 @@ public void updateWiFi(LatLng wifiPosition, float wifiAccuracy) { float distanceSquared = dx * dx + dy * dy; // Calculate weight using Gaussian likelihood - weights[i] = (float) Math.exp(-distanceSquared / (2 * variance)); + weights[i] *= (float) Math.exp(-distanceSquared / (2 * variance)); weightSum += weights[i]; } @@ -170,31 +241,7 @@ public void updateWiFi(LatLng wifiPosition, float wifiAccuracy) { } } - //resample particles - - float[][] newParticles = new float[NUM_PARTICLES][2]; // New array for resampled particles - float step = 1.0f / NUM_PARTICLES; // Step size for resampling - float cumulativeWeight = weights[0]; // Cumulative weight for resampling - - float random1 = random.nextFloat() * step; //random start point for resampling - - int j = 0; // Index for particles - for (int i = 0; i < NUM_PARTICLES; i++) { - float threshold = random1 + i * step; // Threshold for selecting particle - while (threshold > cumulativeWeight && j < NUM_PARTICLES - 1) { // Move to the next particle - j++; - - cumulativeWeight += weights[j]; // Move to next particle - } - newParticles[i][0] = particles[j][0]; // Resample east - newParticles[i][1] = particles[j][1]; // Resample north - } - particles = newParticles; // Replace old particles with resampled particles - - //reset - for (int i = 0; i < NUM_PARTICLES; i++) { - weights[i] = 1.0f / NUM_PARTICLES; // Reset weights to uniform after resampling - } + resample(); Log.d(TAG, "wifi UPDATE" + " WiFi position: " + wifiPosition + " accuracy: " + wifiAccuracy + "m" + (mx + ", " + my)); @@ -218,4 +265,3 @@ public LatLng getFusedPosition() { } } - 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 0db85113..98d4ac88 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -2,6 +2,7 @@ import android.content.Context; import android.content.SharedPreferences; +import android.graphics.PointF; import android.hardware.Sensor; import android.hardware.SensorEvent; import android.hardware.SensorEventListener; @@ -12,6 +13,9 @@ import android.os.Looper; import android.os.SystemClock; import android.widget.Toast; +// import android.graphics.PointF; +//import com.openpositioning.PositionMe.utils.WiFiPositioning; +import com.openpositioning.PositionMe.utils.WallGeometryBuilder; import androidx.annotation.NonNull; import androidx.preference.PreferenceManager; @@ -24,8 +28,8 @@ import com.openpositioning.PositionMe.utils.PdrProcessing; import com.openpositioning.PositionMe.utils.TrajectoryValidator; import com.openpositioning.PositionMe.data.remote.ServerCommunications; -import com.openpositioning.PositionMe.utils.WallGeometryBuilder; -import android.graphics.PointF; +import com.openpositioning.PositionMe.utils.UtilFunctions; +import com.openpositioning.PositionMe.utils.BuildingPolygon; import java.util.ArrayList; import java.util.HashMap; @@ -235,9 +239,67 @@ public void update(Object[] objList) { * *

Delegates to {@link SensorEventHandler#handleSensorEvent(SensorEvent)}.

*/ + + //floor updates particle fusion @Override public void onSensorChanged(SensorEvent sensorEvent) { eventHandler.handleSensorEvent(sensorEvent); + if (sensorEvent.sensor.getType() == Sensor.TYPE_PRESSURE && recorder.isRecording()) { + updateFloorLogic(); + } + } + + private void updateFloorLogic() { + if (!pdrProcessing.getElevationList().isFull()) return; + + float finishAvg = (float) pdrProcessing.getElevationList().getListCopy().stream() + .mapToDouble(f -> f).average().orElse(0.0); + float diff = finishAvg - pdrProcessing.getStartElevation(); + int floorHeight = pdrProcessing.getFloorHeightValue(); + + if (Math.abs(diff) > floorHeight) { + int targetFloor = pdrProcessing.getCurrentFloor() + (int)(diff / floorHeight); + LatLng pos = getFusedPosition(); + if (pos == null) return; + + FloorplanApiClient.BuildingInfo building = getFloorplanBuilding(getSelectedBuildingId()); + if (building == null) return; + + int currentFloorIdx = pdrProcessing.getCurrentFloor() + 1; // Assuming bias 1 for Nucleus/Murchison + if (currentFloorIdx < 0 || currentFloorIdx >= building.getFloorShapesList().size()) return; + + FloorplanApiClient.FloorShapes floorShapes = building.getFloorShapesList().get(currentFloorIdx); + boolean nearTransition = false; + String type = "unknown"; + + for (FloorplanApiClient.MapShapeFeature feature : floorShapes.getFeatures()) { + String fType = feature.getIndoorType(); + if ("lift".equals(fType) || "stairs".equals(fType)) { + for (List part : feature.getParts()) { + if (BuildingPolygon.pointInPolygon(pos, part)) { + nearTransition = true; + type = fType; + break; + } + } + } + if (nearTransition) break; + } + + if (nearTransition) { + // Classification logic: lift vs stairs + // We check if the user is moving horizontally during the elevation change + float horizontalMovement = (float) Math.sqrt( + Math.pow(state.filteredAcc[0], 2) + Math.pow(state.filteredAcc[1], 2) + ); + + boolean isLift = horizontalMovement < 0.2f; // threshold for "near zero" horizontal stride/acceleration + + if (("lift".equals(type) && isLift) || ("stairs".equals(type) && !isLift)) { + pdrProcessing.setCurrentFloor(targetFloor); + } + } + } } /** {@inheritDoc} */ @@ -368,6 +430,15 @@ public void startRecording() { } } + // /** + // * Inject wall polylines (meters) into PDR for collision correction. + // */ + // public void setPdrWalls(List> walls) { + // if (pdrProcessing != null) { + // pdrProcessing.setWalls(walls); + // } + // } + /** * Disables saving sensor values to the trajectory object. * Also stops the foreground service since background collection is no longer needed. @@ -429,15 +500,6 @@ public void setSelectedBuildingId(String buildingId) { recorder.setSelectedBuildingId(buildingId); } - /** - * Inject wall polylines (meters) into PDR for collision correction. - */ - public void setPdrWalls(List> walls) { - if (pdrProcessing != null) { - pdrProcessing.setWalls(walls); - } - } - /** * Gets the selected building identifier for the current recording session. * @@ -679,6 +741,24 @@ public void logSensorFrequencies() { eventHandler.logSensorFrequencies(); } + public ParticleFilter getParticleFilter() { + return particleFilter; + } + + // Inject wall polylines (meters) into PDR for collision correction. + + public void setPdrWalls(List> wallPolylines) { //JAPJOT -- i have added this function to convert wall polylines into segments and pass to particle + List segments = new ArrayList<>(); + for (List polyline : wallPolylines) { + for (int i = 0; i < polyline.size() - 1; i++) { + PointF p1 = polyline.get(i); + PointF p2 = polyline.get(i + 1); //convert each pair of consecutive points into a line segment represented as [x1, y1, x2, y2] + segments.add(new float[]{p1.x, p1.y, p2.x, p2.y}); + } + } + particleFilter.setWalls(segments); + } + //endregion //region Location listener 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 87510104..b2dd2b37 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java @@ -2,7 +2,6 @@ import android.content.Context; import android.content.SharedPreferences; -import android.graphics.PointF; import android.hardware.SensorManager; import androidx.preference.PreferenceManager; @@ -11,14 +10,14 @@ import java.util.Arrays; import java.util.Collections; +import java.util.List; import java.util.OptionalDouble; import java.util.Objects; import java.util.stream.Collectors; -import java.util.List; /** * Processes data recorded in the {@link SensorFusion} class and calculates live PDR estimates. - * Coordinates are relative to the session start, in meters (x east, y north). Elevation is + * * Coordinates are relative to the session start, in meters (x east, y north). Elevation is * relative to the initial barometer median. Attempts to estimate current floor and elevator use. * * @author Mate Stodulka @@ -44,8 +43,9 @@ public class PdrProcessing { // Settings for accessing shared variables private SharedPreferences settings; + // Centralized map-matching thresholds (currently informational only) - private final MapMatchingConfig mapMatchingConfig; + private MapMatchingConfig mapMatchingConfig; // Step length private float stepLength; @@ -57,13 +57,14 @@ public class PdrProcessing { private float positionY; // Optional wall geometry in meters (polylines) - private List> walls; + //private List> walls; // Vertical movement calculation private Float[] startElevationBuffer; private float startElevation; private int setupIndex = 0; private float elevation; + // Floor-to-floor height in meters (manual setting) private int floorHeight; private int currentFloor; @@ -87,15 +88,12 @@ public class PdrProcessing { * @param context Application context for variable access. */ public PdrProcessing(Context context) { - this(context, new MapMatchingConfig()); - } - - public PdrProcessing(Context context, MapMatchingConfig mapMatchingConfig) { // Initialise settings this.settings = PreferenceManager.getDefaultSharedPreferences(context); - this.mapMatchingConfig = mapMatchingConfig; // Check if estimate or manual values should be used this.useManualStep = this.settings.getBoolean("manual_step_values", false); + + // this.mapMatchingConfig = mapMatchingConfig; if(useManualStep) { try { // Retrieve manual step length @@ -135,11 +133,12 @@ public PdrProcessing(Context context, MapMatchingConfig mapMatchingConfig) { } // Distance between floors is building dependent, use manual value - this.floorHeight = settings.getInt("floor_height", (int) mapMatchingConfig.baroHeightThreshold); + this.floorHeight = settings.getInt("floor_height", 4); // Array for holding initial values this.startElevationBuffer = new Float[3]; // Start floor - assumed to be zero this.currentFloor = 0; + } /** @@ -182,16 +181,9 @@ public float[] updatePdr(long currentStepEnd, List accelMagnitudeOvertim float x = (float) (stepLength * Math.cos(adaptedHeading)); float y = (float) (stepLength * Math.sin(adaptedHeading)); - // Apply wall collision correction if walls are provided - PointF previous = new PointF(this.positionX, this.positionY); - PointF candidate = new PointF(this.positionX + x, this.positionY + y); - if (walls != null && !walls.isEmpty()) { - candidate = WallCollisionCorrector.correct(previous, candidate, walls); - } - // Update position values - this.positionX = candidate.x; - this.positionY = candidate.y; + this.positionX += x; + this.positionY += y; // return current position return new float[]{this.positionX, this.positionY}; @@ -224,20 +216,6 @@ public float updateElevation(float absoluteElevation) { // Add to buffer this.elevationList.putNewest(absoluteElevation); - // Check if there was floor movement - // Check if there is enough data to evaluate - if(this.elevationList.isFull()) { - // Check average of elevation array - List elevationMemory = this.elevationList.getListCopy(); - OptionalDouble currentAvg = elevationMemory.stream().mapToDouble(f -> f).average(); - float finishAvg = currentAvg.isPresent() ? (float) currentAvg.getAsDouble() : 0; - - // Check if we moved floor by comparing with start position - if(Math.abs(finishAvg - startElevation) > this.floorHeight) { - // Change floors - 'floor' division - this.currentFloor += (finishAvg - startElevation)/this.floorHeight; - } - } // Return current elevation return elevation; } @@ -301,6 +279,14 @@ public float getCurrentElevation() { return this.elevation; } + public float getStartElevation() { + return this.startElevation; + } + + public CircularFloatBuffer getElevationList() { + return this.elevationList; + } + /** * Get the current floor number as estimated by the PDR class. * @@ -310,6 +296,14 @@ public int getCurrentFloor() { return this.currentFloor; } + public void setCurrentFloor(int floor) { + this.currentFloor = floor; + } + + public int getFloorHeightValue() { + return this.floorHeight; + } + /** * Estimates if the user is currently taking an elevator. * From the gravity and gravity-removed acceleration values the magnitude of horizontal and @@ -409,7 +403,7 @@ public void resetPDR() { } // Distance between floors is building dependent, use manual value - this.floorHeight = settings.getInt("floor_height", (int) mapMatchingConfig.baroHeightThreshold); + this.floorHeight = settings.getInt("floor_height", 4); // Array for holding initial values this.startElevationBuffer = new Float[3]; // Start floor - assumed to be zero @@ -433,11 +427,4 @@ public float getAverageStepLength(){ return averageStepLength; } - /** - * Inject wall polylines (meters) for collision correction. Optional. - */ - public void setWalls(List> walls) { - this.walls = walls; - } - } From d4608efa8cbe6da9c58724146c380a9f9c0dfefb Mon Sep 17 00:00:00 2001 From: JP10JOT <97289379+JapjotS@users.noreply.github.com> Date: Wed, 1 Apr 2026 12:12:14 +0100 Subject: [PATCH 14/36] Update TrajectoryMapFragment.java --- .../presentation/fragment/TrajectoryMapFragment.java | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java index d1178b05..fdfa4431 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 @@ -158,6 +158,11 @@ public void onMapReady(@NonNull GoogleMap googleMap) { drawBuildingPolygon(); + // Draw green outlines around buildings that have indoor map data + if (indoorMapManager != null) { + indoorMapManager.setIndicationOfIndoorMap(); + } + Log.d("TrajectoryMapFragment", "onMapReady: Map is ready!"); From 9978e233b136ae53ede43e0ae9ad45ab7d196144 Mon Sep 17 00:00:00 2001 From: JP10JOT <97289379+JapjotS@users.noreply.github.com> Date: Wed, 1 Apr 2026 12:13:36 +0100 Subject: [PATCH 15/36] changed --- MAP_MATCHING_CHANGELOG.md | 22 -------------- MAP_MATCHING_CONSTRAINTS.md | 57 ------------------------------------- 2 files changed, 79 deletions(-) delete mode 100644 MAP_MATCHING_CHANGELOG.md delete mode 100644 MAP_MATCHING_CONSTRAINTS.md diff --git a/MAP_MATCHING_CHANGELOG.md b/MAP_MATCHING_CHANGELOG.md deleted file mode 100644 index d547d256..00000000 --- a/MAP_MATCHING_CHANGELOG.md +++ /dev/null @@ -1,22 +0,0 @@ -y# Map Matching Change Log (branch snapshot) - -This file tracks code additions and modifications versus `main` for the map-matching/floor logic. Update it whenever related code changes. - -## Added files -- `app/src/main/java/com/openpositioning/PositionMe/utils/MapMatchingConfig.java` — centralizes thresholds (baro, lift/stairs displacement, wall padding, feature proximity). -- `app/src/main/java/com/openpositioning/PositionMe/utils/CrossFloorClassifier.java` — pure function to label cross-floor moves as LIFT/STAIRS/UNKNOWN. -- `app/src/main/java/com/openpositioning/PositionMe/utils/WallCollisionCorrector.java` — segment-intersection check; blocks steps that cross wall segments. -- `app/src/test/java/com/openpositioning/PositionMe/utils/MapMatchingConfigTest.java` — verifies default/custom config values. -- `app/src/test/java/com/openpositioning/PositionMe/utils/CrossFloorClassifierTest.java` — tests lift/stairs/unknown boundaries. -- `app/src/test/java/com/openpositioning/PositionMe/utils/WallCollisionCorrectorTest.java` — tests wall-crossing block and pass-through. - -## Modified files -- `MAP_MATCHING_CONSTRAINTS.md` — translated to English; added rules for minimal diffs, English comments, config centralization, and checklists. -- `app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java` — accepts `MapMatchingConfig`; aligns default floor height with config; clarified units; applies `WallCollisionCorrector` when walls are provided via `setWalls(...)`. -- `app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java` — baro floor switch now requires height threshold and proximity to stairs/lift; fallback to config floor height when metadata missing. -- `app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java` — tracks last location; adds `isNearCrossFloorFeature` proximity helper and `getCurrentFloorShape`; clarifies floor height units. - -## Open tasks (to keep in sync with code) -- [x] Wire `WallCollisionCorrector` using actual wall geometry from floorplan data (convert GeoJSON to meter-based polylines and align frames). -- [x] Log `CrossFloorClassifier` results for floor switches (lift vs stairs) to aid device validation. -- [ ] Device-test barometric floor switching with current thresholds; tune `crossFeatureProximity`/`baroHeightThreshold` if needed. diff --git a/MAP_MATCHING_CONSTRAINTS.md b/MAP_MATCHING_CONSTRAINTS.md deleted file mode 100644 index fc0be0be..00000000 --- a/MAP_MATCHING_CONSTRAINTS.md +++ /dev/null @@ -1,57 +0,0 @@ -# Map Matching Constraints (Java project) - -Rules to follow when implementing or modifying indoor map matching and floor estimation. Review this file before any change touching positioning, motion model, or map data. - -## Scope -- New or changed logic for position estimation, floor estimation, trajectory correction, or sensor fusion (IMU, barometer). -- Reading, storing, or updating building map data with `wall` / `stairs` / `lift` features. - -## Core data assumptions -- Map is layered by floor; each floor has its own feature set. Feature types are limited to `wall`, `stairs`, `lift`. -- Coordinate system and units must stay consistent with existing map data (typically meters). Always state units and reference frame. -- `wall`: impassable area; corrected paths must not intersect wall geometry. -- `stairs`: combine horizontal displacement with height change; horizontal step length must be respected. -- `lift`: height change with near‑zero horizontal displacement or constrained inside the elevator shaft footprint. - -## Motion model and wall constraints -- Predict position with the motion model first; if the prediction intersects a `wall`, apply collision correction (projection/slide/constraint inside walkable polygon). -- No teleport through walls: if a step crosses a wall, truncate or slide along the wall instead of jumping past it. -- Wall handling must work with polygons/segments; do not assume rectangles. - -## Floor switching with barometer -- Allow floor changes only when the current position is near a `stairs` or `lift` feature **and** the barometric height delta exceeds the configured threshold. -- Thresholds must come from project configuration; if hardcoded, place them in a single config file and document the source. -- If no nearby cross‑floor feature exists, ignore height changes and mark the reading as low confidence instead of switching floors. - -## Distinguish lift vs stairs -- Using motion cues: - - Horizontal displacement < `liftHorizontalMax` with significant height change → lift event. - - Horizontal displacement ≥ `stairsHorizontalMin` with stair‑height accumulation → stairs event. -- Record every event with start/end time, height delta, horizontal distance, and candidate feature ID. -- When both fit, pick the type whose feature is geometrically closer. - -## Error handling and fallback -- On sensor anomalies (pressure spikes, IMU saturation), keep the last trusted state and mark the current estimate as low confidence; do not write permanent tracks. -- If map lookup fails or is missing, do not disable wall checks; provide a degraded but explainable path (motion model only, flagged unreliable). - -## Implementation requirements (Java) -- New classes/methods must have unit tests covering: wall crossing correction, floor change rejection, and lift vs stairs boundary cases. -- Use existing geometry libraries where possible; new libs require license/size review and must be recorded in dependency notes. -- Centralize parameters (e.g., `config/map_matching.properties` or DI config); no scattered magic numbers. -- Public methods must declare input/output units and coordinate frame; state whether height is relative or absolute (sea level vs floor datum). -- **Do not modify unrelated code when adding/updating map-matching logic. Keep diffs minimal and targeted.** - -## Debugging and logging -- Log at controllable levels for key decisions: wall collision handling, accepted/rejected floor switches, lift vs stairs classification. -- Logs should include floor index, position, feature ID, and summarized sensor values; avoid dumping full raw streams. - -## Change checklist (self‑review before commit) -- [ ] No path passes through walls; add regression/simulation if needed. -- [ ] Floor changes happen only near `stairs`/`lift` and when height thresholds are met. -- [ ] Lift/stairs decision is explainable via horizontal displacement or feature distance. -- [ ] All new parameters are centralized with defaults; no magic numbers. -- [ ] Unit tests cover core rules and pass. -- [ ] New or edited comments are concise, in English, and style‑consistent. -- [ ] Update `MAP_MATCHING_CHANGELOG.md` with added/modified files and open tasks whenever map-matching code changes. - -Exceptions to these constraints must be documented in design notes and approved before merging. From c82e8dbf78aec74bc72a0513dac621b0423dfa8d Mon Sep 17 00:00:00 2001 From: JP10JOT <97289379+JapjotS@users.noreply.github.com> Date: Wed, 1 Apr 2026 12:48:50 +0100 Subject: [PATCH 16/36] Improve indoor visuals, floorplan fetch & autofloor Parse and apply API-provided visual properties (stroke_color, fill_color, stroke_width) to MapShapeFeature and use them when drawing polygons/polylines. Add clickable building polygons with tags and Toasts, and introduce one-shot floorplan fetching that caches BuildingInfo into SensorFusion and updates IndoorMapManager. Support pre-rendered PNG ground overlays for Nucleus/Library (with bounds and transparency) and fall back to vector shapes for other buildings; clear overlays correctly. Harden auto-floor logic: only trust WiFi floor when the last WiFi fix is fresh (30s), record WiFi fix timestamps, expose isWifiPositionFresh(), add horizontal acceleration magnitude to distinguish LIFT vs STAIRS and reject UNKNOWN mode. Misc: reset fetch state on stop, center camera on building when indoor map first appears, and minor UI/stroke defaults and robustness fixes. --- .../data/remote/FloorplanApiClient.java | 43 +++- .../fragment/TrajectoryMapFragment.java | 186 +++++++++++++----- .../PositionMe/sensors/ParticleFilter.java | 89 ++++++--- .../PositionMe/sensors/SensorFusion.java | 24 +++ .../PositionMe/sensors/WiFiPositioning.java | 14 ++ .../sensors/WifiPositionManager.java | 13 ++ .../PositionMe/utils/IndoorMapManager.java | 125 ++++++++++-- 7 files changed, 405 insertions(+), 89 deletions(-) diff --git a/app/src/main/java/com/openpositioning/PositionMe/data/remote/FloorplanApiClient.java b/app/src/main/java/com/openpositioning/PositionMe/data/remote/FloorplanApiClient.java index cadb6037..9b6c27de 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/data/remote/FloorplanApiClient.java +++ b/app/src/main/java/com/openpositioning/PositionMe/data/remote/FloorplanApiClient.java @@ -51,6 +51,9 @@ public static class MapShapeFeature { private final String indoorType; private final String geometryType; private final List> parts; + private final String strokeColor; // hex string from API, e.g. "#666666", or null + private final String fillColor; // hex string from API, or null + private final float strokeWidth; // 0 means not specified by API /** * Constructs a MapShapeFeature. @@ -58,12 +61,19 @@ public static class MapShapeFeature { * @param indoorType feature type from properties (e.g. "wall", "room") * @param geometryType GeoJSON geometry type (e.g. "MultiLineString", "MultiPolygon") * @param parts coordinate lists: each inner list is a line or polygon ring + * @param strokeColor stroke colour from API properties, or null + * @param fillColor fill colour from API properties, or null + * @param strokeWidth stroke width from API properties, 0 if not set */ public MapShapeFeature(String indoorType, String geometryType, - List> parts) { + List> parts, + String strokeColor, String fillColor, float strokeWidth) { this.indoorType = indoorType; this.geometryType = geometryType; this.parts = parts; + this.strokeColor = strokeColor; + this.fillColor = fillColor; + this.strokeWidth = strokeWidth; } /** Returns the indoor feature type (e.g. "wall", "room"). */ @@ -74,6 +84,15 @@ public MapShapeFeature(String indoorType, String geometryType, /** Returns coordinate parts: lines for MultiLineString, rings for MultiPolygon. */ public List> getParts() { return parts; } + + /** Returns the API-provided stroke colour string, or null if not set. */ + public String getStrokeColor() { return strokeColor; } + + /** Returns the API-provided fill colour string, or null if not set. */ + public String getFillColor() { return fillColor; } + + /** Returns the API-provided stroke width, or 0 if not set. */ + public float getStrokeWidth() { return strokeWidth; } } /** @@ -444,7 +463,27 @@ private MapShapeFeature parseMapShapeFeature(JSONObject feature) { return null; } - return new MapShapeFeature(indoorType, geoType, parts); + // Parse visual properties — try the same field names the private repo uses + String strokeColor = null; + String fillColor = null; + float strokeWidth = 0f; + if (properties != null) { + // stroke colour: stroke_color → line_color → color + if (properties.has("stroke_color")) strokeColor = properties.optString("stroke_color", null); + else if (properties.has("line_color")) strokeColor = properties.optString("line_color", null); + else if (properties.has("color")) strokeColor = properties.optString("color", null); + + // fill colour: fill_color → fill + if (properties.has("fill_color")) fillColor = properties.optString("fill_color", null); + else if (properties.has("fill")) fillColor = properties.optString("fill", null); + + // stroke width: stroke_width → line_width → width + if (properties.has("stroke_width")) strokeWidth = (float) properties.optDouble("stroke_width", 0.0); + else if (properties.has("line_width")) strokeWidth = (float) properties.optDouble("line_width", 0.0); + else if (properties.has("width")) strokeWidth = (float) properties.optDouble("width", 0.0); + } + + return new MapShapeFeature(indoorType, geoType, parts, strokeColor, fillColor, strokeWidth); } catch (JSONException e) { Log.e(TAG, "Failed to parse map_shapes feature", e); return null; 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 33fc19cc..a482018a 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 @@ -37,10 +37,6 @@ import com.google.android.gms.maps.model.*; import java.util.ArrayList; -import java.util.List; - -import com.google.android.gms.maps.model.Circle; -import com.google.android.gms.maps.model.CircleOptions; /** @@ -86,6 +82,11 @@ public class TrajectoryMapFragment extends Fragment { private IndoorMapManager indoorMapManager; // Manages indoor mapping private SensorFusion sensorFusion; + // Floorplan live-fetch state + private FloorplanApiClient floorplanApiClient = new FloorplanApiClient(); + private boolean floorplanFetchAttempted = false; + private boolean wasIndoorMapSet = false; + // Auto-floor state private static final String TAG = "TrajectoryMapFragment"; private static final long AUTO_FLOOR_DEBOUNCE_MS = 3000; @@ -110,6 +111,9 @@ public class TrajectoryMapFragment extends Fragment { private TextView floorLabel; private Button switchColorButton; private Polygon buildingPolygon; + private Polygon nkmlPolygon; + private Polygon fjbPolygon; + private Polygon faradayPolygon; // --- Last N observation display state --- private static final int MAX_OBSERVATIONS = 20; @@ -657,7 +661,20 @@ public void updateUserLocation(@NonNull LatLng newLocation, float orientation) { // Update indoor map overlay if (indoorMapManager != null) { indoorMapManager.setCurrentLocation(displayLocation); - setFloorControlsVisibility(indoorMapManager.getIsIndoorMapSet() ? View.VISIBLE : View.GONE); + boolean nowIndoorMapSet = indoorMapManager.getIsIndoorMapSet(); + setFloorControlsVisibility(nowIndoorMapSet ? View.VISIBLE : View.GONE); + + // When we first enter a building with floor data, center the camera on it + if (!wasIndoorMapSet && nowIndoorMapSet) { + int building = indoorMapManager.getCurrentBuilding(); + String apiName = buildingConstantToApiName(building); + FloorplanApiClient.BuildingInfo info = (sensorFusion != null && apiName != null) + ? sensorFusion.getFloorplanBuilding(apiName) : null; + if (info != null) { + gMap.moveCamera(CameraUpdateFactory.newLatLngZoom(info.getCenter(), 19f)); + } + } + wasIndoorMapSet = nowIndoorMapSet; } } @@ -719,6 +736,7 @@ public void updateGNSS(@NonNull LatLng gnssLocation) { if (gMap == null) return; addObservation(gnssHistory, gnssLocation); redrawObservationOverlays(); + fetchFloorplanIfNeeded(gnssLocation); if (!isGnssOn) return; if (gnssMarker == null) { @@ -811,6 +829,8 @@ public void clearMapAndReset() { } lastGnssLocation = null; currentLocation = null; + floorplanFetchAttempted = false; + wasIndoorMapSet = false; // Clear test point markers for (Marker m : testPointMarkers) { @@ -890,50 +910,67 @@ private void drawBuildingPolygon() { PolygonOptions buildingPolygonOptions = new PolygonOptions() .add(nucleus1, nucleus2, nucleus3, nucleus4, nucleus5) - .strokeColor(Color.RED) // Red border - .strokeWidth(10f) // Border width - //.fillColor(Color.argb(50, 255, 0, 0)) // Semi-transparent red fill - .zIndex(1); // Set a higher zIndex to ensure it appears above other overlays + .strokeColor(Color.RED) + .strokeWidth(10f) + .fillColor(Color.argb(70, 255, 0, 0)) + .clickable(true) + .zIndex(1); - // Options for the new polygon PolygonOptions buildingPolygonOptions2 = new PolygonOptions() .add(nkml1, nkml2, nkml3, nkml4, nkml1) - .strokeColor(Color.BLUE) // Blue border - .strokeWidth(10f) // Border width - // .fillColor(Color.argb(50, 0, 0, 255)) // Semi-transparent blue fill - .zIndex(1); // Set a higher zIndex to ensure it appears above other overlays + .strokeColor(Color.BLUE) + .strokeWidth(10f) + .fillColor(Color.argb(50, 0, 0, 255)) + .clickable(true) + .zIndex(1); PolygonOptions buildingPolygonOptions3 = new PolygonOptions() .add(fjb1, fjb2, fjb3, fjb4, fjb1) - .strokeColor(Color.GREEN) // Green border - .strokeWidth(10f) // Border width - //.fillColor(Color.argb(50, 0, 255, 0)) // Semi-transparent green fill - .zIndex(1); // Set a higher zIndex to ensure it appears above other overlays + .strokeColor(Color.GREEN) + .strokeWidth(10f) + .fillColor(Color.argb(70, 0, 255, 0)) + .clickable(true) + .zIndex(1); PolygonOptions buildingPolygonOptions4 = new PolygonOptions() .add(faraday1, faraday2, faraday3, faraday4, faraday1) - .strokeColor(Color.YELLOW) // Yellow border - .strokeWidth(10f) // Border width - //.fillColor(Color.argb(50, 255, 255, 0)) // Semi-transparent yellow fill - .zIndex(1); // Set a higher zIndex to ensure it appears above other overlays - - - // Remove the old polygon if it exists - if (buildingPolygon != null) { - buildingPolygon.remove(); - } + .strokeColor(Color.YELLOW) + .strokeWidth(10f) + .fillColor(Color.argb(70, 255, 255, 0)) + .clickable(true) + .zIndex(1); + + // Remove old polygons if they exist (e.g. after map reset) + if (buildingPolygon != null) buildingPolygon.remove(); + if (nkmlPolygon != null) nkmlPolygon.remove(); + if (fjbPolygon != null) fjbPolygon.remove(); + if (faradayPolygon != null) faradayPolygon.remove(); - // Add the polygon to the map buildingPolygon = gMap.addPolygon(buildingPolygonOptions); - gMap.addPolygon(buildingPolygonOptions2); - gMap.addPolygon(buildingPolygonOptions3); - gMap.addPolygon(buildingPolygonOptions4); + nkmlPolygon = gMap.addPolygon(buildingPolygonOptions2); + fjbPolygon = gMap.addPolygon(buildingPolygonOptions3); + faradayPolygon = gMap.addPolygon(buildingPolygonOptions4); + + // Tag each polygon so the click listener can identify the building by name + buildingPolygon.setTag("Nucleus Building"); + nkmlPolygon.setTag("Noreen & Kenneth Murray Library"); + fjbPolygon.setTag("Frank Jarvis Building"); + faradayPolygon.setTag("Faraday Building"); + + // Show the building name in a Toast when the user taps a polygon + gMap.setOnPolygonClickListener(polygon -> { + Object tag = polygon.getTag(); + if (tag != null && getContext() != null) { + android.widget.Toast.makeText( + getContext(), (String) tag, android.widget.Toast.LENGTH_SHORT).show(); + } + }); + Log.d(TAG, "Building polygon added, vertex count: " + buildingPolygon.getPoints().size()); } //region Auto-floor logic // Uses WiFi floor when available; otherwise barometric elevation divided by floor height (meters). - // Behavior unchanged; proximity to stairs/lift will be added in later steps. /** * Starts the periodic auto-floor evaluation task. Checks every second @@ -973,13 +1010,14 @@ private void applyImmediateFloor() { updateWallsForPdr(); int candidateFloor; - if (sensorFusion.getLatLngWifiPositioning() != null) { + // Use WiFi floor only when the last fix is fresh (within 30 s); otherwise fall through + // to the barometric path so stale WiFi data does not permanently override it. + if (sensorFusion.getLatLngWifiPositioning() != null && sensorFusion.isWifiPositionFresh()) { candidateFloor = sensorFusion.getWifiFloor(); } else { float elevation = sensorFusion.getElevation(); float floorHeight = indoorMapManager.getFloorHeight(); if (floorHeight <= 0) { - // Fallback to config default if building metadata is missing floorHeight = mapMatchingConfig.baroHeightThreshold; } if (Math.abs(elevation) < mapMatchingConfig.baroHeightThreshold) { @@ -988,16 +1026,23 @@ private void applyImmediateFloor() { if (floorHeight <= 0) return; candidateFloor = Math.round(elevation / floorHeight); - // Require proximity to stairs/lift when using barometer path + // Require proximity to stairs/lift before changing floors boolean nearFeature = indoorMapManager.isNearCrossFloorFeature(mapMatchingConfig.crossFeatureProximity); if (!nearFeature) { return; } + // Use real horizontal acceleration to distinguish lift from stairs + float horizAccel = sensorFusion.getHorizontalAccelMagnitude(); CrossFloorClassifier.Mode mode = - CrossFloorClassifier.classify(0.0, elevation, 0.0, mapMatchingConfig); + CrossFloorClassifier.classify(horizAccel, elevation, 0.0, mapMatchingConfig); Log.d(TAG, "Auto-floor (baro) mode=" + mode + " elevation=" + elevation - + " floorHeight=" + floorHeight); + + " horizAccel=" + horizAccel); + + // Only accept LIFT or STAIRS; UNKNOWN means not enough signal to change floor + if (mode == CrossFloorClassifier.Mode.UNKNOWN) { + return; + } } indoorMapManager.setCurrentFloor(candidateFloor, true); @@ -1032,15 +1077,15 @@ private void evaluateAutoFloor() { int candidateFloor; - // Priority 1: WiFi-based floor (only if WiFi positioning has returned data) - if (sensorFusion.getLatLngWifiPositioning() != null) { + // Priority 1: WiFi floor — only when a fresh fix is available (within 30 s). + // Stale WiFi data must not permanently suppress the barometric path. + if (sensorFusion.getLatLngWifiPositioning() != null && sensorFusion.isWifiPositionFresh()) { candidateFloor = sensorFusion.getWifiFloor(); } else { - // Fallback: barometric elevation estimate + // Fallback: barometric elevation estimate with map-matching guards float elevation = sensorFusion.getElevation(); float floorHeight = indoorMapManager.getFloorHeight(); if (floorHeight <= 0) { - // Fallback to config default if building metadata is missing floorHeight = mapMatchingConfig.baroHeightThreshold; } if (Math.abs(elevation) < mapMatchingConfig.baroHeightThreshold) { @@ -1053,10 +1098,17 @@ private void evaluateAutoFloor() { if (floorHeight <= 0) return; candidateFloor = Math.round(elevation / floorHeight); + // Use real horizontal acceleration so LIFT (low horiz) vs STAIRS (high horiz) is distinguished + float horizAccel = sensorFusion.getHorizontalAccelMagnitude(); CrossFloorClassifier.Mode mode = - CrossFloorClassifier.classify(0.0, elevation, 0.0, mapMatchingConfig); + CrossFloorClassifier.classify(horizAccel, elevation, 0.0, mapMatchingConfig); Log.d(TAG, "Auto-floor (baro) mode=" + mode + " elevation=" + elevation - + " floorHeight=" + floorHeight); + + " horizAccel=" + horizAccel); + + // Reject UNKNOWN — not enough movement signal to commit to a floor change + if (mode == CrossFloorClassifier.Mode.UNKNOWN) { + return; + } } // Debounce: require the same floor reading for AUTO_FLOOR_DEBOUNCE_MS @@ -1077,6 +1129,52 @@ private void evaluateAutoFloor() { //endregion + /** + * Fetches floorplan data from the server once per recording session. + * On success, caches the buildings in SensorFusion and refreshes indoor map overlays. + */ + private void fetchFloorplanIfNeeded(LatLng gnssLocation) { + if (floorplanFetchAttempted) return; + if (sensorFusion == null) return; + floorplanFetchAttempted = true; + + floorplanApiClient.requestFloorplan( + gnssLocation.latitude, + gnssLocation.longitude, + new ArrayList<>(), + new FloorplanApiClient.FloorplanCallback() { + @Override + public void onSuccess(List buildings) { + if (sensorFusion == null) return; + sensorFusion.setFloorplanBuildings(buildings); + if (indoorMapManager != null) { + indoorMapManager.setIndicationOfIndoorMap(); + if (currentLocation != null) { + indoorMapManager.setCurrentLocation(currentLocation); + } + } + Log.d(TAG, "Floorplan fetched: " + buildings.size() + " buildings"); + } + + @Override + public void onFailure(String error) { + Log.w(TAG, "Floorplan fetch failed: " + error); + floorplanFetchAttempted = false; // allow retry on next update + } + } + ); + } + + /** Maps an IndoorMapManager building constant to the API building name. */ + private String buildingConstantToApiName(int building) { + switch (building) { + case IndoorMapManager.BUILDING_NUCLEUS: return "nucleus_building"; + case IndoorMapManager.BUILDING_LIBRARY: return "library"; + case IndoorMapManager.BUILDING_MURCHISON: return "murchison_house"; + default: return null; + } + } + private void updateWallsForPdr() { if (sensorFusion == null || indoorMapManager == null) return; if (!indoorMapManager.getIsIndoorMapSet()) return; diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java index f4840360..5ff0d67f 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java @@ -55,8 +55,13 @@ public void setWalls(List wallSegments) { } - public static final float HEADING_NOISE_STD = 0.05f; //TODO CHANGE LATER? -- JAPJOT - public static final float STRIDE_LENGTH_NOISE_STD = 0.05f; // WE CAN TUNE THESE NOISE PARAMETERS BASED ON EXPECTED SENSOR ACCURACY AND ENVIRONMENTAL CONDITIONS + public static final float HEADING_NOISE_STD = 0.05f; + public static final float STRIDE_LENGTH_NOISE_STD = 0.05f; + + // Small positional noise added to each particle after resampling. + // Prevents sample impoverishment: without roughening, particles copied from the + // same parent are identical and provide no extra information on the next step. + private static final float ROUGHENING_STD = 0.3f; // metres public void predict(float deltaEasting, float deltaNorthing) { if (!initialized) return; // ignore if not initialized @@ -99,7 +104,9 @@ public void predict(float deltaEasting, float deltaNorthing) { particles[i][1] = newY; } } - normalizeWeightsAndResample(); + // Weights are intentionally NOT updated or resampled here. + // predict() only propagates particle positions; weight updates happen + // exclusively in updateGNSS() and updateWiFi() when a new observation arrives. } private boolean intersectsWall(float x1, float y1, float x2, float y2) { @@ -125,27 +132,32 @@ private boolean doIntersect(float x1, float y1, float x2, float y2, float x3, fl return ua >= 0 && ua <= 1 && ub >= 0 && ub <= 1; } - private void normalizeWeightsAndResample() { - float weightSum = 0f; - for (float w : weights) weightSum += w; - - if (weightSum < 1e-6) { - // All particles hit a wall or died, re-initialize around current best or uniform - for (int i = 0; i < NUM_PARTICLES; i++) { - weights[i] = 1.0f / NUM_PARTICLES; - // Add some jitter to avoid collapse if we just reset positions - } - return; - } - - for (int i = 0; i < NUM_PARTICLES; i++) { - weights[i] /= weightSum; + /** + * Computes the effective sample size: N_eff = 1 / Σ(w_i²). + * + *

N_eff equals NUM_PARTICLES when all weights are equal (maximum diversity) + * and approaches 1 when a single particle carries all the weight (collapsed). + * Resampling is triggered when N_eff falls below NUM_PARTICLES / 2.

+ */ + private float computeEffectiveSampleSize() { + float sumSquared = 0f; + for (float w : weights) { + sumSquared += w * w; } - - // Resample only if effective sample size is low or every step (simple version: every step) - resample(); + return sumSquared < 1e-10f ? NUM_PARTICLES : 1.0f / sumSquared; } + /** + * Systematic resampling followed by roughening. + * + *

Systematic resampling draws NUM_PARTICLES new particles proportional to + * their weights using a single random offset, which has lower variance than + * multinomial resampling.

+ * + *

Roughening adds small Gaussian noise after resampling. Without it, + * particles copied from the same parent are identical and degenerate into + * a point mass on the next predict step.

+ */ private void resample() { float[][] newParticles = new float[NUM_PARTICLES][2]; float step = 1.0f / NUM_PARTICLES; @@ -162,7 +174,17 @@ private void resample() { newParticles[i][1] = particles[j][1]; } particles = newParticles; - for (int i = 0; i < NUM_PARTICLES; i++) weights[i] = 1.0f / NUM_PARTICLES; + + // Roughening: add small noise so copied particles diverge on the next step + for (int i = 0; i < NUM_PARTICLES; i++) { + particles[i][0] += (float) (random.nextGaussian() * ROUGHENING_STD); + particles[i][1] += (float) (random.nextGaussian() * ROUGHENING_STD); + } + + // Reset to uniform weights after resampling + for (int i = 0; i < NUM_PARTICLES; i++) { + weights[i] = 1.0f / NUM_PARTICLES; + } } public void updateGNSS(LatLng gnssPosition, float gnssAccuracy) { @@ -191,19 +213,23 @@ public void updateGNSS(LatLng gnssPosition, float gnssAccuracy) { } if (weightSum < 1e-6) { - // Avoid division by zero, reinitialize weights uniformly for (int i = 0; i < NUM_PARTICLES; i++) { weights[i] = 1.0f / NUM_PARTICLES; } } else { - // Normalize weights for (int i = 0; i < NUM_PARTICLES; i++) { weights[i] /= weightSum; } } - resample(); - Log.d(TAG, "GNSS UPDATE" + " GNSS position: " + gnssPosition + " accuracy: " + gnssAccuracy + "m" + (mx + ", " + my)); + // Resample only when particle diversity is low (N_eff < N/2). + // Resampling on every observation wastes diversity; N_eff-gating preserves it. + float nEff = computeEffectiveSampleSize(); + if (nEff < NUM_PARTICLES / 2.0f) { + resample(); + Log.d(TAG, "GNSS resampled: Neff=" + nEff); + } + Log.d(TAG, "GNSS update: pos=" + gnssPosition + " accuracy=" + gnssAccuracy + "m Neff=" + nEff); } @@ -230,19 +256,22 @@ public void updateWiFi(LatLng wifiPosition, float wifiAccuracy) { } if (weightSum < 1e-6) { - // Avoid division by zero, reinitialize weights uniformly for (int i = 0; i < NUM_PARTICLES; i++) { weights[i] = 1.0f / NUM_PARTICLES; } } else { - // Normalize weights for (int i = 0; i < NUM_PARTICLES; i++) { weights[i] /= weightSum; } } - resample(); - Log.d(TAG, "wifi UPDATE" + " WiFi position: " + wifiPosition + " accuracy: " + wifiAccuracy + "m" + (mx + ", " + my)); + // Resample only when particle diversity is low (N_eff < N/2) + float nEff = computeEffectiveSampleSize(); + if (nEff < NUM_PARTICLES / 2.0f) { + resample(); + Log.d(TAG, "WiFi resampled: Neff=" + nEff); + } + Log.d(TAG, "WiFi update: pos=" + wifiPosition + " accuracy=" + wifiAccuracy + "m Neff=" + nEff); } 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 98d4ac88..d26186b4 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -734,6 +734,30 @@ public int getWifiFloor() { return wifiPositionManager.getWifiFloor(); } + /** + * Returns true if the most recent WiFi position fix arrived within the last 30 seconds. + * Used by auto-floor logic to avoid permanently bypassing barometric guards once WiFi + * has fired even once during a recording session. + * + * @return true if WiFi position data is fresh enough to trust for floor detection + */ + public boolean isWifiPositionFresh() { + return wifiPositionManager.isWifiPositionFresh(30_000L); + } + + /** + * Returns the magnitude of horizontal filtered acceleration (m/s²). + * Used by CrossFloorClassifier to distinguish lift (low horizontal movement) + * from stairs (higher horizontal movement) during floor transitions. + * + * @return horizontal acceleration magnitude in m/s² + */ + public float getHorizontalAccelMagnitude() { + return (float) Math.sqrt( + Math.pow(state.filteredAcc[0], 2) + Math.pow(state.filteredAcc[1], 2) + ); + } + /** * Utility function to log the event frequency of each sensor. */ 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..4fc80e7c 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/WiFiPositioning.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/WiFiPositioning.java @@ -43,6 +43,18 @@ public LatLng getWifiLocation() { // Store user's location obtained using WiFi positioning private LatLng wifiLocation; + + // Timestamp (System.currentTimeMillis) of the most recent successful WiFi fix + private long lastWifiFixTimeMs = 0; + + /** + * Returns the wall-clock time of the most recent WiFi position fix. + * Returns 0 if no fix has been received yet. + */ + public long getWifiLocationTimestampMs() { + return lastWifiFixTimeMs; + } + /** * Getter for the WiFi positioning floor obtained using openpositioning API * @return the user's location based on openpositioning API @@ -89,6 +101,7 @@ public void request(JSONObject jsonWifiFeatures) { try { wifiLocation = new LatLng(response.getDouble("lat"),response.getDouble("lon")); floor = response.getInt("floor"); + lastWifiFixTimeMs = System.currentTimeMillis(); } catch (JSONException e) { // Error log to keep record of errors (for secure programming and maintainability) Log.e("jsonErrors","Error parsing response: "+e.getMessage()+" "+ response); @@ -140,6 +153,7 @@ public void request( JSONObject jsonWifiFeatures, final VolleyCallback callback) Log.d("jsonObject",response.toString()); wifiLocation = new LatLng(response.getDouble("lat"),response.getDouble("lon")); floor = response.getInt("floor"); + lastWifiFixTimeMs = System.currentTimeMillis(); callback.onSuccess(wifiLocation,floor); } catch (JSONException e) { Log.e("jsonErrors","Error parsing response: "+e.getMessage()+" "+ response); 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 ee7c398e..0ddd9971 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java @@ -118,6 +118,19 @@ public int getWifiFloor() { return this.wiFiPositioning.getFloor(); } + /** + * Returns true if the most recent WiFi position fix is newer than {@code maxAgeMs} milliseconds. + * Used to prevent stale WiFi fixes from permanently overriding the barometric floor path. + * + * @param maxAgeMs maximum acceptable age of the WiFi fix in milliseconds + * @return true if a fresh fix is available + */ + public boolean isWifiPositionFresh(long maxAgeMs) { + long ts = wiFiPositioning.getWifiLocationTimestampMs(); + if (ts == 0) return false; + return (System.currentTimeMillis() - ts) <= maxAgeMs; + } + /** * Returns the most recent list of WiFi scan results. * diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java b/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java index 262e2546..0bc15b63 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java @@ -4,15 +4,21 @@ import android.util.Log; import com.google.android.gms.maps.GoogleMap; +import com.google.android.gms.maps.model.BitmapDescriptorFactory; +import com.google.android.gms.maps.model.GroundOverlay; +import com.google.android.gms.maps.model.GroundOverlayOptions; import com.google.android.gms.maps.model.LatLng; +import com.google.android.gms.maps.model.LatLngBounds; import com.google.android.gms.maps.model.Polygon; import com.google.android.gms.maps.model.PolygonOptions; import com.google.android.gms.maps.model.Polyline; import com.google.android.gms.maps.model.PolylineOptions; +import com.openpositioning.PositionMe.R; import com.openpositioning.PositionMe.data.remote.FloorplanApiClient; import com.openpositioning.PositionMe.sensors.SensorFusion; import java.util.ArrayList; +import java.util.Arrays; import java.util.List; /** @@ -35,6 +41,20 @@ public class IndoorMapManager { public static final int BUILDING_LIBRARY = 2; public static final int BUILDING_MURCHISON = 3; + // Pre-rendered PNG floor plan images for Nucleus and Library (same as PositionMe_Private) + private static final List NUCLEUS_MAPS = Arrays.asList( + R.drawable.nucleuslg, R.drawable.nucleusg, R.drawable.nucleus1, + R.drawable.nucleus2, R.drawable.nucleus3); + private static final List LIBRARY_MAPS = Arrays.asList( + R.drawable.libraryg, R.drawable.library1, R.drawable.library2, + R.drawable.library3); + + // Lat/lng bounds for positioning ground overlay images on the map + private static final LatLngBounds NUCLEUS_BOUNDS = new LatLngBounds( + BuildingPolygon.NUCLEUS_SW, BuildingPolygon.NUCLEUS_NE); + private static final LatLngBounds LIBRARY_BOUNDS = new LatLngBounds( + BuildingPolygon.LIBRARY_SW, BuildingPolygon.LIBRARY_NE); + private GoogleMap gMap; private LatLng currentLocation; private boolean isIndoorMapSet = false; @@ -43,6 +63,9 @@ public class IndoorMapManager { // Floor height in meters derived from building metadata private float floorHeight; + // Ground overlay for Nucleus and Library (pre-rendered PNG floor plan images) + private GroundOverlay groundOverlay; + // Vector shapes currently drawn on the map (cleared on floor switch or exit) private final List drawnPolygons = new ArrayList<>(); private final List drawnPolylines = new ArrayList<>(); @@ -181,6 +204,14 @@ public String getCurrentFloorDisplayName() { && currentFloor < currentFloorShapes.size()) { return currentFloorShapes.get(currentFloor).getDisplayName(); } + // Fallback display names for PNG buildings when API data not yet loaded + if (currentBuilding == BUILDING_NUCLEUS) { + String[] names = {"LG", "G", "1", "2", "3"}; + if (currentFloor >= 0 && currentFloor < names.length) return names[currentFloor]; + } else if (currentBuilding == BUILDING_LIBRARY) { + String[] names = {"G", "1", "2", "3"}; + if (currentFloor >= 0 && currentFloor < names.length) return names[currentFloor]; + } return String.valueOf(currentFloor); } @@ -211,16 +242,27 @@ public int getAutoFloorBias() { * @param autoFloor true if called by auto-floor feature */ public void setCurrentFloor(int newFloor, boolean autoFloor) { - if (currentFloorShapes == null || currentFloorShapes.isEmpty()) return; + boolean hasPng = (currentBuilding == BUILDING_NUCLEUS + || currentBuilding == BUILDING_LIBRARY); + + // For PNG buildings we don't need API data; for others we do + if (!hasPng && (currentFloorShapes == null || currentFloorShapes.isEmpty())) return; if (autoFloor) { newFloor += getAutoFloorBias(); } - if (newFloor >= 0 && newFloor < currentFloorShapes.size() - && newFloor != this.currentFloor) { + int maxFloor = hasPng + ? (currentBuilding == BUILDING_NUCLEUS ? NUCLEUS_MAPS.size() : LIBRARY_MAPS.size()) + : currentFloorShapes.size(); + + if (newFloor >= 0 && newFloor < maxFloor && newFloor != this.currentFloor) { this.currentFloor = newFloor; - drawFloorShapes(newFloor); + if (hasPng) { + showGroundOverlay(currentBuilding, newFloor); + } else { + drawFloorShapes(newFloor); + } } } @@ -277,20 +319,24 @@ private void setBuildingOverlay() { return; } - // Load floor shapes from cached API data + // Always load floor shapes from cached API data (used by particle filter / auto-floor) FloorplanApiClient.BuildingInfo building = SensorFusion.getInstance().getFloorplanBuilding(apiName); if (building != null) { currentFloorShapes = building.getFloorShapesList(); } - if (currentFloorShapes != null && !currentFloorShapes.isEmpty()) { + // Display: PNG ground overlay for Nucleus/Library; API vectors for Murchison + if (detected == BUILDING_NUCLEUS || detected == BUILDING_LIBRARY) { + showGroundOverlay(detected, currentFloor); + isIndoorMapSet = true; + } else if (currentFloorShapes != null && !currentFloorShapes.isEmpty()) { drawFloorShapes(currentFloor); isIndoorMapSet = true; } } else if (!inAnyBuilding && isIndoorMapSet) { - clearDrawnShapes(); + clearDrawnShapes(); // also removes groundOverlay isIndoorMapSet = false; currentBuilding = BUILDING_NONE; currentFloor = 0; @@ -301,6 +347,36 @@ private void setBuildingOverlay() { } } + /** + * Displays the pre-rendered PNG floor plan image for Nucleus or Library as a + * ground overlay. Replaces any existing ground overlay. + * + * @param building BUILDING_NUCLEUS or BUILDING_LIBRARY + * @param floorIndex floor index matching the NUCLEUS_MAPS / LIBRARY_MAPS list + */ + private void showGroundOverlay(int building, int floorIndex) { + if (groundOverlay != null) { + groundOverlay.remove(); + groundOverlay = null; + } + List maps; + LatLngBounds bounds; + if (building == BUILDING_NUCLEUS) { + maps = NUCLEUS_MAPS; + bounds = NUCLEUS_BOUNDS; + } else if (building == BUILDING_LIBRARY) { + maps = LIBRARY_MAPS; + bounds = LIBRARY_BOUNDS; + } else { + return; + } + if (floorIndex < 0 || floorIndex >= maps.size()) return; + groundOverlay = gMap.addGroundOverlay(new GroundOverlayOptions() + .image(BitmapDescriptorFactory.fromResource(maps.get(floorIndex))) + .positionFromBounds(bounds) + .transparency(0.1f)); // slight transparency so satellite map is visible underneath + } + /** * Draws all vector shapes for the given floor index on the Google Map. * Clears any previously drawn shapes before drawing the new floor. @@ -315,17 +391,23 @@ private void drawFloorShapes(int floorIndex) { FloorplanApiClient.FloorShapes floor = currentFloorShapes.get(floorIndex); for (FloorplanApiClient.MapShapeFeature feature : floor.getFeatures()) { - String geoType = feature.getGeometryType(); + String geoType = feature.getGeometryType(); String indoorType = feature.getIndoorType(); + // Prefer API-provided colours; fall back to type-based defaults + int stroke = parseApiColor(feature.getStrokeColor(), getStrokeColor(indoorType)); + int fill = parseApiColor(feature.getFillColor(), getFillColor(indoorType)); + float width = feature.getStrokeWidth() > 0 + ? Math.max(feature.getStrokeWidth(), 4f) : 5f; + if ("MultiPolygon".equals(geoType) || "Polygon".equals(geoType)) { for (List ring : feature.getParts()) { if (ring.size() < 3) continue; Polygon p = gMap.addPolygon(new PolygonOptions() .addAll(ring) - .strokeColor(getStrokeColor(indoorType)) - .strokeWidth(5f) - .fillColor(getFillColor(indoorType))); + .strokeColor(stroke) + .strokeWidth(width) + .fillColor(fill)); drawnPolygons.add(p); } } else if ("MultiLineString".equals(geoType) @@ -334,14 +416,27 @@ private void drawFloorShapes(int floorIndex) { if (line.size() < 2) continue; Polyline pl = gMap.addPolyline(new PolylineOptions() .addAll(line) - .color(getStrokeColor(indoorType)) - .width(6f)); + .color(stroke) + .width(width)); drawnPolylines.add(pl); } } } } + /** + * Parses an API colour string (e.g. "#666666") to an ARGB int. + * Returns {@code fallback} if the string is null, empty, or unparseable. + */ + private int parseApiColor(String colorStr, int fallback) { + if (colorStr == null || colorStr.trim().isEmpty()) return fallback; + try { + return Color.parseColor(colorStr.trim()); + } catch (IllegalArgumentException ex) { + return fallback; + } + } + /** * Removes all vector shapes currently drawn on the map. */ @@ -350,6 +445,10 @@ private void clearDrawnShapes() { for (Polyline p : drawnPolylines) p.remove(); drawnPolygons.clear(); drawnPolylines.clear(); + if (groundOverlay != null) { + groundOverlay.remove(); + groundOverlay = null; + } } /** From 421e1dbfe083baf11ed92b506ada06203295258a Mon Sep 17 00:00:00 2001 From: JP10JOT <97289379+JapjotS@users.noreply.github.com> Date: Wed, 1 Apr 2026 12:53:37 +0100 Subject: [PATCH 17/36] Set GNSS/WiFi/PDR switches on and load walls Make GNSS/WiFi/PDR switches checked-by-default in the layout and set isGnssOn to true in TrajectoryMapFragment to match the UI default. Also call updateWallsForPdr() immediately after a floorplan is fetched so wall geometry is available to the particle filter as soon as the floorplan arrives (improves PDR/wall constraint behavior even when auto-flooring is disabled). --- .../presentation/fragment/TrajectoryMapFragment.java | 6 +++++- app/src/main/res/layout/fragment_trajectory_map.xml | 3 +++ 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java index a482018a..0c51cd1a 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 @@ -71,7 +71,7 @@ public class TrajectoryMapFragment extends Fragment { private Polyline polyline; // Polyline representing user's movement path private boolean isRed = true; // Tracks whether the polyline color is red - private boolean isGnssOn = false; // Tracks if GNSS tracking is enabled + private boolean isGnssOn = true; // Tracks if GNSS tracking is enabled (matches default android:checked="true") private Polyline gnssPolyline; // Polyline for GNSS path private LatLng lastGnssLocation = null; // Stores the last GNSS location @@ -1152,6 +1152,10 @@ public void onSuccess(List buildings) { if (currentLocation != null) { indoorMapManager.setCurrentLocation(currentLocation); } + // Load wall geometry immediately so the particle filter has wall + // constraints from the moment the floorplan arrives, even if + // auto-floor is not enabled. + updateWallsForPdr(); } Log.d(TAG, "Floorplan fetched: " + buildings.size() + " buildings"); } diff --git a/app/src/main/res/layout/fragment_trajectory_map.xml b/app/src/main/res/layout/fragment_trajectory_map.xml index b88c35f4..cb689330 100644 --- a/app/src/main/res/layout/fragment_trajectory_map.xml +++ b/app/src/main/res/layout/fragment_trajectory_map.xml @@ -35,18 +35,21 @@ android:id="@+id/gnssSwitch" android:layout_width="wrap_content" android:layout_height="wrap_content" + android:checked="true" android:text="@string/gnssSwitch"/> From 41c98986391399d1877ec1fa90b60e686af2560a Mon Sep 17 00:00:00 2001 From: JP10JOT <97289379+JapjotS@users.noreply.github.com> Date: Wed, 1 Apr 2026 13:31:22 +0100 Subject: [PATCH 18/36] 41m --- .../fragment/TrajectoryMapFragment.java | 19 ++++++++++++++++ .../PositionMe/sensors/SensorFusion.java | 22 +++++++++++++++++-- .../PositionMe/utils/BuildingPolygon.java | 18 +++++++++------ .../PositionMe/utils/IndoorMapManager.java | 15 +++++++++++-- .../PositionMe/utils/PdrProcessing.java | 2 +- 5 files changed, 64 insertions(+), 12 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 0c51cd1a..0e12f484 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 @@ -737,6 +737,25 @@ public void updateGNSS(@NonNull LatLng gnssLocation) { addObservation(gnssHistory, gnssLocation); redrawObservationOverlays(); fetchFloorplanIfNeeded(gnssLocation); + + // Drive building detection from raw GNSS so the floorplan appears as soon + // as GPS is available, even before the particle filter produces a fused position. + if (indoorMapManager != null) { + indoorMapManager.setCurrentLocation(gnssLocation); + boolean nowIndoorMapSet = indoorMapManager.getIsIndoorMapSet(); + setFloorControlsVisibility(nowIndoorMapSet ? View.VISIBLE : View.GONE); + if (!wasIndoorMapSet && nowIndoorMapSet) { + int building = indoorMapManager.getCurrentBuilding(); + String apiName = buildingConstantToApiName(building); + FloorplanApiClient.BuildingInfo info = (sensorFusion != null && apiName != null) + ? sensorFusion.getFloorplanBuilding(apiName) : null; + if (info != null) { + gMap.moveCamera(CameraUpdateFactory.newLatLngZoom(info.getCenter(), 19f)); + } + wasIndoorMapSet = true; + } + } + if (!isGnssOn) return; if (gnssMarker == null) { 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 d26186b4..057af62f 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -100,6 +100,11 @@ public class SensorFusion implements SensorEventListener { //particle filter private final ParticleFilter particleFilter = new ParticleFilter(); + // Last GNSS position actually sent to the particle filter. + // Used to suppress stationary noise: indoors, GPS bounces 10-30 m even when + // the device is still, which drags particles out of the building. + private LatLng lastGnssForFilter = null; + // Floorplan API cache (latest result from start-location step) @@ -800,11 +805,24 @@ public void onLocationChanged(@NonNull Location location) { recorder.addGnssData(location); LatLng gnssPos = new LatLng(location.getLatitude(), location.getLongitude()); - float accuracy = location.hasAccuracy() ? location.getAccuracy() : 20f; // Default to 20m if accuracy is unavailable + float accuracy = location.hasAccuracy() ? location.getAccuracy() : 20f; + if (!particleFilter.isInitialized()) { particleFilter.initialise(gnssPos, accuracy); + lastGnssForFilter = gnssPos; } else { - particleFilter.updateGNSS(gnssPos, accuracy); + // Only feed GNSS into the particle filter if the reported position has moved + // at least half the stated accuracy radius since the last accepted fix. + // This suppresses the 10-30 m noise bounce that GPS produces indoors when + // the device is stationary, which would otherwise drag particles through walls. + float minDisplacement = Math.max(accuracy * 0.5f, 3.0f); + boolean shouldUpdate = (lastGnssForFilter == null) + || (UtilFunctions.distanceBetweenPoints(lastGnssForFilter, gnssPos) + >= minDisplacement); + if (shouldUpdate) { + particleFilter.updateGNSS(gnssPos, accuracy); + lastGnssForFilter = gnssPos; + } } } } diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/BuildingPolygon.java b/app/src/main/java/com/openpositioning/PositionMe/utils/BuildingPolygon.java index 06f48a46..db6e7ee5 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/BuildingPolygon.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/BuildingPolygon.java @@ -14,15 +14,19 @@ */ public class BuildingPolygon { // Defining the coordinates of the building boundaries (rectangular boundaries based on floor map shape) - // North-East and South-West Coordinates for the Nucleus Building - public static final LatLng NUCLEUS_NE=new LatLng(55.92332001571212, -3.1738768212979593); - public static final LatLng NUCLEUS_SW=new LatLng(55.92282257022002, -3.1745956532857647); + // Detection-only shift — leave at 0.0. To move the PNG overlay, use OVERLAY_SHIFT_* in IndoorMapManager instead. + static final double SHIFT_LAT = 0.0; + static final double SHIFT_LNG = 0.0; + + public static final LatLng NUCLEUS_NE=new LatLng(55.92332001571212+SHIFT_LAT, -3.1738768212979593+SHIFT_LNG); + public static final LatLng NUCLEUS_SW=new LatLng(55.92282257022002+SHIFT_LAT, -3.1745956532857647+SHIFT_LNG); + // North-East and South-West Coordinates for the Kenneth and Murray Library Building - public static final LatLng LIBRARY_NE=new LatLng(55.92306692576906, -3.174771893078224); - public static final LatLng LIBRARY_SW=new LatLng(55.92281045664704, -3.175184089079065); + public static final LatLng LIBRARY_NE=new LatLng(55.92306692576906+SHIFT_LAT, -3.174771893078224+SHIFT_LNG); + public static final LatLng LIBRARY_SW=new LatLng(55.92281045664704+SHIFT_LAT, -3.175184089079065+SHIFT_LNG); // North-East and South-West Coordinates for Murchison House - public static final LatLng MURCHISON_NE=new LatLng(55.92240, -3.17150); - public static final LatLng MURCHISON_SW=new LatLng(55.92170, -3.17280); + public static final LatLng MURCHISON_NE=new LatLng(55.92240+SHIFT_LAT, -3.17150+SHIFT_LNG); + public static final LatLng MURCHISON_SW=new LatLng(55.92170+SHIFT_LAT, -3.17280+SHIFT_LNG); // Boundary coordinates of the Nucleus building (clockwise) public static final List NUCLEUS_POLYGON = new ArrayList() {{ diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java b/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java index 0bc15b63..163b0734 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java @@ -49,11 +49,22 @@ public class IndoorMapManager { R.drawable.libraryg, R.drawable.library1, R.drawable.library2, R.drawable.library3); + // Fine-tune these to shift the PNG floor-plan overlay without affecting building detection. + // +0.00005 ≈ +5 m north, -0.00001 ≈ -1 m west (1 deg lat ≈ 111 km, 1 deg lng ≈ 70 km here) + private static final double OVERLAY_SHIFT_LAT = 0.0001; + private static final double OVERLAY_SHIFT_LNG = 0.0; + // Lat/lng bounds for positioning ground overlay images on the map private static final LatLngBounds NUCLEUS_BOUNDS = new LatLngBounds( - BuildingPolygon.NUCLEUS_SW, BuildingPolygon.NUCLEUS_NE); + new LatLng(BuildingPolygon.NUCLEUS_SW.latitude + OVERLAY_SHIFT_LAT, + BuildingPolygon.NUCLEUS_SW.longitude + OVERLAY_SHIFT_LNG), + new LatLng(BuildingPolygon.NUCLEUS_NE.latitude + OVERLAY_SHIFT_LAT, + BuildingPolygon.NUCLEUS_NE.longitude + OVERLAY_SHIFT_LNG)); private static final LatLngBounds LIBRARY_BOUNDS = new LatLngBounds( - BuildingPolygon.LIBRARY_SW, BuildingPolygon.LIBRARY_NE); + new LatLng(BuildingPolygon.LIBRARY_SW.latitude + OVERLAY_SHIFT_LAT, + BuildingPolygon.LIBRARY_SW.longitude + OVERLAY_SHIFT_LNG), + new LatLng(BuildingPolygon.LIBRARY_NE.latitude + OVERLAY_SHIFT_LAT, + BuildingPolygon.LIBRARY_NE.longitude + OVERLAY_SHIFT_LNG)); private GoogleMap gMap; private LatLng currentLocation; 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 b2dd2b37..fc305ed5 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java @@ -45,7 +45,7 @@ public class PdrProcessing { // Centralized map-matching thresholds (currently informational only) - private MapMatchingConfig mapMatchingConfig; + //private MapMatchingConfig mapMatchingConfig; // Step length private float stepLength; From 6b06f7b88fbddaa5ff204ee93d1d5eac1404855e Mon Sep 17 00:00:00 2001 From: gayathrikjp Date: Wed, 1 Apr 2026 14:46:28 +0100 Subject: [PATCH 19/36] Updated UI Added setting button and then all switches, gnss, etc are visible --- .../fragment/TrajectoryMapFragment.java | 66 ++++++++-- .../res/layout/fragment_trajectory_map.xml | 114 ++++++++++-------- app/src/main/res/values/strings.xml | 4 +- 3 files changed, 122 insertions(+), 62 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 a482018a..c158bb1c 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 @@ -155,6 +155,10 @@ private enum SmoothingType { // Exponential smoothing strength private static final double ALPHA = 0.25; + private com.google.android.material.button.MaterialButton settingsButton; + private View settingsContainer; + + /** * Adds a new position observation to a rolling history list. * Maintains only the most recent MAX_OBSERVATIONS points. @@ -207,6 +211,9 @@ public void onViewCreated(@NonNull View view, floorLabel = view.findViewById(R.id.floorLabel); switchColorButton = view.findViewById(R.id.lineColorButton); + settingsButton = view.findViewById(R.id.settingsButton); + settingsContainer = view.findViewById(R.id.settingsContainer); + // Setup floor up/down UI hidden initially until we know there's an indoor map setFloorControlsVisibility(View.GONE); @@ -281,6 +288,17 @@ public void onMapReady(@NonNull GoogleMap googleMap) { } }); + // Setting Button + settingsButton.setOnClickListener(v -> { + if (settingsContainer.getVisibility() == View.VISIBLE) { + settingsContainer.setVisibility(View.GONE); + settingsButton.setText("Settings"); + } else { + settingsContainer.setVisibility(View.VISIBLE); + settingsButton.setText("Hide Settings"); + } + }); + // Auto-floor toggle: start/stop periodic floor evaluation sensorFusion = SensorFusion.getInstance(); autoFloorSwitch.setOnCheckedChangeListener((compoundButton, isChecked) -> { @@ -435,35 +453,55 @@ private void initMapSettings(GoogleMap map) { */ private void initMapTypeSpinner() { if (switchMapSpinner == null) return; + String[] maps = new String[]{ - getString(R.string.hybrid), - getString(R.string.normal), - getString(R.string.satellite) + "Select Map Type", + "Hybrid", + "Normal", + "Satellite" }; - ArrayAdapter adapter = new ArrayAdapter<>( + + ArrayAdapter adapter = new ArrayAdapter( requireContext(), android.R.layout.simple_spinner_dropdown_item, maps - ); + ) { + @Override + public boolean isEnabled(int position) { + return position != 0; // disable hint item + } + }; + switchMapSpinner.setAdapter(adapter); + // Show hint initially + switchMapSpinner.setSelection(0); + switchMapSpinner.setOnItemSelectedListener(new AdapterView.OnItemSelectedListener() { @Override public void onItemSelected(AdapterView parent, View view, int position, long id) { + if (gMap == null) return; - switch (position){ + + switch (position) { case 0: + return; // ignore hint + + case 1: gMap.setMapType(GoogleMap.MAP_TYPE_HYBRID); break; - case 1: + + case 2: gMap.setMapType(GoogleMap.MAP_TYPE_NORMAL); break; - case 2: + + case 3: gMap.setMapType(GoogleMap.MAP_TYPE_SATELLITE); break; } } + @Override public void onNothingSelected(AdapterView parent) {} }); @@ -476,6 +514,7 @@ public void onNothingSelected(AdapterView parent) {} private void initSmoothingSpinner() { String[] options = new String[]{ + "Select Display Smoothing", "Raw", "Moving Average", "Exponential" @@ -489,19 +528,24 @@ private void initSmoothingSpinner() { smoothingSpinner.setAdapter(adapter); + // Show hint item initially + smoothingSpinner.setSelection(0); + smoothingSpinner.setOnItemSelectedListener(new AdapterView.OnItemSelectedListener() { @Override public void onItemSelected(AdapterView parent, View view, int position, long id) { - // Update selected smoothing mode switch (position) { case 0: + // Hint row selected, do nothing + return; + case 1: smoothingType = SmoothingType.RAW; break; - case 1: + case 2: smoothingType = SmoothingType.MOVING_AVERAGE; break; - case 2: + case 3: smoothingType = SmoothingType.EXPONENTIAL; break; } diff --git a/app/src/main/res/layout/fragment_trajectory_map.xml b/app/src/main/res/layout/fragment_trajectory_map.xml index b88c35f4..242f34c2 100644 --- a/app/src/main/res/layout/fragment_trajectory_map.xml +++ b/app/src/main/res/layout/fragment_trajectory_map.xml @@ -31,59 +31,75 @@ android:padding="8dp" android:gravity="center"> - - - - - - - - - - - + + - - + android:layout_height="wrap_content" + android:orientation="vertical" + android:gravity="center" + android:visibility="gone"> + + + + + + + + + + + + + + + diff --git a/app/src/main/res/values/strings.xml b/app/src/main/res/values/strings.xml index 8133b578..04a14db1 100644 --- a/app/src/main/res/values/strings.xml +++ b/app/src/main/res/values/strings.xml @@ -114,8 +114,8 @@ Floor height in meters Color 🛰🔵 Show GNSS - 📶🟢 WiFi - 🚶🔴 PDR + 📶🟢 Show WiFi + 🚶🔴 Show PDR Floor Down button Floor Up button Choose Map From 353ec1dfbc366b8385aee090b65e23345f65c12f Mon Sep 17 00:00:00 2001 From: gayathrikjp Date: Wed, 1 Apr 2026 15:03:40 +0100 Subject: [PATCH 20/36] Update with settings icon --- app/src/main/res/layout/fragment_trajectory_map.xml | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/app/src/main/res/layout/fragment_trajectory_map.xml b/app/src/main/res/layout/fragment_trajectory_map.xml index 242f34c2..61345d84 100644 --- a/app/src/main/res/layout/fragment_trajectory_map.xml +++ b/app/src/main/res/layout/fragment_trajectory_map.xml @@ -35,10 +35,13 @@ android:id="@+id/settingsButton" android:layout_width="wrap_content" android:layout_height="wrap_content" - android:backgroundTint="@android:color/white" android:text="Settings" + android:backgroundTint="#FFCDD2" android:textColor="@android:color/black" android:textStyle="bold" + app:icon="@android:drawable/ic_menu_manage" + app:iconTint="@android:color/black" + app:iconPadding="6dp" app:elevation="5dp" /> Date: Wed, 1 Apr 2026 15:25:31 +0100 Subject: [PATCH 21/36] Improve WiFi handling, particle reset, PDR init Prevent duplicate WiFi points and empty API calls: RecordingFragment now tracks lastSentWifiPosition to avoid flooding history with identical fixes; WifiPositionManager skips requests when no APs were scanned and improves debug logging with AP count. Allow reinitialisation of particle filter: ParticleFilter.reset() clears state so the filter can be re-seeded. SensorFusion.setStartGNSSLatitude now resets and re-seeds the particle filter at the user-chosen start location (5 m spread) and updates lastGnssForFilter. PDR initialization tweaks: PdrProcessing now has a non-null MapMatchingConfig (initialized with defaults) and reset logic clears elevation/setup indices so elevation baseline is recalibrated on reset. --- .../presentation/fragment/RecordingFragment.java | 9 +++++++-- .../PositionMe/sensors/ParticleFilter.java | 8 ++++++++ .../openpositioning/PositionMe/sensors/SensorFusion.java | 8 ++++++++ .../PositionMe/sensors/WifiPositionManager.java | 7 ++++++- .../openpositioning/PositionMe/utils/PdrProcessing.java | 9 +++++++-- 5 files changed, 36 insertions(+), 5 deletions(-) diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/RecordingFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/RecordingFragment.java index 06b5d5b9..b711f69b 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,9 @@ public class RecordingFragment extends Fragment { // Last fused point that was actually rendered private LatLng lastSentFusedPosition = null; + // Last WiFi location sent to the map — avoids flooding wifiHistory with the same point + private LatLng lastSentWifiPosition = null; + private final Runnable refreshDataTask = new Runnable() { @Override public void run() { @@ -401,9 +404,11 @@ private void updateUIandPosition() { // WiFi observation logic for colour-coded last N updates if (trajectoryMapFragment != null) { LatLng wifiLocation = sensorFusion.getLatLngWifiPositioning(); - Log.d("WiFiDebug", "RecordingFragment wifiLocation = " + wifiLocation); - if (wifiLocation != null) { + // Only add to history when the location has actually changed (new API response) + if (wifiLocation != null && !wifiLocation.equals(lastSentWifiPosition)) { + Log.d("WiFiDebug", "New WiFi fix: " + wifiLocation); trajectoryMapFragment.updateWiFiObservation(wifiLocation); + lastSentWifiPosition = wifiLocation; } } diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java index 5ff0d67f..32821fba 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java @@ -46,6 +46,14 @@ public boolean isInitialized() { return initialized; } + /** Resets the filter so it can be re-initialised at a new origin. */ + public void reset() { + initialized = false; + origin = null; + particles = null; + weights = null; + } + public LatLng getOrigin() { return origin; } 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 057af62f..d74faab2 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -606,6 +606,14 @@ public LatLng getFusedPosition(){ public void setStartGNSSLatitude(float[] startPosition) { state.startLocation[0] = startPosition[0]; state.startLocation[1] = startPosition[1]; + + // Reset and re-seed the particle filter at the chosen start location. + // The filter may have been initialized earlier from a noisy indoor GPS fix; + // re-seeding here ensures the fused position starts at the user-confirmed location. + particleFilter.reset(); + LatLng chosenStart = new LatLng(startPosition[0], startPosition[1]); + particleFilter.initialise(chosenStart, 5f); // 5 m spread — user just pinpointed their location + lastGnssForFilter = chosenStart; } /** 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 0ddd9971..887a1765 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java @@ -56,8 +56,13 @@ public void update(Object[] wifiList) { /** * Creates a request to obtain a WiFi location for the obtained WiFi fingerprint. + * Skipped if the scan returned no access points (would cause a 422 from the API). */ private void createWifiPositioningRequest() { + if (this.wifiList == null || this.wifiList.isEmpty()) { + Log.d("WiFiDebug", "No APs in scan, skipping API call"); + return; + } try { JSONObject wifiAccessPoints = new JSONObject(); for (Wifi data : this.wifiList) { @@ -65,7 +70,7 @@ private void createWifiPositioningRequest() { } JSONObject wifiFingerPrint = new JSONObject(); wifiFingerPrint.put(WIFI_FINGERPRINT, wifiAccessPoints); - Log.d("WiFiDebug", "Sending fingerprint: " + wifiFingerPrint.toString()); + Log.d("WiFiDebug", "Sending fingerprint with " + this.wifiList.size() + " APs: " + wifiFingerPrint.toString()); this.wiFiPositioning.request(wifiFingerPrint); } catch (JSONException e) { Log.e("jsonErrors", "Error creating json object" + e.toString()); 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 fc305ed5..d3cc6959 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java @@ -45,7 +45,7 @@ public class PdrProcessing { // Centralized map-matching thresholds (currently informational only) - //private MapMatchingConfig mapMatchingConfig; + private final MapMatchingConfig mapMatchingConfig; // Step length private float stepLength; @@ -93,7 +93,9 @@ public PdrProcessing(Context context) { // Check if estimate or manual values should be used this.useManualStep = this.settings.getBoolean("manual_step_values", false); - // this.mapMatchingConfig = mapMatchingConfig; + // Initialize map-matching configuration with defaults + this.mapMatchingConfig = new MapMatchingConfig(); + if(useManualStep) { try { // Retrieve manual step length @@ -408,6 +410,9 @@ public void resetPDR() { this.startElevationBuffer = new Float[3]; // Start floor - assumed to be zero this.currentFloor = 0; + // Must be reset so the elevation baseline is recalibrated from scratch + this.setupIndex = 0; + this.startElevation = 0f; } /** From 6b0510936803c12901ef2217e8d45ce69119d548 Mon Sep 17 00:00:00 2001 From: JP10JOT <97289379+JapjotS@users.noreply.github.com> Date: Wed, 1 Apr 2026 15:46:11 +0100 Subject: [PATCH 22/36] fixed the map --- .../res/layout/fragment_trajectory_map.xml | 31 ++----------------- 1 file changed, 3 insertions(+), 28 deletions(-) diff --git a/app/src/main/res/layout/fragment_trajectory_map.xml b/app/src/main/res/layout/fragment_trajectory_map.xml index 4d1f617b..a0301d25 100644 --- a/app/src/main/res/layout/fragment_trajectory_map.xml +++ b/app/src/main/res/layout/fragment_trajectory_map.xml @@ -31,34 +31,6 @@ android:padding="8dp" android:gravity="center"> - - - - - - - - - Date: Wed, 1 Apr 2026 15:50:29 +0100 Subject: [PATCH 23/36] Always render API vector floor shapes Remove special-case PNG ground overlays for Nucleus/Library and always use API vector drawing. setCurrentFloor now requires currentFloorShapes to be present, uses currentFloorShapes.size() for bounds, and always calls drawFloorShapes. The building-detection path was simplified to always draw vector shapes when available and no longer calls showGroundOverlay. --- .../PositionMe/utils/IndoorMapManager.java | 23 ++++--------------- 1 file changed, 5 insertions(+), 18 deletions(-) diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java b/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java index 163b0734..3e26b7ee 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java @@ -253,27 +253,17 @@ public int getAutoFloorBias() { * @param autoFloor true if called by auto-floor feature */ public void setCurrentFloor(int newFloor, boolean autoFloor) { - boolean hasPng = (currentBuilding == BUILDING_NUCLEUS - || currentBuilding == BUILDING_LIBRARY); - - // For PNG buildings we don't need API data; for others we do - if (!hasPng && (currentFloorShapes == null || currentFloorShapes.isEmpty())) return; + if (currentFloorShapes == null || currentFloorShapes.isEmpty()) return; if (autoFloor) { newFloor += getAutoFloorBias(); } - int maxFloor = hasPng - ? (currentBuilding == BUILDING_NUCLEUS ? NUCLEUS_MAPS.size() : LIBRARY_MAPS.size()) - : currentFloorShapes.size(); + int maxFloor = currentFloorShapes.size(); if (newFloor >= 0 && newFloor < maxFloor && newFloor != this.currentFloor) { this.currentFloor = newFloor; - if (hasPng) { - showGroundOverlay(currentBuilding, newFloor); - } else { - drawFloorShapes(newFloor); - } + drawFloorShapes(newFloor); } } @@ -337,11 +327,8 @@ private void setBuildingOverlay() { currentFloorShapes = building.getFloorShapesList(); } - // Display: PNG ground overlay for Nucleus/Library; API vectors for Murchison - if (detected == BUILDING_NUCLEUS || detected == BUILDING_LIBRARY) { - showGroundOverlay(detected, currentFloor); - isIndoorMapSet = true; - } else if (currentFloorShapes != null && !currentFloorShapes.isEmpty()) { + // Display API vector shapes for all buildings + if (currentFloorShapes != null && !currentFloorShapes.isEmpty()) { drawFloorShapes(currentFloor); isIndoorMapSet = true; } From 7836814ad2d025a902b4c4bb22b00647f868c57a Mon Sep 17 00:00:00 2001 From: JP10JOT <97289379+JapjotS@users.noreply.github.com> Date: Wed, 1 Apr 2026 15:51:39 +0100 Subject: [PATCH 24/36] Update IndoorMapManager.java --- .../PositionMe/utils/IndoorMapManager.java | 32 +++++++++++++++++-- 1 file changed, 29 insertions(+), 3 deletions(-) diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java b/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java index 3e26b7ee..340a9070 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java @@ -320,17 +320,43 @@ private void setBuildingOverlay() { return; } - // Always load floor shapes from cached API data (used by particle filter / auto-floor) + // Mark as indoors immediately so floor controls, auto-floor, and wall + // collision all activate without waiting for the API response. + isIndoorMapSet = true; + + // Load floor shapes from cached API data if already available FloorplanApiClient.BuildingInfo building = SensorFusion.getInstance().getFloorplanBuilding(apiName); if (building != null) { currentFloorShapes = building.getFloorShapesList(); } - // Display API vector shapes for all buildings + // Draw shapes now if available; otherwise they will be drawn when + // setCurrentLocation is called again after the floorplan API responds. if (currentFloorShapes != null && !currentFloorShapes.isEmpty()) { drawFloorShapes(currentFloor); - isIndoorMapSet = true; + } + + } else if (inAnyBuilding && isIndoorMapSet + && (currentFloorShapes == null || currentFloorShapes.isEmpty())) { + // Already marked indoors but shapes weren't available yet — try again now + // that the API may have responded. + String apiName2; + switch (currentBuilding) { + case BUILDING_NUCLEUS: apiName2 = "nucleus_building"; break; + case BUILDING_LIBRARY: apiName2 = "library"; break; + case BUILDING_MURCHISON:apiName2 = "murchison_house"; break; + default: apiName2 = null; + } + if (apiName2 != null) { + FloorplanApiClient.BuildingInfo b = + SensorFusion.getInstance().getFloorplanBuilding(apiName2); + if (b != null) { + currentFloorShapes = b.getFloorShapesList(); + if (currentFloorShapes != null && !currentFloorShapes.isEmpty()) { + drawFloorShapes(currentFloor); + } + } } } else if (!inAnyBuilding && isIndoorMapSet) { From 738128bae95ee703ab19932e533b630a44244c11 Mon Sep 17 00:00:00 2001 From: JP10JOT <97289379+JapjotS@users.noreply.github.com> Date: Wed, 1 Apr 2026 16:32:28 +0100 Subject: [PATCH 25/36] Improve indoor tracking visuals & particle logic MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Multiple changes to reduce visual drift indoors, improve wall handling and show uncertainty. - RecordingFragment: remove time-based forced updates; only update displayed fused point on first point or when movement exceeds 0.5 m (prevents 1s drift updates). Simplified logging and removed last-update timestamp tracking. - TrajectoryMapFragment: add an accuracy circle (radius = particle-cloud RMS) and fade/orient marker when stationary; animate camera instead of jumpy moveCamera; sync floorplan color with trajectory toggle; change history dot rendering to higher base alpha and smaller dots; prevent drawing trajectory segments that would cross floorplan walls by consulting particleFilter.wouldCrossWall(); clean up accuracy circle on reset. - ParticleFilter: reduce roughening std (0.3 -> 0.08 m) to limit stationary drift; avoid reversing particles on wall intersection (keep particle at old position); ensure roughening doesn't place particles through walls; add public wouldCrossWall(LatLng,LatLng) for map-layer collision checks; inflate GNSS measurement uncertainty indoors (×3) to avoid pulling particles through walls; add getPositionUncertaintyMeters() to compute RMS particle spread. - SensorFusion: implement stationary detection using rolling variance of linear-acceleration (window=20) and expose isStationary(); suppress PDR step-prediction and WiFi-driven particle updates while stationary to avoid WiFi/GNSS jitter moving the dot; expose getPositionUncertaintyMeters() for UI. - IndoorMapManager: remove hard overlay shifts, add dynamic floorPlanColor with setter and redrawCurrentFloor(), and use the active color for wall/room stroke & fill so floorplan visuals match trajectory color. These changes reduce perceived jitter when the device is still, prevent trajectories from being drawn through walls, and surface position uncertainty for improved map UX. --- .../fragment/RecordingFragment.java | 51 ++------ .../fragment/TrajectoryMapFragment.java | 123 +++++++++++------- .../PositionMe/sensors/ParticleFilter.java | 71 +++++++--- .../PositionMe/sensors/SensorFusion.java | 79 +++++++++-- .../PositionMe/utils/IndoorMapManager.java | 41 ++++-- 5 files changed, 241 insertions(+), 124 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 b711f69b..704bc1e8 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 @@ -88,14 +88,11 @@ public class RecordingFragment extends Fragment { // References to the child map fragment private TrajectoryMapFragment trajectoryMapFragment; - // Update fused trajectory at least every 1 second - private static final long FUSED_UPDATE_INTERVAL_MS = 1000; + // Minimum displacement before the displayed point is moved. + // Keeps the marker locked while stationary; particle-filter noise is ~0.1-0.15 m + // per resample so 0.5 m gives comfortable headroom above the noise floor. + private static final double MOVEMENT_THRESHOLD_METERS = 0.5; - // Also update immediately when movement exceeds this threshold - private static final double MOVEMENT_THRESHOLD_METERS = 0.75; - - // Last time a fused point was sent to the map - private long lastFusedUpdateTimeMs = 0L; // Last fused point that was actually rendered private LatLng lastSentFusedPosition = null; @@ -307,50 +304,31 @@ private void updateUIandPosition() { // 2. The map fragment is ready to receive updates if (fusedPosition != null && trajectoryMapFragment != null) { - // Get the current system time in milliseconds - long now = System.currentTimeMillis(); - // --- CONDITION 1: First point --- // If no previous fused point has been sent to the map, // this is the very first update → must display it boolean isFirstPoint = (lastSentFusedPosition == null); - // --- CONDITION 2: Time-based update --- - // Check if at least 1 second has passed since the last update - boolean oneSecondElapsed = - (now - lastFusedUpdateTimeMs) >= FUSED_UPDATE_INTERVAL_MS; - - // --- CONDITION 3: Movement-based update --- - // Check if the user has moved a meaningful distance + // --- CONDITION 2: Movement-based update --- + // Only move the displayed point when the fused position has shifted + // at least MOVEMENT_THRESHOLD_METERS from the last displayed position. + // Time-based unconditional updates are intentionally omitted here — + // they caused the dot to drift every second even when stationary. boolean movementDetected = false; - - // Store how far the user moved (for debugging/logging) double movedDistance = 0.0; - // Only compute movement if we have a previous point if (lastSentFusedPosition != null) { - - // Calculate distance between last displayed point and current fused position movedDistance = UtilFunctions.distanceBetweenPoints( lastSentFusedPosition, fusedPosition ); - - // If movement exceeds threshold (e.g., 0.75m), trigger update movementDetected = movedDistance >= MOVEMENT_THRESHOLD_METERS; } - // --- FINAL DECISION --- - // Update the map if ANY of the following is true: - // - first point - // - 1 second passed - // - significant movement detected - if (isFirstPoint || oneSecondElapsed || movementDetected) { + if (isFirstPoint || movementDetected) { - // Log that we are updating the map (useful for debugging) Log.d("FUSED_TEST", "MAP UPDATE -> " + "first=" + isFirstPoint - + ", oneSecondElapsed=" + oneSecondElapsed + ", movementDetected=" + movementDetected + ", movedDistance=" + movedDistance + ", lat=" + fusedPosition.latitude @@ -368,15 +346,8 @@ private void updateUIandPosition() { // Save this position as the last displayed one lastSentFusedPosition = fusedPosition; - // Save the time of this update - lastFusedUpdateTimeMs = now; - } else { - - // Log that this update was skipped (for debugging performance) - Log.d("FUSED_TEST", "SKIPPED -> " - + "movedDistance=" + movedDistance - + ", elapsedMs=" + (now - lastFusedUpdateTimeMs)); + Log.d("FUSED_TEST", "SKIPPED -> movedDistance=" + movedDistance); } } 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 37fb368c..f73904fb 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 @@ -73,6 +73,9 @@ public class TrajectoryMapFragment extends Fragment { private boolean isRed = true; // Tracks whether the polyline color is red private boolean isGnssOn = true; // Tracks if GNSS tracking is enabled (matches default android:checked="true") + // Accuracy circle drawn around the position marker (radius = particle-cloud RMS spread) + private Circle accuracyCircle; + private Polyline gnssPolyline; // Polyline for GNSS path private LatLng lastGnssLocation = null; // Stores the last GNSS location @@ -285,6 +288,11 @@ public void onMapReady(@NonNull GoogleMap googleMap) { polyline.setColor(Color.RED); isRed = true; } + // Keep floorplan lines in sync with trajectory color + if (indoorMapManager != null) { + indoorMapManager.setFloorPlanColor(isRed ? Color.RED : Color.BLACK); + indoorMapManager.redrawCurrentFloor(); + } } }); @@ -361,14 +369,14 @@ private void drawHistory(List history, List rendered, int color) for (int i = 0; i < history.size(); i++) { LatLng point = history.get(i); - int alpha = (int) (255f * (i + 1) / history.size()); + // Fade oldest→newest but keep alpha high (150–255) so dots are always visible + int alpha = 150 + (int) (105f * (i + 1) / history.size()); int fadedColor = Color.argb(alpha, Color.red(color), Color.green(color), Color.blue(color)); Circle circle = gMap.addCircle(new CircleOptions() .center(point) - .radius(1.5) - .strokeWidth(2f) - .strokeColor(fadedColor) + .radius(0.6) + .strokeWidth(0f) .fillColor(fadedColor)); rendered.add(circle); @@ -662,6 +670,13 @@ public void updateUserLocation(@NonNull LatLng newLocation, float orientation) { LatLng oldLocation = this.currentLocation; this.currentLocation = displayLocation; + // Determine visual state from sensor fusion + boolean stationary = (sensorFusion != null) && sensorFusion.isStationary(); + float uncertainty = (sensorFusion != null) + ? sensorFusion.getPositionUncertaintyMeters() : 2f; + // Cap displayed radius: show at least 1 m, at most 15 m + double circleRadius = Math.min(Math.max(uncertainty, 1f), 15f); + // If no marker, create it if (orientationMarker == null) { orientationMarker = gMap.addMarker(new MarkerOptions() @@ -672,13 +687,38 @@ public void updateUserLocation(@NonNull LatLng newLocation, float orientation) { UtilFunctions.getBitmapFromVector(requireContext(), R.drawable.ic_baseline_navigation_24))) ); - gMap.moveCamera(CameraUpdateFactory.newLatLngZoom(displayLocation, 19f)); + gMap.animateCamera(CameraUpdateFactory.newLatLngZoom(displayLocation, 19f)); } else { - // Update marker position + orientation orientationMarker.setPosition(displayLocation); orientationMarker.setRotation(orientation); - // Move camera a bit - gMap.moveCamera(CameraUpdateFactory.newLatLng(displayLocation)); + // Smooth pan — animateCamera avoids the jarring jump of moveCamera + gMap.animateCamera(CameraUpdateFactory.newLatLng(displayLocation)); + } + + // Fade the marker when stationary so the user can clearly see the lock state + if (orientationMarker != null) { + orientationMarker.setAlpha(stationary ? 0.55f : 1.0f); + } + + // Draw / update the accuracy circle around the position marker + if (accuracyCircle == null) { + accuracyCircle = gMap.addCircle(new CircleOptions() + .center(displayLocation) + .radius(circleRadius) + .strokeWidth(1.5f) + .strokeColor(Color.argb(180, 33, 150, 243)) // blue outline + .fillColor(Color.argb(40, 33, 150, 243))); // faint blue fill + } else { + accuracyCircle.setCenter(displayLocation); + accuracyCircle.setRadius(circleRadius); + // Tint the circle green when stationary to give instant visual feedback + if (stationary) { + accuracyCircle.setStrokeColor(Color.argb(180, 76, 175, 80)); + accuracyCircle.setFillColor(Color.argb(40, 76, 175, 80)); + } else { + accuracyCircle.setStrokeColor(Color.argb(180, 33, 150, 243)); + accuracyCircle.setFillColor(Color.argb(40, 33, 150, 243)); + } } // Extend polyline if movement occurred @@ -696,9 +736,15 @@ public void updateUserLocation(@NonNull LatLng newLocation, float orientation) { points.add(displayLocation); polyline.setPoints(points); } else if (!oldLocation.equals(displayLocation)) { - // Subsequent movement: append a new polyline point - points.add(displayLocation); - polyline.setPoints(points); + // Wall guard: never draw a trajectory segment that crosses a floorplan wall. + // The particle filter holds the same wall geometry used to constrain particles, + // so we reuse its intersection test here before committing the point. + boolean crossesWall = sensorFusion != null + && sensorFusion.getParticleFilter().wouldCrossWall(oldLocation, displayLocation); + if (!crossesWall) { + points.add(displayLocation); + polyline.setPoints(points); + } } } @@ -890,6 +936,10 @@ public void clearMapAndReset() { gnssMarker.remove(); gnssMarker = null; } + if (accuracyCircle != null) { + accuracyCircle.remove(); + accuracyCircle = null; + } lastGnssLocation = null; currentLocation = null; floorplanFetchAttempted = false; @@ -1062,9 +1112,15 @@ public void run() { } /** - * Applies the best-guess floor immediately without debounce. - * Called once when auto-floor is first toggled on, so the user - * sees an instant correction after manually browsing wrong floors. + * Applies the sensor-authoritative floor immediately without debounce. + * Called once when auto-floor is re-enabled so the map display snaps back + * to the floor the sensors believe the user is on, regardless of any + * manual floor browsing the user did while auto-floor was off. + * + *

Priority: WiFi floor (if fresh) → PDR/barometric floor. + * Unlike {@link #evaluateAutoFloor()}, this path has no elevation-magnitude + * or proximity guards — those guards exist to avoid jittery periodic changes, + * but here we always want an immediate, unconditional sync.

*/ private void applyImmediateFloor() { if (sensorFusion == null || indoorMapManager == null) return; @@ -1073,44 +1129,21 @@ private void applyImmediateFloor() { updateWallsForPdr(); int candidateFloor; - // Use WiFi floor only when the last fix is fresh (within 30 s); otherwise fall through - // to the barometric path so stale WiFi data does not permanently override it. if (sensorFusion.getLatLngWifiPositioning() != null && sensorFusion.isWifiPositionFresh()) { + // WiFi gives the most reliable absolute floor when a fresh fix is available candidateFloor = sensorFusion.getWifiFloor(); } else { - float elevation = sensorFusion.getElevation(); - float floorHeight = indoorMapManager.getFloorHeight(); - if (floorHeight <= 0) { - floorHeight = mapMatchingConfig.baroHeightThreshold; - } - if (Math.abs(elevation) < mapMatchingConfig.baroHeightThreshold) { - return; // Ignore small height changes - } - if (floorHeight <= 0) return; - candidateFloor = Math.round(elevation / floorHeight); - - // Require proximity to stairs/lift before changing floors - boolean nearFeature = indoorMapManager.isNearCrossFloorFeature(mapMatchingConfig.crossFeatureProximity); - if (!nearFeature) { - return; - } - - // Use real horizontal acceleration to distinguish lift from stairs - float horizAccel = sensorFusion.getHorizontalAccelMagnitude(); - CrossFloorClassifier.Mode mode = - CrossFloorClassifier.classify(horizAccel, elevation, 0.0, mapMatchingConfig); - Log.d(TAG, "Auto-floor (baro) mode=" + mode + " elevation=" + elevation - + " horizAccel=" + horizAccel); - - // Only accept LIFT or STAIRS; UNKNOWN means not enough signal to change floor - if (mode == CrossFloorClassifier.Mode.UNKNOWN) { - return; - } + // Fall back to the PDR pipeline's continuously-maintained floor counter. + // This already incorporates every barometric transition since recording started, + // so it is correct even when the current elevation delta is near zero (e.g. ground floor). + candidateFloor = sensorFusion.getPdrCurrentFloor(); } indoorMapManager.setCurrentFloor(candidateFloor, true); updateFloorLabel(); - // Seed the debounce state so subsequent checks don't re-trigger immediately + Log.d(TAG, "applyImmediateFloor -> floor=" + candidateFloor); + + // Seed the debounce state so the first periodic evaluation doesn't re-trigger immediately lastCandidateFloor = candidateFloor; lastCandidateTime = SystemClock.elapsedRealtime(); } diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java index 32821fba..2e033591 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java @@ -69,7 +69,7 @@ public void setWalls(List wallSegments) { // Small positional noise added to each particle after resampling. // Prevents sample impoverishment: without roughening, particles copied from the // same parent are identical and provide no extra information on the next step. - private static final float ROUGHENING_STD = 0.3f; // metres + private static final float ROUGHENING_STD = 0.08f; // metres — reduced to limit stationary drift public void predict(float deltaEasting, float deltaNorthing) { if (!initialized) return; // ignore if not initialized @@ -99,14 +99,9 @@ public void predict(float deltaEasting, float deltaNorthing) { float newY = oldY + noisyStride * (float) Math.sin(noisyHeading); if (intersectsWall(oldX, oldY, newX, newY)) { - // Simple reflection: reverse direction and reduce stride (simulate energy loss) - float reflectedHeading = (float) (noisyHeading + Math.PI); // reverse direction - float reducedStride = noisyStride * 0.5f; // reduce stride to simulate energy loss - - particles[i][0] = oldX + reducedStride * (float) Math.cos(reflectedHeading); - particles[i][1] = oldY + reducedStride * (float) Math.sin(reflectedHeading); - - //weights[i] = 0f; could also delete the particle + // Stop at old position — can't pass through a wall. + // Keeping oldX/oldY is more stable than reversing direction, + // which can send the particle into an opposite wall. } else { particles[i][0] = newX; particles[i][1] = newY; @@ -117,7 +112,22 @@ public void predict(float deltaEasting, float deltaNorthing) { // exclusively in updateGNSS() and updateWiFi() when a new observation arrives. } - private boolean intersectsWall(float x1, float y1, float x2, float y2) { + /** + * Returns true if the straight line between two geographic positions would + * cross any loaded wall segment. Used by the map layer to prevent the + * trajectory polyline from being drawn through floorplan walls. + * + * @param from start of the candidate segment (WGS-84) + * @param to end of the candidate segment (WGS-84) + */ + public boolean wouldCrossWall(LatLng from, LatLng to) { + if (origin == null || walls == null || walls.isEmpty()) return false; + float[] fromENU = UtilFunctions.convertWGS84ToENU(origin, from); + float[] toENU = UtilFunctions.convertWGS84ToENU(origin, to); + return intersectsWall(fromENU[0], fromENU[1], toENU[0], toENU[1]); + } + + private boolean intersectsWall(float x1, float y1, float x2, float y2) { // Check if the line segment from (x1, y1) to (x2, y2) intersects any wall segment if (walls == null || walls.isEmpty()) return false; for (float[] wall : walls) { @@ -183,10 +193,17 @@ private void resample() { } particles = newParticles; - // Roughening: add small noise so copied particles diverge on the next step + // Roughening: add small noise so copied particles diverge on the next step. + // Wall-check each jitter so roughening doesn't land particles inside walls. for (int i = 0; i < NUM_PARTICLES; i++) { - particles[i][0] += (float) (random.nextGaussian() * ROUGHENING_STD); - particles[i][1] += (float) (random.nextGaussian() * ROUGHENING_STD); + float rx = (float) (random.nextGaussian() * ROUGHENING_STD); + float ry = (float) (random.nextGaussian() * ROUGHENING_STD); + float roughenedX = particles[i][0] + rx; + float roughenedY = particles[i][1] + ry; + if (!intersectsWall(particles[i][0], particles[i][1], roughenedX, roughenedY)) { + particles[i][0] = roughenedX; + particles[i][1] = roughenedY; + } } // Reset to uniform weights after resampling @@ -204,7 +221,11 @@ public void updateGNSS(LatLng gnssPosition, float gnssAccuracy) { float mx = mesurementENU[0]; //easting value of the measurement float my = mesurementENU[1]; //northing value of the measurement - float variance = gnssAccuracy * gnssAccuracy; // Convert accuracy to variance sigma^2 + // Indoors, GPS signals reflect off ceilings and walls, giving fixes several + // metres off and often on the wrong side of a wall. Inflate the uncertainty + // so the GNSS observation cannot drag particles through wall segments. + float effectiveAccuracy = walls.isEmpty() ? gnssAccuracy : gnssAccuracy * 3.0f; + float variance = effectiveAccuracy * effectiveAccuracy; // Convert accuracy to variance sigma^2 //gaussian likelihood function @@ -297,8 +318,28 @@ public LatLng getFusedPosition() { // Convert mean ENU back to WGS84 coordinates return UtilFunctions.convertENUToWGS84(origin, new float[]{meanEasting, meanNorthing, 0f}); + } - + /** + * Returns the RMS spread of particles around their weighted mean, in metres. + * Provides a live estimate of position uncertainty — large when particles are + * spread out, small when they are tightly clustered. + */ + public float getPositionUncertaintyMeters() { + if (!initialized) return Float.MAX_VALUE; + float meanE = 0f, meanN = 0f; + for (int i = 0; i < NUM_PARTICLES; i++) { + meanE += particles[i][0] * weights[i]; + meanN += particles[i][1] * weights[i]; + } + float varE = 0f, varN = 0f; + for (int i = 0; i < NUM_PARTICLES; i++) { + float dE = particles[i][0] - meanE; + float dN = particles[i][1] - meanN; + varE += weights[i] * dE * dE; + varN += weights[i] * dN * dN; + } + return (float) Math.sqrt(varE + varN); } } 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 d74faab2..26f67ba2 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -105,6 +105,14 @@ public class SensorFusion implements SensorEventListener { // the device is still, which drags particles out of the building. private LatLng lastGnssForFilter = null; + // Stationary detection — rolling variance of linear-acceleration magnitude. + // When variance stays below the threshold the phone is considered stationary, + // and both step-prediction and WiFi particle updates are suppressed. + private static final int STATIONARY_WINDOW = 20; + private static final float STATIONARY_THRESHOLD = 0.015f; // m²/s⁴ + private final List linearAccelWindow = new ArrayList<>(); + private boolean isStationary = false; + // Floorplan API cache (latest result from start-location step) @@ -183,7 +191,7 @@ public void setContext(Context context) { //particle filter eventHandler.setStepListener((deltaEasting, deltaNorthing) -> { - if (particleFilter.isInitialized()) { + if (particleFilter.isInitialized() && !isStationary) { particleFilter.predict(deltaEasting, deltaNorthing); } }); @@ -222,17 +230,20 @@ public void update(Object[] objList) { } wifiProcessor.registerObserver(objects -> { - new Handler(Looper.getMainLooper()).postDelayed(() -> { - LatLng wifiPosition = wifiPositionManager.getLatLngWifiPositioning(); - if (wifiPosition != null) { // Check for null to avoid errors when no WiFi position is available - if (!particleFilter.isInitialized()) { - particleFilter.initialise(wifiPosition, 20f); // Initial spread of 20 meters, can be tuned based on expected WiFi accuracy - } else { - particleFilter.updateWiFi(wifiPosition, 20f); + new Handler(Looper.getMainLooper()).postDelayed(() -> { + LatLng wifiPosition = wifiPositionManager.getLatLngWifiPositioning(); + if (wifiPosition != null) { + if (!particleFilter.isInitialized()) { + particleFilter.initialise(wifiPosition, 20f); + } else if (!isStationary) { + // Suppress WiFi-driven particle updates while stationary — + // indoor WiFi fluctuates 5-20 m even with no movement, which + // would drag particles around and make the dot drift on screen. + particleFilter.updateWiFi(wifiPosition, 20f); + } } - } - }, 1000); - }); + }, 1000); + }); } //endregion @@ -249,11 +260,38 @@ public void update(Object[] objList) { @Override public void onSensorChanged(SensorEvent sensorEvent) { eventHandler.handleSensorEvent(sensorEvent); + if (sensorEvent.sensor.getType() == Sensor.TYPE_LINEAR_ACCELERATION) { + updateStationaryState(sensorEvent.values[0], sensorEvent.values[1], sensorEvent.values[2]); + } if (sensorEvent.sensor.getType() == Sensor.TYPE_PRESSURE && recorder.isRecording()) { updateFloorLogic(); } } + /** + * Maintains a rolling variance of linear-acceleration magnitude. + * Sets {@code isStationary = true} when variance drops below the threshold, + * meaning the phone has been essentially still for the last STATIONARY_WINDOW samples. + */ + private void updateStationaryState(float x, float y, float z) { + float mag = (float) Math.sqrt(x * x + y * y + z * z); + linearAccelWindow.add(mag); + if (linearAccelWindow.size() > STATIONARY_WINDOW) { + linearAccelWindow.remove(0); + } + if (linearAccelWindow.size() < STATIONARY_WINDOW) return; + + float mean = 0f; + for (float v : linearAccelWindow) mean += v; + mean /= STATIONARY_WINDOW; + + float variance = 0f; + for (float v : linearAccelWindow) variance += (v - mean) * (v - mean); + variance /= STATIONARY_WINDOW; + + isStationary = variance < STATIONARY_THRESHOLD; + } + private void updateFloorLogic() { if (!pdrProcessing.getElevationList().isFull()) return; @@ -782,6 +820,25 @@ public ParticleFilter getParticleFilter() { return particleFilter; } + /** Returns true when the device has been stationary for the last sensor window. */ + public boolean isStationary() { + return isStationary; + } + + /** + * Returns the floor number currently tracked by the PDR/barometric pipeline. + * This is the sensor-authoritative floor — updated continuously by barometric + * transitions in {@link #updateFloorLogic()}, independent of what the map display shows. + */ + public int getPdrCurrentFloor() { + return pdrProcessing.getCurrentFloor(); + } + + /** Returns the RMS particle-cloud spread in metres (live position uncertainty). */ + public float getPositionUncertaintyMeters() { + return particleFilter.getPositionUncertaintyMeters(); + } + // Inject wall polylines (meters) into PDR for collision correction. public void setPdrWalls(List> wallPolylines) { //JAPJOT -- i have added this function to convert wall polylines into segments and pass to particle diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java b/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java index 340a9070..4a0b8d14 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java @@ -49,12 +49,11 @@ public class IndoorMapManager { R.drawable.libraryg, R.drawable.library1, R.drawable.library2, R.drawable.library3); - // Fine-tune these to shift the PNG floor-plan overlay without affecting building detection. - // +0.00005 ≈ +5 m north, -0.00001 ≈ -1 m west (1 deg lat ≈ 111 km, 1 deg lng ≈ 70 km here) - private static final double OVERLAY_SHIFT_LAT = 0.0001; - private static final double OVERLAY_SHIFT_LNG = 0.0; + // Lat/lng bounds for positioning ground overlay images on the map + static final float OVERLAY_SHIFT_LAT = 0; + static final float OVERLAY_SHIFT_LNG = 0; private static final LatLngBounds NUCLEUS_BOUNDS = new LatLngBounds( new LatLng(BuildingPolygon.NUCLEUS_SW.latitude + OVERLAY_SHIFT_LAT, BuildingPolygon.NUCLEUS_SW.longitude + OVERLAY_SHIFT_LNG), @@ -89,11 +88,14 @@ public class IndoorMapManager { public static final float LIBRARY_FLOOR_HEIGHT = 3.6F; public static final float MURCHISON_FLOOR_HEIGHT = 4.0F; - // Colours for different indoor feature types - private static final int WALL_STROKE = Color.argb(200, 80, 80, 80); - private static final int ROOM_STROKE = Color.argb(180, 33, 150, 243); - private static final int ROOM_FILL = Color.argb(40, 33, 150, 243); - private static final int DEFAULT_STROKE = Color.argb(150, 100, 100, 100); + // Active trajectory color — set by the user via the color toggle button. + // Floor plan shapes are drawn in this color so they stay visually consistent. + private int floorPlanColor = Color.RED; // default matches the default polyline color + + /** Call this whenever the user changes the trajectory polyline color. */ + public void setFloorPlanColor(int color) { + this.floorPlanColor = color; + } // Last known user location (lat/lng on map) private LatLng lastLocation; @@ -291,6 +293,13 @@ public void decreaseFloor() { *

Detection priority: floorplan API real polygon outlines first, * then legacy hard-coded rectangular boundaries as fallback.

*/ + /** Redraws the current floor with the latest color settings. */ + public void redrawCurrentFloor() { + if (currentFloorShapes != null && !currentFloorShapes.isEmpty()) { + drawFloorShapes(currentFloor); + } + } + private void setBuildingOverlay() { try { int detected = detectCurrentBuilding(); @@ -482,9 +491,12 @@ private void clearDrawnShapes() { * @return ARGB colour value */ private int getStrokeColor(String indoorType) { - if ("wall".equals(indoorType)) return WALL_STROKE; - if ("room".equals(indoorType)) return ROOM_STROKE; - return DEFAULT_STROKE; + int r = Color.red(floorPlanColor); + int g = Color.green(floorPlanColor); + int b = Color.blue(floorPlanColor); + if ("wall".equals(indoorType)) return Color.argb(220, r, g, b); + if ("room".equals(indoorType)) return Color.argb(160, r, g, b); + return Color.argb(180, r, g, b); } /** @@ -494,7 +506,10 @@ private int getStrokeColor(String indoorType) { * @return ARGB colour value */ private int getFillColor(String indoorType) { - if ("room".equals(indoorType)) return ROOM_FILL; + int r = Color.red(floorPlanColor); + int g = Color.green(floorPlanColor); + int b = Color.blue(floorPlanColor); + if ("room".equals(indoorType)) return Color.argb(40, r, g, b); return Color.TRANSPARENT; } From 32b6f6dca5108d3b153ab47e3fc3aa20abc780e0 Mon Sep 17 00:00:00 2001 From: JP10JOT <97289379+JapjotS@users.noreply.github.com> Date: Wed, 1 Apr 2026 16:43:22 +0100 Subject: [PATCH 26/36] Avoid consuming first point if map not ready MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Have TrajectoryMapFragment.updateUserLocation return a boolean indicating whether the GoogleMap was ready and the update was rendered, and only update RecordingFragment.lastSentFusedPosition when that call returns true. This prevents losing the "first-point" slot if gMap is still null during the fragment creation → onMapReady window, allowing the next tick to retry the initial render instead of skipping it. --- .../fragment/RecordingFragment.java | 16 +++++++------- .../fragment/TrajectoryMapFragment.java | 11 ++++++++-- .../PositionMe/sensors/ParticleFilter.java | 21 +++++++++++-------- 3 files changed, 30 insertions(+), 18 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 704bc1e8..dc52a6f2 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 @@ -334,17 +334,19 @@ private void updateUIandPosition() { + ", lat=" + fusedPosition.latitude + ", lng=" + fusedPosition.longitude); - // Send the fused position to the map: - // - updates marker position - // - rotates marker (orientation) - // - extends trajectory polyline - trajectoryMapFragment.updateUserLocation( + // Send the fused position to the map. + // updateUserLocation returns false when the Google Map is not ready yet + // (gMap == null during the brief window between fragment creation and + // onMapReady firing). In that case do NOT consume the first-point slot — + // keep lastSentFusedPosition null so the next tick retries as a first point. + boolean rendered = trajectoryMapFragment.updateUserLocation( fusedPosition, (float) Math.toDegrees(sensorFusion.passOrientation()) ); - // Save this position as the last displayed one - lastSentFusedPosition = fusedPosition; + if (rendered) { + lastSentFusedPosition = fusedPosition; + } } else { Log.d("FUSED_TEST", "SKIPPED -> movedDistance=" + movedDistance); 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 f73904fb..8f107337 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 @@ -657,8 +657,14 @@ public void addPdrObservation(@NonNull LatLng point) { * @param newLocation The new location to plot. * @param orientation The user’s heading (e.g. from sensor fusion). */ - public void updateUserLocation(@NonNull LatLng newLocation, float orientation) { - if (gMap == null) return; + /** + * Update the user's current location on the map. + * + * @return true if the map was ready and the update was rendered; + * false if {@code gMap} was not yet initialised (caller should retry). + */ + public boolean updateUserLocation(@NonNull LatLng newLocation, float orientation) { + if (gMap == null) return false; // Apply selected smoothing filter before rendering LatLng displayLocation = applySmoothing(newLocation); @@ -766,6 +772,7 @@ public void updateUserLocation(@NonNull LatLng newLocation, float orientation) { } wasIndoorMapSet = nowIndoorMapSet; } + return true; } diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java index 2e033591..2fa4284d 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java @@ -14,7 +14,11 @@ public class ParticleFilter { private static final int NUM_PARTICLES = 200; // Number of particles private float[][] particles; - //[num particles][2] is {easting, northing} + // [num particles][3] = {easting (m), northing (m), headingBias (rad)} + // headingBias is the estimated error between the phone's compass heading and the + // true walking direction. It starts near zero and is implicitly corrected during + // movement as particles with the right bias land closer to GNSS/WiFi observations + // and therefore accumulate higher weights. private float[] weights; // Weights for each particle private LatLng origin; // Origin point for ENU conversion @@ -25,21 +29,20 @@ public class ParticleFilter { private List walls = new ArrayList<>(); public void initialise(LatLng firstFix, float accuracyMeters) { - if (initialized) return; // ignore subsequent calls, DONE + if (initialized) return; // ignore subsequent calls this.origin = firstFix; - this.particles = new float[NUM_PARTICLES][2]; - - - this.weights = new float[NUM_PARTICLES]; + this.particles = new float[NUM_PARTICLES][3]; + this.weights = new float[NUM_PARTICLES]; float spread = accuracyMeters; for (int i = 0; i < NUM_PARTICLES; i++) { - particles[i][0] = (float) (random.nextGaussian() * spread); // east - particles[i][1] = (float) (random.nextGaussian() * spread); // north + particles[i][0] = (float) (random.nextGaussian() * spread); // east + particles[i][1] = (float) (random.nextGaussian() * spread); // north + particles[i][2] = (float) (random.nextGaussian() * INITIAL_BIAS_STD); // heading bias weights[i] = 1.0f / NUM_PARTICLES; } initialized = true; - Log.d(TAG, "Initialized at " + firstFix + " accuraccy is =" + accuracyMeters + "m"); + Log.d(TAG, "Initialized at " + firstFix + " accuracy=" + accuracyMeters + "m"); } public boolean isInitialized() { From 132013c993d1e92b2a5bfd700772ee0cff5b4046 Mon Sep 17 00:00:00 2001 From: JP10JOT <97289379+JapjotS@users.noreply.github.com> Date: Wed, 1 Apr 2026 17:26:23 +0100 Subject: [PATCH 27/36] Improve floor detection and particle filter robustness Rework floor-change and map-matching behavior and harden the particle filter. - SensorFusion: rewrite updateFloorLogic to use a fixed Nucleus floor height (5.5 m), require a 40% floor-height threshold before considering a change, compute absolute target floor, and only commit lift changes when barometric elevation is within a snap tolerance (0.8 m) and horizontal movement is low. Adds constants for nucleus floor height and lift snap tolerance and logs committed changes. - TrajectoryMapFragment: add a lift snap guard that prevents committing floor transitions mid-ascent/descent unless barometer is within 15% of the target level when horizontal accel is very low. - RecordingFragment: display barometric elevation immediately (no PDR needed) by moving elevation UI update earlier in updateUIandPosition(). - ParticleFilter: add per-particle heading bias (initial bias and slow random-walk drift), apply bias when predicting headings, persist bias during resampling, and switch GNSS/WiFi likelihoods to a robust Student-t (nu=4) formulation with tighter weight-underflow thresholds. Keep wall intersection handling and propagation logic. - IndoorMapManager & MapMatchingConfig: update default Nucleus floor height to 5.5 m and adjust map-matching defaults (baro threshold and feature proximity) to better match the new floor-height assumptions. These changes aim to reduce false floor detections (especially in lifts), improve resilience to GNSS/WiFi outliers, and model systematic compass/heading bias in the particle filter. --- .../fragment/RecordingFragment.java | 8 +- .../fragment/TrajectoryMapFragment.java | 11 +++ .../PositionMe/sensors/ParticleFilter.java | 56 +++++------ .../PositionMe/sensors/SensorFusion.java | 99 +++++++++++-------- .../PositionMe/utils/IndoorMapManager.java | 2 +- .../PositionMe/utils/MapMatchingConfig.java | 4 +- 6 files changed, 104 insertions(+), 76 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 dc52a6f2..3d4b5658 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 @@ -265,6 +265,10 @@ private void onAddTestPoint() { * Update the UI with sensor data and pass map updates to TrajectoryMapFragment. */ private void updateUIandPosition() { + // Elevation comes from the barometer — available immediately, no PDR needed. + float elevationVal = sensorFusion.getElevation(); + elevation.setText(getString(R.string.elevation, String.format("%.1f", elevationVal))); + float[] pdrValues = sensorFusion.getSensorValueMap().get(SensorTypes.PDR); if (pdrValues == null) return; @@ -273,10 +277,6 @@ private void updateUIandPosition() { + Math.pow(pdrValues[1] - previousPosY, 2)); distanceTravelled.setText(getString(R.string.meter, String.format("%.2f", distance))); - // Elevation - 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 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 8f107337..e53a88a8 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 @@ -1203,6 +1203,17 @@ private void evaluateAutoFloor() { // Use real horizontal acceleration so LIFT (low horiz) vs STAIRS (high horiz) is distinguished float horizAccel = sensorFusion.getHorizontalAccelMagnitude(); + + // Lift snap guard: while inside a lift (near-zero horizontal movement) only commit + // once the barometer is within 15% of floor height from the exact target level, + // preventing false triggers mid-ascent/descent. + if (horizAccel < 0.3f) { + float targetElev = candidateFloor * floorHeight; + if (Math.abs(elevation - targetElev) > floorHeight * 0.15f) { + return; + } + } + CrossFloorClassifier.Mode mode = CrossFloorClassifier.classify(horizAccel, elevation, 0.0, mapMatchingConfig); Log.d(TAG, "Auto-floor (baro) mode=" + mode + " elevation=" + elevation diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java index 2fa4284d..734e0459 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java @@ -28,6 +28,11 @@ public class ParticleFilter { // Wall segments: {x1, y1, x2, y2} in ENU private List walls = new ArrayList<>(); + // ~17° initial heading-bias uncertainty, matching Woodman & Harle (UbiComp 2008). + private static final float INITIAL_BIAS_STD = 0.3f; + // Slow random-walk drift on heading bias per step (rad). + private static final float BIAS_DRIFT_STD = 0.01f; + public void initialise(LatLng firstFix, float accuracyMeters) { if (initialized) return; // ignore subsequent calls this.origin = firstFix; @@ -82,19 +87,14 @@ public void predict(float deltaEasting, float deltaNorthing) { float heading = (float) Math.atan2(deltaNorthing, deltaEasting); // calculate heading from deltas for (int i = 0; i < NUM_PARTICLES; i++) { - // Add noise to heading and stride - float noisyHeading = heading + (float) (random.nextGaussian() * HEADING_NOISE_STD); - float noisyStride = stride + (float) (random.nextGaussian() * STRIDE_LENGTH_NOISE_STD); - - // Update particle position based on noisy heading and stride - - //JAPJOT: ive commented this out to add wall collision checking, we need to check if the line from old position to new position intersects any walls, if it does, we can either discard the particle (set weight to 0) - // or reflect it off the wall (more complex). and particles that hit walls will be reflected in opposite direction - //like dvd logo - - // particles[i][0] += noisyStride * Math.cos(noisyHeading); // update east - // particles[i][1] += noisyStride * Math.sin(noisyHeading); // update north - + // Apply this particle's estimated heading bias before adding step noise. + // Based on: Woodman, O.J. & Harle, R. (2008). "Pedestrian localisation for + // indoor environments." UbiComp 2008, pp. 114-123. + // The bias corrects systematic compass error so particles that are aligned + // with the true walking direction accumulate higher weights over time. + float correctedHeading = heading + particles[i][2]; + float noisyHeading = correctedHeading + (float) (random.nextGaussian() * HEADING_NOISE_STD); + float noisyStride = stride + (float) (random.nextGaussian() * STRIDE_LENGTH_NOISE_STD); float oldX = particles[i][0]; float oldY = particles[i][1]; @@ -103,12 +103,13 @@ public void predict(float deltaEasting, float deltaNorthing) { if (intersectsWall(oldX, oldY, newX, newY)) { // Stop at old position — can't pass through a wall. - // Keeping oldX/oldY is more stable than reversing direction, - // which can send the particle into an opposite wall. } else { particles[i][0] = newX; particles[i][1] = newY; } + + // Heading bias random walk: bias drifts slowly each step. + particles[i][2] += (float) (random.nextGaussian() * BIAS_DRIFT_STD); } // Weights are intentionally NOT updated or resampled here. // predict() only propagates particle positions; weight updates happen @@ -180,7 +181,7 @@ private float computeEffectiveSampleSize() { * a point mass on the next predict step.

*/ private void resample() { - float[][] newParticles = new float[NUM_PARTICLES][2]; + float[][] newParticles = new float[NUM_PARTICLES][3]; float step = 1.0f / NUM_PARTICLES; float cumulativeWeight = weights[0]; float randomStart = random.nextFloat() * step; @@ -193,6 +194,7 @@ private void resample() { } newParticles[i][0] = particles[j][0]; newParticles[i][1] = particles[j][1]; + newParticles[i][2] = particles[j][2]; } particles = newParticles; @@ -231,20 +233,20 @@ public void updateGNSS(LatLng gnssPosition, float gnssAccuracy) { float variance = effectiveAccuracy * effectiveAccuracy; // Convert accuracy to variance sigma^2 - //gaussian likelihood function - + // Student-t likelihood (ν=4) — heavier tails than Gaussian, robust to outlier GNSS fixes. + // Based on: Nurminen, H. et al. (2013). "Particle Filter and Smoother for Indoor + // Localization." IPIN 2013. w_i ∝ (1 + d²/(ν·σ²))^(-(ν+2)/2), ν=4 → exponent=-3. + final float nu = 4.0f; float weightSum = 0f; for (int i = 0; i < NUM_PARTICLES; i++) { float dx = particles[i][0] - mx; float dy = particles[i][1] - my; float distanceSquared = dx * dx + dy * dy; - - // Calculate weight using Gaussian likelihood - weights[i] *= (float) Math.exp(-distanceSquared / (2 * variance)); + weights[i] *= (float) Math.pow(1.0f + distanceSquared / (nu * variance), -(nu + 2f) / 2f); weightSum += weights[i]; } - if (weightSum < 1e-6) { + if (weightSum < 1e-10f) { for (int i = 0; i < NUM_PARTICLES; i++) { weights[i] = 1.0f / NUM_PARTICLES; } @@ -274,20 +276,18 @@ public void updateWiFi(LatLng wifiPosition, float wifiAccuracy) { float variance = wifiAccuracy * wifiAccuracy; // Convert accuracy to variance sigma^2 - //gaussian likelihood function - + // Student-t likelihood (ν=4) — same robust formulation as updateGNSS(). + final float nu = 4.0f; float weightSum = 0f; for (int i = 0; i < NUM_PARTICLES; i++) { float dx = particles[i][0] - mx; float dy = particles[i][1] - my; float distanceSquared = dx * dx + dy * dy; - - // Calculate weight using Gaussian likelihood - weights[i] *= (float) Math.exp(-distanceSquared / (2 * variance)); + weights[i] *= (float) Math.pow(1.0f + distanceSquared / (nu * variance), -(nu + 2f) / 2f); weightSum += weights[i]; } - if (weightSum < 1e-6) { + if (weightSum < 1e-10f) { for (int i = 0; i < NUM_PARTICLES; i++) { weights[i] = 1.0f / NUM_PARTICLES; } 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 26f67ba2..f491ed28 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -292,56 +292,73 @@ private void updateStationaryState(float x, float y, float z) { isStationary = variance < STATIONARY_THRESHOLD; } + // Nucleus floor-to-floor height in metres (GF=0, F1=5.5, F2=11, F3=16.5). + // Used for absolute floor index computation from barometric elevation. + private static final float NUCLEUS_FLOOR_HEIGHT_M = 5.5f; + // Lift snap tolerance: only commit a lift floor change when the barometer elevation + // is within this distance of the exact target level (prevents mid-ascent false triggers). + private static final float LIFT_SNAP_TOLERANCE_M = 0.8f; + private void updateFloorLogic() { if (!pdrProcessing.getElevationList().isFull()) return; - + float finishAvg = (float) pdrProcessing.getElevationList().getListCopy().stream() .mapToDouble(f -> f).average().orElse(0.0); float diff = finishAvg - pdrProcessing.getStartElevation(); - int floorHeight = pdrProcessing.getFloorHeightValue(); - - if (Math.abs(diff) > floorHeight) { - int targetFloor = pdrProcessing.getCurrentFloor() + (int)(diff / floorHeight); - LatLng pos = getFusedPosition(); - if (pos == null) return; - - FloorplanApiClient.BuildingInfo building = getFloorplanBuilding(getSelectedBuildingId()); - if (building == null) return; - - int currentFloorIdx = pdrProcessing.getCurrentFloor() + 1; // Assuming bias 1 for Nucleus/Murchison - if (currentFloorIdx < 0 || currentFloorIdx >= building.getFloorShapesList().size()) return; - - FloorplanApiClient.FloorShapes floorShapes = building.getFloorShapesList().get(currentFloorIdx); - boolean nearTransition = false; - String type = "unknown"; - - for (FloorplanApiClient.MapShapeFeature feature : floorShapes.getFeatures()) { - String fType = feature.getIndoorType(); - if ("lift".equals(fType) || "stairs".equals(fType)) { - for (List part : feature.getParts()) { - if (BuildingPolygon.pointInPolygon(pos, part)) { - nearTransition = true; - type = fType; - break; - } + float floorHeight = NUCLEUS_FLOOR_HEIGHT_M; + + // Require at least 40% of floor height before evaluating a change. + if (Math.abs(diff) < floorHeight * 0.4f) return; + + // Absolute floor index from recording origin (not relative to current floor). + // Math.round avoids cumulative integer-truncation errors. + int targetFloor = Math.round(diff / floorHeight); + if (targetFloor == pdrProcessing.getCurrentFloor()) return; + + LatLng pos = getFusedPosition(); + if (pos == null) return; + + FloorplanApiClient.BuildingInfo building = getFloorplanBuilding(getSelectedBuildingId()); + if (building == null) return; + + // Use current floor + bias to index into the building's floor list. + int currentFloorIdx = pdrProcessing.getCurrentFloor() + 1; // +1 bias for Nucleus/Murchison (GF at index 1) + if (currentFloorIdx < 0 || currentFloorIdx >= building.getFloorShapesList().size()) return; + + FloorplanApiClient.FloorShapes floorShapes = building.getFloorShapesList().get(currentFloorIdx); + boolean nearTransition = false; + String type = "unknown"; + + for (FloorplanApiClient.MapShapeFeature feature : floorShapes.getFeatures()) { + String fType = feature.getIndoorType(); + if ("lift".equals(fType) || "stairs".equals(fType)) { + for (List part : feature.getParts()) { + if (BuildingPolygon.pointInPolygon(pos, part)) { + nearTransition = true; + type = fType; + break; } } - if (nearTransition) break; } + if (nearTransition) break; + } - if (nearTransition) { - // Classification logic: lift vs stairs - // We check if the user is moving horizontally during the elevation change - float horizontalMovement = (float) Math.sqrt( - Math.pow(state.filteredAcc[0], 2) + Math.pow(state.filteredAcc[1], 2) - ); - - boolean isLift = horizontalMovement < 0.2f; // threshold for "near zero" horizontal stride/acceleration - - if (("lift".equals(type) && isLift) || ("stairs".equals(type) && !isLift)) { - pdrProcessing.setCurrentFloor(targetFloor); - } - } + if (!nearTransition) return; + + float horizontalMovement = (float) Math.sqrt( + Math.pow(state.filteredAcc[0], 2) + Math.pow(state.filteredAcc[1], 2)); + boolean isLift = horizontalMovement < 0.2f; + + if ("lift".equals(type) && isLift) { + // Snap-to-floor guard: only commit once the barometer reads within + // LIFT_SNAP_TOLERANCE_M of the exact target level (GF=0, F1=5.5, F2=11 …). + float targetElev = targetFloor * floorHeight; + if (Math.abs(diff - targetElev) > LIFT_SNAP_TOLERANCE_M) return; + pdrProcessing.setCurrentFloor(targetFloor); + Log.d(TAG, "Floor change (lift) → " + targetFloor + " diff=" + diff); + } else if ("stairs".equals(type) && !isLift) { + pdrProcessing.setCurrentFloor(targetFloor); + Log.d(TAG, "Floor change (stairs) → " + targetFloor + " diff=" + diff); } } diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java b/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java index 4a0b8d14..197d8508 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java @@ -84,7 +84,7 @@ public class IndoorMapManager { private List currentFloorShapes; // Average floor heights per building (meters), used for barometric auto-floor - public static final float NUCLEUS_FLOOR_HEIGHT = 4.2F; + public static final float NUCLEUS_FLOOR_HEIGHT = 5.5F; public static final float LIBRARY_FLOOR_HEIGHT = 3.6F; public static final float MURCHISON_FLOOR_HEIGHT = 4.0F; diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/MapMatchingConfig.java b/app/src/main/java/com/openpositioning/PositionMe/utils/MapMatchingConfig.java index b8ff4dc0..258ccb9a 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/MapMatchingConfig.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/MapMatchingConfig.java @@ -23,8 +23,8 @@ public class MapMatchingConfig { public final float crossFeatureProximity; public MapMatchingConfig() { - // Defaults chosen to align with current app behavior (floor height ~4m, small wall padding) - this(4.0f, 1.5f, 2.0f, 0.2f, 5.0f); + // Defaults: Nucleus floor height 5.5 m, 3 m baro threshold, 10 m feature proximity + this(3.0f, 1.5f, 2.0f, 0.2f, 10.0f); } public MapMatchingConfig(float baroHeightThreshold, From eb7d52acaf35858eba344f4f2605bad94899c79c Mon Sep 17 00:00:00 2001 From: JP10JOT <97289379+JapjotS@users.noreply.github.com> Date: Wed, 1 Apr 2026 17:37:54 +0100 Subject: [PATCH 28/36] Refine sensor fusion WiFi/GNSS handling and logs MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Tighten and clarify sensor-fusion behaviour and add debug logging. Imports android.util.Log and adjusts WiFi initialization to prefer WiFi indoors (initial spread 15m). Adds divergence recovery: compares fused position vs WiFi anchor using particle uncertainty and resets+re-initialises the filter if distance > max(2.5×uncertainty, 20m), otherwise applies WiFi update. Slightly changes WiFi update flow to guard null fused state. Inflate GNSS initialization spread aggressively (initSpread = max(accuracy*4, 25m) ) to account for indoor multipath so WiFi corrections can converge without frequent resets. Add debug TAGs/logs for floor-change handling and temporarily comment out stairs floor-set to avoid unintended updates. --- .../PositionMe/sensors/SensorFusion.java | 42 +++++++++++++++---- 1 file changed, 35 insertions(+), 7 deletions(-) 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 f491ed28..b8904a0e 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -12,6 +12,7 @@ import android.os.Handler; import android.os.Looper; import android.os.SystemClock; +import android.util.Log; import android.widget.Toast; // import android.graphics.PointF; //import com.openpositioning.PositionMe.utils.WiFiPositioning; @@ -234,12 +235,30 @@ public void update(Object[] objList) { LatLng wifiPosition = wifiPositionManager.getLatLngWifiPositioning(); if (wifiPosition != null) { if (!particleFilter.isInitialized()) { - particleFilter.initialise(wifiPosition, 20f); + // WiFi is the preferred initial anchor indoors — more accurate than GNSS. + particleFilter.initialise(wifiPosition, 15f); } else if (!isStationary) { - // Suppress WiFi-driven particle updates while stationary — - // indoor WiFi fluctuates 5-20 m even with no movement, which - // would drag particles around and make the dot drift on screen. - particleFilter.updateWiFi(wifiPosition, 20f); + // Divergence recovery (Thrun, Burgard & Fox 2005, "Probabilistic Robotics", + // If WiFi contradicts the current estimate by more than 2.5× the particle + // spread, the filter has likely converged to a wrong location (e.g. from a + // bad indoor GNSS fix). Reset at the WiFi anchor so re-convergence can happen. + LatLng fused = particleFilter.getFusedPosition(); + float uncertainty = particleFilter.getPositionUncertaintyMeters(); + if (fused != null) { + double wifiDist = UtilFunctions.distanceBetweenPoints(fused, wifiPosition); + float divergenceThreshold = Math.max(uncertainty * 2.5f, 20f); + if (wifiDist > divergenceThreshold) { + Log.d("SensorFusion", "WiFi-reset: filter diverged " + + (int) wifiDist + " m (threshold " + (int) divergenceThreshold + " m)"); + particleFilter.reset(); + particleFilter.initialise(wifiPosition, 15f); + lastGnssForFilter = wifiPosition; // re-anchor GNSS displacement gating + } else { + particleFilter.updateWiFi(wifiPosition, 20f); + } + } else { + particleFilter.updateWiFi(wifiPosition, 20f); + } } } }, 1000); @@ -355,9 +374,12 @@ private void updateFloorLogic() { float targetElev = targetFloor * floorHeight; if (Math.abs(diff - targetElev) > LIFT_SNAP_TOLERANCE_M) return; pdrProcessing.setCurrentFloor(targetFloor); + String TAG = "FLOOR CHANGE LIFT"; Log.d(TAG, "Floor change (lift) → " + targetFloor + " diff=" + diff); } else if ("stairs".equals(type) && !isLift) { - pdrProcessing.setCurrentFloor(targetFloor); + //pdrProcessing.setCurrentFloor(targetFloor); + String TAG = "FLOOR CHANGE STAIRS"; + Log.d(TAG, "Floor change (stairs) → " + targetFloor + " diff=" + diff); } } @@ -890,7 +912,13 @@ public void onLocationChanged(@NonNull Location location) { float accuracy = location.hasAccuracy() ? location.getAccuracy() : 20f; if (!particleFilter.isInitialized()) { - particleFilter.initialise(gnssPos, accuracy); + // Inflate spread aggressively for indoor GNSS initialization. + // Reported accuracy is typically 5-15 m, but indoor multipath error is 20-100 m. + // A 25 m minimum spread ensures particles cover enough area for WiFi corrections + // to work without a full filter reset. Ref: Davidson et al. (2010), "Application + // of particle filters to a map-aided indoor positioning system," IEEE/ION PLANS. + float initSpread = Math.max(accuracy * 4f, 25f); + particleFilter.initialise(gnssPos, initSpread); lastGnssForFilter = gnssPos; } else { // Only feed GNSS into the particle filter if the reported position has moved From 205acb7df2193aadf775ecc0f24b9a285f30d89f Mon Sep 17 00:00:00 2001 From: JP10JOT <97289379+JapjotS@users.noreply.github.com> Date: Wed, 1 Apr 2026 17:47:37 +0100 Subject: [PATCH 29/36] Align walls to PF origin; sync PDR; reject GNSS MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Ensure wall geometry is built in the same ENU frame as the particle filter by using the particle filter origin when available (prevents particles appearing to pass through walls). Add SensorEventHandler.resetStepOrigin() and call it when starting recording so the PDR step-delta baseline is synced after resetPDR(), avoiding a large spurious first-step delta. Add GNSS outlier gating in SensorFusion: when filter uncertainty is low, reject GNSS fixes that are implausibly far from the fused position (offset > max(2.5×accuracy, 20m)) and log rejections. Minor comments and formatting adjustments included. --- .../fragment/TrajectoryMapFragment.java | 15 ++++++++-- .../sensors/SensorEventHandler.java | 11 ++++++++ .../PositionMe/sensors/SensorFusion.java | 28 ++++++++++++++++--- 3 files changed, 47 insertions(+), 7 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 e53a88a8..d696a176 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 @@ -1299,15 +1299,24 @@ private void updateWallsForPdr() { LatLng current = indoorMapManager.getLastLocation(); if (current == null) return; - if (wallOrigin == null) { + // CRITICAL: build wall geometry in the SAME ENU frame as the particle positions. + // The particle filter's origin is the anchor for all (easting, northing) coordinates. + // If wallOrigin differs from particleFilter.origin, wall segments are offset and + // particles appear to phase through them even though the intersection check is correct. + LatLng pfOrigin = sensorFusion.getParticleFilter().getOrigin(); + if (pfOrigin != null) { + // Particle filter is initialized — use its origin so both frames are aligned. + wallOrigin = pfOrigin; + } else if (wallOrigin == null) { + // Filter not yet initialized; use current location as a temporary origin. + // It will be recomputed once the filter has an origin. wallOrigin = current; } FloorplanApiClient.FloorShapes floor = indoorMapManager.getCurrentFloorShape(); if (floor == null) return; - List> walls = WallGeometryBuilder.buildWalls( - floor, wallOrigin); + List> walls = WallGeometryBuilder.buildWalls(floor, wallOrigin); sensorFusion.setPdrWalls(walls); } } 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 7068775e..5d57eaeb 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java @@ -35,6 +35,17 @@ public void setStepListener(StepListener listener) { this.stepListener = listener; // Set the step listener for particle filter updates } + /** + * Resets the PDR step-delta baseline to the current PDR origin (zero). + * Must be called after {@link PdrProcessing#resetPDR()} so that the first step of a new + * recording session does not produce a huge spurious delta against the previous session's + * accumulated position. + */ + public void resetStepOrigin() { + lastEasting = 0f; + lastNorthing = 0f; + } + // END OF PARTICLE FILTER 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 b8904a0e..d1ce55dc 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -239,7 +239,7 @@ public void update(Object[] objList) { particleFilter.initialise(wifiPosition, 15f); } else if (!isStationary) { // Divergence recovery (Thrun, Burgard & Fox 2005, "Probabilistic Robotics", - // If WiFi contradicts the current estimate by more than 2.5× the particle + // If WiFi contradicts the current estimate by more than 2.5× the particle // spread, the filter has likely converged to a wrong location (e.g. from a // bad indoor GNSS fix). Reset at the WiFi anchor so re-convergence can happen. LatLng fused = particleFilter.getFusedPosition(); @@ -501,8 +501,11 @@ private void stopWirelessCollectors() { * @see SensorCollectionService */ public void startRecording() { - recorder.startRecording(pdrProcessing); + recorder.startRecording(pdrProcessing); // calls pdrProcessing.resetPDR() internally eventHandler.resetBootTime(recorder.getBootTime()); + // Sync step-delta baseline with the freshly-zeroed PDR so the first step of this + // session doesn't fire a massive spurious delta from the previous session's position. + eventHandler.resetStepOrigin(); // Handover WiFi/BLE scan lifecycle from activity callbacks to foreground service. stopWirelessCollectors(); @@ -930,8 +933,25 @@ public void onLocationChanged(@NonNull Location location) { || (UtilFunctions.distanceBetweenPoints(lastGnssForFilter, gnssPos) >= minDisplacement); if (shouldUpdate) { - particleFilter.updateGNSS(gnssPos, accuracy); - lastGnssForFilter = gnssPos; + // Outlier gate: if the filter has converged (low uncertainty) and the GNSS + // fix is implausibly far from the current estimate, reject it as indoor + // multipath. This protects a good WiFi-anchored position from being + // overridden by a bad GPS reflection. + float uncertainty = particleFilter.getPositionUncertaintyMeters(); + LatLng fused = particleFilter.getFusedPosition(); + boolean outlier = false; + if (fused != null && uncertainty < 12f) { + double offset = UtilFunctions.distanceBetweenPoints(fused, gnssPos); + if (offset > Math.max(accuracy * 2.5f, 20f)) { + outlier = true; + Log.d("SensorFusion", "GNSS outlier rejected: " + (int) offset + + "m offset, uncertainty=" + (int) uncertainty + "m"); + } + } + if (!outlier) { + particleFilter.updateGNSS(gnssPos, accuracy); + lastGnssForFilter = gnssPos; + } } } } From b537a1c7a8c0512f07059e0fe35b1c32df7d8aa7 Mon Sep 17 00:00:00 2001 From: JP10JOT <97289379+JapjotS@users.noreply.github.com> Date: Wed, 1 Apr 2026 19:33:34 +0100 Subject: [PATCH 30/36] Align walls to PF origin; sync PDR; reject GNSS MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Ensure wall geometry is built in the same ENU frame as the particle filter by using the particle filter origin when available (prevents particles appearing to pass through walls). Add SensorEventHandler.resetStepOrigin() and call it when starting recording so the PDR step-delta baseline is synced after resetPDR(), avoiding a large spurious first-step delta. Add GNSS outlier gating in SensorFusion: when filter uncertainty is low, reject GNSS fixes that are implausibly far from the fused position (offset > max(2.5×accuracy, 20m)) and log rejections. Minor comments and formatting adjustments included. --- .../activity/RecordingActivity.java | 87 ++++------------ .../fragment/RecordingFragment.java | 85 +++++++++------- .../fragment/TrajectoryMapFragment.java | 12 +++ .../sensors/SensorEventHandler.java | 10 +- .../PositionMe/sensors/SensorFusion.java | 99 +++++++++++++++---- .../PositionMe/utils/PathView.java | 35 ++++--- 6 files changed, 188 insertions(+), 140 deletions(-) diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/activity/RecordingActivity.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/activity/RecordingActivity.java index 9497848e..0ca473bf 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/presentation/activity/RecordingActivity.java +++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/activity/RecordingActivity.java @@ -13,36 +13,20 @@ import com.openpositioning.PositionMe.R; import com.openpositioning.PositionMe.sensors.SensorFusion; import com.openpositioning.PositionMe.service.SensorCollectionService; -import com.openpositioning.PositionMe.presentation.fragment.StartLocationFragment; import com.openpositioning.PositionMe.presentation.fragment.RecordingFragment; import com.openpositioning.PositionMe.presentation.fragment.CorrectionFragment; /** - * The RecordingActivity manages the recording flow of the application, guiding the user through a sequence - * of screens for location selection, recording, and correction before finalizing the process. - *

- * This activity follows a structured workflow: - *

    - *
  1. StartLocationFragment - Allows users to select their starting location.
    2. - *
  2. RecordingFragment - Handles the recording process and contains a TrajectoryMapFragment.
    3. - *
  3. CorrectionFragment - Enables users to review and correct recorded data before completion.
    4. - *
- *

- * The activity ensures that the screen remains on during the recording process to prevent interruptions. - * It also provides fragment transactions for seamless navigation between different stages of the workflow. - *

- * This class is referenced in various fragments such as HomeFragment, StartLocationFragment, - * RecordingFragment, and CorrectionFragment to control navigation through the recording flow. + * Manages the recording flow: name dialog → live recording → correction. + * The start location is anchored automatically from the first GPS fix; + * no manual pin-drop screen is required. * - * @see StartLocationFragment The first step in the recording process where users select their starting location. - * @see RecordingFragment Handles data recording and map visualization. - * @see CorrectionFragment Allows users to review and make corrections before finalizing the process. - * @see com.openpositioning.PositionMe.R.layout#activity_recording The associated layout for this activity. + * @see RecordingFragment live map and sensor data during walking. + * @see CorrectionFragment review/correct before upload. * * @author ShuGu */ - public class RecordingActivity extends AppCompatActivity { @Override @@ -51,32 +35,18 @@ protected void onCreate(@Nullable Bundle savedInstanceState) { setContentView(R.layout.activity_recording); if (savedInstanceState == null) { - // Show trajectory name input dialog before proceeding to start location showTrajectoryNameDialog(); } - // Keep screen on getWindow().addFlags(WindowManager.LayoutParams.FLAG_KEEP_SCREEN_ON); } - /** - * {@inheritDoc} - * Re-registers sensor listeners so that IMU, step detection, barometer and other - * movement sensors remain active while this activity is in the foreground. - * Without this, sensors are unregistered when {@link MainActivity#onPause()} fires - * during the activity transition, leaving PDR and elevation updates dead. - */ @Override protected void onResume() { super.onResume(); SensorFusion.getInstance().resumeListening(); } - /** - * {@inheritDoc} - * Stops sensor listeners when this activity is no longer visible, unless - * the foreground {@link SensorCollectionService} is running (recording in progress). - */ @Override protected void onPause() { super.onPause(); @@ -86,9 +56,8 @@ protected void onPause() { } /** - * Shows an AlertDialog prompting the user to enter a trajectory name. - * The name is stored in SensorFusion as trajectory_id and later written to the protobuf. - * After input, proceeds to StartLocationFragment. + * Prompts for a trajectory name, then starts recording immediately. + * The GPS anchor is written automatically on the first location fix. */ private void showTrajectoryNameDialog() { EditText input = new EditText(this); @@ -101,36 +70,24 @@ private void showTrajectoryNameDialog() { .setMessage("Enter a name for this recording session:") .setView(input) .setCancelable(false) - .setPositiveButton("Save", (dialog, which) -> { + .setPositiveButton("Start", (dialog, which) -> { String name = input.getText().toString().trim(); - if (name.isEmpty()) { - // Default name based on timestamp - name = "traj_" + System.currentTimeMillis(); - } - SensorFusion.getInstance().setTrajectoryId(name); - showStartLocationScreen(); + if (name.isEmpty()) name = "traj_" + System.currentTimeMillis(); + SensorFusion sf = SensorFusion.getInstance(); + sf.setTrajectoryId(name); + sf.startRecording(); + showRecordingScreen(); }) .setNegativeButton("Skip", (dialog, which) -> { - // Use default name - SensorFusion.getInstance().setTrajectoryId( - "traj_" + System.currentTimeMillis()); - showStartLocationScreen(); + SensorFusion sf = SensorFusion.getInstance(); + sf.setTrajectoryId("traj_" + System.currentTimeMillis()); + sf.startRecording(); + showRecordingScreen(); }) .show(); } - /** - * Show the StartLocationFragment (beginning of flow). - */ - public void showStartLocationScreen() { - FragmentTransaction ft = getSupportFragmentManager().beginTransaction(); - ft.replace(R.id.mainFragmentContainer, new StartLocationFragment()); - ft.commit(); - } - - /** - * Show the RecordingFragment, which contains the TrajectoryMapFragment internally. - */ + /** Show the RecordingFragment (live map + sensors). */ public void showRecordingScreen() { FragmentTransaction ft = getSupportFragmentManager().beginTransaction(); ft.replace(R.id.mainFragmentContainer, new RecordingFragment()); @@ -138,9 +95,7 @@ public void showRecordingScreen() { ft.commit(); } - /** - * Show the CorrectionFragment after the user stops recording. - */ + /** Show the CorrectionFragment after the user stops recording. */ public void showCorrectionScreen() { FragmentTransaction ft = getSupportFragmentManager().beginTransaction(); ft.replace(R.id.mainFragmentContainer, new CorrectionFragment()); @@ -148,9 +103,7 @@ public void showCorrectionScreen() { ft.commit(); } - /** - * Finish the Activity (or do any final steps) once corrections are done. - */ + /** Finish the Activity once corrections are done. */ public void finishFlow() { getWindow().clearFlags(WindowManager.LayoutParams.FLAG_KEEP_SCREEN_ON); finish(); 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 3d4b5658..ad2617d5 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 @@ -93,10 +93,19 @@ public class RecordingFragment extends Fragment { // per resample so 0.5 m gives comfortable headroom above the noise floor. private static final double MOVEMENT_THRESHOLD_METERS = 0.5; + // Maximum single-tick displacement accepted from the fused estimate. + // Jumps larger than this are treated as filter teleports: the PDR dead-reckoning + // fallback is used instead so the trajectory stays physically continuous. + private static final double MAX_JUMP_METERS = 8.0; // Last fused point that was actually rendered private LatLng lastSentFusedPosition = null; + // PDR {x, y} coordinates (meters from recording origin) at the last accepted render. + // Used to compute the PDR delta when the fused estimate teleports. + private float lastAcceptedPdrX = 0f; + private float lastAcceptedPdrY = 0f; + // Last WiFi location sent to the map — avoids flooding wifiHistory with the same point private LatLng lastSentWifiPosition = null; @@ -299,57 +308,57 @@ private void updateUIandPosition() { // Get the latest fused position from SensorFusion (best estimate of user location) LatLng fusedPosition = sensorFusion.getFusedPosition(); - // Only proceed if: - // 1. We have a valid fused position - // 2. The map fragment is ready to receive updates if (fusedPosition != null && trajectoryMapFragment != null) { - // --- CONDITION 1: First point --- - // If no previous fused point has been sent to the map, - // this is the very first update → must display it boolean isFirstPoint = (lastSentFusedPosition == null); - - // --- CONDITION 2: Movement-based update --- - // Only move the displayed point when the fused position has shifted - // at least MOVEMENT_THRESHOLD_METERS from the last displayed position. - // Time-based unconditional updates are intentionally omitted here — - // they caused the dot to drift every second even when stationary. - boolean movementDetected = false; double movedDistance = 0.0; + boolean movementDetected = false; if (lastSentFusedPosition != null) { movedDistance = UtilFunctions.distanceBetweenPoints( - lastSentFusedPosition, - fusedPosition - ); + lastSentFusedPosition, fusedPosition); movementDetected = movedDistance >= MOVEMENT_THRESHOLD_METERS; } if (isFirstPoint || movementDetected) { - - Log.d("FUSED_TEST", "MAP UPDATE -> " - + "first=" + isFirstPoint - + ", movementDetected=" + movementDetected - + ", movedDistance=" + movedDistance - + ", lat=" + fusedPosition.latitude - + ", lng=" + fusedPosition.longitude); - - // Send the fused position to the map. - // updateUserLocation returns false when the Google Map is not ready yet - // (gMap == null during the brief window between fragment creation and - // onMapReady firing). In that case do NOT consume the first-point slot — - // keep lastSentFusedPosition null so the next tick retries as a first point. - boolean rendered = trajectoryMapFragment.updateUserLocation( - fusedPosition, - (float) Math.toDegrees(sensorFusion.passOrientation()) - ); - - if (rendered) { - lastSentFusedPosition = fusedPosition; + LatLng positionToRender; + + if (!isFirstPoint && movedDistance > MAX_JUMP_METERS) { + // ---- TELEPORT DETECTED ---- + // The fused estimate jumped implausibly (bad GNSS/filter divergence). + // Fall back to PDR dead-reckoning: apply the PDR step delta to the + // last confirmed anchor so the trajectory stays physically continuous. + float pdrDeltaX = pdrValues[0] - lastAcceptedPdrX; + float pdrDeltaY = pdrValues[1] - lastAcceptedPdrY; + double pdrStep = Math.sqrt(pdrDeltaX * pdrDeltaX + pdrDeltaY * pdrDeltaY); + + if (pdrStep >= MOVEMENT_THRESHOLD_METERS && pdrStep < MAX_JUMP_METERS) { + positionToRender = UtilFunctions.convertENUToWGS84( + lastSentFusedPosition, + new float[]{pdrDeltaX, pdrDeltaY, 0f}); + Log.d("FUSED_TEST", "Teleport " + (int) movedDistance + + "m → PDR fallback " + String.format("%.2f", pdrStep) + "m"); + } else { + // PDR also hasn't moved enough — stay put this tick + positionToRender = null; + Log.d("FUSED_TEST", "Teleport " + (int) movedDistance + + "m rejected, no PDR movement yet"); + } + } else { + // Normal update — fused estimate is within plausible range + positionToRender = fusedPosition; } - } else { - Log.d("FUSED_TEST", "SKIPPED -> movedDistance=" + movedDistance); + if (positionToRender != null) { + boolean rendered = trajectoryMapFragment.updateUserLocation( + positionToRender, + (float) Math.toDegrees(sensorFusion.passOrientation())); + if (rendered) { + lastSentFusedPosition = positionToRender; + lastAcceptedPdrX = pdrValues[0]; + lastAcceptedPdrY = pdrValues[1]; + } + } } } diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java index d696a176..002be3b0 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 @@ -764,6 +764,12 @@ public boolean updateUserLocation(@NonNull LatLng newLocation, float orientation if (!wasIndoorMapSet && nowIndoorMapSet) { int building = indoorMapManager.getCurrentBuilding(); String apiName = buildingConstantToApiName(building); + // Auto-set building ID so floor-change logic works without StartLocationFragment + if (sensorFusion != null && apiName != null + && (sensorFusion.getSelectedBuildingId() == null + || sensorFusion.getSelectedBuildingId().isEmpty())) { + sensorFusion.setSelectedBuildingId(apiName); + } FloorplanApiClient.BuildingInfo info = (sensorFusion != null && apiName != null) ? sensorFusion.getFloorplanBuilding(apiName) : null; if (info != null) { @@ -844,6 +850,12 @@ public void updateGNSS(@NonNull LatLng gnssLocation) { if (!wasIndoorMapSet && nowIndoorMapSet) { int building = indoorMapManager.getCurrentBuilding(); String apiName = buildingConstantToApiName(building); + // Auto-set building ID so floor-change logic works without StartLocationFragment + if (sensorFusion != null && apiName != null + && (sensorFusion.getSelectedBuildingId() == null + || sensorFusion.getSelectedBuildingId().isEmpty())) { + sensorFusion.setSelectedBuildingId(apiName); + } FloorplanApiClient.BuildingInfo info = (sensorFusion != null && apiName != null) ? sensorFusion.getFloorplanBuilding(apiName) : null; if (info != 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 5d57eaeb..408ac385 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java @@ -30,6 +30,7 @@ public interface StepListener { private StepListener stepListener; // Listener for step events to update the particle filter private float lastEasting = 0f; // Track last easting for calculating deltas private float lastNorthing = 0f;// Track last northing for calculating deltas + private boolean orientationInitialized = false; // True once the first rotation-vector event has arrived public void setStepListener(StepListener listener) { this.stepListener = listener; // Set the step listener for particle filter updates @@ -44,6 +45,7 @@ public void setStepListener(StepListener listener) { public void resetStepOrigin() { lastEasting = 0f; lastNorthing = 0f; + orientationInitialized = false; } // END OF PARTICLE FILTER @@ -120,12 +122,11 @@ public void handleSensorEvent(SensorEvent sensorEvent) { } break; - // NOTE: intentional fall-through from GYROSCOPE to LINEAR_ACCELERATION - // (existing behavior preserved during refactoring) case Sensor.TYPE_GYROSCOPE: state.angularVelocity[0] = sensorEvent.values[0]; state.angularVelocity[1] = sensorEvent.values[1]; state.angularVelocity[2] = sensorEvent.values[2]; + break; case Sensor.TYPE_LINEAR_ACCELERATION: state.filteredAcc[0] = sensorEvent.values[0]; @@ -171,9 +172,14 @@ public void handleSensorEvent(SensorEvent sensorEvent) { float[] rotationVectorDCM = new float[9]; SensorManager.getRotationMatrixFromVector(rotationVectorDCM, state.rotation); SensorManager.getOrientation(rotationVectorDCM, state.orientation); + orientationInitialized = true; break; case Sensor.TYPE_STEP_DETECTOR: + if (!orientationInitialized) { + Log.d("SensorFusion", "Step detected but orientation not yet ready — skipping PDR update"); + break; + } long stepTime = SystemClock.uptimeMillis() - bootTime; if (currentTime - lastStepTime < 20) { 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 d1ce55dc..6b1b25ea 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -114,6 +114,14 @@ public class SensorFusion implements SensorEventListener { private final List linearAccelWindow = new ArrayList<>(); private boolean isStationary = false; + // Timestamp (elapsedRealtime ms) of the last setStartGNSSLatitude() call. + // GNSS updates are suppressed for 10 s so a bad indoor GPS fix cannot drag + // particles away from the user-confirmed anchor immediately after anchoring. + private long filterAnchorTimeMs = 0L; + + // True once the first GNSS fix during this recording session has been used to + // anchor startLocation and write initial protobuf metadata (auto-init path). + private boolean initialMetadataWritten = false; // Floorplan API cache (latest result from start-location step) @@ -507,6 +515,14 @@ public void startRecording() { // session doesn't fire a massive spurious delta from the previous session's position. eventHandler.resetStepOrigin(); + // Reset per-session positioning state so each recording starts clean. + initialMetadataWritten = false; + state.startLocation[0] = 0f; + state.startLocation[1] = 0f; + filterAnchorTimeMs = 0L; + lastGnssForFilter = null; + particleFilter.reset(); + // Handover WiFi/BLE scan lifecycle from activity callbacks to foreground service. stopWirelessCollectors(); @@ -670,12 +686,29 @@ public float[] getGNSSLatitude(boolean start) { } /** - * Getter function for fused position from particle filter. + * Returns the best available position estimate. + * Priority: (1) particle-filter fused estimate; (2) PDR dead-reckoning from + * the GNSS-anchored start location when WiFi/GNSS are unavailable (deep indoors). + * This keeps the trajectory live and fluid even when external signals fail. * - * @return LatLng of the current estimated position. JAPJOT + * @return current position estimate, or null if no fix is available yet. */ - public LatLng getFusedPosition(){ - return particleFilter.getFusedPosition(); + public LatLng getFusedPosition() { + LatLng filtered = particleFilter.getFusedPosition(); + if (filtered != null) return filtered; + + // PDR fallback: startLocation + accumulated step displacement. + float startLat = state.startLocation[0]; + float startLon = state.startLocation[1]; + if (startLat == 0f && startLon == 0f) { + startLat = state.latitude; // fall back to latest raw GNSS if not anchored yet + startLon = state.longitude; + } + if (startLat == 0f && startLon == 0f) return null; + float[] pdr = pdrProcessing.getPDRMovement(); + LatLng origin = new LatLng(startLat, startLon); + if (pdr[0] == 0f && pdr[1] == 0f) return origin; + return UtilFunctions.convertENUToWGS84(origin, new float[]{pdr[0], pdr[1], 0f}); } /** @@ -693,6 +726,7 @@ public void setStartGNSSLatitude(float[] startPosition) { particleFilter.reset(); LatLng chosenStart = new LatLng(startPosition[0], startPosition[1]); particleFilter.initialise(chosenStart, 5f); // 5 m spread — user just pinpointed their location + filterAnchorTimeMs = SystemClock.elapsedRealtime(); // start 10 s GNSS grace period lastGnssForFilter = chosenStart; } @@ -914,38 +948,63 @@ public void onLocationChanged(@NonNull Location location) { LatLng gnssPos = new LatLng(location.getLatitude(), location.getLongitude()); float accuracy = location.hasAccuracy() ? location.getAccuracy() : 20f; + // Auto-initialise: first GNSS fix during recording anchors start location and + // writes initial metadata automatically — no manual StartLocationFragment needed. + if (recorder.isRecording() && !initialMetadataWritten) { + state.startLocation[0] = state.latitude; + state.startLocation[1] = state.longitude; + if (!particleFilter.isInitialized()) { + // WiFi hasn't provided a better anchor yet; seed with GNSS + generous spread. + float initSpread = Math.max(accuracy * 4f, 25f); + particleFilter.initialise(gnssPos, initSpread); + lastGnssForFilter = gnssPos; + } + writeInitialMetadata(); + initialMetadataWritten = true; + Log.d("SensorFusion", "Auto-anchored: " + state.latitude + "," + state.longitude); + return; + } + + // Grace period: for 10 s after setStartGNSSLatitude() suppress ALL GNSS so a bad + // indoor GPS fix cannot drag particles away from the confirmed anchor. + if (filterAnchorTimeMs > 0 + && SystemClock.elapsedRealtime() - filterAnchorTimeMs < 10_000L) { + return; + } + if (!particleFilter.isInitialized()) { // Inflate spread aggressively for indoor GNSS initialization. // Reported accuracy is typically 5-15 m, but indoor multipath error is 20-100 m. - // A 25 m minimum spread ensures particles cover enough area for WiFi corrections - // to work without a full filter reset. Ref: Davidson et al. (2010), "Application - // of particle filters to a map-aided indoor positioning system," IEEE/ION PLANS. + // A 25 m minimum spread ensures particles cover enough area for WiFi corrections. + // Ref: Davidson et al. (2010), "Application of particle filters to a map-aided + // indoor positioning system," IEEE/ION PLANS. float initSpread = Math.max(accuracy * 4f, 25f); particleFilter.initialise(gnssPos, initSpread); lastGnssForFilter = gnssPos; } else { - // Only feed GNSS into the particle filter if the reported position has moved - // at least half the stated accuracy radius since the last accepted fix. - // This suppresses the 10-30 m noise bounce that GPS produces indoors when - // the device is stationary, which would otherwise drag particles through walls. + // Displacement gate: only update if the fix has actually moved enough. + // Suppresses the 10-30 m stationary bounce GPS produces indoors. float minDisplacement = Math.max(accuracy * 0.5f, 3.0f); boolean shouldUpdate = (lastGnssForFilter == null) || (UtilFunctions.distanceBetweenPoints(lastGnssForFilter, gnssPos) >= minDisplacement); if (shouldUpdate) { - // Outlier gate: if the filter has converged (low uncertainty) and the GNSS - // fix is implausibly far from the current estimate, reject it as indoor - // multipath. This protects a good WiFi-anchored position from being - // overridden by a bad GPS reflection. - float uncertainty = particleFilter.getPositionUncertaintyMeters(); + // Outlier gate: reject spurious GNSS jumps. + // Hard limit: >50 m is physically impossible from GPS; always reject. + // Soft limit: once filter has partially converged (uncertainty < 30 m), + // reject fixes that are >2.5× accuracy away from the fused estimate. LatLng fused = particleFilter.getFusedPosition(); boolean outlier = false; - if (fused != null && uncertainty < 12f) { + if (fused != null) { double offset = UtilFunctions.distanceBetweenPoints(fused, gnssPos); - if (offset > Math.max(accuracy * 2.5f, 20f)) { + float uncertainty = particleFilter.getPositionUncertaintyMeters(); + if (offset > 50f) { + outlier = true; + Log.d("SensorFusion", "GNSS hard-rejected: " + (int) offset + "m"); + } else if (uncertainty < 30f && offset > Math.max(accuracy * 2.5f, 20f)) { outlier = true; - Log.d("SensorFusion", "GNSS outlier rejected: " + (int) offset - + "m offset, uncertainty=" + (int) uncertainty + "m"); + Log.d("SensorFusion", "GNSS outlier: " + (int) offset + + "m offset, σ=" + (int) uncertainty + "m"); } } if (!outlier) { diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/PathView.java b/app/src/main/java/com/openpositioning/PositionMe/utils/PathView.java index 5a5efa8d..c9d2ea11 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/PathView.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/PathView.java @@ -102,8 +102,8 @@ protected void onDraw(Canvas canvas) { // Start a new path at the center of the view path.moveTo(getWidth()/2, getHeight()/2); - // Draw line between last point and this point - for (int i = 1; i < xCoords.size(); i++) { + // Draw line between last point and this point (start at 0 to include first step) + for (int i = 0; i < xCoords.size(); i++) { path.lineTo(xCoords.get(i), yCoords.get(i)); } @@ -134,8 +134,8 @@ else if(reDraw){ // Start a new path at the center of the view path.moveTo(getWidth()/2, getHeight()/2); - // Draw line between last point and this point - for (int i = 1; i < xCoords.size(); i++) { + // Draw line between last point and this point (start at 0 to include first step) + for (int i = 0; i < xCoords.size(); i++) { path.lineTo(xCoords.get(i), yCoords.get(i)); } @@ -153,8 +153,8 @@ else if(reDraw){ // Start a new path at the center of the view path.moveTo(getWidth()/2, getHeight()/2); - // Draw line between last point and this point - for (int i = 1; i < xCoords.size(); i++) { + // Draw line between last point and this point (start at 0 to include first step) + for (int i = 0; i < xCoords.size(); i++) { path.lineTo(xCoords.get(i), yCoords.get(i)); } @@ -174,6 +174,8 @@ public void drawTrajectory(float[] newCords) { // Negate the y coordinate and add it to the yCoords list, since screen coordinates // start from top to bottom yCoords.add(-newCords[1]); + // postInvalidate() is thread-safe and schedules a redraw on the UI thread + postInvalidate(); } /** @@ -186,20 +188,27 @@ private void scaleTrajectory() { int centerX = getWidth() / 2; int centerY = getHeight() / 2; - // Calculate the scaling that would be required in each direction - float xRightRange = (getWidth() / 2) / (Math.abs(Collections.max(xCoords))); - float xLeftRange = (getWidth() / 2) / (Math.abs(Collections.min(xCoords))); - float yTopRange = (getHeight() / 2) / (Math.abs(Collections.max(yCoords))); - float yBottomRange = (getHeight() / 2) / (Math.abs(Collections.min(yCoords))); + // Compute the maximum absolute extent from the origin in each axis. + // Using max(|max|, |min|) avoids division-by-zero when the walk is entirely + // on one side of the origin (e.g. all-positive X during the first leg of a square). + float xExtent = Math.max(Math.abs(Collections.max(xCoords)), + Math.abs(Collections.min(xCoords))); + float yExtent = Math.max(Math.abs(Collections.max(yCoords)), + Math.abs(Collections.min(yCoords))); + + // Fallback to 1 if the trajectory has no movement in an axis (prevents /0) + if (xExtent == 0) xExtent = 1f; + if (yExtent == 0) yExtent = 1f; // Take the minimum scaling ratio to ensure all points fit within the view - float minRatio = Math.min(Math.min(xRightRange, xLeftRange), Math.min(yTopRange, yBottomRange)); + float minRatio = Math.min((getWidth() / 2f) / xExtent, + (getHeight() / 2f) / yExtent); // Add margins to the scaling ratio scalingRatio = 0.9f * minRatio; // Limit scaling ratio to an equivalent of zoom of 21 in google maps - if (scalingRatio >= 23.926) { + if (scalingRatio >= 23.926f) { scalingRatio = 23.926f; } System.out.println("Adjusted scaling ratio: " + scalingRatio); From e1871f79af82e7e4010bfb38c046f2c5f639f605 Mon Sep 17 00:00:00 2001 From: JP10JOT <97289379+JapjotS@users.noreply.github.com> Date: Wed, 1 Apr 2026 20:08:23 +0100 Subject: [PATCH 31/36] Improve PDR, wall handling, and autofloor logic MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Multiple fixes and enhancements to positioning, map rendering, and autofloor behavior: - RecordingFragment: increased UI refresh rate from 200ms to 16ms for ~60fps marker/camera animation. - TrajectoryMapFragment: - Track particle-filter origin and rebuild wall geometry when PF origin changes to keep ENU frames aligned. - Use lastPolylinePoint as the drawn-anchor so rejected segments don't advance the anchor; prevents permanent stuck trajectories when wall collisions occur. - Reset lastPolylinePoint and lastWallBuildOrigin when clearing state. - applyImmediateFloor and evaluateAutoFloor: remove WiFi priority; auto-floor now uses PDR/barometric floor only and is restricted to lift proximity with low horizontal accel and snap-to-floor tolerance (prevents staircase/adjacent-floor WiFi false triggers). Debounce retained. - ParticleFilter: when a simulated step intersects a wall, attempt sliding the particle along the first blocking wall's tangent (if clear) instead of freezing — reduces particle clustering at boundaries. - SensorFusion: set rotation sensor sampling to 10000 (100 Hz) so heading updates faster than step rate. - IndoorMapManager: added isNearLift(radius) used by auto-floor guards; feature stroke/fill colors adjusted so walls/stairs/lifts are visually distinct (wall=red, stairs=yellow, lift=green). Rationale: smoother UI animation, correct wall-intersection behavior by keeping ENU frames in sync, avoid false floor changes near stairs or due to WiFi bleed, reduce particle-filter bias at walls, and ensure heading is sampled quickly enough for PDR accuracy. --- .../fragment/RecordingFragment.java | 4 +- .../fragment/TrajectoryMapFragment.java | 145 +++++++++--------- .../PositionMe/sensors/ParticleFilter.java | 33 +++- .../PositionMe/sensors/SensorFusion.java | 2 +- .../PositionMe/utils/IndoorMapManager.java | 52 ++++++- 5 files changed, 151 insertions(+), 85 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 ad2617d5..4e35db59 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 @@ -113,8 +113,8 @@ public class RecordingFragment extends Fragment { @Override public void run() { updateUIandPosition(); - // Loop again - refreshDataHandler.postDelayed(refreshDataTask, 200); + // Loop again — 16 ms ≈ 60 fps for smooth marker and camera animation + refreshDataHandler.postDelayed(refreshDataTask, 16); } }; 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 002be3b0..f04bb33c 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 @@ -101,6 +101,15 @@ public class TrajectoryMapFragment extends Fragment { private final MapMatchingConfig mapMatchingConfig = new MapMatchingConfig(); private LatLng wallOrigin; + // Tracks the particle-filter origin that was used for the last wall build. + // Walls must be rebuilt whenever the PF origin changes so both share the same ENU frame. + private LatLng lastWallBuildOrigin = null; + + // The last position that was ACTUALLY added to the polyline. + // Used as the "from" point in the wall-crossing check so that the anchor never + // advances through a wall: if a segment is rejected, the anchor stays at the last + // good point and the next segment is checked from there instead. + private LatLng lastPolylinePoint = null; // UI private Spinner switchMapSpinner; @@ -733,24 +742,43 @@ public boolean updateUserLocation(@NonNull LatLng newLocation, float orientation points.add(newLocation); polyline.setPoints(points); }*/ - // Extend polyline + // Rebuild wall geometry if the particle-filter origin has changed since the last build. + // Both the wall segments and wouldCrossWall() must use the same ENU origin; if the + // filter was reset (WiFi anchor, GNSS fix, setStartGNSSLatitude) the origin shifts and + // the old walls are misaligned, causing segments to pass through them undetected. + if (sensorFusion != null && indoorMapManager != null && indoorMapManager.getIsIndoorMapSet()) { + LatLng pfOrigin = sensorFusion.getParticleFilter().getOrigin(); + if (pfOrigin != null && !pfOrigin.equals(lastWallBuildOrigin)) { + updateWallsForPdr(); + } + } + + // Extend polyline. + // IMPORTANT: use lastPolylinePoint (not currentLocation / oldLocation) as the + // "from" anchor for the wall check. currentLocation always advances to displayLocation + // (so the marker and camera stay correct), but lastPolylinePoint only advances when a + // point is actually drawn. This prevents the anchor from moving to the wrong side of + // a wall: once a segment is rejected, the next check still starts from the last valid + // drawn point, so the trajectory can resume as soon as the fused position re-enters + // valid territory — it never gets permanently stuck. if (polyline != null) { List points = new ArrayList<>(polyline.getPoints()); - // First position fix: add the first polyline point - if (oldLocation == null) { + if (lastPolylinePoint == null) { + // First drawn point — no wall check needed. points.add(displayLocation); polyline.setPoints(points); - } else if (!oldLocation.equals(displayLocation)) { - // Wall guard: never draw a trajectory segment that crosses a floorplan wall. - // The particle filter holds the same wall geometry used to constrain particles, - // so we reuse its intersection test here before committing the point. + lastPolylinePoint = displayLocation; + } else if (!lastPolylinePoint.equals(displayLocation)) { boolean crossesWall = sensorFusion != null - && sensorFusion.getParticleFilter().wouldCrossWall(oldLocation, displayLocation); + && sensorFusion.getParticleFilter() + .wouldCrossWall(lastPolylinePoint, displayLocation); if (!crossesWall) { points.add(displayLocation); polyline.setPoints(points); + lastPolylinePoint = displayLocation; } + // else: lastPolylinePoint stays at the last good anchor } } @@ -961,6 +989,8 @@ public void clearMapAndReset() { } lastGnssLocation = null; currentLocation = null; + lastPolylinePoint = null; + lastWallBuildOrigin = null; floorplanFetchAttempted = false; wasIndoorMapSet = false; @@ -1132,14 +1162,8 @@ public void run() { /** * Applies the sensor-authoritative floor immediately without debounce. - * Called once when auto-floor is re-enabled so the map display snaps back - * to the floor the sensors believe the user is on, regardless of any - * manual floor browsing the user did while auto-floor was off. - * - *

Priority: WiFi floor (if fresh) → PDR/barometric floor. - * Unlike {@link #evaluateAutoFloor()}, this path has no elevation-magnitude - * or proximity guards — those guards exist to avoid jittery periodic changes, - * but here we always want an immediate, unconditional sync.

+ * Uses the PDR/barometric floor counter — WiFi floor is intentionally excluded + * because near staircases WiFi fingerprints bleed across floors and cause false triggers. */ private void applyImmediateFloor() { if (sensorFusion == null || indoorMapManager == null) return; @@ -1147,16 +1171,9 @@ private void applyImmediateFloor() { updateWallsForPdr(); - int candidateFloor; - if (sensorFusion.getLatLngWifiPositioning() != null && sensorFusion.isWifiPositionFresh()) { - // WiFi gives the most reliable absolute floor when a fresh fix is available - candidateFloor = sensorFusion.getWifiFloor(); - } else { - // Fall back to the PDR pipeline's continuously-maintained floor counter. - // This already incorporates every barometric transition since recording started, - // so it is correct even when the current elevation delta is near zero (e.g. ground floor). - candidateFloor = sensorFusion.getPdrCurrentFloor(); - } + // Barometer-only: use the PDR pipeline's continuously-maintained floor counter + // which is already gated on lift proximity and 5.5 m snap tolerance in SensorFusion. + int candidateFloor = sensorFusion.getPdrCurrentFloor(); indoorMapManager.setCurrentFloor(candidateFloor, true); updateFloorLabel(); @@ -1180,9 +1197,12 @@ private void stopAutoFloor() { } /** - * Evaluates the current floor using WiFi positioning (priority) or - * barometric elevation (fallback). Applies a 3-second debounce window - * to prevent jittery floor switching. + * Evaluates the current floor using barometric elevation only. + * Floor changes are permitted ONLY when the user is physically near a lift AND + * the barometer shows a change that is a whole multiple of the building's floor height + * (5.5 m for Nucleus). WiFi floor is intentionally excluded: near staircases the WiFi + * fingerprints from adjacent floors cause false triggers that move the map to the wrong + * floor and break the wall-collision geometry. */ private void evaluateAutoFloor() { if (sensorFusion == null || indoorMapManager == null) return; @@ -1190,52 +1210,32 @@ private void evaluateAutoFloor() { updateWallsForPdr(); - int candidateFloor; + // Barometric elevation from the PDR pipeline + float elevation = sensorFusion.getElevation(); - // Priority 1: WiFi floor — only when a fresh fix is available (within 30 s). - // Stale WiFi data must not permanently suppress the barometric path. - if (sensorFusion.getLatLngWifiPositioning() != null && sensorFusion.isWifiPositionFresh()) { - candidateFloor = sensorFusion.getWifiFloor(); - } else { - // Fallback: barometric elevation estimate with map-matching guards - float elevation = sensorFusion.getElevation(); - float floorHeight = indoorMapManager.getFloorHeight(); - if (floorHeight <= 0) { - floorHeight = mapMatchingConfig.baroHeightThreshold; - } - if (Math.abs(elevation) < mapMatchingConfig.baroHeightThreshold) { - return; // Ignore small height changes - } - boolean nearFeature = indoorMapManager.isNearCrossFloorFeature(mapMatchingConfig.crossFeatureProximity); - if (!nearFeature) { - return; - } - if (floorHeight <= 0) return; - candidateFloor = Math.round(elevation / floorHeight); - - // Use real horizontal acceleration so LIFT (low horiz) vs STAIRS (high horiz) is distinguished - float horizAccel = sensorFusion.getHorizontalAccelMagnitude(); - - // Lift snap guard: while inside a lift (near-zero horizontal movement) only commit - // once the barometer is within 15% of floor height from the exact target level, - // preventing false triggers mid-ascent/descent. - if (horizAccel < 0.3f) { - float targetElev = candidateFloor * floorHeight; - if (Math.abs(elevation - targetElev) > floorHeight * 0.15f) { - return; - } - } + // Use the building's known floor height (5.5 m for Nucleus) + float floorHeight = indoorMapManager.getFloorHeight(); + if (floorHeight <= 0) floorHeight = IndoorMapManager.NUCLEUS_FLOOR_HEIGHT; - CrossFloorClassifier.Mode mode = - CrossFloorClassifier.classify(horizAccel, elevation, 0.0, mapMatchingConfig); - Log.d(TAG, "Auto-floor (baro) mode=" + mode + " elevation=" + elevation - + " horizAccel=" + horizAccel); + // Guard 1: ignore trivially small elevation changes (noise / gentle ramp) + if (Math.abs(elevation) < mapMatchingConfig.baroHeightThreshold) return; - // Reject UNKNOWN — not enough movement signal to commit to a floor change - if (mode == CrossFloorClassifier.Mode.UNKNOWN) { - return; - } - } + // Guard 2: only commit near a lift — not near stairs, not in the middle of a corridor. + // This is the key fix: staircase proximity must NOT trigger floor changes. + if (!indoorMapManager.isNearLift(mapMatchingConfig.crossFeatureProximity)) return; + + // Guard 3: require low horizontal movement (lift ≈ still, stairs ≈ walking) + float horizAccel = sensorFusion.getHorizontalAccelMagnitude(); + if (horizAccel >= 0.3f) return; // user is walking — probably stairs, not lift + + // Guard 4: snap-to-floor — barometer must be within 15 % of an exact floor level. + // Prevents committing a floor change mid-ascent when the lift is between floors. + int candidateFloor = Math.round(elevation / floorHeight); + float targetElev = candidateFloor * floorHeight; + if (Math.abs(elevation - targetElev) > floorHeight * 0.15f) return; + + Log.d(TAG, "Auto-floor (baro/lift) elevation=" + elevation + + " candidate=" + candidateFloor + " horizAccel=" + horizAccel); // Debounce: require the same floor reading for AUTO_FLOOR_DEBOUNCE_MS long now = SystemClock.elapsedRealtime(); @@ -1330,5 +1330,6 @@ private void updateWallsForPdr() { List> walls = WallGeometryBuilder.buildWalls(floor, wallOrigin); sensorFusion.setPdrWalls(walls); + lastWallBuildOrigin = wallOrigin; } } diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java index 734e0459..899fa137 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java @@ -90,8 +90,6 @@ public void predict(float deltaEasting, float deltaNorthing) { // Apply this particle's estimated heading bias before adding step noise. // Based on: Woodman, O.J. & Harle, R. (2008). "Pedestrian localisation for // indoor environments." UbiComp 2008, pp. 114-123. - // The bias corrects systematic compass error so particles that are aligned - // with the true walking direction accumulate higher weights over time. float correctedHeading = heading + particles[i][2]; float noisyHeading = correctedHeading + (float) (random.nextGaussian() * HEADING_NOISE_STD); float noisyStride = stride + (float) (random.nextGaussian() * STRIDE_LENGTH_NOISE_STD); @@ -102,7 +100,36 @@ public void predict(float deltaEasting, float deltaNorthing) { float newY = oldY + noisyStride * (float) Math.sin(noisyHeading); if (intersectsWall(oldX, oldY, newX, newY)) { - // Stop at old position — can't pass through a wall. + // Wall hit: slide along the first blocking wall's tangent instead of freezing. + // Frozen particles cluster at wall boundaries and pull the weighted mean (fused + // position) towards walls even as the user walks forward. Sliding lets particles + // continue moving in the corridor direction, keeping the cloud well-distributed. + boolean moved = false; + for (float[] wall : walls) { + if (!doIntersect(oldX, oldY, newX, newY, + wall[0], wall[1], wall[2], wall[3])) continue; + // Wall tangent unit vector + float wx = wall[2] - wall[0]; + float wy = wall[3] - wall[1]; + float wLen = (float) Math.sqrt(wx * wx + wy * wy); + if (wLen < 1e-6f) break; + wx /= wLen; + wy /= wLen; + // Project desired step onto the wall tangent + float dx = newX - oldX; + float dy = newY - oldY; + float proj = dx * wx + dy * wy; + float slideX = oldX + proj * wx; + float slideY = oldY + proj * wy; + // Accept only if the slide itself is wall-free + if (!intersectsWall(oldX, oldY, slideX, slideY)) { + particles[i][0] = slideX; + particles[i][1] = slideY; + moved = true; + } + break; // handle only the first blocking wall + } + // If no valid slide found, keep at old position (fallback) } else { particles[i][0] = newX; particles[i][1] = newY; 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 6b1b25ea..a3c75a15 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -425,7 +425,7 @@ public void resumeListening() { stepDetectionSensor.sensorManager.registerListener(this, stepDetectionSensor.sensor, SensorManager.SENSOR_DELAY_NORMAL); rotationSensor.sensorManager.registerListener(this, - rotationSensor.sensor, (int) 1e6); + rotationSensor.sensor, 10000); // 100 Hz — heading must update faster than step rate // Foreground service owns WiFi/BLE scanning during recording. if (!recorder.isRecording()) { startWirelessCollectors(); diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java b/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java index 197d8508..4fbecae8 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java @@ -172,6 +172,32 @@ public FloorplanApiClient.FloorShapes getCurrentFloorShape() { return currentFloorShapes.get(currentFloor); } + /** + * Returns true if the last known location is within the given radius (meters) + * of a lift feature on the current floor. + * Used by auto-floor logic to restrict barometric floor changes to lift areas only. + * + * @param radiusMeters proximity threshold in meters + * @return true when near a lift; false otherwise or when data missing + */ + public boolean isNearLift(float radiusMeters) { + if (lastLocation == null || currentFloorShapes == null) return false; + if (currentFloor < 0 || currentFloor >= currentFloorShapes.size()) return false; + FloorplanApiClient.FloorShapes floor = currentFloorShapes.get(currentFloor); + if (floor == null || floor.getFeatures() == null) return false; + for (FloorplanApiClient.MapShapeFeature feature : floor.getFeatures()) { + if (!"lift".equals(feature.getIndoorType())) continue; + for (List part : feature.getParts()) { + for (LatLng point : part) { + if (UtilFunctions.distanceBetweenPoints(lastLocation, point) <= radiusMeters) { + return true; + } + } + } + } + return false; + } + /** * Returns true if the last known location is within the given radius (meters) * of any stairs or lift feature on the current floor. @@ -486,17 +512,24 @@ private void clearDrawnShapes() { /** * Returns the stroke colour for a given indoor feature type. + * Feature colours are fixed regardless of the user's trajectory colour choice: + * wall → red (impassable boundary) + * stairs → yellow (cross-floor via stairs) + * lift → green (cross-floor via lift) + * room → current trajectory colour (neutral room outline) * * @param indoorType the indoor_type property value * @return ARGB colour value */ private int getStrokeColor(String indoorType) { + if ("wall".equals(indoorType)) return Color.argb(220, 220, 30, 30); // red + if ("stairs".equals(indoorType)) return Color.argb(220, 220, 180, 0); // yellow + if ("lift".equals(indoorType)) return Color.argb(220, 30, 180, 30); // green + // room / other — use the user-chosen trajectory colour int r = Color.red(floorPlanColor); int g = Color.green(floorPlanColor); int b = Color.blue(floorPlanColor); - if ("wall".equals(indoorType)) return Color.argb(220, r, g, b); - if ("room".equals(indoorType)) return Color.argb(160, r, g, b); - return Color.argb(180, r, g, b); + return Color.argb(160, r, g, b); } /** @@ -506,10 +539,15 @@ private int getStrokeColor(String indoorType) { * @return ARGB colour value */ private int getFillColor(String indoorType) { - int r = Color.red(floorPlanColor); - int g = Color.green(floorPlanColor); - int b = Color.blue(floorPlanColor); - if ("room".equals(indoorType)) return Color.argb(40, r, g, b); + if ("wall".equals(indoorType)) return Color.argb( 60, 220, 30, 30); // faint red + if ("stairs".equals(indoorType)) return Color.argb( 60, 220, 180, 0); // faint yellow + if ("lift".equals(indoorType)) return Color.argb( 60, 30, 180, 30); // faint green + if ("room".equals(indoorType)) { + int r = Color.red(floorPlanColor); + int g = Color.green(floorPlanColor); + int b = Color.blue(floorPlanColor); + return Color.argb(40, r, g, b); + } return Color.TRANSPARENT; } From 5874e726c132a44037d232d5e46fe09f4eec79d3 Mon Sep 17 00:00:00 2001 From: JP10JOT <97289379+JapjotS@users.noreply.github.com> Date: Wed, 1 Apr 2026 20:13:10 +0100 Subject: [PATCH 32/36] Update TrajectoryMapFragment.java --- .../fragment/TrajectoryMapFragment.java | 75 ++++++++++--------- 1 file changed, 38 insertions(+), 37 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 f04bb33c..ec5a6a78 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 @@ -1139,6 +1139,33 @@ private void drawBuildingPolygon() { * and applies floor changes only after the debounce window (3 seconds * of consistent readings). */ + //endregion + + /** + * Maps barometer elevation to floor level based on height ranges. + * - 9-11m: Floor 3 + * - 3-6m: Floor 2 + * - -1 to 2m: Floor 1 + * - -4 to -7: Ground floor + * - -8 and below: Lower ground + * + * @param elevation Barometer elevation in meters + * @return Floor level (0 = ground, 1 = first floor, -1 = lower ground, etc.) + */ + private int getFloorFromBarometerHeight(float elevation) { + if (elevation >= 9 && elevation <= 11) { + return 3; + } else if (elevation >= 3 && elevation < 9) { + return 2; + } else if (elevation >= -1 && elevation < 3) { + return 1; + } else if (elevation >= -7 && elevation < -1) { + return 0; // Ground floor + } else { // elevation < -8 + return -1; // Lower ground + } + } + private void startAutoFloor() { if (autoFloorHandler == null) { autoFloorHandler = new Handler(Looper.getMainLooper()); @@ -1161,9 +1188,8 @@ public void run() { } /** - * Applies the sensor-authoritative floor immediately without debounce. - * Uses the PDR/barometric floor counter — WiFi floor is intentionally excluded - * because near staircases WiFi fingerprints bleed across floors and cause false triggers. + * Applies the barometer-based floor immediately without debounce. + * Maps barometer elevation to floor level using the configured height ranges. */ private void applyImmediateFloor() { if (sensorFusion == null || indoorMapManager == null) return; @@ -1171,13 +1197,13 @@ private void applyImmediateFloor() { updateWallsForPdr(); - // Barometer-only: use the PDR pipeline's continuously-maintained floor counter - // which is already gated on lift proximity and 5.5 m snap tolerance in SensorFusion. - int candidateFloor = sensorFusion.getPdrCurrentFloor(); + // Get barometer elevation and convert to floor + float elevation = sensorFusion.getElevation(); + int candidateFloor = getFloorFromBarometerHeight(elevation); indoorMapManager.setCurrentFloor(candidateFloor, true); updateFloorLabel(); - Log.d(TAG, "applyImmediateFloor -> floor=" + candidateFloor); + Log.d(TAG, "applyImmediateFloor -> elevation=" + elevation + " floor=" + candidateFloor); // Seed the debounce state so the first periodic evaluation doesn't re-trigger immediately lastCandidateFloor = candidateFloor; @@ -1197,12 +1223,8 @@ private void stopAutoFloor() { } /** - * Evaluates the current floor using barometric elevation only. - * Floor changes are permitted ONLY when the user is physically near a lift AND - * the barometer shows a change that is a whole multiple of the building's floor height - * (5.5 m for Nucleus). WiFi floor is intentionally excluded: near staircases the WiFi - * fingerprints from adjacent floors cause false triggers that move the map to the wrong - * floor and break the wall-collision geometry. + * Evaluates the current floor using barometric elevation with debounce. + * Maps barometer height directly to floor level based on configured ranges. */ private void evaluateAutoFloor() { if (sensorFusion == null || indoorMapManager == null) return; @@ -1210,32 +1232,11 @@ private void evaluateAutoFloor() { updateWallsForPdr(); - // Barometric elevation from the PDR pipeline + // Get barometer elevation and convert to floor float elevation = sensorFusion.getElevation(); + int candidateFloor = getFloorFromBarometerHeight(elevation); - // Use the building's known floor height (5.5 m for Nucleus) - float floorHeight = indoorMapManager.getFloorHeight(); - if (floorHeight <= 0) floorHeight = IndoorMapManager.NUCLEUS_FLOOR_HEIGHT; - - // Guard 1: ignore trivially small elevation changes (noise / gentle ramp) - if (Math.abs(elevation) < mapMatchingConfig.baroHeightThreshold) return; - - // Guard 2: only commit near a lift — not near stairs, not in the middle of a corridor. - // This is the key fix: staircase proximity must NOT trigger floor changes. - if (!indoorMapManager.isNearLift(mapMatchingConfig.crossFeatureProximity)) return; - - // Guard 3: require low horizontal movement (lift ≈ still, stairs ≈ walking) - float horizAccel = sensorFusion.getHorizontalAccelMagnitude(); - if (horizAccel >= 0.3f) return; // user is walking — probably stairs, not lift - - // Guard 4: snap-to-floor — barometer must be within 15 % of an exact floor level. - // Prevents committing a floor change mid-ascent when the lift is between floors. - int candidateFloor = Math.round(elevation / floorHeight); - float targetElev = candidateFloor * floorHeight; - if (Math.abs(elevation - targetElev) > floorHeight * 0.15f) return; - - Log.d(TAG, "Auto-floor (baro/lift) elevation=" + elevation - + " candidate=" + candidateFloor + " horizAccel=" + horizAccel); + Log.d(TAG, "Auto-floor evaluation: elevation=" + elevation + " candidateFloor=" + candidateFloor); // Debounce: require the same floor reading for AUTO_FLOOR_DEBOUNCE_MS long now = SystemClock.elapsedRealtime(); From 2fb2fec810f1be8659ae4f042f2d38e2609378b4 Mon Sep 17 00:00:00 2001 From: Davidhqm Date: Wed, 1 Apr 2026 20:33:37 +0100 Subject: [PATCH 33/36] fixed initial draggable marker --- .../fragment/StartLocationFragment.java | 26 ++----- .../PositionMe/sensors/SensorFusion.java | 70 +++++++++++++++++++ 2 files changed, 76 insertions(+), 20 deletions(-) diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/StartLocationFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/StartLocationFragment.java index 0951e85a..06a480f9 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 @@ -149,24 +149,9 @@ private void setupMap() { startMarker = mMap.addMarker(new MarkerOptions() .position(position) .title("Start Position") - .draggable(true)); + .draggable(false)); mMap.animateCamera(CameraUpdateFactory.newLatLngZoom(position, zoom)); - // Marker drag listener to update the start position when dragged - mMap.setOnMarkerDragListener(new GoogleMap.OnMarkerDragListener() { - @Override - public void onMarkerDragStart(Marker marker) {} - - @Override - public void onMarkerDragEnd(Marker marker) { - startPosition[0] = (float) marker.getPosition().latitude; - startPosition[1] = (float) marker.getPosition().longitude; - } - - @Override - public void onMarkerDrag(Marker marker) {} - }); - // Polygon click listener for building selection mMap.setOnPolygonClickListener(polygon -> { String buildingName = (String) polygon.getTag(); @@ -468,11 +453,12 @@ public void onViewCreated(@NonNull View view, @Nullable Bundle savedInstanceStat } if (requireActivity() instanceof RecordingActivity) { - // Start sensor recording + set the start location + // Start sensor recording and seed initial position autonomously (GNSS/WiFi). sensorFusion.startRecording(); - sensorFusion.setStartGNSSLatitude(startPosition); - // Write trajectory_id, initial_position and initial heading to protobuf - sensorFusion.writeInitialMetadata(); + // If no fix is available yet, listeners will populate initial metadata later. + if (sensorFusion.initializeStartPositionAutonomously()) { + sensorFusion.writeInitialMetadata(); + } // Switch to the recording screen ((RecordingActivity) requireActivity()).showRecordingScreen(); 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 a3c75a15..831f5470 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -269,6 +269,7 @@ public void update(Object[] objList) { } } } + autoSeedStartAndMetadataIfNeeded(); }, 1000); }); } @@ -730,6 +731,73 @@ public void setStartGNSSLatitude(float[] startPosition) { lastGnssForFilter = chosenStart; } + private boolean hasValidStartLocation() { + return isValidCoordinate(state.startLocation[0], state.startLocation[1]); + } + + private boolean isValidCoordinate(double lat, double lon) { + if (Double.isNaN(lat) || Double.isNaN(lon) + || Double.isInfinite(lat) || Double.isInfinite(lon)) { + return false; + } + if (lat < -90.0 || lat > 90.0 || lon < -180.0 || lon > 180.0) { + return false; + } + // Treat (0,0) as unset in this app. + return !(lat == 0.0 && lon == 0.0); + } + + /** + * Seeds initial position from sensors without user interaction. + * Priority: GNSS -> WiFi -> fused particle estimate. + * + * @return true if a valid initial position is available + */ + public boolean initializeStartPositionAutonomously() { + if (hasValidStartLocation()) { + return true; + } + + if (isValidCoordinate(state.latitude, state.longitude)) { + setStartGNSSLatitude(new float[]{state.latitude, state.longitude}); + return true; + } + + LatLng wifiPosition = (wifiPositionManager != null) + ? wifiPositionManager.getLatLngWifiPositioning() + : null; + if (wifiPosition != null + && isValidCoordinate(wifiPosition.latitude, wifiPosition.longitude)) { + setStartGNSSLatitude(new float[]{ + (float) wifiPosition.latitude, + (float) wifiPosition.longitude + }); + return true; + } + + LatLng fusedPosition = particleFilter.getFusedPosition(); + if (fusedPosition != null + && isValidCoordinate(fusedPosition.latitude, fusedPosition.longitude)) { + setStartGNSSLatitude(new float[]{ + (float) fusedPosition.latitude, + (float) fusedPosition.longitude + }); + return true; + } + + return false; + } + + private void autoSeedStartAndMetadataIfNeeded() { + if (!recorder.isRecording() || hasValidStartLocation()) { + return; + } + + if (initializeStartPositionAutonomously()) { + writeInitialMetadata(); + } + } + /** * Function to redraw path in corrections fragment. * @@ -1013,6 +1081,8 @@ public void onLocationChanged(@NonNull Location location) { } } } + + autoSeedStartAndMetadataIfNeeded(); } } From 4474475e47fd02009d58e7c32593e75c72a57020 Mon Sep 17 00:00:00 2001 From: JP10JOT <97289379+JapjotS@users.noreply.github.com> Date: Wed, 1 Apr 2026 21:30:15 +0100 Subject: [PATCH 34/36] Refine PDR, auto-floor, heading and wall checks MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Multiple fixes and improvements across sensor fusion, PDR, mapping and utilities: - TrajectoryMapFragment: add floor transition sentinel, minimum polyline distance, use position uncertainty to gate wall-cross checks, auto-enable auto-floor switch when indoor map is set, tighten barometer->floor thresholds and skip auto-floor during transition zone, add logging. - ParticleFilter: add hasWalls(), make GNSS variance much larger when walls present to avoid dragging particles through walls. - SensorEventHandler: robust step handling — debounce min step interval, heading jump smoothing/acceptance, require peak acceleration, clear/reset step origin/state updates and avoid spurious updates during noisy input; reorder and simplify logic. - SensorFusion: heading-bias calibration while stationary, expose getHeadingBias(), reset calibration on session start, stricter GNSS rejection indoors, call heading calibration update on rotation-vector events. - IndoorMapManager: simplify auto-floor bias (now 0), more defensive setCurrentFloor handling when floor shapes missing, default currentFloor adjustments for detected buildings. - PdrProcessing: add simple Kalman-style smoothing for step length with process/measurement noise and bounds, safe bounce calc, reset KF state on init, return sensible default average step length when none recorded. - UtilFunctions: fix degreesToMetersLng math (use cos instead of divide). These changes aim to improve trajectory drawing stability, reduce false floor switches and GNSS-induced jumps indoors, and stabilize step length estimation. --- .../fragment/TrajectoryMapFragment.java | 59 +++--- .../PositionMe/sensors/ParticleFilter.java | 6 +- .../sensors/SensorEventHandler.java | 109 ++++++----- .../PositionMe/sensors/SensorFusion.java | 43 ++++- .../PositionMe/utils/IndoorMapManager.java | 26 +-- .../PositionMe/utils/PathRouter.java | 171 ++++++++++++++++++ .../PositionMe/utils/PdrProcessing.java | 38 ++-- .../PositionMe/utils/UtilFunctions.java | 2 +- .../PositionMe/utils/WallGeometryBuilder.java | 23 ++- 9 files changed, 345 insertions(+), 132 deletions(-) create mode 100644 app/src/main/java/com/openpositioning/PositionMe/utils/PathRouter.java 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 ec5a6a78..f5e4b831 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 @@ -94,6 +94,8 @@ public class TrajectoryMapFragment extends Fragment { private static final String TAG = "TrajectoryMapFragment"; private static final long AUTO_FLOOR_DEBOUNCE_MS = 3000; private static final long AUTO_FLOOR_CHECK_INTERVAL_MS = 1000; + private static final int FLOOR_TRANSITION_ZONE = Integer.MIN_VALUE; + private static final double MIN_POLYLINE_DISTANCE_M = 0.5; private Handler autoFloorHandler; private Runnable autoFloorTask; private int lastCandidateFloor = Integer.MIN_VALUE; @@ -763,22 +765,24 @@ public boolean updateUserLocation(@NonNull LatLng newLocation, float orientation // valid territory — it never gets permanently stuck. if (polyline != null) { List points = new ArrayList<>(polyline.getPoints()); + uncertainty = (sensorFusion != null) ? sensorFusion.getPositionUncertaintyMeters() : 2f; if (lastPolylinePoint == null) { - // First drawn point — no wall check needed. points.add(displayLocation); polyline.setPoints(points); lastPolylinePoint = displayLocation; - } else if (!lastPolylinePoint.equals(displayLocation)) { - boolean crossesWall = sensorFusion != null - && sensorFusion.getParticleFilter() - .wouldCrossWall(lastPolylinePoint, displayLocation); - if (!crossesWall) { - points.add(displayLocation); - polyline.setPoints(points); - lastPolylinePoint = displayLocation; + } else { + double dist = UtilFunctions.distanceBetweenPoints(lastPolylinePoint, displayLocation); + if (dist >= MIN_POLYLINE_DISTANCE_M) { + boolean crossesWall = sensorFusion != null + && sensorFusion.getParticleFilter() + .wouldCrossWall(lastPolylinePoint, displayLocation); + if (!crossesWall) { + points.add(displayLocation); + polyline.setPoints(points); + lastPolylinePoint = displayLocation; + } } - // else: lastPolylinePoint stays at the last good anchor } } @@ -788,11 +792,9 @@ public boolean updateUserLocation(@NonNull LatLng newLocation, float orientation boolean nowIndoorMapSet = indoorMapManager.getIsIndoorMapSet(); setFloorControlsVisibility(nowIndoorMapSet ? View.VISIBLE : View.GONE); - // When we first enter a building with floor data, center the camera on it if (!wasIndoorMapSet && nowIndoorMapSet) { int building = indoorMapManager.getCurrentBuilding(); String apiName = buildingConstantToApiName(building); - // Auto-set building ID so floor-change logic works without StartLocationFragment if (sensorFusion != null && apiName != null && (sensorFusion.getSelectedBuildingId() == null || sensorFusion.getSelectedBuildingId().isEmpty())) { @@ -803,6 +805,9 @@ public boolean updateUserLocation(@NonNull LatLng newLocation, float orientation if (info != null) { gMap.moveCamera(CameraUpdateFactory.newLatLngZoom(info.getCenter(), 19f)); } + if (autoFloorSwitch != null && !autoFloorSwitch.isChecked()) { + autoFloorSwitch.setChecked(true); + } } wasIndoorMapSet = nowIndoorMapSet; } @@ -878,7 +883,6 @@ public void updateGNSS(@NonNull LatLng gnssLocation) { if (!wasIndoorMapSet && nowIndoorMapSet) { int building = indoorMapManager.getCurrentBuilding(); String apiName = buildingConstantToApiName(building); - // Auto-set building ID so floor-change logic works without StartLocationFragment if (sensorFusion != null && apiName != null && (sensorFusion.getSelectedBuildingId() == null || sensorFusion.getSelectedBuildingId().isEmpty())) { @@ -889,6 +893,9 @@ public void updateGNSS(@NonNull LatLng gnssLocation) { if (info != null) { gMap.moveCamera(CameraUpdateFactory.newLatLngZoom(info.getCenter(), 19f)); } + if (autoFloorSwitch != null && !autoFloorSwitch.isChecked()) { + autoFloorSwitch.setChecked(true); + } wasIndoorMapSet = true; } } @@ -1153,17 +1160,12 @@ private void drawBuildingPolygon() { * @return Floor level (0 = ground, 1 = first floor, -1 = lower ground, etc.) */ private int getFloorFromBarometerHeight(float elevation) { - if (elevation >= 9 && elevation <= 11) { - return 3; - } else if (elevation >= 3 && elevation < 9) { - return 2; - } else if (elevation >= -1 && elevation < 3) { - return 1; - } else if (elevation >= -7 && elevation < -1) { - return 0; // Ground floor - } else { // elevation < -8 - return -1; // Lower ground - } + if (elevation >= 9f && elevation <= 11f) return 3; + if (elevation >= 3f && elevation <= 6f) return 2; + if (elevation >= -1f && elevation <= 2f) return 1; + if (elevation >= -7f && elevation <= -4f) return 0; + if (elevation <= -8f) return -1; + return FLOOR_TRANSITION_ZONE; } private void startAutoFloor() { @@ -1197,15 +1199,14 @@ private void applyImmediateFloor() { updateWallsForPdr(); - // Get barometer elevation and convert to floor float elevation = sensorFusion.getElevation(); int candidateFloor = getFloorFromBarometerHeight(elevation); + if (candidateFloor == FLOOR_TRANSITION_ZONE) return; indoorMapManager.setCurrentFloor(candidateFloor, true); updateFloorLabel(); Log.d(TAG, "applyImmediateFloor -> elevation=" + elevation + " floor=" + candidateFloor); - // Seed the debounce state so the first periodic evaluation doesn't re-trigger immediately lastCandidateFloor = candidateFloor; lastCandidateTime = SystemClock.elapsedRealtime(); } @@ -1232,13 +1233,11 @@ private void evaluateAutoFloor() { updateWallsForPdr(); - // Get barometer elevation and convert to floor float elevation = sensorFusion.getElevation(); int candidateFloor = getFloorFromBarometerHeight(elevation); - Log.d(TAG, "Auto-floor evaluation: elevation=" + elevation + " candidateFloor=" + candidateFloor); + if (candidateFloor == FLOOR_TRANSITION_ZONE) return; - // Debounce: require the same floor reading for AUTO_FLOOR_DEBOUNCE_MS long now = SystemClock.elapsedRealtime(); if (candidateFloor != lastCandidateFloor) { lastCandidateFloor = candidateFloor; @@ -1249,8 +1248,8 @@ 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; + Log.d(TAG, "Auto-floor applied: elevation=" + elevation + " floor=" + candidateFloor); } } diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java index 899fa137..829c22ad 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java @@ -70,6 +70,10 @@ public void setWalls(List wallSegments) { this.walls = wallSegments; } + public boolean hasWalls() { + return walls != null && !walls.isEmpty(); + } + public static final float HEADING_NOISE_STD = 0.05f; public static final float STRIDE_LENGTH_NOISE_STD = 0.05f; @@ -256,7 +260,7 @@ public void updateGNSS(LatLng gnssPosition, float gnssAccuracy) { // Indoors, GPS signals reflect off ceilings and walls, giving fixes several // metres off and often on the wrong side of a wall. Inflate the uncertainty // so the GNSS observation cannot drag particles through wall segments. - float effectiveAccuracy = walls.isEmpty() ? gnssAccuracy : gnssAccuracy * 3.0f; + float effectiveAccuracy = walls.isEmpty() ? gnssAccuracy : Math.max(gnssAccuracy * 10f, 50f); float variance = effectiveAccuracy * effectiveAccuracy; // Convert accuracy to variance sigma^2 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 408ac385..aebc56f1 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java @@ -27,25 +27,24 @@ public interface StepListener { void onStep(float deltaEasting, float deltaNorthing); } - private StepListener stepListener; // Listener for step events to update the particle filter - private float lastEasting = 0f; // Track last easting for calculating deltas - private float lastNorthing = 0f;// Track last northing for calculating deltas - private boolean orientationInitialized = false; // True once the first rotation-vector event has arrived + private StepListener stepListener; + private float lastEasting = 0f; + private float lastNorthing = 0f; + private boolean orientationInitialized = false; + + private float lastAcceptedHeading = Float.NaN; + private static final float MAX_HEADING_JUMP_RAD = (float) (Math.PI / 6.0); + private static final long MIN_STEP_INTERVAL_MS = 300; public void setStepListener(StepListener listener) { - this.stepListener = listener; // Set the step listener for particle filter updates + this.stepListener = listener; } - /** - * Resets the PDR step-delta baseline to the current PDR origin (zero). - * Must be called after {@link PdrProcessing#resetPDR()} so that the first step of a new - * recording session does not produce a huge spurious delta against the previous session's - * accumulated position. - */ public void resetStepOrigin() { - lastEasting = 0f; + lastEasting = 0f; lastNorthing = 0f; orientationInitialized = false; + lastAcceptedHeading = Float.NaN; } // END OF PARTICLE FILTER @@ -176,58 +175,58 @@ public void handleSensorEvent(SensorEvent sensorEvent) { break; case Sensor.TYPE_STEP_DETECTOR: - if (!orientationInitialized) { - Log.d("SensorFusion", "Step detected but orientation not yet ready — skipping PDR update"); - break; - } + if (!orientationInitialized) break; long stepTime = SystemClock.uptimeMillis() - bootTime; - if (currentTime - lastStepTime < 20) { - Log.e("SensorFusion", "Ignoring step event, too soon after last step event:" - + (currentTime - lastStepTime) + " ms"); - break; - } else { - lastStepTime = currentTime; - - if (accelMagnitude.isEmpty()) { - Log.e("SensorFusion", - "stepDetection triggered, but accelMagnitude is empty! " + - "This can cause updatePdr(...) to fail or return bad results."); + long timeSinceLastStep = currentTime - lastStepTime; + if (timeSinceLastStep < MIN_STEP_INTERVAL_MS) break; + lastStepTime = currentTime; + + float rawHeading = state.orientation[0]; + float headingForStep; + if (!Float.isNaN(lastAcceptedHeading)) { + float delta = rawHeading - lastAcceptedHeading; + while (delta > (float) Math.PI) delta -= (float) (2 * Math.PI); + while (delta < -(float) Math.PI) delta += (float) (2 * Math.PI); + if (Math.abs(delta) > MAX_HEADING_JUMP_RAD && timeSinceLastStep >= 2000L) { + headingForStep = lastAcceptedHeading; } else { - Log.d("SensorFusion", - "stepDetection triggered, accelMagnitude size = " - + accelMagnitude.size()); - } - - float[] newCords = this.pdrProcessing.updatePdr( - stepTime, - this.accelMagnitude, - state.orientation[0] - ); - - this.accelMagnitude.clear(); - - if (recorder.isRecording()) { - this.pathView.drawTrajectory(newCords); - state.stepCounter++; - recorder.addPdrData( - SystemClock.uptimeMillis() - bootTime, - newCords[0], newCords[1]); + lastAcceptedHeading = rawHeading; + headingForStep = rawHeading; } + } else { + lastAcceptedHeading = rawHeading; + headingForStep = rawHeading; + } - // Update the particle filter with the new step data - if (stepListener != null) { - float deltaEasting = newCords[0] - lastEasting; - float deltaNorthing = newCords[1] - lastNorthing; - stepListener.onStep(deltaEasting, deltaNorthing); - } + if (accelMagnitude.isEmpty()) { + break; + } + double peakAccel = 0.0; + for (double v : accelMagnitude) if (v > peakAccel) peakAccel = v; + if (peakAccel < 0.5) { + accelMagnitude.clear(); + break; + } - lastEasting = newCords[0]; - lastNorthing = newCords[1]; + float[] newCords = pdrProcessing.updatePdr(stepTime, accelMagnitude, headingForStep); + accelMagnitude.clear(); + if (recorder.isRecording()) { + pathView.drawTrajectory(newCords); + state.stepCounter++; + recorder.addPdrData(SystemClock.uptimeMillis() - bootTime, newCords[0], newCords[1]); + } - break; + if (stepListener != null) { + float deltaEasting = newCords[0] - lastEasting; + float deltaNorthing = newCords[1] - lastNorthing; + stepListener.onStep(deltaEasting, deltaNorthing); } + + lastEasting = newCords[0]; + lastNorthing = newCords[1]; + break; } } 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 a3c75a15..2b359180 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -106,14 +106,17 @@ public class SensorFusion implements SensorEventListener { // the device is still, which drags particles out of the building. private LatLng lastGnssForFilter = null; - // Stationary detection — rolling variance of linear-acceleration magnitude. - // When variance stays below the threshold the phone is considered stationary, - // and both step-prediction and WiFi particle updates are suppressed. private static final int STATIONARY_WINDOW = 20; - private static final float STATIONARY_THRESHOLD = 0.015f; // m²/s⁴ + private static final float STATIONARY_THRESHOLD = 0.015f; private final List linearAccelWindow = new ArrayList<>(); private boolean isStationary = false; + private final List headingCalibSamples = new ArrayList<>(); + private float headingBias = 0f; + private boolean headingCalibDone = false; + private long headingCalibStartMs = 0L; + private static final long HEADING_CALIB_DURATION_MS = 10_000L; + // Timestamp (elapsedRealtime ms) of the last setStartGNSSLatitude() call. // GNSS updates are suppressed for 10 s so a bad indoor GPS fix cannot drag // particles away from the user-confirmed anchor immediately after anchoring. @@ -260,7 +263,8 @@ public void update(Object[] objList) { + (int) wifiDist + " m (threshold " + (int) divergenceThreshold + " m)"); particleFilter.reset(); particleFilter.initialise(wifiPosition, 15f); - lastGnssForFilter = wifiPosition; // re-anchor GNSS displacement gating + lastGnssForFilter = wifiPosition; + filterAnchorTimeMs = SystemClock.elapsedRealtime(); } else { particleFilter.updateWiFi(wifiPosition, 20f); } @@ -283,18 +287,38 @@ public void update(Object[] objList) { *

Delegates to {@link SensorEventHandler#handleSensorEvent(SensorEvent)}.

*/ - //floor updates particle fusion @Override public void onSensorChanged(SensorEvent sensorEvent) { eventHandler.handleSensorEvent(sensorEvent); if (sensorEvent.sensor.getType() == Sensor.TYPE_LINEAR_ACCELERATION) { updateStationaryState(sensorEvent.values[0], sensorEvent.values[1], sensorEvent.values[2]); } + if (sensorEvent.sensor.getType() == Sensor.TYPE_ROTATION_VECTOR && recorder.isRecording()) { + updateHeadingCalibration(); + } if (sensorEvent.sensor.getType() == Sensor.TYPE_PRESSURE && recorder.isRecording()) { updateFloorLogic(); } } + private void updateHeadingCalibration() { + if (headingCalibDone || !isStationary) return; + long now = android.os.SystemClock.elapsedRealtime(); + if (headingCalibStartMs == 0L) headingCalibStartMs = now; + headingCalibSamples.add(state.orientation[0]); + if (now - headingCalibStartMs >= HEADING_CALIB_DURATION_MS && headingCalibSamples.size() >= 10) { + float sum = 0f; + for (float h : headingCalibSamples) sum += h; + headingBias = sum / headingCalibSamples.size(); + headingCalibDone = true; + Log.d("SensorFusion", "Heading bias calibrated: " + (float) Math.toDegrees(headingBias) + "°"); + } + } + + public float getHeadingBias() { + return headingCalibDone ? headingBias : 0f; + } + /** * Maintains a rolling variance of linear-acceleration magnitude. * Sets {@code isStationary = true} when variance drops below the threshold, @@ -515,13 +539,15 @@ public void startRecording() { // session doesn't fire a massive spurious delta from the previous session's position. eventHandler.resetStepOrigin(); - // Reset per-session positioning state so each recording starts clean. initialMetadataWritten = false; state.startLocation[0] = 0f; state.startLocation[1] = 0f; filterAnchorTimeMs = 0L; lastGnssForFilter = null; particleFilter.reset(); + headingCalibDone = false; + headingCalibSamples.clear(); + headingCalibStartMs = 0L; // Handover WiFi/BLE scan lifecycle from activity callbacks to foreground service. stopWirelessCollectors(); @@ -1001,6 +1027,9 @@ public void onLocationChanged(@NonNull Location location) { if (offset > 50f) { outlier = true; Log.d("SensorFusion", "GNSS hard-rejected: " + (int) offset + "m"); + } else if (particleFilter.hasWalls() && offset > 20f) { + outlier = true; + Log.d("SensorFusion", "GNSS indoor-rejected: " + (int) offset + "m"); } else if (uncertainty < 30f && offset > Math.max(accuracy * 2.5f, 20f)) { outlier = true; Log.d("SensorFusion", "GNSS outlier: " + (int) offset diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java b/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java index 4fbecae8..c103e35c 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java @@ -262,14 +262,7 @@ public String getCurrentFloorDisplayName() { * @return the floor index offset for auto-floor conversion */ public int getAutoFloorBias() { - switch (currentBuilding) { - case BUILDING_NUCLEUS: - case BUILDING_MURCHISON: - return 1; // LG at index 0, so G = index 1 - case BUILDING_LIBRARY: - default: - return 0; // G at index 0 - } + return 0; } /** @@ -281,17 +274,18 @@ public int getAutoFloorBias() { * @param autoFloor true if called by auto-floor feature */ public void setCurrentFloor(int newFloor, boolean autoFloor) { - if (currentFloorShapes == null || currentFloorShapes.isEmpty()) return; - if (autoFloor) { newFloor += getAutoFloorBias(); } - int maxFloor = currentFloorShapes.size(); - - if (newFloor >= 0 && newFloor < maxFloor && newFloor != this.currentFloor) { + if (currentFloorShapes != null && !currentFloorShapes.isEmpty()) { + int maxFloor = currentFloorShapes.size(); + if (newFloor >= 0 && newFloor < maxFloor && newFloor != this.currentFloor) { + this.currentFloor = newFloor; + drawFloorShapes(newFloor); + } + } else if (newFloor >= 0 && newFloor != this.currentFloor) { this.currentFloor = newFloor; - drawFloorShapes(newFloor); } } @@ -338,7 +332,7 @@ private void setBuildingOverlay() { switch (detected) { case BUILDING_NUCLEUS: apiName = "nucleus_building"; - currentFloor = 1; + currentFloor = 0; floorHeight = NUCLEUS_FLOOR_HEIGHT; break; case BUILDING_LIBRARY: @@ -348,7 +342,7 @@ private void setBuildingOverlay() { break; case BUILDING_MURCHISON: apiName = "murchison_house"; - currentFloor = 1; + currentFloor = 0; floorHeight = MURCHISON_FLOOR_HEIGHT; break; default: diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/PathRouter.java b/app/src/main/java/com/openpositioning/PositionMe/utils/PathRouter.java new file mode 100644 index 00000000..c101fe78 --- /dev/null +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/PathRouter.java @@ -0,0 +1,171 @@ +package com.openpositioning.PositionMe.utils; + +import java.util.*; +import java.util.concurrent.atomic.AtomicReference; + +public class PathRouter { + + private static final float EPS = 0.05f; + + private static final class Graph { + final float[][] nodes; + final float[][] adj; + final List solid; + final List trans; + Graph(float[][] nodes, float[][] adj, List solid, List trans) { + this.nodes = nodes; this.adj = adj; this.solid = solid; this.trans = trans; + } + int n() { return nodes.length; } + } + + private final AtomicReference graphRef = new AtomicReference<>(); + + public void rebuild(List solid, List transitions) { + List s = solid != null ? new ArrayList<>(solid) : Collections.emptyList(); + List t = transitions != null ? new ArrayList<>(transitions) : Collections.emptyList(); + new Thread(() -> graphRef.set(buildGraph(s, t))).start(); + } + + public List route(float[] start, float[] end, boolean floorChanged) { + Graph g = graphRef.get(); + if (g == null) return Collections.singletonList(end); + List active = floorChanged ? g.solid : concat(g.solid, g.trans); + if (!crossed(start[0], start[1], end[0], end[1], active)) { + return Collections.singletonList(end); + } + return astar(g, start, end, active, !floorChanged && !g.trans.isEmpty()); + } + + private static Graph buildGraph(List solid, List trans) { + List verts = dedup(concat(solid, trans)); + int n = verts.size(); + float[][] nodes = verts.toArray(new float[0][]); + float[][] adj = new float[n][n]; + for (float[] r : adj) Arrays.fill(r, -1f); + for (int i = 0; i < n; i++) { + for (int j = i + 1; j < n; j++) { + if (!crossed(nodes[i][0], nodes[i][1], nodes[j][0], nodes[j][1], solid)) { + float d = dst(nodes[i], nodes[j]); + adj[i][j] = adj[j][i] = d; + } + } + } + return new Graph(nodes, adj, solid, trans); + } + + private static List astar(Graph g, float[] start, float[] end, + List active, boolean checkTrans) { + int n = g.n(), S = n, E = n + 1, T = n + 2; + float[] gs = fill(T), fs = fill(T); + int[] prev = new int[T]; + boolean[] closed = new boolean[T]; + Arrays.fill(prev, -1); + gs[S] = 0f; + fs[S] = dst(start, end); + PriorityQueue open = new PriorityQueue<>(Comparator.comparingDouble(i -> fs[i])); + open.add(S); + while (!open.isEmpty()) { + int cur = open.poll(); + if (closed[cur]) continue; + closed[cur] = true; + if (cur == E) return buildPath(prev, S, E, n, g, start, end); + for (int nb = 0; nb < T; nb++) { + if (closed[nb]) continue; + float w = ew(g, cur, nb, S, E, start, end, active, checkTrans); + if (w < 0) continue; + float ng = gs[cur] + w; + if (ng < gs[nb]) { + gs[nb] = ng; + fs[nb] = ng + dst(pt(g, nb, S, E, start, end), end); + prev[nb] = cur; + open.add(nb); + } + } + } + return Collections.singletonList(end); + } + + private static float ew(Graph g, int a, int b, int S, int E, + float[] start, float[] end, List active, boolean checkTrans) { + float[] pa = pt(g, a, S, E, start, end); + float[] pb = pt(g, b, S, E, start, end); + if (a < g.n() && b < g.n()) { + if (g.adj[a][b] < 0) return -1f; + if (checkTrans && crossed(pa[0], pa[1], pb[0], pb[1], g.trans)) return -1f; + return g.adj[a][b]; + } + return crossed(pa[0], pa[1], pb[0], pb[1], active) ? -1f : dst(pa, pb); + } + + private static float[] pt(Graph g, int i, int S, int E, float[] s, float[] e) { + if (i == S) return s; + if (i == E) return e; + return g.nodes[i]; + } + + private static List buildPath(int[] prev, int S, int E, int n, + Graph g, float[] s, float[] e) { + List path = new ArrayList<>(); + for (int cur = E; cur != S && cur != -1; cur = prev[cur]) { + path.add(0, pt(g, cur, S, E, s, e)); + } + return path.isEmpty() ? Collections.singletonList(e) : path; + } + + private static boolean crossed(float ax, float ay, float bx, float by, List walls) { + for (float[] w : walls) { + if (coin(ax, ay, w[0], w[1]) || coin(ax, ay, w[2], w[3]) || + coin(bx, by, w[0], w[1]) || coin(bx, by, w[2], w[3])) continue; + if (seg(ax, ay, bx, by, w[0], w[1], w[2], w[3])) return true; + } + return false; + } + + private static boolean coin(float ax, float ay, float bx, float by) { + return Math.abs(ax - bx) < EPS && Math.abs(ay - by) < EPS; + } + + private static boolean seg(float ax, float ay, float bx, float by, + float cx, float cy, float dx, float dy) { + float d1 = ccw(cx, cy, dx, dy, ax, ay), d2 = ccw(cx, cy, dx, dy, bx, by); + float d3 = ccw(ax, ay, bx, by, cx, cy), d4 = ccw(ax, ay, bx, by, dx, dy); + return ((d1 > 0 && d2 < 0) || (d1 < 0 && d2 > 0)) + && ((d3 > 0 && d4 < 0) || (d3 < 0 && d4 > 0)); + } + + private static float ccw(float ox, float oy, float ax, float ay, float bx, float by) { + return (ax - ox) * (by - oy) - (ay - oy) * (bx - ox); + } + + private static float dst(float[] a, float[] b) { + float dx = b[0] - a[0], dy = b[1] - a[1]; + return (float) Math.sqrt(dx * dx + dy * dy); + } + + private static List dedup(List segs) { + List list = new ArrayList<>(); + Set seen = new HashSet<>(); + for (float[] seg : segs) { + add(seg[0], seg[1], list, seen); + add(seg[2], seg[3], list, seen); + } + return list; + } + + private static void add(float x, float y, List list, Set seen) { + long key = (long) Math.round(x / 0.05f) * 4_000_000L + (long) Math.round(y / 0.05f); + if (seen.add(key)) list.add(new float[]{x, y}); + } + + private static List concat(List a, List b) { + List r = new ArrayList<>(a); + r.addAll(b); + return r; + } + + private static float[] fill(int n) { + float[] a = new float[n]; + Arrays.fill(a, Float.MAX_VALUE); + return a; + } +} 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 d3cc6959..6cece6ef 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java @@ -76,9 +76,15 @@ public class PdrProcessing { private CircularFloatBuffer verticalAccel; private CircularFloatBuffer horizontalAccel; - // Step sum and length aggregation variables private float sumStepLength = 0; private int stepCount = 0; + + private float kfStepLength = 0.7f; + private float kfVariance = 0.1f; + private static final float KF_PROCESS_NOISE = 0.005f; + private static final float KF_MEAS_NOISE = 0.04f; + private static final float MIN_STEP_LENGTH = 0.2f; + private static final float MAX_STEP_LENGTH = 1.5f; //endregion /** @@ -167,12 +173,14 @@ public float[] updatePdr(long currentStepEnd, List accelMagnitudeOvertim return new float[]{this.positionX, this.positionY}; } - // Calculate step length - if(!useManualStep) { - //ArrayList accelMagnitudeFiltered = filter(accelMagnitudeOvertime); - // Estimate stride - this.stepLength = weibergMinMax(accelMagnitudeOvertime); - // System.err.println("Step Length" + stepLength); + if (!useManualStep) { + float rawStep = weibergMinMax(accelMagnitudeOvertime); + rawStep = Math.max(MIN_STEP_LENGTH, Math.min(MAX_STEP_LENGTH, rawStep)); + kfVariance += KF_PROCESS_NOISE; + float gain = kfVariance / (kfVariance + KF_MEAS_NOISE); + kfStepLength += gain * (rawStep - kfStepLength); + kfVariance = (1f - gain) * kfVariance; + this.stepLength = kfStepLength; } // Increment aggregate variables @@ -249,8 +257,7 @@ private float weibergMinMax(List accelMagnitude) { double maxAccel = Collections.max(validAccel); double minAccel = Collections.min(validAccel); - // calculate bounce - float bounce = (float) Math.pow((maxAccel - minAccel), 0.25); + float bounce = (float) Math.pow(Math.max(0.001, maxAccel - minAccel), 0.25); // determine which constant to use based on settings if (this.settings.getBoolean("overwrite_constants", false)) { @@ -410,9 +417,12 @@ public void resetPDR() { this.startElevationBuffer = new Float[3]; // Start floor - assumed to be zero this.currentFloor = 0; - // Must be reset so the elevation baseline is recalibrated from scratch this.setupIndex = 0; this.startElevation = 0f; + this.kfStepLength = 0.7f; + this.kfVariance = 0.1f; + this.sumStepLength = 0f; + this.stepCount = 0; } /** @@ -421,14 +431,10 @@ public void resetPDR() { * @return average step length in meters. */ public float getAverageStepLength(){ - //Calculate average step length - float averageStepLength = sumStepLength/(float) stepCount; - - //Reset sum and number of steps + if (stepCount == 0) return 0.75f; + float averageStepLength = sumStepLength / (float) stepCount; stepCount = 0; sumStepLength = 0; - - //Return average step length return averageStepLength; } diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/UtilFunctions.java b/app/src/main/java/com/openpositioning/PositionMe/utils/UtilFunctions.java index 7aab0596..c7dfe576 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/UtilFunctions.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/UtilFunctions.java @@ -97,7 +97,7 @@ public static double degreesToMetersLat(double degreeVal) { * @return double corresponding to the value in meters. */ public static double degreesToMetersLng(double degreeVal, double latitude) { - return degreeVal*DEGREE_IN_M/Math.cos(Math.toRadians(latitude)); + return degreeVal * DEGREE_IN_M * Math.cos(Math.toRadians(latitude)); } /** diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/WallGeometryBuilder.java b/app/src/main/java/com/openpositioning/PositionMe/utils/WallGeometryBuilder.java index dc5c00b3..5f8bf611 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/WallGeometryBuilder.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/WallGeometryBuilder.java @@ -23,10 +23,22 @@ private WallGeometryBuilder() {} * @return list of wall polylines; empty if none. */ public static List> buildWalls(FloorplanApiClient.FloorShapes floor, LatLng origin) { - List> walls = new ArrayList<>(); - if (floor == null || origin == null) return walls; + return buildByType(floor, origin, "wall"); + } + + public static List> buildTransitions(FloorplanApiClient.FloorShapes floor, LatLng origin) { + List> result = new ArrayList<>(); + result.addAll(buildByType(floor, origin, "stairs")); + result.addAll(buildByType(floor, origin, "lift")); + return result; + } + + private static List> buildByType(FloorplanApiClient.FloorShapes floor, + LatLng origin, String type) { + List> result = new ArrayList<>(); + if (floor == null || origin == null) return result; for (FloorplanApiClient.MapShapeFeature feature : floor.getFeatures()) { - if (!"wall".equals(feature.getIndoorType())) continue; + if (!type.equals(feature.getIndoorType())) continue; for (List part : feature.getParts()) { if (part == null || part.size() < 2) continue; List line = new ArrayList<>(part.size()); @@ -35,13 +47,12 @@ public static List> buildWalls(FloorplanApiClient.FloorShapes floor float dy = (float) UtilFunctions.degreesToMetersLat(p.latitude - origin.latitude); line.add(new PointF(dx, dy)); } - // Close ring for polygons so collision sees full wall loop if (!line.isEmpty() && !line.get(0).equals(line.get(line.size() - 1))) { line.add(new PointF(line.get(0).x, line.get(0).y)); } - walls.add(line); + result.add(line); } } - return walls; + return result; } } From 5c60756b80bdbf4a6e36ab6e6feaa9f0cf0a0c1f Mon Sep 17 00:00:00 2001 From: JP10JOT <97289379+JapjotS@users.noreply.github.com> Date: Wed, 1 Apr 2026 23:43:16 +0100 Subject: [PATCH 35/36] comment fixes --- .../PositionMe/data/local/TrajParser.java | 14 +- .../PositionMe/sensors/ParticleFilter.java | 122 +++++++++++++++++- .../sensors/SensorEventHandler.java | 25 ++++ 3 files changed, 147 insertions(+), 14 deletions(-) diff --git a/app/src/main/java/com/openpositioning/PositionMe/data/local/TrajParser.java b/app/src/main/java/com/openpositioning/PositionMe/data/local/TrajParser.java index 2d2b1cbf..22112992 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/data/local/TrajParser.java +++ b/app/src/main/java/com/openpositioning/PositionMe/data/local/TrajParser.java @@ -84,7 +84,6 @@ public ReplayPoint(LatLng pdrLocation, LatLng gnssLocation, float orientation, f } } - /** Represents an IMU (Inertial Measurement Unit) data record used for orientation calculations. */ private static class ImuRecord { public long relativeTimestamp; public float accX, accY, accZ; // Accelerometer values @@ -92,13 +91,11 @@ private static class ImuRecord { public float rotationVectorX, rotationVectorY, rotationVectorZ, rotationVectorW; // Rotation quaternion } - /** Represents a Pedestrian Dead Reckoning (PDR) data record storing position shifts over time. */ private static class PdrRecord { public long relativeTimestamp; public float x, y; // Position relative to the starting point } - /** Represents a GNSS (Global Navigation Satellite System) data record with latitude/longitude. */ private static class GnssRecord { public long relativeTimestamp; public double latitude, longitude; // GNSS coordinates @@ -199,7 +196,6 @@ public static List parseTrajectoryData(String filePath, Context con return result; } -/** Parses IMU data from JSON. */ private static List parseImuData(JsonArray imuArray) { List imuList = new ArrayList<>(); if (imuArray == null) return imuList; @@ -209,7 +205,7 @@ private static List parseImuData(JsonArray imuArray) { imuList.add(record); } return imuList; -}/** Parses PDR data from JSON. */ +} private static List parsePdrData(JsonArray pdrArray) { List pdrList = new ArrayList<>(); if (pdrArray == null) return pdrList; @@ -219,7 +215,7 @@ private static List parsePdrData(JsonArray pdrArray) { pdrList.add(record); } return pdrList; -}/** Parses GNSS data from JSON. */ +} private static List parseGnssData(JsonArray gnssArray) { List gnssList = new ArrayList<>(); if (gnssArray == null) return gnssList; @@ -229,17 +225,17 @@ private static List parseGnssData(JsonArray gnssArray) { gnssList.add(record); } return gnssList; -}/** Finds the closest IMU record to the given timestamp. */ +} private static ImuRecord findClosestImuRecord(List imuList, long targetTimestamp) { return imuList.stream().min(Comparator.comparingLong(imu -> Math.abs(imu.relativeTimestamp - targetTimestamp))) .orElse(null); -}/** Finds the closest GNSS record to the given timestamp. */ +} private static GnssRecord findClosestGnssRecord(List gnssList, long targetTimestamp) { return gnssList.stream().min(Comparator.comparingLong(gnss -> Math.abs(gnss.relativeTimestamp - targetTimestamp))) .orElse(null); -}/** Computes the orientation from a rotation vector. */ +} private static float computeOrientationFromRotationVector(float rx, float ry, float rz, float rw, Context context) { float[] rotationVector = new float[]{rx, ry, rz, rw}; float[] rotationMatrix = new float[9]; diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java index 829c22ad..f23cfca8 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java @@ -7,8 +7,32 @@ import java.util.List; -import java.util.Random; //FOR PARTICLE FILTER - +import java.util.Random; +/** + * Sequential Monte Carlo (particle filter) for fused indoor positioning. + * + *

Maintains a cloud of NUM_PARTICLES weighted hypothesis particles, each + * representing a candidate position (easting, northing) and heading-bias offset in a + * local ENU frame centred on the first fix. Three update sources drive the filter:

+ *
    + *
  • PDR propagates particles using the pedestrian dead-reckoning step displacement + * with per-particle heading-bias correction and Gaussian noise, then slides any + * particle that would cross a wall along the wall tangent so it remains inside + * the building.
    • + *
  • GNSS weights particles with a Student-t likelihood (ν=4) that is robust to + * the multipath outliers common indoors. Accuracy is inflated 10× when wall + * segments are loaded to prevent bad GPS fixes from dragging the cloud outside + * the building.
    • + *
  • WiFi applies the same Student-t likelihood using positions from the + * openpositioning fingerprinting API, which are the primary indoor anchor. The + * filter is automatically reset to a WiFi fix whenever the estimated position + * has diverged by more than 2.5× the particle-cloud spread.
    • + *
+ * + *

Systematic resampling with Gaussian roughening (Thrun, Burgard & Fox 2005, + * Probabilistic Robotics) is applied whenever the effective sample size N_eff drops + * below N/2 to prevent sample impoverishment.

+ */ public class ParticleFilter { private static final String TAG = "ParticleFilter"; private static final int NUM_PARTICLES = 200; // Number of particles @@ -33,6 +57,22 @@ public class ParticleFilter { // Slow random-walk drift on heading bias per step (rad). private static final float BIAS_DRIFT_STD = 0.01f; + /** + * Initialises the filter at the given geographic position. + * Particles are spread in a Gaussian cloud of radius {@code accuracyMeters} around + * {@code firstFix}, which becomes the ENU origin for all subsequent computations. + * Each particle is also assigned a random initial heading-bias drawn from + * {@link #INITIAL_BIAS_STD}. Subsequent calls are ignored once the filter is + * initialised; call {@link #reset()} first to re-seed at a new location. + * It's best to initialise with a GNSS fix when outdoors, but the filter can also be + * seeded with a WiFi position or manual anchor when GNSS is unavailable or unreliable indoors.. + * + * -- Japjot + * + * @param firstFix WGS-84 position used as the ENU origin and initial mean + * @param accuracyMeters 1-sigma spread of the initial particle cloud in metres; + * use a larger value when the fix is uncertain (e.g. GNSS indoors) + */ public void initialise(LatLng firstFix, float accuracyMeters) { if (initialized) return; // ignore subsequent calls this.origin = firstFix; @@ -54,7 +94,7 @@ public boolean isInitialized() { return initialized; } - /** Resets the filter so it can be re-initialised at a new origin. */ + public void reset() { initialized = false; origin = null; @@ -62,14 +102,31 @@ public void reset() { weights = null; } + public LatLng getOrigin() { return origin; } + /** + * Loads wall segments into the filter for collision detection. + * Each element is a float array {@code {x1, y1, x2, y2}} in ENU metres relative to + * {@link #getOrigin()}. These segments are used in {@link #predict} to prevent + * particles from passing through building walls, and in {@link #wouldCrossWall} for + * trajectory display validation. + * + * @param wallSegments list of wall line segments; may be empty to disable collision checks + */ public void setWalls(List wallSegments) { this.walls = wallSegments; } + /** + * Returns {@code true} if at least one wall segment has been loaded via {@link #setWalls}. + * Used to distinguish indoor operation (walls present) from outdoor/unloaded state so + * that GNSS noise inflation and outlier thresholds can be tightened appropriately. + * + * @return {@code true} when the wall list is non-null and non-empty + */ public boolean hasWalls() { return walls != null && !walls.isEmpty(); } @@ -83,6 +140,23 @@ public boolean hasWalls() { // same parent are identical and provide no extra information on the next step. private static final float ROUGHENING_STD = 0.08f; // metres — reduced to limit stationary drift + /** + * Propagates each particle forward by one PDR step using a noisy motion model. + * The step displacement is derived from the PDR easting/northing deltas. Per-particle + * heading-bias (see {@link #INITIAL_BIAS_STD}) is applied before adding Gaussian noise + * ({@link #HEADING_NOISE_STD}, {@link #STRIDE_LENGTH_NOISE_STD}) to the heading and stride. + * Particles that would cross a wall segment are instead slid along the wall tangent so they + * remain inside the building without clustering at the boundary. + *

+ * Weights are intentionally unchanged here; weight updates occur only in + * {@link #updateGNSS} and {@link #updateWiFi} when an observation arrives. + * + * @param deltaEasting eastward PDR displacement since the last step, in metres + * @param deltaNorthing northward PDR displacement since the last step, in metres + * + * written by -- Japjot + * @JapjotS + */ public void predict(float deltaEasting, float deltaNorthing) { if (!initialized) return; // ignore if not initialized @@ -186,7 +260,7 @@ private boolean doIntersect(float x1, float y1, float x2, float y2, float x3, fl } /** - * Computes the effective sample size: N_eff = 1 / Σ(w_i²). + * Computes the effective sample size: N_eff = 1 / sum(w_i²). * *

N_eff equals NUM_PARTICLES when all weights are equal (maximum diversity) * and approaches 1 when a single particle carries all the weight (collapsed). @@ -248,6 +322,17 @@ private void resample() { } } + /** + * Updates particle weights from a GNSS observation using a Student-t likelihood function. + * The Student-t distribution (ν=4) has heavier tails than a Gaussian, making it robust to + * the multipath and NLOS outlier fixes common indoors. When wall segments are loaded the + * effective accuracy is inflated to {@code max(10 × gnssAccuracy, 50 m)} to prevent indoor + * GPS noise from dragging particles through walls. Systematic resampling is triggered when + * the effective sample size N_eff drops below N/2. + * + * @param gnssPosition WGS-84 position reported by the GNSS subsystem + * @param gnssAccuracy 1-sigma horizontal accuracy reported by the GNSS fix, in metres + */ public void updateGNSS(LatLng gnssPosition, float gnssAccuracy) { if (!initialized) return; // ignore if not initialized @@ -266,7 +351,7 @@ public void updateGNSS(LatLng gnssPosition, float gnssAccuracy) { // Student-t likelihood (ν=4) — heavier tails than Gaussian, robust to outlier GNSS fixes. // Based on: Nurminen, H. et al. (2013). "Particle Filter and Smoother for Indoor - // Localization." IPIN 2013. w_i ∝ (1 + d²/(ν·σ²))^(-(ν+2)/2), ν=4 → exponent=-3. + // Localization." IPIN 2013. w_i isproportional to (1 + d²/(ν·sigma²))^(-(ν+2)/2), ν=4 → exponent=-3. final float nu = 4.0f; float weightSum = 0f; for (int i = 0; i < NUM_PARTICLES; i++) { @@ -299,6 +384,16 @@ public void updateGNSS(LatLng gnssPosition, float gnssAccuracy) { } + /** + * Updates particle weights from a WiFi fingerprinting observation using a Student-t + * likelihood function identical in form to {@link #updateGNSS}. WiFi fixes are the + * preferred indoor anchor because they are unaffected by multipath; the filter is + * initialised here if no prior fix exists, and reset to this position if the current + * estimate has diverged by more than 2.5× the particle-cloud spread. + * + * @param wifiPosition WGS-84 position returned by the openpositioning WiFi API + * @param wifiAccuracy assumed 1-sigma accuracy of the WiFi fix, in metres + */ public void updateWiFi(LatLng wifiPosition, float wifiAccuracy) { if (!initialized) return; // ignore if not initialized float[] mesurementENU = UtilFunctions.convertWGS84ToENU(origin, wifiPosition); @@ -339,6 +434,14 @@ public void updateWiFi(LatLng wifiPosition, float wifiAccuracy) { } + /** + * Computes and returns the fused position estimate as the weighted mean of all particles. + * The mean is computed in ENU space and then converted back to WGS-84. This is the + * primary output of the filter, representing the best current estimate of the user's + * location given all PDR, GNSS, and WiFi observations received so far. + * + * @return fused WGS-84 position, or {@code null} if the filter has not been initialised + */ public LatLng getFusedPosition() { if (!initialized) return null; // Return null if not initialized @@ -359,6 +462,15 @@ public LatLng getFusedPosition() { * Provides a live estimate of position uncertainty — large when particles are * spread out, small when they are tightly clustered. */ + /** + * Returns the RMS spread of particles around their weighted mean, in metres. + * Computed as {@code sqrt(weightedVariance_E + weightedVariance_N)}, this provides a + * live estimate of positional uncertainty: large when the cloud is spread out (poor + * convergence or divergence), small when particles are tightly clustered (high confidence). + * Used by the map display to scale the accuracy circle and by the GNSS outlier gate. + * + * @return position uncertainty in metres, or {@link Float#MAX_VALUE} if not initialised + */ public float getPositionUncertaintyMeters() { if (!initialized) return Float.MAX_VALUE; float meanE = 0f, meanN = 0f; 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 aebc56f1..8ff5da47 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java @@ -23,7 +23,18 @@ public class SensorEventHandler { // FOR PARTICLE FILTER + /** + * Callback invoked by {@link SensorEventHandler} every time a valid step is detected. + * The deltas are in ENU metres relative to the session origin, computed from the + * accumulated PDR position at the time of the step event. + */ public interface StepListener { + /** + * Called once per accepted step. + * + * @param deltaEasting eastward displacement of this step in metres + * @param deltaNorthing northward displacement of this step in metres + */ void onStep(float deltaEasting, float deltaNorthing); } @@ -36,10 +47,24 @@ public interface StepListener { private static final float MAX_HEADING_JUMP_RAD = (float) (Math.PI / 6.0); private static final long MIN_STEP_INTERVAL_MS = 300; + /** + * Registers a listener to receive per-step ENU displacement notifications. + * The listener is invoked from the sensor thread each time a step passes all quality + * gates (minimum interval, peak acceleration, heading outlier check). Pass {@code null} + * to remove a previously registered listener. + * + * @param listener the {@link StepListener} to notify on each accepted step, or {@code null} + */ public void setStepListener(StepListener listener) { this.stepListener = listener; } + /** + * Resets the PDR step-delta baseline to zero and clears the heading history. + * Must be called at the start of each recording session so that the first step of the + * new session does not fire a large spurious delta derived from the previous session's + * accumulated PDR position. + */ public void resetStepOrigin() { lastEasting = 0f; lastNorthing = 0f; From d2cc95135cba3dffeaed0d9a30fb5b9933a98ed5 Mon Sep 17 00:00:00 2001 From: JP10JOT <97289379+JapjotS@users.noreply.github.com> Date: Wed, 1 Apr 2026 23:48:27 +0100 Subject: [PATCH 36/36] changes --- .../fragment/RecordingFragment.java | 21 +++++++++---------- .../PositionMe/sensors/ParticleFilter.java | 10 ++++----- .../PositionMe/sensors/SensorFusion.java | 10 ++++----- .../PositionMe/utils/IndoorMapManager.java | 9 +++----- 4 files changed, 22 insertions(+), 28 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 4e35db59..37f3d373 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 @@ -106,15 +106,13 @@ public class RecordingFragment extends Fragment { private float lastAcceptedPdrX = 0f; private float lastAcceptedPdrY = 0f; - // Last WiFi location sent to the map — avoids flooding wifiHistory with the same point private LatLng lastSentWifiPosition = null; private final Runnable refreshDataTask = new Runnable() { @Override public void run() { updateUIandPosition(); - // Loop again — 16 ms ≈ 60 fps for smooth marker and camera animation - refreshDataHandler.postDelayed(refreshDataTask, 16); + refreshDataHandler.postDelayed(refreshDataTask, 16); //loop faster } }; @@ -274,7 +272,7 @@ private void onAddTestPoint() { * Update the UI with sensor data and pass map updates to TrajectoryMapFragment. */ private void updateUIandPosition() { - // Elevation comes from the barometer — available immediately, no PDR needed. + // Elevation comes from the barometer so is available immediately, no PDR needed should work FINE float elevationVal = sensorFusion.getElevation(); elevation.setText(getString(R.string.elevation, String.format("%.1f", elevationVal))); @@ -312,6 +310,8 @@ private void updateUIandPosition() { boolean isFirstPoint = (lastSentFusedPosition == null); double movedDistance = 0.0; + + // Determine if the fused position has moved enough to warrant a map update. boolean movementDetected = false; if (lastSentFusedPosition != null) { @@ -320,7 +320,7 @@ private void updateUIandPosition() { movementDetected = movedDistance >= MOVEMENT_THRESHOLD_METERS; } - if (isFirstPoint || movementDetected) { + if (isFirstPoint || movementDetected) { // Only update the map if it's the first point or if the fused position has moved enough LatLng positionToRender; if (!isFirstPoint && movedDistance > MAX_JUMP_METERS) { @@ -328,7 +328,8 @@ private void updateUIandPosition() { // The fused estimate jumped implausibly (bad GNSS/filter divergence). // Fall back to PDR dead-reckoning: apply the PDR step delta to the // last confirmed anchor so the trajectory stays physically continuous. - float pdrDeltaX = pdrValues[0] - lastAcceptedPdrX; + //i think this should work + float pdrDeltaX = pdrValues[0] - lastAcceptedPdrX; // Compute PDR step delta since last accepted point float pdrDeltaY = pdrValues[1] - lastAcceptedPdrY; double pdrStep = Math.sqrt(pdrDeltaX * pdrDeltaX + pdrDeltaY * pdrDeltaY); @@ -336,16 +337,15 @@ private void updateUIandPosition() { positionToRender = UtilFunctions.convertENUToWGS84( lastSentFusedPosition, new float[]{pdrDeltaX, pdrDeltaY, 0f}); - Log.d("FUSED_TEST", "Teleport " + (int) movedDistance - + "m → PDR fallback " + String.format("%.2f", pdrStep) + "m"); + } else { - // PDR also hasn't moved enough — stay put this tick + // PDR also hasn't moved enough so stay put this tick positionToRender = null; Log.d("FUSED_TEST", "Teleport " + (int) movedDistance + "m rejected, no PDR movement yet"); } } else { - // Normal update — fused estimate is within plausible range + // Normal update hence fused estimate is within plausible range positionToRender = fusedPosition; } @@ -364,7 +364,6 @@ private void updateUIandPosition() { - // GNSS logic if you want to show GNSS error, etc. float[] gnss = sensorFusion.getSensorValueMap().get(SensorTypes.GNSSLATLONG); if (gnss != null && trajectoryMapFragment != null) { // If user toggles showing GNSS in the map, call e.g. diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java index f23cfca8..b96fabbe 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java @@ -19,7 +19,7 @@ * with per-particle heading-bias correction and Gaussian noise, then slides any * particle that would cross a wall along the wall tangent so it remains inside * the building. - *

  • GNSS weights particles with a Student-t likelihood (ν=4) that is robust to + *
  • GNSS weights particles with a Student-t likelihood (v=4) that is robust to * the multipath outliers common indoors. Accuracy is inflated 10× when wall * segments are loaded to prevent bad GPS fixes from dragging the cloud outside * the building.
    • @@ -324,7 +324,7 @@ private void resample() { /** * Updates particle weights from a GNSS observation using a Student-t likelihood function. - * The Student-t distribution (ν=4) has heavier tails than a Gaussian, making it robust to + * The Student-t distribution (v=4) has heavier tails than a Gaussian, making it robust to * the multipath and NLOS outlier fixes common indoors. When wall segments are loaded the * effective accuracy is inflated to {@code max(10 × gnssAccuracy, 50 m)} to prevent indoor * GPS noise from dragging particles through walls. Systematic resampling is triggered when @@ -349,9 +349,9 @@ public void updateGNSS(LatLng gnssPosition, float gnssAccuracy) { float variance = effectiveAccuracy * effectiveAccuracy; // Convert accuracy to variance sigma^2 - // Student-t likelihood (ν=4) — heavier tails than Gaussian, robust to outlier GNSS fixes. + // Student-t likelihood (v=4) — heavier tails than Gaussian, robust to outlier GNSS fixes. // Based on: Nurminen, H. et al. (2013). "Particle Filter and Smoother for Indoor - // Localization." IPIN 2013. w_i isproportional to (1 + d²/(ν·sigma²))^(-(ν+2)/2), ν=4 → exponent=-3. + // Localization." IPIN 2013. w_i isproportional to (1 + d²/(v·sigma²))^(-(v+2)/2), v=4 leasd to exponent=-3. final float nu = 4.0f; float weightSum = 0f; for (int i = 0; i < NUM_PARTICLES; i++) { @@ -402,7 +402,7 @@ public void updateWiFi(LatLng wifiPosition, float wifiAccuracy) { float variance = wifiAccuracy * wifiAccuracy; // Convert accuracy to variance sigma^2 - // Student-t likelihood (ν=4) — same robust formulation as updateGNSS(). + // Student-t likelihood (v=4) — same robust formulation as updateGNSS(). final float nu = 4.0f; float weightSum = 0f; for (int i = 0; i < NUM_PARTICLES; i++) { 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 a6c95546..2b57465a 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -312,7 +312,7 @@ private void updateHeadingCalibration() { for (float h : headingCalibSamples) sum += h; headingBias = sum / headingCalibSamples.size(); headingCalibDone = true; - Log.d("SensorFusion", "Heading bias calibrated: " + (float) Math.toDegrees(headingBias) + "°"); + Log.d("SensorFusion", "Heading bias calibrated: " + (float) Math.toDegrees(headingBias) + "degrees"); } } @@ -408,12 +408,10 @@ private void updateFloorLogic() { if (Math.abs(diff - targetElev) > LIFT_SNAP_TOLERANCE_M) return; pdrProcessing.setCurrentFloor(targetFloor); String TAG = "FLOOR CHANGE LIFT"; - Log.d(TAG, "Floor change (lift) → " + targetFloor + " diff=" + diff); } else if ("stairs".equals(type) && !isLift) { //pdrProcessing.setCurrentFloor(targetFloor); String TAG = "FLOOR CHANGE STAIRS"; - Log.d(TAG, "Floor change (stairs) → " + targetFloor + " diff=" + diff); } } @@ -967,11 +965,11 @@ public boolean isWifiPositionFresh() { } /** - * Returns the magnitude of horizontal filtered acceleration (m/s²). + * Returns the magnitude of horizontal filtered acceleration (m/s squared). * Used by CrossFloorClassifier to distinguish lift (low horizontal movement) * from stairs (higher horizontal movement) during floor transitions. * - * @return horizontal acceleration magnitude in m/s² + * @return horizontal acceleration magnitude in m/ss quared */ public float getHorizontalAccelMagnitude() { return (float) Math.sqrt( @@ -1101,7 +1099,7 @@ public void onLocationChanged(@NonNull Location location) { } else if (uncertainty < 30f && offset > Math.max(accuracy * 2.5f, 20f)) { outlier = true; Log.d("SensorFusion", "GNSS outlier: " + (int) offset - + "m offset, σ=" + (int) uncertainty + "m"); + + "m offset, sigma=" + (int) uncertainty + "m"); } } if (!outlier) { diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java b/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java index c103e35c..a3dc1c15 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java @@ -88,7 +88,7 @@ public class IndoorMapManager { public static final float LIBRARY_FLOOR_HEIGHT = 3.6F; public static final float MURCHISON_FLOOR_HEIGHT = 4.0F; - // Active trajectory color — set by the user via the color toggle button. + // Active trajectory color set by the user via the color toggle button. // Floor plan shapes are drawn in this color so they stay visually consistent. private int floorPlanColor = Color.RED; // default matches the default polyline color @@ -507,10 +507,7 @@ private void clearDrawnShapes() { /** * Returns the stroke colour for a given indoor feature type. * Feature colours are fixed regardless of the user's trajectory colour choice: - * wall → red (impassable boundary) - * stairs → yellow (cross-floor via stairs) - * lift → green (cross-floor via lift) - * room → current trajectory colour (neutral room outline) + * * @param indoorType the indoor_type property value * @return ARGB colour value @@ -519,7 +516,7 @@ private int getStrokeColor(String indoorType) { if ("wall".equals(indoorType)) return Color.argb(220, 220, 30, 30); // red if ("stairs".equals(indoorType)) return Color.argb(220, 220, 180, 0); // yellow if ("lift".equals(indoorType)) return Color.argb(220, 30, 180, 30); // green - // room / other — use the user-chosen trajectory colour + // room / other use the user-chosen trajectory colour int r = Color.red(floorPlanColor); int g = Color.green(floorPlanColor); int b = Color.blue(floorPlanColor);