From f4c45eb62bbfd60391869ce768eaeb1ae86e2bae Mon Sep 17 00:00:00 2001 From: Joshua Bray Date: Sat, 28 Mar 2026 20:06:45 +0000 Subject: [PATCH 01/14] Initial estimation logic, local co-ordinate frame for WGS84 centre, easting/northing space --- .../PositionMe/sensors/ParticleFilter.java | 200 ++++++++++++++++++ secrets.properties | 6 +- 2 files changed, 203 insertions(+), 3 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..f026dd58 --- /dev/null +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java @@ -0,0 +1,200 @@ +package com.openpositioning.PositionMe.sensors; + +import com.google.android.gms.maps.model.LatLng; + +import android.util.Log; + +import java.util.Random; + +/** + * The Particle Filter class represents a sensor fusion algorithm + */ + +public class ParticleFilter { + + // Number of particles maintained by the filter + private static final int Num_Particles = 200; + + // Meters conversion constant for WGS84 to easting, northing space + private static final double Meters_Per_Degree = 111111.0; + + // Initial standard deviation spread from GNSS/WiFi fix. + private static final float GNSS_Init_STD = 15.0f; + private static final float WiFi_Init_STD = 8.0f; + + // The defined particle set + private final Particle[] particles; + + // WGS84 reference point used as the local coordinate frame origin + private LatLng originLatLng; + + // Weighted estimate easting and northing of the current position estimate + private float estimatedX; + private float estimatedY; + + // True once initial position estimation has been established + private boolean initialised; + + private final Random random; + private final SensorFusion sensorFusion; + + // Defines a single hypothesis about the user's position + private static class Particle{ + // Easting offset from local origin + float x; + + //Northing offset from local origin + float y; + + // Normalised importance weight + float weight; + + Particle(float x, float y, float weight) { + this.x = x; + this.y = y; + this.weight = weight; + } + } + + /** + * Constructor for the Particle Filter class. + * + * Attempts to initialise position and particles estimate from available WiFi and GNSS data. + * If neither are available at construction, tryInitialise() attempts again on later call. + */ + public ParticleFilter(){ + this.sensorFusion = SensorFusion.getInstance(); + this.particles = new Particle[Num_Particles]; + this.random = new Random(); + this.initialised = false; + + initialisePosition(); + } + + /** + * Automatically estimates the first available position from WiFi or GNSS + * and defines the particle set around this WGS84 point as an isotropic 2D + * Gaussian distribution. + * + * WiFi estimation prioritised for its greater indoor positioning accuracy. + * If unavailable, GNSS is used for the initial estimation instead. + * If neither is available, tryIinitialise() is called later. + */ + private void initialisePosition() { + float[] gnssLatLon = sensorFusion.getGNSSLatitude(false); + LatLng wifiLatLng = sensorFusion.getLatLngWifiPositioning(); + + // GNSS positioning returns null if no reading is available + boolean hasGNSS = gnssLatLon != null + && (gnssLatLon[0] != 0f || gnssLatLon[1] != 0f); + + // WiFi positioning returns null if no reading is available + boolean hasWifi = wifiLatLng != null; + + // Defer initialisation if neither GNSS or WiFi have a valid reading + if (!hasGNSS && !hasWifi) { + return; // Initialisation retried via tryInitialise() on later call + } + + // Initialisation source and particle spread based on priority + LatLng initLatLng; + float spreadStd; + + // Uses WiFi positioning if available, otherwise GNSS as fallback + if (hasWifi) { + initLatLng = wifiLatLng; + spreadStd = WiFi_Init_STD; + } else { + initLatLng = new LatLng(gnssLatLon[0], gnssLatLon[1]); + spreadStd = GNSS_Init_STD; + } + + // Set local frame origin at initial estimated position + originLatLng = initLatLng; + Log.d("ParticleFilter", "Origin set to: " + + originLatLng.latitude + ", " + + originLatLng.longitude); + + // Distribute particles around (0 , 0) in local frame + spreadParticles(0.0f, 0.0f, spreadStd); + initialised = true; + } + + /** + * Function to retry initial position estimation if it was deferred at construction + */ + public void tryInitialise() { + if (!initialised) { + initialisePosition(); + } + } + + /** + * Distributes particles as an isotropic 2D gaussian distribution around + * (centerX, centerY) and assigns equal weights to each particle (1/N). + */ + private void spreadParticles(float centerX, float centerY, float stdM) { + float initWeight = 1.0f / Num_Particles; + for (int i = 0; i < Num_Particles; i++) { + float x = centerX + (float) (random.nextGaussian() * stdM); + float y = centerY + (float) (random.nextGaussian() * stdM); + particles[i] = new Particle(x, y, initWeight); + } + + estimatedX = centerX; + estimatedY = centerY; + } + + /** + * Converts a WGS84 LatLng to a local easting/northing offset in meters + * relative to the local frame origin (originLatLng). + */ + public float[] latLngToLocal(LatLng point) { + if (originLatLng == null) return new float[]{0f, 0f}; + + double latDiff = point.latitude - originLatLng.latitude; + double lonDiff = point.longitude - originLatLng.longitude; + + float northing = (float) (latDiff * Meters_Per_Degree); + float easting = (float) (lonDiff * Meters_Per_Degree + * Math.cos(Math.toRadians(originLatLng.latitude))); + + return new float[]{easting, northing}; + } + + /** + * Converts a local easting/northing offset in meters back to WGS84. + */ + public LatLng localToLatLng(float eastingM, float northingM) { + if (originLatLng == null) return null; + + double lat = originLatLng.latitude + + northingM / Meters_Per_Degree; + double lon = originLatLng.longitude + + eastingM / (Meters_Per_Degree + * Math.cos(Math.toRadians(originLatLng.latitude))); + + return new LatLng(lat, lon); + } + + /** + * State accessors defined below. + */ + + public boolean isInitialised() { + return initialised; + } + + public LatLng getEstimatedPosition() { + if (!initialised) return null; + return localToLatLng(estimatedX, estimatedY); + } + + public float[] getEstimatedLocalPosition() { + return new float[]{estimatedX, estimatedY}; + } + + public LatLng getOrigin() { + return originLatLng; + } +} diff --git a/secrets.properties b/secrets.properties index f0dc54fd..893161db 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=AIzaSyC13ios2NevfsRVS6yIj1arAM29ecy0GxQ +OPENPOSITIONING_API_KEY=c64dIaytmbCb84kU1-uuXw +OPENPOSITIONING_MASTER_KEY=ewireless From 9cc07b2063c3b92218bbdbea83b953b9bd88e841 Mon Sep 17 00:00:00 2001 From: Joshua Bray Date: Sun, 29 Mar 2026 14:59:49 +0100 Subject: [PATCH 02/14] PDR predict functionality on local WPG84 frame --- .../PositionMe/sensors/ParticleFilter.java | 37 +++++++++++++++++++ .../sensors/SensorEventHandler.java | 18 ++++++++- .../PositionMe/sensors/SensorFusion.java | 7 +++- 3 files changed, 60 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 f026dd58..0b9d7f98 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java @@ -22,6 +22,9 @@ public class ParticleFilter { private static final float GNSS_Init_STD = 15.0f; private static final float WiFi_Init_STD = 8.0f; + // PDR process noise standard deviation approximation (adjustable) + public static final float PDR_Process_Noise_STD = 0.4f; + // The defined particle set private final Particle[] particles; @@ -145,6 +148,40 @@ private void spreadParticles(float centerX, float centerY, float stdM) { estimatedY = centerY; } + /** + * PDR motion model prediction + * + * Moves all defined particles according to PDR displacement with + * added Gaussian process noise to account for uncertainty in step length + * and heading. + */ + public void predict(float deltaEast, float deltaNorth, float PDRstd) { + // Early return if filter not initialised + if (!initialised) { + return; + } + + // Shift each particle with Gaussian noise considered + for (int i = 0; i < Num_Particles; i++) { + float noiseX = (float) (random.nextGaussian() * PDRstd); + float noiseY = (float) (random.nextGaussian() * PDRstd); + particles[i].x += deltaEast + noiseX; + particles[i].y += deltaNorth + noiseY; + } + + // Update weighted mean estimate + estimatedX = 0f; + estimatedY = 0f; + for (int i = 0; i < Num_Particles; i++) { + estimatedX += particles[i].x * particles[i].weight; + estimatedY += particles[i].y * particles[i].weight; + } + + //Debug test + Log.d("ParticleFilter", "Delta: (" + deltaEast + ", " + deltaNorth + + " ; Estimate: ("+ estimatedX + ", " + estimatedY +")"); + } + /** * Converts a WGS84 LatLng to a local easting/northing offset in meters * relative to the local frame origin (originLatLng). 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..bbe65bb7 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java @@ -29,6 +29,7 @@ public class SensorEventHandler { private final PdrProcessing pdrProcessing; private final PathView pathView; private final TrajectoryRecorder recorder; + private final ParticleFilter particleFilter; // Timestamp tracking private final HashMap lastEventTimestamps = new HashMap<>(); @@ -39,6 +40,10 @@ public class SensorEventHandler { // Acceleration magnitude buffer between steps private final List accelMagnitude = new ArrayList<>(); + // Previous PDR position for displacement deltas computation + private float previousPDRX = 0f; + private float previousPDRY = 0f; + /** * Creates a new SensorEventHandler. * @@ -46,15 +51,17 @@ public class SensorEventHandler { * @param pdrProcessing PDR processor for step-length and position calculation * @param pathView path drawing view for trajectory visualisation * @param recorder trajectory recorder for checking recording state and writing PDR data + * @param particleFilter particle filter sensor fusion algorithm * @param bootTime initial boot time offset */ public SensorEventHandler(SensorState state, PdrProcessing pdrProcessing, PathView pathView, TrajectoryRecorder recorder, - long bootTime) { + ParticleFilter particleFilter, long bootTime) { this.state = state; this.pdrProcessing = pdrProcessing; this.pathView = pathView; this.recorder = recorder; + this.particleFilter = particleFilter; this.bootTime = bootTime; } @@ -174,6 +181,15 @@ public void handleSensorEvent(SensorEvent sensorEvent) { this.accelMagnitude.clear(); if (recorder.isRecording()) { + // Compute PDR displacement and shift particles + float deltaX = newCords[0] - previousPDRX; + float deltaY = newCords[1] - previousPDRY; + particleFilter.predict(deltaX, deltaY, ParticleFilter.PDR_Process_Noise_STD); + + // Update previous PDR position for next step + previousPDRX = newCords[0]; + previousPDRY = newCords[1]; + this.pathView.drawTrajectory(newCords); state.stepCounter++; recorder.addPdrData( 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..27f6926d 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -63,6 +63,7 @@ public class SensorFusion implements SensorEventListener { private SensorEventHandler eventHandler; private TrajectoryRecorder recorder; private WifiPositionManager wifiPositionManager; + private ParticleFilter particleFilter; // Movement sensor instances (lifecycle managed here) private MovementSensor accelerometerSensor; @@ -160,9 +161,12 @@ public void setContext(Context context) { this.wifiPositionManager = new WifiPositionManager(wiFiPositioning, recorder); + // Initialise particle filter + this.particleFilter = new ParticleFilter(); + long bootTime = SystemClock.uptimeMillis(); this.eventHandler = new SensorEventHandler( - state, pdrProcessing, pathView, recorder, bootTime); + state, pdrProcessing, pathView, recorder, particleFilter, bootTime); // Register WiFi observer on WifiPositionManager (not on SensorFusion) this.wifiProcessor = new WifiDataProcessor(context); @@ -328,6 +332,7 @@ private void stopWirelessCollectors() { public void startRecording() { recorder.startRecording(pdrProcessing); eventHandler.resetBootTime(recorder.getBootTime()); + particleFilter.tryInitialise(); // Handover WiFi/BLE scan lifecycle from activity callbacks to foreground service. stopWirelessCollectors(); From 16ec151770a18d416637912f3e808985e4f7c71c Mon Sep 17 00:00:00 2001 From: Joshua Bray Date: Sun, 29 Mar 2026 20:46:28 +0100 Subject: [PATCH 03/14] GNSS update + particle weighting --- .../PositionMe/sensors/ParticleFilter.java | 56 +++++++++++++++++++ .../PositionMe/sensors/SensorFusion.java | 7 +++ 2 files changed, 63 insertions(+) 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 0b9d7f98..6714c757 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java @@ -25,6 +25,10 @@ public class ParticleFilter { // PDR process noise standard deviation approximation (adjustable) public static final float PDR_Process_Noise_STD = 0.4f; + // Min and max GNSS measurment accuracy for weight update (define rejected measurements) + private static final float GNSS_Min_Accuracy = 5.0f; + private static final float GNSS_Max_Accuracy = 50.0f; + // The defined particle set private final Particle[] particles; @@ -182,6 +186,58 @@ public void predict(float deltaEast, float deltaNorth, float PDRstd) { + " ; Estimate: ("+ estimatedX + ", " + estimatedY +")"); } + /** + * Particle weight update + */ + public void updateWeights(LatLng measurementLatLng, float accuracy) { + // Early return if not initialised + if (!initialised || measurementLatLng == null) { + return; + } + + // Early return if out of accuracy range (poor measurement) + if (accuracy <= 0 || accuracy >= GNSS_Max_Accuracy) { + return; + } + float sigma = Math.max(accuracy, GNSS_Min_Accuracy); + + // Convert received measurement to local frame (meters) + float[] Measurement_Local = latLngToLocal(measurementLatLng); + float mx = Measurement_Local[0]; + float my = Measurement_Local[1]; + + // Calculate each particle weighting + float twoSigmaSquared = 2.0f * sigma * sigma; + for (int i = 0; i < Num_Particles; i++) { + // Calculate distance in meters from particle to measurement + float dx = particles[i].x - mx; + float dy = particles[i].y - my; + + // Gaussian likelihood equation + float d = (float) Math.sqrt(dx * dx + dy * dy); + float dSquared = d * d; + float likelihood = (float) Math.exp(-dSquared / twoSigmaSquared); + particles[i].weight *= likelihood; + } + + // Normalise weights and to sum = 1.0 + float weightSum = 0f; + for (int i = 0; i < Num_Particles; i++) { + weightSum += particles[i].weight; + } + for (int i = 0; i < Num_Particles; i++) { + particles[i].weight /= weightSum; + } + + // Recalculate weighted mean estimate of position + estimatedX = 0f; + estimatedY = 0f; + for (int i = 0; i < Num_Particles; i++) { + estimatedX += particles[i].x * particles[i].weight; + estimatedY += particles[i].y * particles[i].weight; + } + } + /** * Converts a WGS84 LatLng to a local easting/northing offset in meters * relative to the local frame origin (originLatLng). 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 27f6926d..281e86b2 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -650,6 +650,13 @@ public void onLocationChanged(@NonNull Location location) { state.latitude = (float) location.getLatitude(); state.longitude = (float) location.getLongitude(); recorder.addGnssData(location); + + // Update particle weights with GNSS measurement + if (particleFilter.isInitialised()) { + LatLng gnssLatLng = new LatLng(location.getLatitude(), location.getLongitude()); + float accuracy = location.getAccuracy(); + particleFilter.updateWeights(gnssLatLng, accuracy); + } } } From 0cbc7c56b0beb5cb7871bb36d572fadd359cfe3f Mon Sep 17 00:00:00 2001 From: Joshua Bray Date: Mon, 30 Mar 2026 13:03:52 +0100 Subject: [PATCH 04/14] WiFi update + weighting --- .../PositionMe/sensors/ParticleFilter.java | 18 ++++++++++++- .../PositionMe/sensors/SensorFusion.java | 8 +++--- .../PositionMe/sensors/WiFiPositioning.java | 25 ++++++++++++++++++- 3 files changed, 46 insertions(+), 5 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 6714c757..900d44f9 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java @@ -25,7 +25,7 @@ public class ParticleFilter { // PDR process noise standard deviation approximation (adjustable) public static final float PDR_Process_Noise_STD = 0.4f; - // Min and max GNSS measurment accuracy for weight update (define rejected measurements) + // Min and max GNSS measurement accuracy for weight update (define rejected measurements) private static final float GNSS_Min_Accuracy = 5.0f; private static final float GNSS_Max_Accuracy = 50.0f; @@ -236,6 +236,22 @@ public void updateWeights(LatLng measurementLatLng, float accuracy) { estimatedX += particles[i].x * particles[i].weight; estimatedY += particles[i].y * particles[i].weight; } + + // Debugging code for terminal output and observation + float maxWeight = 0f; + int maxIndex = 0; + for (int i = 0; i < Num_Particles; i++) { + if (particles[i].weight > maxWeight) { + maxWeight = particles[i].weight; + maxIndex = i; + } + } + + Log.d("ParticleFilter", "Update called - " + + "Measurement: (" + mx + ", " + my + ") " + + "Estimate: (" + estimatedX + ", " + estimatedY + ") " + + "Best particle: (" + particles[maxIndex].x + ", " + particles[maxIndex].y + ") " + + "Max weight: " + maxWeight); } /** 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 281e86b2..d9af5ed9 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -151,7 +151,7 @@ public void setContext(Context context) { SharedPreferences settings = PreferenceManager.getDefaultSharedPreferences(context); this.pdrProcessing = new PdrProcessing(context); this.pathView = new PathView(context, null); - WiFiPositioning wiFiPositioning = new WiFiPositioning(context); + WiFiPositioning wiFiPositioning = new WiFiPositioning(context, null); // Create internal modules this.recorder = new TrajectoryRecorder(appContext, state, serverCommunications, settings); @@ -164,6 +164,8 @@ public void setContext(Context context) { // Initialise particle filter this.particleFilter = new ParticleFilter(); + wiFiPositioning.setParticleFilter(particleFilter); + long bootTime = SystemClock.uptimeMillis(); this.eventHandler = new SensorEventHandler( state, pdrProcessing, pathView, recorder, particleFilter, bootTime); @@ -651,8 +653,8 @@ public void onLocationChanged(@NonNull Location location) { state.longitude = (float) location.getLongitude(); recorder.addGnssData(location); - // Update particle weights with GNSS measurement - if (particleFilter.isInitialised()) { + // Update particle weights with GNSS measurement + if (particleFilter.isInitialised()) { LatLng gnssLatLng = new LatLng(location.getLatitude(), location.getLongitude()); float accuracy = location.getAccuracy(); particleFilter.updateWeights(gnssLatLng, accuracy); 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..baceea8d 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/WiFiPositioning.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/WiFiPositioning.java @@ -54,6 +54,7 @@ public int getFloor() { // Store current floor of user, default value 0 (ground floor) private int floor=0; + private ParticleFilter particleFilter; /** * Constructor to create the WiFi positioning object @@ -62,9 +63,14 @@ public int getFloor() { * * @param context Context of object calling */ - public WiFiPositioning(Context context){ + public WiFiPositioning(Context context, ParticleFilter particleFilter){ // Initialising the Request queue this.requestQueue = Volley.newRequestQueue(context.getApplicationContext()); + this.particleFilter = particleFilter; + } + + public void setParticleFilter(ParticleFilter particleFilter) { + this.particleFilter = particleFilter; } /** @@ -89,6 +95,14 @@ public void request(JSONObject jsonWifiFeatures) { try { wifiLocation = new LatLng(response.getDouble("lat"),response.getDouble("lon")); floor = response.getInt("floor"); + + //Update particle filter weights with WiFi + if (particleFilter != null && particleFilter.isInitialised()) { + // Update particle filter weights with WiFi measurement + Log.d("ParticleFilter", "WiFi weight update: lat=" + wifiLocation.latitude + + ", lon=" + wifiLocation.longitude); + particleFilter.updateWeights(wifiLocation, 8.0f); + } } 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 +154,15 @@ 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"); + + //Update particle filter weights with WiFi + if (particleFilter != null && particleFilter.isInitialised()) { + // Update particle filter weights with WiFi measurement + Log.d("ParticleFilter", "WiFi weight update: lat=" + wifiLocation.latitude + + ", lon=" + wifiLocation.longitude); + particleFilter.updateWeights(wifiLocation, 8.0f); + } + callback.onSuccess(wifiLocation,floor); } catch (JSONException e) { Log.e("jsonErrors","Error parsing response: "+e.getMessage()+" "+ response); From 01ad8be3ebad879ffd8adde1835b35fa4e68a5d1 Mon Sep 17 00:00:00 2001 From: Joshua Bray Date: Mon, 30 Mar 2026 17:11:05 +0100 Subject: [PATCH 05/14] Initial resampling attempt --- .../PositionMe/sensors/ParticleFilter.java | 52 +++++++++++++++++++ 1 file changed, 52 insertions(+) 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 900d44f9..3d39ddc0 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java @@ -252,6 +252,58 @@ public void updateWeights(LatLng measurementLatLng, float accuracy) { + "Estimate: (" + estimatedX + ", " + estimatedY + ") " + "Best particle: (" + particles[maxIndex].x + ", " + particles[maxIndex].y + ") " + "Max weight: " + maxWeight); + + resample(); + } + + /** + * Systematic resampling of particles for weight degeneration solution (SIR) + */ + private void resample() { + // Calculate effective sample size as 1 / sum of particle weights squared + float sumWeightsSquared = 0f; + for (int i = 0; i < Num_Particles; i++) { + sumWeightsSquared += particles[i].weight * particles[i].weight; + } + float N_eff = 1.0f / sumWeightsSquared; + + // Early return if effective sample size >= set threshold + float threshold = Num_Particles / 2.0f; + if (N_eff >= threshold) { + Log.d("ParticleFilter", "Resampling skipped"); + return; + } + + // Build cumulative sum array from normalised weights + float[] cumulativeSum = new float[Num_Particles]; + cumulativeSum[0] = particles[0].weight; + for (int i = 1; i < Num_Particles; i++) { + cumulativeSum[i] = cumulativeSum[i-1] + particles[i].weight; + } + + // Set uniform staring point + float overN = 1.0f / Num_Particles; + float u1 = random.nextFloat() * overN; + + Particle[] resampledParticles = new Particle[Num_Particles]; + float uniformWeighting = overN; + int j = 0; + + for (int i = 0; i < Num_Particles; i++) { + float u = u1 + i * overN; + + while (j < Num_Particles - 1 && u > cumulativeSum[j]) { + j++; + } + + resampledParticles[i] = new Particle( + particles[j].x, particles[j].y, uniformWeighting); + } + + // Replace particle array with resampled particles + System.arraycopy(resampledParticles, 0, particles, 0, Num_Particles); + + Log.d("ParticleFilter", "Resampling complete: all weights reset to " + uniformWeighting); } /** From ee705c88fbb742d2f04ef351dce02b7185211c3b Mon Sep 17 00:00:00 2001 From: Greg Date: Mon, 30 Mar 2026 21:05:20 +0100 Subject: [PATCH 06/14] Add map matching bullet 1 - wall-based particle rejection --- .../PositionMe/sensors/MapGeometry.java | 90 ++++++ .../PositionMe/sensors/MapMatcher.java | 293 ++++++++++++++++++ .../PositionMe/sensors/ParticleFilter.java | 120 ++++++- .../PositionMe/sensors/SensorFusion.java | 35 ++- .../PositionMe/utils/PdrProcessing.java | 12 +- 5 files changed, 539 insertions(+), 11 deletions(-) create mode 100644 app/src/main/java/com/openpositioning/PositionMe/sensors/MapGeometry.java create mode 100644 app/src/main/java/com/openpositioning/PositionMe/sensors/MapMatcher.java diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/MapGeometry.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/MapGeometry.java new file mode 100644 index 00000000..246f29e4 --- /dev/null +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/MapGeometry.java @@ -0,0 +1,90 @@ +package com.openpositioning.PositionMe.sensors; + +import android.util.Log; + +import java.util.ArrayList; +import java.util.List; + +// NOTE: doesSegmentCross* methods are intentionally omitted — wall-crossing +// correction belongs to the separate "movement model / wall constraint" requirement. + +/** + * Pure static geometry helpers for map-matching. + * + * No instance state. No Android dependencies beyond Log. + * All coordinates are in the local easting/northing meter frame used by + * ParticleFilter. + * + * Logcat tag: MapGeometry (self-test results only) + */ +class MapGeometry { + + private static final String TAG = "MapGeometry"; + + // No instances needed + private MapGeometry() {} + + // ------------------------------------------------------------------------- + // Point-in-polygon + // ------------------------------------------------------------------------- + + /** + * Ray-casting point-in-polygon test. + * + * @param x easting (meters, local frame) + * @param y northing (meters, local frame) + * @param polygon list of float[]{eastingM, northingM} vertices + * @return true if (x,y) is strictly inside the polygon + */ + static boolean isPointInsidePolygon(float x, float y, List polygon) { + if (polygon == null || polygon.size() < 3) return false; + + int n = polygon.size(); + boolean inside = false; + int j = n - 1; + + for (int i = 0; i < n; i++) { + float xi = polygon.get(i)[0], yi = polygon.get(i)[1]; + float xj = polygon.get(j)[0], yj = polygon.get(j)[1]; + + if (((yi > y) != (yj > y)) + && (x < (xj - xi) * (y - yi) / (yj - yi) + xi)) { + inside = !inside; + } + j = i; + } + return inside; + } + + // ------------------------------------------------------------------------- + // Self-test (Step 2) + // ------------------------------------------------------------------------- + + /** + * Validates isPointInsidePolygon against known-correct answers. + * Called once after building map data is loaded. Tag: MapGeometry. + */ + static void selfTest() { + // Unit square: (0,0) → (10,0) → (10,10) → (0,10) + List square = new ArrayList<>(); + square.add(new float[]{0f, 0f}); + square.add(new float[]{10f, 0f}); + square.add(new float[]{10f, 10f}); + square.add(new float[]{0f, 10f}); + + check("isPointInsidePolygon(5,5) == true", + isPointInsidePolygon(5f, 5f, square), true); + + check("isPointInsidePolygon(15,5) == false", + isPointInsidePolygon(15f, 5f, square), false); + } + + private static void check(String name, boolean result, boolean expected) { + if (result == expected) { + Log.d(TAG, "PASS: " + name); + } else { + Log.e(TAG, "FAIL: " + name + + " (got " + result + ", expected " + expected + ")"); + } + } +} diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/MapMatcher.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/MapMatcher.java new file mode 100644 index 00000000..e9454381 --- /dev/null +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/MapMatcher.java @@ -0,0 +1,293 @@ +package com.openpositioning.PositionMe.sensors; + +import android.util.Log; + +import com.google.android.gms.maps.model.LatLng; +import com.openpositioning.PositionMe.data.remote.FloorplanApiClient; +import com.openpositioning.PositionMe.utils.BuildingPolygon; + +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; + + +class MapMatcher { + + private static final String TAG_LOAD = "MapMatcher"; + + /** Constant: earth meters per degree (matches ParticleFilter). */ + private static final double METERS_PER_DEGREE = 111111.0; + + // ========================================================================= + // Inner data class + // ========================================================================= + + /** + * A single wall polygon/polyline in the local coordinate frame. + */ + static class WallFeature { + /** Vertices as float[]{eastingM, northingM} in the local frame. */ + final List localPoints; + /** True for MultiPolygon/Polygon features; false for LineString features. */ + final boolean isPolygon; + + WallFeature(List pts, boolean isPolygon) { + this.localPoints = pts; + this.isPolygon = isPolygon; + } + } + + // ========================================================================= + // State + // ========================================================================= + + private final SensorFusion sensorFusion; + + // Null until tryLoadBuilding() succeeds + private String loadedBuildingId = null; + private LatLng mapOrigin = null; + private int numFloors = 0; + // Maps floor displayName ("GF", "F1", ...) to its floorShapes index. + // Built at load time; used by getLikelyFloorIndex() to resolve WiFi floor integers. + private Map displayNameToIndex = null; + + // Per-floor wall feature arrays (size = numFloors, indexed 0..numFloors-1) + // Null until tryLoadBuilding() succeeds; getWallsForFloor() guards against this. + @SuppressWarnings("unchecked") + private List[] wallsByFloor = null; + + // ========================================================================= + // Constructor + // ========================================================================= + + /** + * Creates a MapMatcher that reads data from the given SensorFusion instance. + * No loading is performed until tryLoadBuilding() is called. + * + * @param sf the SensorFusion singleton + */ + MapMatcher(SensorFusion sf) { + this.sensorFusion = sf; + } + + // ========================================================================= + // Step 1: loading + // ========================================================================= + + /** + * Loads wall/stair/lift features from the floorplan cache for the current building. + * + * Detection order: + * 1. Direct look-up by preferredBuildingId if non-null. + * 2. Ray-cast against API outline polygons using origin. + * 3. Closest building center as last resort. + * + * Silently returns if origin is null or no building is found. + * Runs the MapGeometry self-test (MapGeometry.selfTest()) on success. + * + * @param preferredBuildingId building name key (e.g. "nucleus_building"); may be null + * @param origin local coordinate frame origin from ParticleFilter + */ + @SuppressWarnings("unchecked") + void tryLoadBuilding(String preferredBuildingId, LatLng origin) { + if (origin == null) { + Log.w(TAG_LOAD, "tryLoadBuilding: origin is null — aborting"); + return; + } + + // 1. Preferred building by name + FloorplanApiClient.BuildingInfo building = null; + if (preferredBuildingId != null && !preferredBuildingId.isEmpty()) { + building = sensorFusion.getFloorplanBuilding(preferredBuildingId); + if (building != null) { + Log.d(TAG_LOAD, "Found preferred building: " + preferredBuildingId); + } + } + + // 2. Polygon detection against all cached buildings + if (building == null) { + for (FloorplanApiClient.BuildingInfo b : sensorFusion.getFloorplanBuildings()) { + List outline = b.getOutlinePolygon(); + if (outline != null && outline.size() >= 3 + && BuildingPolygon.pointInPolygon(origin, outline)) { + building = b; + Log.d(TAG_LOAD, "Building detected via polygon: " + b.getName()); + break; + } + } + } + + // 3. Closest center fallback + if (building == null) { + building = closestBuilding(origin); + if (building != null) { + Log.d(TAG_LOAD, "Building selected by closest center: " + building.getName()); + } + } + + if (building == null) { + Log.w(TAG_LOAD, "tryLoadBuilding: no building found in cache — map matching disabled"); + return; + } + + List floorShapes = building.getFloorShapesList(); + if (floorShapes == null || floorShapes.isEmpty()) { + Log.w(TAG_LOAD, "tryLoadBuilding: building has no floor shapes"); + return; + } + + // Allocate per-floor lists + numFloors = floorShapes.size(); + mapOrigin = origin; + loadedBuildingId = building.getName(); + + // Build displayName → floorIndex map for WiFi floor resolution + displayNameToIndex = new HashMap<>(); + for (int f = 0; f < numFloors; f++) { + String name = floorShapes.get(f).getDisplayName(); + if (name != null) displayNameToIndex.put(name, f); + } + + wallsByFloor = new List[numFloors]; + + for (int f = 0; f < numFloors; f++) { + wallsByFloor[f] = new ArrayList<>(); + + FloorplanApiClient.FloorShapes floor = floorShapes.get(f); + for (FloorplanApiClient.MapShapeFeature feature : floor.getFeatures()) { + if (!"wall".equals(feature.getIndoorType())) continue; + + String geoType = feature.getGeometryType(); + boolean isPoly = "MultiPolygon".equals(geoType) || "Polygon".equals(geoType); + + for (List part : feature.getParts()) { + List localPts = new ArrayList<>(part.size()); + for (LatLng ll : part) { + localPts.add(latLngToLocal(ll, origin)); + } + if (localPts.size() < 2) continue; + wallsByFloor[f].add(new WallFeature(localPts, isPoly)); + } + } + + Log.d(TAG_LOAD, String.format("Floor %d (%s): walls=%d", + f, floor.getDisplayName(), wallsByFloor[f].size())); + } + + Log.d(TAG_LOAD, "Loaded: building=" + loadedBuildingId + + " floors=" + numFloors); + + // Step 2: run geometry self-test + MapGeometry.selfTest(); + } + + /** + * Returns true once building data has been successfully loaded. + * All three conditions must hold: building ID set, origin set, and at least one floor parsed. + */ + boolean isInitialised() { + return loadedBuildingId != null && mapOrigin != null && numFloors > 0; + } + + // ========================================================================= + // Step 2: floor accessor + // ========================================================================= + + /** Returns the wall features for the given floor, or an empty list if out of range. */ + List getWallsForFloor(int floorIndex) { + if (wallsByFloor == null || floorIndex < 0 || floorIndex >= numFloors) { + return new ArrayList<>(); + } + return wallsByFloor[floorIndex]; + } + + // ========================================================================= + // Utilities + // ========================================================================= + + /** Returns the loaded building ID, or null if not loaded. */ + String getLoadedBuildingId() { + return loadedBuildingId; + } + + /** + * Returns the floor index most likely occupied by the user, derived from + * the current WiFi floor reading mapped to a floor display name. + * + * WiFi integers are converted to display names per building. The display name is then looked up in the index map built + * at load time, so the result is correct regardless of API floor ordering. + * + * Falls back to 0 if WiFi returns an unrecognised value. + */ + int getLikelyFloorIndex() { + if (displayNameToIndex == null) return 0; + int wifiFloor = sensorFusion.getWifiFloor(); + String name = wifiFloorToDisplayName(wifiFloor); + Integer idx = (name != null) ? displayNameToIndex.get(name) : null; + int result = (idx != null) ? idx : 0; + Log.d(TAG_LOAD, "getLikelyFloorIndex: wifiFloor=" + wifiFloor + + " elevation=" + sensorFusion.getElevation() + + " name=" + name + " index=" + result); + return result; + } + + /** + * Converts a WiFi floor integer to the display name used in the floorplan API + * for the currently loaded building. + * + * Nucleus / Murchison: 0=GF, 1=F1, 2=F2, 3=F3 (LG not available via WiFi). + * Generic fallback: treats the integer as a display-name string ("0", "1", ...). + */ + private String wifiFloorToDisplayName(int wifiFloor) { + if ("nucleus_building".equals(loadedBuildingId) + || "murchison_house".equals(loadedBuildingId)) { + switch (wifiFloor) { + case 0: return "GF"; + case 1: return "F1"; + case 2: return "F2"; + case 3: return "F3"; + default: + // Floor not covered by WiFi (e.g. LG) — infer from barometer elevation + return (sensorFusion.getElevation() < -1.5f) ? "LG" : "GF"; + } + } + return String.valueOf(wifiFloor); + } + + + /** + * Converts a WGS84 LatLng to a local easting/northing offset (meters) + * relative to origin, using the same formula as ParticleFilter. + */ + private static float[] latLngToLocal(LatLng point, LatLng origin) { + double latDiff = point.latitude - origin.latitude; + double lonDiff = point.longitude - origin.longitude; + float northing = (float) (latDiff * METERS_PER_DEGREE); + float easting = (float) (lonDiff * METERS_PER_DEGREE + * Math.cos(Math.toRadians(origin.latitude))); + return new float[]{easting, northing}; + } + + /** + * Returns the cached building whose centre is geographically closest to origin. + * Used as a last-resort fallback when no polygon contains the origin. + + * Distance is computed as squared Euclidean in lat/lon degrees. + */ + private FloorplanApiClient.BuildingInfo closestBuilding(LatLng origin) { + FloorplanApiClient.BuildingInfo best = null; + double bestDist = Double.MAX_VALUE; + for (FloorplanApiClient.BuildingInfo b : sensorFusion.getFloorplanBuildings()) { + LatLng center = b.getCenter(); + double dl = center.latitude - origin.latitude; + double dn = center.longitude - origin.longitude; + double dist = dl * dl + dn * dn; + if (dist < bestDist) { + bestDist = dist; + best = b; + } + } + return best; + } +} 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 900d44f9..3c98a543 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java @@ -4,6 +4,7 @@ import android.util.Log; +import java.util.List; import java.util.Random; /** @@ -45,6 +46,9 @@ public class ParticleFilter { private final Random random; private final SensorFusion sensorFusion; + // Map matcher for wall-constrained prediction (null-safe: if null, predict runs unchanged) + private MapMatcher mapMatcher; + // Defines a single hypothesis about the user's position private static class Particle{ // Easting offset from local origin @@ -129,13 +133,24 @@ private void initialisePosition() { /** * Function to retry initial position estimation if it was deferred at construction - */ + */ public void tryInitialise() { if (!initialised) { initialisePosition(); } } + /** + * Wires in a MapMatcher for wall-constrained particle prediction. + * When set, predict() will reject movements that enter or cross walls. + * Safe to call with null to disable map matching. + * + * @param mm the MapMatcher instance, or null + */ + public void setMapMatcher(MapMatcher mm) { + this.mapMatcher = mm; + } + /** * Distributes particles as an isotropic 2D gaussian distribution around * (centerX, centerY) and assigns equal weights to each particle (1/N). @@ -173,6 +188,53 @@ public void predict(float deltaEast, float deltaNorth, float PDRstd) { particles[i].y += deltaNorth + noiseY; } + // Wall rejection: zero weights of particles that land inside a wall polygon + Log.d("ParticleFilter", "predict() reached wall check — mapMatcher=" + + (mapMatcher == null ? "null" : (mapMatcher.isInitialised() ? "ready" : "not initialised"))); + if (mapMatcher == null) { + Log.w("ParticleFilter", "Wall rejection skipped: mapMatcher is null"); + } else if (!mapMatcher.isInitialised()) { + Log.w("ParticleFilter", "Wall rejection skipped: mapMatcher not initialised" + + " (building=" + mapMatcher.getLoadedBuildingId() + ")"); + } else { + int floorIndex = mapMatcher.getLikelyFloorIndex(); + List walls = mapMatcher.getWallsForFloor(floorIndex); + int zeroed = 0; + for (int i = 0; i < Num_Particles; i++) { + for (MapMatcher.WallFeature wall : walls) { + if (!wall.isPolygon) continue; + if (MapGeometry.isPointInsidePolygon( + particles[i].x, particles[i].y, wall.localPoints)) { + particles[i].weight = 0f; + zeroed++; + break; + } + } + } + Log.d("ParticleFilter", "Wall rejection (floor " + floorIndex + "): " + + zeroed + "/" + Num_Particles + " particles zeroed this step"); + + // Renormalise remaining weights + float weightSum = 0f; + for (int i = 0; i < Num_Particles; i++) { + weightSum += particles[i].weight; + } + if (weightSum == 0f) { + Log.w("ParticleFilter", "All particles rejected — recovering with uniform respread around estimate"); + float spread = 2.0f; + float uniform = 1.0f / Num_Particles; + for (int i = 0; i < Num_Particles; i++) { + particles[i].x = estimatedX + (float) (random.nextGaussian() * spread); + particles[i].y = estimatedY + (float) (random.nextGaussian() * spread); + particles[i].weight = uniform; + } + } else { + for (int i = 0; i < Num_Particles; i++) { + particles[i].weight /= weightSum; + } + } + } + // Update weighted mean estimate estimatedX = 0f; estimatedY = 0f; @@ -183,7 +245,7 @@ public void predict(float deltaEast, float deltaNorth, float PDRstd) { //Debug test Log.d("ParticleFilter", "Delta: (" + deltaEast + ", " + deltaNorth - + " ; Estimate: ("+ estimatedX + ", " + estimatedY +")"); + + " ; Estimate: ("+ estimatedX + ", " + estimatedY +")"); } /** @@ -252,6 +314,58 @@ public void updateWeights(LatLng measurementLatLng, float accuracy) { + "Estimate: (" + estimatedX + ", " + estimatedY + ") " + "Best particle: (" + particles[maxIndex].x + ", " + particles[maxIndex].y + ") " + "Max weight: " + maxWeight); + + resample(); + } + + /** + * Systematic resampling of particles for weight degeneration solution (SIR) + */ + private void resample() { + // Calculate effective sample size as 1 / sum of particle weights squared + float sumWeightsSquared = 0f; + for (int i = 0; i < Num_Particles; i++) { + sumWeightsSquared += particles[i].weight * particles[i].weight; + } + float N_eff = 1.0f / sumWeightsSquared; + + // Early return if effective sample size >= set threshold + float threshold = Num_Particles / 2.0f; + if (N_eff >= threshold) { + Log.d("ParticleFilter", "Resampling skipped"); + return; + } + + // Build cumulative sum array from normalised weights + float[] cumulativeSum = new float[Num_Particles]; + cumulativeSum[0] = particles[0].weight; + for (int i = 1; i < Num_Particles; i++) { + cumulativeSum[i] = cumulativeSum[i-1] + particles[i].weight; + } + + // Set uniform staring point + float overN = 1.0f / Num_Particles; + float u1 = random.nextFloat() * overN; + + Particle[] resampledParticles = new Particle[Num_Particles]; + float uniformWeighting = overN; + int j = 0; + + for (int i = 0; i < Num_Particles; i++) { + float u = u1 + i * overN; + + while (j < Num_Particles - 1 && u > cumulativeSum[j]) { + j++; + } + + resampledParticles[i] = new Particle( + particles[j].x, particles[j].y, uniformWeighting); + } + + // Replace particle array with resampled particles + System.arraycopy(resampledParticles, 0, particles, 0, Num_Particles); + + Log.d("ParticleFilter", "Resampling complete: all weights reset to " + uniformWeighting); } /** @@ -306,4 +420,4 @@ public float[] getEstimatedLocalPosition() { public LatLng getOrigin() { return originLatLng; } -} +} \ No newline at end of file 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 d9af5ed9..42c16ef3 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -11,6 +11,7 @@ import android.os.Handler; import android.os.Looper; import android.os.SystemClock; +import android.util.Log; import android.widget.Toast; import androidx.annotation.NonNull; @@ -64,6 +65,7 @@ public class SensorFusion implements SensorEventListener { private TrajectoryRecorder recorder; private WifiPositionManager wifiPositionManager; private ParticleFilter particleFilter; + private MapMatcher mapMatcher; // Movement sensor instances (lifecycle managed here) private MovementSensor accelerometerSensor; @@ -164,6 +166,10 @@ public void setContext(Context context) { // Initialise particle filter this.particleFilter = new ParticleFilter(); + // Create map matcher and wire into particle filter + this.mapMatcher = new MapMatcher(this); + particleFilter.setMapMatcher(mapMatcher); + wiFiPositioning.setParticleFilter(particleFilter); long bootTime = SystemClock.uptimeMillis(); @@ -336,6 +342,9 @@ public void startRecording() { eventHandler.resetBootTime(recorder.getBootTime()); particleFilter.tryInitialise(); + // Try to load building map now that particles are initialised + tryTriggerMapMatcher(getSelectedBuildingId()); + // Handover WiFi/BLE scan lifecycle from activity callbacks to foreground service. stopWirelessCollectors(); @@ -429,6 +438,9 @@ public void setFloorplanBuildings(List building } floorplanBuildingCache.put(building.getName(), building); } + + // Attempt to load building map now that the cache is populated + tryTriggerMapMatcher(getSelectedBuildingId()); } /** @@ -453,6 +465,25 @@ public List getFloorplanBuildings() { return new ArrayList<>(floorplanBuildingCache.values()); } + /** + * Attempts to load building map data into the map matcher when all required + * conditions are met: particle filter initialised, origin set, and floorplan + * cache non-empty. + * + *

Called from both {@link #setFloorplanBuildings} and {@link #startRecording} + * because either can fire first; all guards are checked each time.

+ * + * @param preferredBuildingId building name hint; may be null for auto-detection + */ + private void tryTriggerMapMatcher(String preferredBuildingId) { + if (mapMatcher == null) return; + if (!particleFilter.isInitialised()) return; + if (particleFilter.getOrigin() == null) return; + if (floorplanBuildingCache.isEmpty()) return; + mapMatcher.tryLoadBuilding(preferredBuildingId, particleFilter.getOrigin()); + Log.d("Debug", "MapMatcher ready: " + mapMatcher.isInitialised()); + } + /** * Writes the initial position and heading into the trajectory protobuf. * Should be called after startRecording() and setStartGNSSLatitude(). @@ -653,8 +684,8 @@ public void onLocationChanged(@NonNull Location location) { state.longitude = (float) location.getLongitude(); recorder.addGnssData(location); - // Update particle weights with GNSS measurement - if (particleFilter.isInitialised()) { + // Update particle weights with GNSS measurement + if (particleFilter.isInitialised()) { LatLng gnssLatLng = new LatLng(location.getLatitude(), location.getLongitude()); float accuracy = location.getAccuracy(); particleFilter.updateWeights(gnssLatLng, accuracy); 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..0784b9ce 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java @@ -142,7 +142,7 @@ public PdrProcessing(Context context) { public float[] updatePdr(long currentStepEnd, List accelMagnitudeOvertime, float headingRad) { if (accelMagnitudeOvertime == null || accelMagnitudeOvertime.size() < MIN_REQUIRED_SAMPLES) { return new float[]{this.positionX, this.positionY}; // Return current position without update - // - TODO - temporary solution of the empty list issue + // - TODO - temporary solution of the empty list issue } // Change angle so zero rad is east @@ -153,7 +153,7 @@ public float[] updatePdr(long currentStepEnd, List accelMagnitudeOvertim // return current position, do not update return new float[]{this.positionX, this.positionY}; } - + // Calculate step length if(!useManualStep) { //ArrayList accelMagnitudeFiltered = filter(accelMagnitudeOvertime); @@ -308,12 +308,12 @@ public boolean estimateElevator(float[] gravity, float[] acc) { // get horizontal and vertical acceleration magnitude float verticalAcc = (float) Math.sqrt( Math.pow((acc[0] * gravity[0]/g),2) + - Math.pow((acc[1] * gravity[1]/g), 2) + - Math.pow((acc[2] * gravity[2]/g), 2)); + Math.pow((acc[1] * gravity[1]/g), 2) + + Math.pow((acc[2] * gravity[2]/g), 2)); float horizontalAcc = (float) Math.sqrt( Math.pow((acc[0] * (1 - gravity[0]/g)), 2) + - Math.pow((acc[1] * (1 - gravity[1]/g)), 2) + - Math.pow((acc[2] * (1 - gravity[2]/g)), 2)); + Math.pow((acc[1] * (1 - gravity[1]/g)), 2) + + Math.pow((acc[2] * (1 - gravity[2]/g)), 2)); // Save into buffer to compare with past values this.verticalAccel.putNewest(verticalAcc); this.horizontalAccel.putNewest(horizontalAcc); From 9c383c138c94cf734e1e78faa23ddb967b404823 Mon Sep 17 00:00:00 2001 From: Juraj02 Date: Tue, 31 Mar 2026 16:24:41 +0100 Subject: [PATCH 07/14] Add trajectory map data display and UI improvements --- .../fragment/RecordingFragment.java | 93 ++++++- .../fragment/TrajectoryMapFragment.java | 233 +++++++++++++++--- .../PositionMe/sensors/SensorFusion.java | 14 ++ app/src/main/res/drawable/legend_dot.xml | 5 + .../res/layout/fragment_trajectory_map.xml | 105 ++++++++ app/src/main/res/values/strings.xml | 5 + gradlew | 0 7 files changed, 415 insertions(+), 40 deletions(-) create mode 100644 app/src/main/res/drawable/legend_dot.xml mode change 100644 => 100755 gradlew 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..243f7004 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 @@ -84,6 +84,14 @@ public class RecordingFragment extends Fragment { private float previousPosX = 0f; private float previousPosY = 0f; + // Fused trajectory update loop (1-second interval) + private Handler fusedTrajectoryHandler; + private LatLng lastFusedPos = null; + private LatLng lastGnssObsPos = null; + private LatLng lastWifiObsPos = null; + private float previousObsPosX = 0f; + private float previousObsPosY = 0f; + // References to the child map fragment private TrajectoryMapFragment trajectoryMapFragment; @@ -96,6 +104,15 @@ public void run() { } }; + /** Updates the fused best-estimate marker and trajectory polyline every 1 second. */ + private final Runnable fusedTrajectoryTask = new Runnable() { + @Override + public void run() { + updateFusedDisplay(); + fusedTrajectoryHandler.postDelayed(this, 1000); + } + }; + public RecordingFragment() { // Required empty public constructor } @@ -107,6 +124,7 @@ public void onCreate(Bundle savedInstanceState) { Context context = requireActivity(); this.settings = PreferenceManager.getDefaultSharedPreferences(context); this.refreshDataHandler = new Handler(); + this.fusedTrajectoryHandler = new Handler(); } @Nullable @@ -193,6 +211,9 @@ public void onViewCreated(@NonNull View view, // The blinking effect for recIcon blinkingRecordingIcon(); + // Start the 1-second fused position + trajectory update loop + fusedTrajectoryHandler.postDelayed(fusedTrajectoryTask, 1000); + // Start the timed or indefinite UI refresh if (this.settings.getBoolean("split_trajectory", false)) { // A maximum recording time is set @@ -247,6 +268,30 @@ private void onAddTestPoint() { trajectoryMapFragment.addTestPointMarker(idx, ts, cur); } + /** + * Updates the purple fused trajectory polyline every 1 second. + * Uses the WiFi fix as the best estimate when available; falls back to the current + * PDR-derived position. Raw GNSS is intentionally excluded here to avoid polyline jumps. + * The position marker (arrow) is updated separately in the 200ms loop via + * {@link #updateUIandPosition}. + */ + private void updateFusedDisplay() { + if (trajectoryMapFragment == null) return; + + // Prefer WiFi fix; fall back to PDR-derived current location (no raw GNSS) + LatLng bestEstimate = sensorFusion.getLatLngWifiPositioning(); + if (bestEstimate == null) { + bestEstimate = trajectoryMapFragment.getCurrentLocation(); + } + if (bestEstimate == null) return; + + // Append to fused trajectory only if the position has moved > 0.3 m + if (lastFusedPos == null + || UtilFunctions.distanceBetweenPoints(lastFusedPos, bestEstimate) > 0.3) { + trajectoryMapFragment.updateFusedTrajectory(bestEstimate); + lastFusedPos = bestEstimate; + } + } /** * Update the UI with sensor data and pass map updates to TrajectoryMapFragment. @@ -276,10 +321,14 @@ private void updateUIandPosition() { new float[]{ pdrValues[0] - previousPosX, pdrValues[1] - previousPosY } ); - // Pass the location + orientation to the map + // Update the red PDR polyline and move the position marker (with optional smoothing). + // Prefer the particle filter estimate for the marker; fall back to PDR-derived position. if (trajectoryMapFragment != null) { - trajectoryMapFragment.updateUserLocation(newLocation, - (float) Math.toDegrees(sensorFusion.passOrientation())); + float orientation = (float) Math.toDegrees(sensorFusion.passOrientation()); + trajectoryMapFragment.updateUserLocation(newLocation, orientation); + LatLng fusedPos = sensorFusion.getFusedPosition(); + trajectoryMapFragment.updateFusedPosition( + fusedPos != null ? fusedPos : newLocation, orientation); } } @@ -302,6 +351,42 @@ private void updateUIandPosition() { } } + // --- Colour-coded observation markers --- + + // GNSS observation: add a blue marker whenever the raw GNSS fix changes + float[] gnssRaw = sensorFusion.getSensorValueMap().get(SensorTypes.GNSSLATLONG); + if (gnssRaw != null) { + LatLng gnssObs = new LatLng(gnssRaw[0], gnssRaw[1]); + if (!gnssObs.equals(lastGnssObsPos)) { + trajectoryMapFragment.addObservationMarker(gnssObs, + TrajectoryMapFragment.ObservationSource.GNSS); + lastGnssObsPos = gnssObs; + } + } + + // WiFi observation: add an orange marker whenever a new WiFi fix arrives + LatLng wifiObs = sensorFusion.getLatLngWifiPositioning(); + if (wifiObs != null && !wifiObs.equals(lastWifiObsPos)) { + trajectoryMapFragment.addObservationMarker(wifiObs, + TrajectoryMapFragment.ObservationSource.WIFI); + lastWifiObsPos = wifiObs; + } + + // PDR observation: add a red marker whenever PDR position has moved ≥ 1 m + LatLng currentLoc = trajectoryMapFragment.getCurrentLocation(); + if (currentLoc != null) { + double pdrDelta = Math.sqrt( + Math.pow(pdrValues[0] - previousObsPosX, 2) + + Math.pow(pdrValues[1] - previousObsPosY, 2)); + if (pdrDelta >= 1.0) { + trajectoryMapFragment.addObservationMarker(currentLoc, + TrajectoryMapFragment.ObservationSource.PDR); + previousObsPosX = pdrValues[0]; + previousObsPosY = pdrValues[1]; + } + } + + // Update previous previousPosX = pdrValues[0]; previousPosY = pdrValues[1]; @@ -323,6 +408,7 @@ private void blinkingRecordingIcon() { public void onPause() { super.onPause(); refreshDataHandler.removeCallbacks(refreshDataTask); + fusedTrajectoryHandler.removeCallbacks(fusedTrajectoryTask); } @Override @@ -331,6 +417,7 @@ public void onResume() { if(!this.settings.getBoolean("split_trajectory", false)) { refreshDataHandler.postDelayed(refreshDataTask, 500); } + fusedTrajectoryHandler.postDelayed(fusedTrajectoryTask, 1000); } private int testPointIndex = 0; 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..89490637 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 @@ -36,11 +36,11 @@ /** * A fragment responsible for displaying a trajectory map using Google Maps. - *

+ * * The TrajectoryMapFragment provides a map interface for visualizing movement trajectories, * GNSS tracking, and indoor mapping. It manages map settings, user interactions, and real-time * updates to user location and GNSS markers. - *

+ * * Key Features: * - Displays a Google Map with support for different map types (Hybrid, Normal, Satellite). * - Tracks and visualizes user movement using polylines. @@ -57,6 +57,18 @@ public class TrajectoryMapFragment extends Fragment { + /** Sources for colour-coded position observations on the map. */ + public enum ObservationSource { GNSS, WIFI, PDR } + + // Observation marker colours + private static final int COLOR_GNSS_OBS = 0xFF2196F3; // blue (unused directly; hue used below) + private static final int COLOR_WIFI_OBS = 0xFFFF9800; // orange + private static final int COLOR_PDR_OBS = 0xFFF44336; // red + private static final int COLOR_FUSED = 0xFF9C27B0; // purple – fused trajectory + + /** Weight applied to the newest sample in the EMA smoothing filter (0 < α ≤ 1). */ + private static final float EMA_ALPHA = 0.3f; + private GoogleMap gMap; // Google Maps instance private LatLng currentLocation; // Stores the user's current location private Marker orientationMarker; // Marker representing user's heading @@ -71,6 +83,20 @@ public class TrajectoryMapFragment extends Fragment { private Polyline gnssPolyline; // Polyline for GNSS path private LatLng lastGnssLocation = null; // Stores the last GNSS location + // Fused trajectory – updated every 1 s or on movement + private Polyline fusedTrajectoryPolyline; + private LatLng lastFusedTrajectoryPoint = null; + + // One moving marker per source — each updates in place rather than accumulating + private Marker gnssObsMarker = null; + private Marker wifiObsMarker = null; + private Marker pdrObsMarker = null; + + // EMA smoothing state + private boolean smoothingEnabled = false; + private double smoothedLat = Double.NaN; + private double smoothedLng = Double.NaN; + private LatLng pendingCameraPosition = null; // Stores pending camera movement private boolean hasPendingCameraMove = false; // Tracks if camera needs to move @@ -91,6 +117,7 @@ public class TrajectoryMapFragment extends Fragment { private SwitchMaterial gnssSwitch; private SwitchMaterial autoFloorSwitch; + private SwitchMaterial smoothSwitch; private com.google.android.material.floatingactionbutton.FloatingActionButton floorUpButton, floorDownButton; private TextView floorLabel; @@ -120,6 +147,7 @@ public void onViewCreated(@NonNull View view, switchMapSpinner = view.findViewById(R.id.mapSwitchSpinner); gnssSwitch = view.findViewById(R.id.gnssSwitch); autoFloorSwitch = view.findViewById(R.id.autoFloor); + smoothSwitch = view.findViewById(R.id.smoothSwitch); floorUpButton = view.findViewById(R.id.floorUpButton); floorDownButton = view.findViewById(R.id.floorDownButton); floorLabel = view.findViewById(R.id.floorLabel); @@ -183,6 +211,12 @@ public void onMapReady(@NonNull GoogleMap googleMap) { } }); + // Smoothing toggle + if (smoothSwitch != null) { + smoothSwitch.setOnCheckedChangeListener((btn, isChecked) -> + setSmoothingEnabled(isChecked)); + } + // Auto-floor toggle: start/stop periodic floor evaluation sensorFusion = SensorFusion.getInstance(); autoFloorSwitch.setOnCheckedChangeListener((compoundButton, isChecked) -> { @@ -246,6 +280,13 @@ private void initMapSettings(GoogleMap map) { .width(5f) .add() // start empty ); + + // Fused best-estimate trajectory in purple + fusedTrajectoryPolyline = map.addPolyline(new PolylineOptions() + .color(COLOR_FUSED) + .width(8f) + .add() // start empty + ); } @@ -301,60 +342,31 @@ public void onNothingSelected(AdapterView parent) {} } /** - * Update the user's current location on the map, create or move orientation marker, - * and append to polyline if the user actually moved. + * Records the user’s current PDR-derived location and extends the red trajectory polyline. + * Does NOT move the orientation marker — marker updates are handled exclusively by + * {@link #updateFusedPosition} to avoid competing updates from different loops. * - * @param newLocation The new location to plot. - * @param orientation The user’s heading (e.g. from sensor fusion). + * @param newLocation The new PDR-derived location. + * @param orientation Unused here; kept for API compatibility. */ public void updateUserLocation(@NonNull LatLng newLocation, float orientation) { if (gMap == null) return; - // Keep track of current location LatLng oldLocation = this.currentLocation; this.currentLocation = newLocation; - // If no marker, create it - if (orientationMarker == null) { - orientationMarker = gMap.addMarker(new MarkerOptions() - .position(newLocation) - .flat(true) - .title("Current Position") - .icon(BitmapDescriptorFactory.fromBitmap( - UtilFunctions.getBitmapFromVector(requireContext(), - R.drawable.ic_baseline_navigation_24))) - ); - gMap.moveCamera(CameraUpdateFactory.newLatLngZoom(newLocation, 19f)); - } else { - // Update marker position + orientation - orientationMarker.setPosition(newLocation); - orientationMarker.setRotation(orientation); - // Move camera a bit - gMap.moveCamera(CameraUpdateFactory.newLatLng(newLocation)); - } - - // Extend polyline if movement occurred - /*if (oldLocation != null && !oldLocation.equals(newLocation) && polyline != null) { - List points = new ArrayList<>(polyline.getPoints()); - points.add(newLocation); - polyline.setPoints(points); - }*/ - // Extend polyline + // Extend the red PDR polyline if (polyline != null) { List points = new ArrayList<>(polyline.getPoints()); - - // First position fix: add the first polyline point if (oldLocation == null) { points.add(newLocation); polyline.setPoints(points); } else if (!oldLocation.equals(newLocation)) { - // Subsequent movement: append a new polyline point points.add(newLocation); polyline.setPoints(points); } } - // Update indoor map overlay if (indoorMapManager != null) { indoorMapManager.setCurrentLocation(newLocation); @@ -443,6 +455,134 @@ public void updateGNSS(@NonNull LatLng gnssLocation) { } + /** + * Updates the user's best-estimate position on the map. + * When smoothing is enabled an Exponential Moving Average filter (α = {@value #EMA_ALPHA}) + * is applied before moving the orientation marker, producing visibly smoother motion. + * + * @param pos Raw best-estimate position from the fusion / fallback pipeline. + * @param orientation Device heading in degrees (clockwise from north). + */ + public void updateFusedPosition(@NonNull LatLng pos, float orientation) { + if (gMap == null) return; + + LatLng displayPos; + if (smoothingEnabled) { + if (Double.isNaN(smoothedLat)) { + // Seed the filter on first call + smoothedLat = pos.latitude; + smoothedLng = pos.longitude; + } else { + smoothedLat = EMA_ALPHA * pos.latitude + (1.0 - EMA_ALPHA) * smoothedLat; + smoothedLng = EMA_ALPHA * pos.longitude + (1.0 - EMA_ALPHA) * smoothedLng; + } + displayPos = new LatLng(smoothedLat, smoothedLng); + } else { + displayPos = pos; + } + + if (orientationMarker == null) { + orientationMarker = gMap.addMarker(new MarkerOptions() + .position(displayPos) + .flat(true) + .title("Current Position") + .icon(BitmapDescriptorFactory.fromBitmap( + UtilFunctions.getBitmapFromVector(requireContext(), + R.drawable.ic_baseline_navigation_24)))); + gMap.moveCamera(CameraUpdateFactory.newLatLngZoom(displayPos, 19f)); + } else { + orientationMarker.setPosition(displayPos); + orientationMarker.setRotation(orientation); + gMap.moveCamera(CameraUpdateFactory.newLatLng(displayPos)); + } + + currentLocation = displayPos; + + if (indoorMapManager != null) { + indoorMapManager.setCurrentLocation(displayPos); + setFloorControlsVisibility(indoorMapManager.getIsIndoorMapSet() ? View.VISIBLE : View.GONE); + } + } + + /** + * Appends a point to the purple fused-trajectory polyline. + * Only adds a point when the position has actually changed to avoid duplicate vertices. + * + * @param pos New fused position to record. + */ + public void updateFusedTrajectory(@NonNull LatLng pos) { + if (gMap == null || fusedTrajectoryPolyline == null) return; + if (pos.equals(lastFusedTrajectoryPoint)) return; + + List points = new ArrayList<>(fusedTrajectoryPolyline.getPoints()); + points.add(pos); + fusedTrajectoryPolyline.setPoints(points); + lastFusedTrajectoryPoint = pos; + } + + /** + * Places or moves a colour-coded observation marker for the given source. + * Each source maintains exactly one marker on the map — it moves to the latest position + * rather than accumulating multiple markers. + * + * @param pos Absolute position of the observation. + * @param source Which positioning source produced this observation. + */ + public void addObservationMarker(@NonNull LatLng pos, @NonNull ObservationSource source) { + if (gMap == null) return; + + Marker existing; + float hue; + String title; + switch (source) { + case GNSS: + existing = gnssObsMarker; + hue = BitmapDescriptorFactory.HUE_AZURE; + title = "GNSS"; + break; + case WIFI: + existing = wifiObsMarker; + hue = BitmapDescriptorFactory.HUE_ORANGE; + title = "WiFi"; + break; + default: // PDR + existing = pdrObsMarker; + hue = BitmapDescriptorFactory.HUE_RED; + title = "PDR"; + break; + } + + if (existing != null) { + existing.setPosition(pos); + } else { + Marker m = gMap.addMarker(new MarkerOptions() + .position(pos) + .title(title) + .anchor(0.5f, 0.5f) + .icon(BitmapDescriptorFactory.defaultMarker(hue))); + switch (source) { + case GNSS: gnssObsMarker = m; break; + case WIFI: wifiObsMarker = m; break; + default: pdrObsMarker = m; break; + } + } + } + + /** + * Enables or disables the EMA position smoothing filter. + * Disabling resets the filter state so the next call to + * {@link #updateFusedPosition} re-seeds it from the raw position. + * + * @param enabled {@code true} to enable smoothing. + */ + public void setSmoothingEnabled(boolean enabled) { + this.smoothingEnabled = enabled; + if (!enabled) { + smoothedLat = Double.NaN; + smoothedLng = Double.NaN; + } + } + /** * Remove GNSS marker if user toggles it off */ @@ -509,6 +649,21 @@ public void clearMapAndReset() { } testPointMarkers.clear(); + // Clear all per-source observation markers + if (gnssObsMarker != null) { gnssObsMarker.remove(); gnssObsMarker = null; } + if (wifiObsMarker != null) { wifiObsMarker.remove(); wifiObsMarker = null; } + if (pdrObsMarker != null) { pdrObsMarker.remove(); pdrObsMarker = null; } + + // Clear fused trajectory + if (fusedTrajectoryPolyline != null) { + fusedTrajectoryPolyline.remove(); + fusedTrajectoryPolyline = null; + } + lastFusedTrajectoryPoint = null; + + // Reset EMA filter + smoothedLat = Double.NaN; + smoothedLng = Double.NaN; // Re-create empty polylines with your chosen colors if (gMap != null) { @@ -520,6 +675,10 @@ public void clearMapAndReset() { .color(Color.BLUE) .width(5f) .add()); + fusedTrajectoryPolyline = gMap.addPolyline(new PolylineOptions() + .color(COLOR_FUSED) + .width(8f) + .add()); } } 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 d9af5ed9..fea3609c 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -621,6 +621,20 @@ public LatLng getLatLngWifiPositioning() { return wifiPositionManager.getLatLngWifiPositioning(); } + /** + * Returns the best estimated position of the user from the particle filter. + * Returns {@code null} if the filter has not yet initialised, in which case + * the caller should fall back to the PDR-derived absolute position. + * + * @return particle filter position estimate, or {@code null} if not yet initialised. + */ + public LatLng getFusedPosition() { + if (particleFilter != null && particleFilter.isInitialised()) { + return particleFilter.getEstimatedPosition(); + } + return null; + } + /** * Returns the current floor the user is on, obtained using WiFi positioning. * diff --git a/app/src/main/res/drawable/legend_dot.xml b/app/src/main/res/drawable/legend_dot.xml new file mode 100644 index 00000000..bb6f7dec --- /dev/null +++ b/app/src/main/res/drawable/legend_dot.xml @@ -0,0 +1,5 @@ + + + + \ No newline at end of file diff --git a/app/src/main/res/layout/fragment_trajectory_map.xml b/app/src/main/res/layout/fragment_trajectory_map.xml index a72425bf..bcfd77e5 100644 --- a/app/src/main/res/layout/fragment_trajectory_map.xml +++ b/app/src/main/res/layout/fragment_trajectory_map.xml @@ -43,6 +43,12 @@ android:layout_height="wrap_content" android:text="@string/auto_floor" /> + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Floor Up button Choose Map ❇️ Auto Floor + Smooth + GNSS fix + WiFi fix + PDR step + Fused path GNSS error: Satellite Normal diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 From 2eef39d7c48e7951b070c2f247fd4b302209339e Mon Sep 17 00:00:00 2001 From: Juraj02 Date: Wed, 1 Apr 2026 15:21:55 +0100 Subject: [PATCH 08/14] Add collapsible controls panel and numbered observation circles --- .../fragment/RecordingFragment.java | 9 +- .../fragment/TrajectoryMapFragment.java | 140 ++++++++++++------ .../PositionMe/sensors/SensorFusion.java | 13 ++ .../res/layout/fragment_trajectory_map.xml | 25 ++++ app/src/main/res/values/strings.xml | 1 + 5 files changed, 143 insertions(+), 45 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 243f7004..cdd20af8 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 @@ -321,12 +321,13 @@ private void updateUIandPosition() { new float[]{ pdrValues[0] - previousPosX, pdrValues[1] - previousPosY } ); - // Update the red PDR polyline and move the position marker (with optional smoothing). - // Prefer the particle filter estimate for the marker; fall back to PDR-derived position. + // Update the red PDR polyline and move the position marker. + // Priority: particle filter → WiFi fix → PDR-derived fallback. if (trajectoryMapFragment != null) { float orientation = (float) Math.toDegrees(sensorFusion.passOrientation()); trajectoryMapFragment.updateUserLocation(newLocation, orientation); LatLng fusedPos = sensorFusion.getFusedPosition(); + if (fusedPos == null) fusedPos = sensorFusion.getLatLngWifiPositioning(); trajectoryMapFragment.updateFusedPosition( fusedPos != null ? fusedPos : newLocation, orientation); } @@ -364,11 +365,13 @@ private void updateUIandPosition() { } } - // WiFi observation: add an orange marker whenever a new WiFi fix arrives + // WiFi observation: add an orange marker and correct the particle filter + // whenever a new WiFi fix arrives LatLng wifiObs = sensorFusion.getLatLngWifiPositioning(); if (wifiObs != null && !wifiObs.equals(lastWifiObsPos)) { trajectoryMapFragment.addObservationMarker(wifiObs, TrajectoryMapFragment.ObservationSource.WIFI); + sensorFusion.correctWithWifiPosition(wifiObs); lastWifiObsPos = wifiObs; } 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 89490637..49e41a31 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 @@ -30,7 +30,14 @@ import com.google.android.gms.maps.SupportMapFragment; import com.google.android.gms.maps.model.*; +import android.graphics.Bitmap; +import android.graphics.Canvas; +import android.graphics.Paint; +import android.graphics.Typeface; + +import java.util.ArrayDeque; import java.util.ArrayList; +import java.util.Deque; import java.util.List; @@ -87,10 +94,11 @@ public enum ObservationSource { GNSS, WIFI, PDR } private Polyline fusedTrajectoryPolyline; private LatLng lastFusedTrajectoryPoint = null; - // One moving marker per source — each updates in place rather than accumulating - private Marker gnssObsMarker = null; - private Marker wifiObsMarker = null; - private Marker pdrObsMarker = null; + // Last 3 observation circle-markers per source; index 0 = newest (label "1") + private static final int OBS_HISTORY = 3; + private final Deque gnssObsMarkers = new ArrayDeque<>(); + private final Deque wifiObsMarkers = new ArrayDeque<>(); + private final Deque pdrObsMarkers = new ArrayDeque<>(); // EMA smoothing state private boolean smoothingEnabled = false; @@ -118,11 +126,14 @@ public enum ObservationSource { GNSS, WIFI, PDR } private SwitchMaterial gnssSwitch; private SwitchMaterial autoFloorSwitch; private SwitchMaterial smoothSwitch; + private SwitchMaterial pdrPathSwitch; private com.google.android.material.floatingactionbutton.FloatingActionButton floorUpButton, floorDownButton; private TextView floorLabel; private Button switchColorButton; private Polygon buildingPolygon; + private android.widget.LinearLayout switchesPanel; + private android.widget.ImageButton toggleControlsButton; public TrajectoryMapFragment() { @@ -148,10 +159,22 @@ public void onViewCreated(@NonNull View view, gnssSwitch = view.findViewById(R.id.gnssSwitch); autoFloorSwitch = view.findViewById(R.id.autoFloor); smoothSwitch = view.findViewById(R.id.smoothSwitch); + pdrPathSwitch = view.findViewById(R.id.pdrPathSwitch); floorUpButton = view.findViewById(R.id.floorUpButton); floorDownButton = view.findViewById(R.id.floorDownButton); floorLabel = view.findViewById(R.id.floorLabel); switchColorButton = view.findViewById(R.id.lineColorButton); + switchesPanel = view.findViewById(R.id.switchesPanel); + toggleControlsButton = view.findViewById(R.id.toggleControlsButton); + toggleControlsButton.setOnClickListener(v -> { + if (switchesPanel.getVisibility() == View.VISIBLE) { + switchesPanel.setVisibility(View.GONE); + toggleControlsButton.setImageResource(android.R.drawable.arrow_down_float); + } else { + switchesPanel.setVisibility(View.VISIBLE); + toggleControlsButton.setImageResource(android.R.drawable.arrow_up_float); + } + }); // Setup floor up/down UI hidden initially until we know there's an indoor map setFloorControlsVisibility(View.GONE); @@ -216,6 +239,13 @@ public void onMapReady(@NonNull GoogleMap googleMap) { smoothSwitch.setOnCheckedChangeListener((btn, isChecked) -> setSmoothingEnabled(isChecked)); } + + // PDR path toggle — hidden by default, shown only when user enables it + if (pdrPathSwitch != null) { + pdrPathSwitch.setOnCheckedChangeListener((btn, isChecked) -> { + if (polyline != null) polyline.setVisible(isChecked); + }); + } // Auto-floor toggle: start/stop periodic floor evaluation sensorFusion = SensorFusion.getInstance(); @@ -267,11 +297,12 @@ private void initMapSettings(GoogleMap map) { // Initialize indoor manager indoorMapManager = new IndoorMapManager(map); - // Initialize an empty polyline + // Initialize an empty polyline — hidden by default, toggled via pdrPathSwitch polyline = map.addPolyline(new PolylineOptions() .color(Color.RED) .width(5f) - .add() // start empty + .visible(false) + .add() ); // GNSS path in blue @@ -520,52 +551,73 @@ public void updateFusedTrajectory(@NonNull LatLng pos) { lastFusedTrajectoryPoint = pos; } - /** - * Places or moves a colour-coded observation marker for the given source. - * Each source maintains exactly one marker on the map — it moves to the latest position - * rather than accumulating multiple markers. - * - * @param pos Absolute position of the observation. - * @param source Which positioning source produced this observation. - */ + // Show a numbered circle marker for the given source, keeping last 3 positions. + // Circle 1 = latest, 3 = oldest. public void addObservationMarker(@NonNull LatLng pos, @NonNull ObservationSource source) { if (gMap == null) return; - Marker existing; - float hue; + Deque deque; + int solidColor; String title; switch (source) { case GNSS: - existing = gnssObsMarker; - hue = BitmapDescriptorFactory.HUE_AZURE; - title = "GNSS"; + deque = gnssObsMarkers; + solidColor = COLOR_GNSS_OBS; + title = "GNSS"; break; case WIFI: - existing = wifiObsMarker; - hue = BitmapDescriptorFactory.HUE_ORANGE; - title = "WiFi"; + deque = wifiObsMarkers; + solidColor = COLOR_WIFI_OBS; + title = "WiFi"; break; default: // PDR - existing = pdrObsMarker; - hue = BitmapDescriptorFactory.HUE_RED; - title = "PDR"; + deque = pdrObsMarkers; + solidColor = COLOR_PDR_OBS; + title = "PDR"; break; } - if (existing != null) { - existing.setPosition(pos); - } else { - Marker m = gMap.addMarker(new MarkerOptions() - .position(pos) - .title(title) - .anchor(0.5f, 0.5f) - .icon(BitmapDescriptorFactory.defaultMarker(hue))); - switch (source) { - case GNSS: gnssObsMarker = m; break; - case WIFI: wifiObsMarker = m; break; - default: pdrObsMarker = m; break; - } + if (deque.size() >= OBS_HISTORY) { + deque.removeLast().remove(); } + + // Bump labels on existing markers + int newLabel = deque.size() + 1; + for (Marker m : deque) { + m.setIcon(BitmapDescriptorFactory.fromBitmap(makeObsCircleBitmap(solidColor, newLabel))); + newLabel--; + } + + Marker newest = gMap.addMarker(new MarkerOptions() + .position(pos) + .title(title) + .anchor(0.5f, 0.5f) + .zIndex(1f) + .icon(BitmapDescriptorFactory.fromBitmap(makeObsCircleBitmap(solidColor, 1)))); + deque.addFirst(newest); + } + + // Creates a semi-transparent filled circle bitmap with a number in the centre + private Bitmap makeObsCircleBitmap(int solidColor, int number) { + int size = 64; + Bitmap bmp = Bitmap.createBitmap(size, size, Bitmap.Config.ARGB_8888); + Canvas canvas = new Canvas(bmp); + + int fillColor = Color.argb(160, Color.red(solidColor), Color.green(solidColor), Color.blue(solidColor)); + Paint fillPaint = new Paint(Paint.ANTI_ALIAS_FLAG); + fillPaint.setStyle(Paint.Style.FILL); + fillPaint.setColor(fillColor); + canvas.drawCircle(size / 2f, size / 2f, size / 2f - 2, fillPaint); + + Paint textPaint = new Paint(Paint.ANTI_ALIAS_FLAG); + textPaint.setColor(Color.WHITE); + textPaint.setTypeface(Typeface.DEFAULT_BOLD); + textPaint.setTextSize(size * 0.45f); + textPaint.setTextAlign(Paint.Align.CENTER); + float textY = size / 2f - (textPaint.descent() + textPaint.ascent()) / 2f; + canvas.drawText(String.valueOf(number), size / 2f, textY, textPaint); + + return bmp; } /** @@ -649,10 +701,13 @@ public void clearMapAndReset() { } testPointMarkers.clear(); - // Clear all per-source observation markers - if (gnssObsMarker != null) { gnssObsMarker.remove(); gnssObsMarker = null; } - if (wifiObsMarker != null) { wifiObsMarker.remove(); wifiObsMarker = null; } - if (pdrObsMarker != null) { pdrObsMarker.remove(); pdrObsMarker = null; } + // Clear all per-source observation circle-markers + for (Marker m : gnssObsMarkers) m.remove(); + gnssObsMarkers.clear(); + for (Marker m : wifiObsMarkers) m.remove(); + wifiObsMarkers.clear(); + for (Marker m : pdrObsMarkers) m.remove(); + pdrObsMarkers.clear(); // Clear fused trajectory if (fusedTrajectoryPolyline != null) { @@ -670,6 +725,7 @@ public void clearMapAndReset() { polyline = gMap.addPolyline(new PolylineOptions() .color(Color.RED) .width(5f) + .visible(pdrPathSwitch != null && pdrPathSwitch.isChecked()) .add()); gnssPolyline = gMap.addPolyline(new PolylineOptions() .color(Color.BLUE) 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 fea3609c..d1f26f4b 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -635,6 +635,19 @@ public LatLng getFusedPosition() { return null; } + /** + * Feeds a WiFi position fix into the particle filter as a correction measurement. + * WiFi is more reliable indoors than GNSS, so this prevents PDR drift accumulating + * in environments where GNSS accuracy is too poor to pass the filter's threshold. + * + * @param wifiLatLng WiFi-derived position from the OpenPositioning API. + */ + public void correctWithWifiPosition(LatLng wifiLatLng) { + if (particleFilter != null && particleFilter.isInitialised() && wifiLatLng != null) { + particleFilter.updateWeights(wifiLatLng, 8.0f); + } + } + /** * Returns the current floor the user is on, obtained using WiFi positioning. * diff --git a/app/src/main/res/layout/fragment_trajectory_map.xml b/app/src/main/res/layout/fragment_trajectory_map.xml index bcfd77e5..78625d59 100644 --- a/app/src/main/res/layout/fragment_trajectory_map.xml +++ b/app/src/main/res/layout/fragment_trajectory_map.xml @@ -31,6 +31,22 @@ android:padding="8dp" android:gravity="center"> + + + + + + + + diff --git a/app/src/main/res/values/strings.xml b/app/src/main/res/values/strings.xml index 6f9f1484..ca814cee 100644 --- a/app/src/main/res/values/strings.xml +++ b/app/src/main/res/values/strings.xml @@ -119,6 +119,7 @@ Choose Map ❇️ Auto Floor Smooth + PDR Path GNSS fix WiFi fix PDR step From 56f738ccb7251de7905e2d90d23b43eec61bf73a Mon Sep 17 00:00:00 2001 From: Juraj02 Date: Wed, 1 Apr 2026 17:50:42 +0100 Subject: [PATCH 09/14] Clean up Javadoc and HTML tags in comments --- .../fragment/RecordingFragment.java | 6 +- .../presentation/fragment/ReplayFragment.java | 8 +-- .../fragment/TrajectoryMapFragment.java | 56 ++++--------------- 3 files changed, 15 insertions(+), 55 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 cdd20af8..427d69c6 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 @@ -278,11 +278,7 @@ private void onAddTestPoint() { private void updateFusedDisplay() { if (trajectoryMapFragment == null) return; - // Prefer WiFi fix; fall back to PDR-derived current location (no raw GNSS) - LatLng bestEstimate = sensorFusion.getLatLngWifiPositioning(); - if (bestEstimate == null) { - bestEstimate = trajectoryMapFragment.getCurrentLocation(); - } + LatLng bestEstimate = sensorFusion.getFusedPosition(); if (bestEstimate == null) return; // Append to fused trajectory only if the position has moved > 0.3 m diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/ReplayFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/ReplayFragment.java index d15a4a83..253c84b3 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/ReplayFragment.java +++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/ReplayFragment.java @@ -25,17 +25,15 @@ /** * Sub fragment of Replay Activity. Fragment that replays trajectory data on a map. - *

- * The ReplayFragment is responsible for visualizing and replaying trajectory data captured during - * previous recordings. It loads trajectory data from a JSON file, updates the map with user movement, + * Loads trajectory data from a JSON file, updates the map with user movement, * and provides UI controls for playback, pause, and seek functionalities. - *

+ * * Features: * - Loads trajectory data from a file and displays it on a map. * - Provides playback controls including play, pause, restart, and go to end. * - Updates the trajectory dynamically as playback progresses. * - Allows users to manually seek through the recorded trajectory. - * - Integrates with {@link TrajectoryMapFragment} for map visualization. + * - Integrates with TrajectoryMapFragment for map visualization. * * @see TrajectoryMapFragment The map fragment displaying the trajectory. * @see ReplayActivity The activity managing the replay workflow. 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 49e41a31..79f9d042 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 @@ -277,11 +277,8 @@ public void onMapReady(@NonNull GoogleMap googleMap) { /** * Initialize the map settings with the provided GoogleMap instance. - *

- * The method sets basic map settings, initializes the indoor map manager, - * and creates an empty polyline for user movement tracking. - * The method also initializes the GNSS polyline for tracking GNSS path. - * The method sets the map type to Hybrid and initializes the map with these settings. + * Sets basic map settings, initializes the indoor map manager, + * and creates an empty polyline for user movement tracking. * * @param map */ @@ -322,19 +319,7 @@ private void initMapSettings(GoogleMap map) { /** - * Initialize the map type spinner with the available map types. - *

- * The spinner allows the user to switch between different map types - * (e.g. Hybrid, Normal, Satellite) to customize their map view. - * The spinner is populated with the available map types and listens - * for user selection to update the map accordingly. - * The map type is updated directly on the GoogleMap instance. - *

- * Note: The spinner is initialized with the default map type (Hybrid). - * The map type is updated on user selection. - *

- *

- * @see com.google.android.gms.maps.GoogleMap The GoogleMap instance to update map type. + * Initialize the map type spinner with the available map types (Hybrid, Normal, Satellite). */ private void initMapTypeSpinner() { if (switchMapSpinner == null) return; @@ -375,7 +360,7 @@ public void onNothingSelected(AdapterView parent) {} /** * Records the user’s current PDR-derived location and extends the red trajectory polyline. * Does NOT move the orientation marker — marker updates are handled exclusively by - * {@link #updateFusedPosition} to avoid competing updates from different loops. + * updateFusedPosition() to avoid competing updates from different loops. * * @param newLocation The new PDR-derived location. * @param orientation Unused here; kept for API compatibility. @@ -409,12 +394,8 @@ public void updateUserLocation(@NonNull LatLng newLocation, float orientation) { /** * Set the initial camera position for the map. - *

- * The method sets the initial camera position for the map when it is first loaded. - * If the map is already ready, the camera is moved immediately. - * If the map is not ready, the camera position is stored until the map is ready. - * The method also tracks if there is a pending camera move. - *

+ * If the map is not ready yet, the position is stored and applied once it is. + * * @param startLocation The initial camera position to set. */ public void setInitialCameraPosition(@NonNull LatLng startLocation) { @@ -488,8 +469,7 @@ public void updateGNSS(@NonNull LatLng gnssLocation) { /** * Updates the user's best-estimate position on the map. - * When smoothing is enabled an Exponential Moving Average filter (α = {@value #EMA_ALPHA}) - * is applied before moving the orientation marker, producing visibly smoother motion. + * When smoothing is enabled an EMA filter is applied before moving the orientation marker. * * @param pos Raw best-estimate position from the fusion / fallback pipeline. * @param orientation Device heading in degrees (clockwise from north). @@ -622,10 +602,9 @@ private Bitmap makeObsCircleBitmap(int solidColor, int number) { /** * Enables or disables the EMA position smoothing filter. - * Disabling resets the filter state so the next call to - * {@link #updateFusedPosition} re-seeds it from the raw position. + * Disabling resets the filter so the next call re-seeds from the raw position. * - * @param enabled {@code true} to enable smoothing. + * @param enabled true to enable smoothing. */ public void setSmoothingEnabled(boolean enabled) { this.smoothingEnabled = enabled; @@ -739,21 +718,8 @@ public void clearMapAndReset() { } /** - * Draw the building polygon on the map - *

- * The method draws a polygon representing the building on the map. - * The polygon is drawn with specific vertices and colors to represent - * different buildings or areas on the map. - * The method removes the old polygon if it exists and adds the new polygon - * to the map with the specified options. - * The method logs the number of vertices in the polygon for debugging. - *

- * - * Note: The method uses hard-coded vertices for the building polygon. - * - *

- * - * See: {@link com.google.android.gms.maps.model.PolygonOptions} The options for the new polygon. + * Draw the building polygon on the map using hard-coded vertices. + * Removes any existing polygon before adding the new one. */ private void drawBuildingPolygon() { if (gMap == null) { From bc3b22e6d707ba1a2f49c38c7209e6325263718a Mon Sep 17 00:00:00 2001 From: Greg Date: Wed, 1 Apr 2026 18:03:11 +0100 Subject: [PATCH 10/14] mapmatching_floorchangingandupdates --- .../fragment/TrajectoryMapFragment.java | 30 +- .../PositionMe/sensors/MapGeometry.java | 123 +++++- .../PositionMe/sensors/MapMatcher.java | 357 +++++++++++++----- .../PositionMe/sensors/ParticleFilter.java | 84 ++--- .../PositionMe/sensors/SensorFusion.java | 215 ++++++++--- 5 files changed, 602 insertions(+), 207 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 479ea51b..24380b83 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 @@ -584,7 +584,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() @@ -653,8 +653,10 @@ private void applyImmediateFloor() { if (!indoorMapManager.getIsIndoorMapSet()) return; int candidateFloor; - if (sensorFusion.getLatLngWifiPositioning() != null) { - candidateFloor = sensorFusion.getWifiFloor(); + // Priority: MapMatcher fused floor + // Falls back to raw WiFi, then barometric estimate if neither is available. Will work when in lift as it detects it. + if (sensorFusion.getLatLngWifiPositioning() != null || sensorFusion.getMapMatcherFloor() >= 0) { + candidateFloor = sensorFusion.getMapMatcherFloor(); } else { float elevation = sensorFusion.getElevation(); float floorHeight = indoorMapManager.getFloorHeight(); @@ -684,7 +686,7 @@ 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. + * to prevent jittery floor switching so it becomes smoother. */ private void evaluateAutoFloor() { if (sensorFusion == null || indoorMapManager == null) return; @@ -692,9 +694,10 @@ private void evaluateAutoFloor() { int candidateFloor; - // Priority 1: WiFi-based floor (only if WiFi positioning has returned data) - if (sensorFusion.getLatLngWifiPositioning() != null) { - candidateFloor = sensorFusion.getWifiFloor(); + // Priority 1: MapMatcher fused floor (barometer override > WiFi > raw barometric). + // getMapMatcherFloor() returns WiFi floor as fallback when MapMatcher not yet loaded. + if (sensorFusion.getLatLngWifiPositioning() != null || sensorFusion.getMapMatcherFloor() >= 0) { + candidateFloor = sensorFusion.getMapMatcherFloor(); } else { // Fallback: barometric elevation estimate float elevation = sensorFusion.getElevation(); @@ -712,12 +715,17 @@ 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 + // Only apply a floor *change* when near stairs or a lift. + // Same-floor confirmations are always allowed (no movement required). + int currentFloor = indoorMapManager.getCurrentFloor(); + if (candidateFloor == currentFloor || sensorFusion.isNearTransition()) { + indoorMapManager.setCurrentFloor(candidateFloor, true); + updateFloorLabel(); + } + // Reset timer regardless, so we re-evaluate next second lastCandidateTime = now; } } //endregion -} +} \ No newline at end of file diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/MapGeometry.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/MapGeometry.java index 246f29e4..a37dd243 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/MapGeometry.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/MapGeometry.java @@ -5,17 +5,14 @@ import java.util.ArrayList; import java.util.List; -// NOTE: doesSegmentCross* methods are intentionally omitted — wall-crossing -// correction belongs to the separate "movement model / wall constraint" requirement. - /** - * Pure static geometry helpers for map-matching. + * Geometry utility class for the map matcher. All methods are static - no instances are created. * - * No instance state. No Android dependencies beyond Log. - * All coordinates are in the local easting/northing meter frame used by - * ParticleFilter. + * Provides a point-in-polygon test used to detect which building the user is in, and a + * segment-crossing test used by ParticleFilter to check whether a particle's movement + * crosses a wall. Also runs a self-test on load to verify both work correctly. * - * Logcat tag: MapGeometry (self-test results only) + * All coordinates are local ENU metres matching the frame used by ParticleFilter and MapMatcher. */ class MapGeometry { @@ -24,17 +21,19 @@ class MapGeometry { // No instances needed private MapGeometry() {} - // ------------------------------------------------------------------------- - // Point-in-polygon - // ------------------------------------------------------------------------- + /** - * Ray-casting point-in-polygon test. + * Tests whether point (x, y) is inside the given polygon using ray-casting. * - * @param x easting (meters, local frame) - * @param y northing (meters, local frame) - * @param polygon list of float[]{eastingM, northingM} vertices - * @return true if (x,y) is strictly inside the polygon + * Casts a ray horizontally to the left from (x, y) and counts how many polygon + * edges it crosses. An odd count means inside, even means outside. + * Returns false for null or degenerate polygons (fewer than 3 vertices). + * + * @param x easting in local ENU metres + * @param y northing in local ENU metres + * @param polygon list of float[]{eastingM, northingM} vertices in order + * @return true if the point is inside the polygon */ static boolean isPointInsidePolygon(float x, float y, List polygon) { if (polygon == null || polygon.size() < 3) return false; @@ -56,16 +55,86 @@ static boolean isPointInsidePolygon(float x, float y, List polygon) { return inside; } + + + /** + * Returns true if the segment from (x1,y1) to (x2,y2) crosses any edge of the polygon. + * + * Bullet 3 (Map Matcher): this is the core wall-crossing test. ParticleFilter.predict() calls this + * for every particle after it is displaced. If the move segment crosses a wall edge, + * the particle is snapped back, correcting any estimate that would pass through a wall. + * + * Called by ParticleFilter.predict() after each particle is displaced. If the move + * segment crosses a wall edge, the particle is snapped back to its previous position. + * Iterates all consecutive vertex pairs of the polygon and checks each edge with + * segmentsIntersect(). The last edge joins the final vertex back to the first. + * + * Returns false for null or single-point polygons. + * + * @param x1 start easting in local ENU metres + * @param y1 start northing in local ENU metres + * @param x2 end easting in local ENU metres + * @param y2 end northing in local ENU metres + * @param polygon wall vertices as float[]{eastingM, northingM} + */ + static boolean doesSegmentCrossPolygon( + float x1, float y1, float x2, float y2, + List polygon) { + if (polygon == null || polygon.size() < 2) return false; + int n = polygon.size(); + for (int i = 0; i < n; i++) { + float[] a = polygon.get(i); + float[] b = polygon.get((i + 1) % n); + if (segmentsIntersect(x1, y1, x2, y2, a[0], a[1], b[0], b[1])) { + return true; + } + } + return false; + } + + /** + * Tests whether segment AB and segment CD intersect. + * + * Uses the parametric cross-product method. Each segment is expressed as a + * start point plus a direction vector. The scalar parameters t and u represent + * how far along each segment the potential crossing point lies. If both t and u + * are in [0, 1], the crossing point is within both segment lengths, so they + * intersect. If the cross product of the two direction vectors is near zero, + * the segments are parallel and cannot intersect. + * + * @param ax start of segment A - easting + * @param ay start of segment A - northing + * @param bx end of segment A - easting + * @param by end of segment A - northing + * @param cx start of segment B - easting + * @param cy start of segment B - northing + * @param dx end of segment B - easting + * @param dy end of segment B - northing + */ + private static boolean segmentsIntersect( + float ax, float ay, float bx, float by, + float cx, float cy, float dx, float dy) { + float d1x = bx - ax, d1y = by - ay; // direction of segment A + float d2x = dx - cx, d2y = dy - cy; // direction of segment B + float cross = d1x * d2y - d1y * d2x; + if (Math.abs(cross) < 1e-6f) return false; // parallel or collinear + float t = ((cx - ax) * d2y - (cy - ay) * d2x) / cross; + float u = ((cx - ax) * d1y - (cy - ay) * d1x) / cross; + return t >= 0f && t <= 1f && u >= 0f && u <= 1f; + } + // ------------------------------------------------------------------------- // Self-test (Step 2) // ------------------------------------------------------------------------- /** - * Validates isPointInsidePolygon against known-correct answers. - * Called once after building map data is loaded. Tag: MapGeometry. + * Runs a set of known-answer checks against isPointInsidePolygon and + * doesSegmentCrossPolygon. Called once from MapMatcher.tryLoadBuilding() + * after building data is parsed. Results are written to Logcat under the + * MapGeometry tag so any geometry regression shows up immediately. */ static void selfTest() { - // Unit square: (0,0) → (10,0) → (10,10) → (0,10) + // Test polygon: 10x10 unit square with corners at (0,0), (10,0), (10,10), (0,10) List square = new ArrayList<>(); square.add(new float[]{0f, 0f}); square.add(new float[]{10f, 0f}); @@ -77,8 +146,22 @@ static void selfTest() { check("isPointInsidePolygon(15,5) == false", isPointInsidePolygon(15f, 5f, square), false); + + // Segment crossing the left edge of the square (x=-2 to x=5, y=5): should cross + // Segment entering the square through the left edge - should cross + check("doesSegmentCrossPolygon(-2,5 -> 5,5) == true", + doesSegmentCrossPolygon(-2f, 5f, 5f, 5f, square), true); + + // Segment that starts and ends inside the square - no edge crossing + check("doesSegmentCrossPolygon(2,2 -> 8,8) == false", + doesSegmentCrossPolygon(2f, 2f, 8f, 8f, square), false); + + // Segment completely outside the square - no crossing + check("doesSegmentCrossPolygon(12,0 -> 15,10) == false", + doesSegmentCrossPolygon(12f, 0f, 15f, 10f, square), false); } + // Logs PASS or FAIL for a single self-test case. private static void check(String name, boolean result, boolean expected) { if (result == expected) { Log.d(TAG, "PASS: " + name); @@ -87,4 +170,4 @@ private static void check(String name, boolean result, boolean expected) { + " (got " + result + ", expected " + expected + ")"); } } -} +} \ No newline at end of file diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/MapMatcher.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/MapMatcher.java index e9454381..9b39162b 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/MapMatcher.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/MapMatcher.java @@ -11,25 +11,31 @@ import java.util.List; import java.util.Map; - +/** + * Loads and stores the floorplan geometry for the current building. Used by the rest of the + * system for three purposes: supplying per-floor wall data to ParticleFilter so particles + * cannot pass through walls, storing stair and lift centroids so SensorFusion can check + * whether the user is near a transition point before accepting a floor change, and resolving + * the current floor index by combining the WiFi floor reading with any barometer-confirmed + * override. Handles buildings where the API floor list is stored in non-physical order + * (e.g. Nucleus stores GF at index 4 rather than index 0). + * + * Call tryLoadBuilding() once at the start of a recording to initialise. + */ class MapMatcher { private static final String TAG_LOAD = "MapMatcher"; - /** Constant: earth meters per degree (matches ParticleFilter). */ + // Equatorial approximation: metres per degree of lat/lon. Matches the constant in ParticleFilter. private static final double METERS_PER_DEGREE = 111111.0; - // ========================================================================= - // Inner data class - // ========================================================================= - /** - * A single wall polygon/polyline in the local coordinate frame. - */ + + // A single wall polygon or polyline from the floorplan API, converted to local ENU metres. static class WallFeature { - /** Vertices as float[]{eastingM, northingM} in the local frame. */ + // Vertices as {eastingM, northingM} relative to the particle filter origin final List localPoints; - /** True for MultiPolygon/Polygon features; false for LineString features. */ + // True for filled Polygon/MultiPolygon shapes; false for LineString wall segments final boolean isPolygon; WallFeature(List pts, boolean isPolygon) { @@ -38,56 +44,65 @@ static class WallFeature { } } - // ========================================================================= - // State - // ========================================================================= private final SensorFusion sensorFusion; - // Null until tryLoadBuilding() succeeds + // Set by tryLoadBuilding(); null/zero until loading succeeds private String loadedBuildingId = null; private LatLng mapOrigin = null; private int numFloors = 0; - // Maps floor displayName ("GF", "F1", ...) to its floorShapes index. - // Built at load time; used by getLikelyFloorIndex() to resolve WiFi floor integers. + + // displayName ("GF", "F1", "LG" etc.) -> floorShapes list index. + // Built at load time. Used by getAdjacentFloorIndex() for reverse-lookup. private Map displayNameToIndex = null; - // Per-floor wall feature arrays (size = numFloors, indexed 0..numFloors-1) - // Null until tryLoadBuilding() succeeds; getWallsForFloor() guards against this. + // Arrays indexed [0..numFloors-1], one list per floor. + // All three are null until tryLoadBuilding() completes. @SuppressWarnings("unchecked") private List[] wallsByFloor = null; - // ========================================================================= - // Constructor - // ========================================================================= + // Centroids of staircase and lift features on each floor, in local ENU metres. + // Used by SensorFusion.isNearTransition() to gate barometer floor changes. + @SuppressWarnings("unchecked") + private List[] stairCentersByFloor = null; + @SuppressWarnings("unchecked") + private List[] liftCentersByFloor = null; + + // When SensorFusion accepts a barometer floor change, it calls setCurrentFloor() to + // store the confirmed API index here. -1 means no override, fall back to WiFi. + private int currentFloorOverride = -1; + + // Physical floor number (LG=-1, GF=0, F1=1 ...) -> API floorShapes list index. + // Built at load time. Needed because some buildings store floors out of physical order + // (e.g. Nucleus: [LG, F1, F2, F3, GF] so GF is at index 4, not 0). + private Map physicalToApiIndex = null; + + /** - * Creates a MapMatcher that reads data from the given SensorFusion instance. - * No loading is performed until tryLoadBuilding() is called. + * Creates a MapMatcher. No geometry is loaded until tryLoadBuilding() is called. * - * @param sf the SensorFusion singleton + * @param sf the SensorFusion singleton used to access the floorplan cache */ MapMatcher(SensorFusion sf) { this.sensorFusion = sf; } - // ========================================================================= - // Step 1: loading - // ========================================================================= /** - * Loads wall/stair/lift features from the floorplan cache for the current building. + * Loads wall, stair, and lift features from the floorplan cache for the current building. * - * Detection order: - * 1. Direct look-up by preferredBuildingId if non-null. - * 2. Ray-cast against API outline polygons using origin. - * 3. Closest building center as last resort. + * Building selection works in order: preferred name first (if provided), then + * point-in-polygon check against all cached building outlines, then closest center + * as a last resort. Returns early without loading if origin is null or no building + * is found in the cache. * - * Silently returns if origin is null or no building is found. - * Runs the MapGeometry self-test (MapGeometry.selfTest()) on success. + * On success, builds the displayName and physicalFloor index lookup maps, converts + * every floor's wall/stair/lift geometry to local ENU metres, and runs + * MapGeometry.selfTest() to verify the geometry helpers. * - * @param preferredBuildingId building name key (e.g. "nucleus_building"); may be null - * @param origin local coordinate frame origin from ParticleFilter + * @param preferredBuildingId building name from floorplan API (e.g. "nucleus_building"); null to auto-detect + * @param origin local frame origin from ParticleFilter */ @SuppressWarnings("unchecked") void tryLoadBuilding(String preferredBuildingId, LatLng origin) { @@ -142,37 +157,72 @@ void tryLoadBuilding(String preferredBuildingId, LatLng origin) { mapOrigin = origin; loadedBuildingId = building.getName(); - // Build displayName → floorIndex map for WiFi floor resolution + // Build both lookup maps: displayName -> index (for adjacent-floor arithmetic) + // and physicalFloor -> index (for WiFi floor conversion) displayNameToIndex = new HashMap<>(); + physicalToApiIndex = new HashMap<>(); for (int f = 0; f < numFloors; f++) { String name = floorShapes.get(f).getDisplayName(); - if (name != null) displayNameToIndex.put(name, f); + if (name != null) { + displayNameToIndex.put(name, f); + physicalToApiIndex.put(displayNameToPhysical(name), f); + } } - wallsByFloor = new List[numFloors]; + wallsByFloor = new List[numFloors]; + stairCentersByFloor = new List[numFloors]; + liftCentersByFloor = new List[numFloors]; + currentFloorOverride = -1; for (int f = 0; f < numFloors; f++) { - wallsByFloor[f] = new ArrayList<>(); + wallsByFloor[f] = new ArrayList<>(); + stairCentersByFloor[f] = new ArrayList<>(); + liftCentersByFloor[f] = new ArrayList<>(); FloorplanApiClient.FloorShapes floor = floorShapes.get(f); + // Bullet 2 (Map Matcher): the floorplan API gives each feature an indoor_type. We sort every + // feature on this floor into one of the three required categories: + // "wall" -> stored as WallFeature for wall-crossing checks (Bullet 3, Map Matcher) + // "stairs"/"staircase" -> centroid stored for proximity gating (Bullets 4 & 5, Map Matcher) + // "lift"/"elevator" -> centroid stored for proximity gating (Bullets 4 & 5, Map Matcher) + // Walk every feature on this floor and sort it into walls, stairs, or lifts. + // Wall parts are stored as WallFeature objects (polygon or polyline). + // Stair and lift parts have their centroid computed and stored for proximity checks. for (FloorplanApiClient.MapShapeFeature feature : floor.getFeatures()) { - if (!"wall".equals(feature.getIndoorType())) continue; + String type = feature.getIndoorType(); + + if ("wall".equals(type)) { + String geoType = feature.getGeometryType(); + boolean isPoly = "MultiPolygon".equals(geoType) || "Polygon".equals(geoType); + + for (List part : feature.getParts()) { + List localPts = new ArrayList<>(part.size()); + for (LatLng ll : part) { + localPts.add(latLngToLocal(ll, origin)); + } + if (localPts.size() < 2) continue; + wallsByFloor[f].add(new WallFeature(localPts, isPoly)); + } - String geoType = feature.getGeometryType(); - boolean isPoly = "MultiPolygon".equals(geoType) || "Polygon".equals(geoType); + } else if ("stairs".equals(type) || "staircase".equals(type)) { + for (List part : feature.getParts()) { + float[] centroid = computeCentroidLocal(part, origin); + if (centroid != null) stairCentersByFloor[f].add(centroid); + } - for (List part : feature.getParts()) { - List localPts = new ArrayList<>(part.size()); - for (LatLng ll : part) { - localPts.add(latLngToLocal(ll, origin)); + } else if ("lift".equals(type) || "elevator".equals(type)) { + for (List part : feature.getParts()) { + float[] centroid = computeCentroidLocal(part, origin); + if (centroid != null) liftCentersByFloor[f].add(centroid); } - if (localPts.size() < 2) continue; - wallsByFloor[f].add(new WallFeature(localPts, isPoly)); } } - Log.d(TAG_LOAD, String.format("Floor %d (%s): walls=%d", - f, floor.getDisplayName(), wallsByFloor[f].size())); + Log.d(TAG_LOAD, String.format("Floor %d (%s): walls=%d stairs=%d lifts=%d", + f, floor.getDisplayName(), + wallsByFloor[f].size(), + stairCentersByFloor[f].size(), + liftCentersByFloor[f].size())); } Log.d(TAG_LOAD, "Loaded: building=" + loadedBuildingId @@ -183,18 +233,15 @@ void tryLoadBuilding(String preferredBuildingId, LatLng origin) { } /** - * Returns true once building data has been successfully loaded. - * All three conditions must hold: building ID set, origin set, and at least one floor parsed. + * Returns true once tryLoadBuilding() has successfully parsed at least one floor. + * All other public methods guard against being called before this returns true. */ boolean isInitialised() { return loadedBuildingId != null && mapOrigin != null && numFloors > 0; } - // ========================================================================= - // Step 2: floor accessor - // ========================================================================= - /** Returns the wall features for the given floor, or an empty list if out of range. */ + // Returns the wall features for the given API floor index, or an empty list if out of range. List getWallsForFloor(int floorIndex) { if (wallsByFloor == null || floorIndex < 0 || floorIndex >= numFloors) { return new ArrayList<>(); @@ -202,53 +249,129 @@ List getWallsForFloor(int floorIndex) { return wallsByFloor[floorIndex]; } - // ========================================================================= - // Utilities - // ========================================================================= - /** Returns the loaded building ID, or null if not loaded. */ + + // Returns the building name that was loaded, or null if tryLoadBuilding() has not succeeded yet. String getLoadedBuildingId() { return loadedBuildingId; } /** - * Returns the floor index most likely occupied by the user, derived from - * the current WiFi floor reading mapped to a floor display name. + * Returns the best guess for which API floorShapes index the user is currently on. + * Index 0 matches floorShapesList[0], index 1 matches floorShapesList[1], and so on. + * + * Bullet 1 (Map Matcher): combines WiFi and barometer data to give the rest of the system a single + * reliable floor number, accounting for buildings where the API list is in non-physical + * order (e.g. Nucleus stores GF at index 4). * - * WiFi integers are converted to display names per building. The display name is then looked up in the index map built - * at load time, so the result is correct regardless of API floor ordering. + * A barometer-confirmed override (set by SensorFusion when near lift) takes + * priority over the WiFi reading. When no override is set, the WiFi floor integer is + * converted to an API index via physicalToApiIndex. Falls back to the bias formula + * if the lookup map is not ready yet (Nucleus/Murchison: WiFi 0 maps to index 1; + * other buildings: WiFi 0 maps to index 0). * - * Falls back to 0 if WiFi returns an unrecognised value. + * Result is clamped to [0, numFloors-1]. */ int getLikelyFloorIndex() { - if (displayNameToIndex == null) return 0; + // Barometer override beats WiFi when SensorFusion has confirmed a floor change + if (currentFloorOverride >= 0 && currentFloorOverride < numFloors) { + Log.d(TAG_LOAD, "getLikelyFloorIndex: using barometer override=" + currentFloorOverride); + return currentFloorOverride; + } int wifiFloor = sensorFusion.getWifiFloor(); - String name = wifiFloorToDisplayName(wifiFloor); - Integer idx = (name != null) ? displayNameToIndex.get(name) : null; - int result = (idx != null) ? idx : 0; + // physicalToApiIndex handles buildings where the API list is not in physical order + // (e.g. Nucleus: [LG, F1, F2, F3, GF] - WiFi 0 = GF = API index 4, not index 1). + // Falls back to the bias formula when the map has not loaded yet. + Integer apiIdx = (physicalToApiIndex != null) ? physicalToApiIndex.get(wifiFloor) : null; + int result = (apiIdx != null) + ? apiIdx + : Math.max(0, Math.min(numFloors - 1, wifiFloor + getAutoFloorBias())); Log.d(TAG_LOAD, "getLikelyFloorIndex: wifiFloor=" + wifiFloor - + " elevation=" + sensorFusion.getElevation() - + " name=" + name + " index=" + result); + + " bias=" + getAutoFloorBias() + " index=" + result); return result; } /** - * Converts a WiFi floor integer to the display name used in the floorplan API - * for the currently loaded building. + * Returns the index offset between WiFi floor 0 (GF) and its position in the API floor list. + * Nucleus and Murchison store LG at index 0, so GF sits at index 1 (bias = 1). + * All other buildings place GF at index 0, so no offset is needed (bias = 0). + * Kept in sync with IndoorMapManager.getAutoFloorBias(). + */ + int getAutoFloorBias() { + if ("nucleus_building".equals(loadedBuildingId) + || "murchison_house".equals(loadedBuildingId)) { + return 1; + } + return 0; + } + + /** + * Returns the approximate floor-to-floor height in metres for the loaded building. + * SensorFusion divides accumulated elevation change by this value to decide how many + * floors the user has moved. Kept in sync with IndoorMapManager's per-building constants. + */ + float getFloorHeight() { + if ("nucleus_building".equals(loadedBuildingId)) return 4.2f; + if ("murchison_house".equals(loadedBuildingId)) return 4.0f; + if ("library".equals(loadedBuildingId)) return 3.6f; + return 4.0f; // generic fallback + } + + /** + * Records a barometer-confirmed floor index so getLikelyFloorIndex() returns it + * instead of the WiFi estimate. Called by SensorFusion.checkAndApplyFloorChange(). + * Pass -1 to clear the override and let WiFi drive floor selection again. + */ + void setCurrentFloor(int floor) { + currentFloorOverride = floor; + } + + // Returns the staircase centroids in local ENU metres for floor f, or an empty list. + List getStairCentersForFloor(int f) { + if (stairCentersByFloor == null || f < 0 || f >= numFloors) return new ArrayList<>(); + return stairCentersByFloor[f]; + } + + // Returns the lift centroids in local ENU metres for floor f, or an empty list. + List getLiftCentersForFloor(int f) { + if (liftCentersByFloor == null || f < 0 || f >= numFloors) return new ArrayList<>(); + return liftCentersByFloor[f]; + } + + /** + * Converts a physical floor number (LG=-1, GF=0, F1=1, F2=2, ...) to its + * position in the API floorShapes list. Returns -1 if not found. * - * Nucleus / Murchison: 0=GF, 1=F1, 2=F2, 3=F3 (LG not available via WiFi). - * Generic fallback: treats the integer as a display-name string ("0", "1", ...). + * SensorFusion calls this when WiFi changes floor rather than going through + * getLikelyFloorIndex(), because getLikelyFloorIndex() already reflects the + * new WiFi value and combining it with getAdjacentFloorIndex() would shift + * one floor too far. + */ + int physicalFloorToApiIndex(int physicalFloor) { + if (physicalToApiIndex == null) return -1; + Integer idx = physicalToApiIndex.get(physicalFloor); + return (idx != null) ? idx : -1; + } + + /** + * Converts a WiFi floor integer to the display name used in the floorplan API. + * + * For Nucleus and Murchison, the API stores floors in this order: + * index 0=LG, 1=F1, 2=F2, 3=F3, 4=GF + * GF is at index 4, so this cannot simply call String.valueOf(wifiFloor). + * For other buildings, the WiFi integer is used as a plain string ("0", "1", etc.). */ private String wifiFloorToDisplayName(int wifiFloor) { if ("nucleus_building".equals(loadedBuildingId) || "murchison_house".equals(loadedBuildingId)) { switch (wifiFloor) { - case 0: return "GF"; + case 0: return "LG"; case 1: return "F1"; case 2: return "F2"; case 3: return "F3"; + case 4: return "GF"; default: - // Floor not covered by WiFi (e.g. LG) — infer from barometer elevation + // Unknown floor — infer from barometer elevation return (sensorFusion.getElevation() < -1.5f) ? "LG" : "GF"; } } @@ -257,8 +380,73 @@ private String wifiFloorToDisplayName(int wifiFloor) { /** - * Converts a WGS84 LatLng to a local easting/northing offset (meters) - * relative to origin, using the same formula as ParticleFilter. + * Returns the API floorShapes index that is physicalDelta floors above (positive) or + * below (negative) currentApiIndex in physical building height (LG=-1, GF=0, F1=1, ...). + * + * A simple +/-1 on the index would be wrong for buildings with non-physical API ordering + * (e.g. Nucleus: GF is at index 4, so going up one floor from GF must land on index 3 for + * F1, not index 5 which does not exist). Instead, we reverse-lookup the current floor's + * display name, convert it to a physical number, add the delta, then look up the resulting + * physical floor in physicalToApiIndex. + * Issue with being on GF and F1 is showed in auto-floor feature so this fixes that issue. + * + * Returns currentApiIndex unchanged if the target floor does not exist in the list. + */ + int getAdjacentFloorIndex(int currentApiIndex, int physicalDelta) { + if (physicalToApiIndex == null || displayNameToIndex == null) return currentApiIndex; + // Reverse-lookup: find the display name that maps to this API index + for (Map.Entry e : displayNameToIndex.entrySet()) { + if (e.getValue() == currentApiIndex) { + int currentPhysical = displayNameToPhysical(e.getKey()); + int targetPhysical = currentPhysical + physicalDelta; + Integer targetApi = physicalToApiIndex.get(targetPhysical); + return (targetApi != null) ? targetApi : currentApiIndex; + } + } + return currentApiIndex; + } + + /** + * Converts a floor display name to a physical floor number. + * LG = -1, GF = 0, F1 = 1, F2 = 2, F3 = 3 + * Unknown names default to 0. + */ + private static int displayNameToPhysical(String name) { + if (name == null) return 0; + switch (name) { + case "LG": return -1; + case "GF": return 0; + default: + if (name.length() > 1 && name.charAt(0) == 'F') { + try { return Integer.parseInt(name.substring(1)); } + catch (NumberFormatException ignored) {} + } + try { return Integer.parseInt(name); } + catch (NumberFormatException ignored) { return 0; } + } + } + + /** + * Computes the centroid of a set of LatLng points in local ENU metres. + * Used to get a single representative point for stairs/lift polygons. + * Returns null if pts is null or empty. + */ + private static float[] computeCentroidLocal(List pts, LatLng origin) { + if (pts == null || pts.isEmpty()) return null; + float sumE = 0f, sumN = 0f; + for (LatLng ll : pts) { + float[] local = latLngToLocal(ll, origin); + sumE += local[0]; + sumN += local[1]; + } + return new float[]{sumE / pts.size(), sumN / pts.size()}; + } + + /** + * Converts a WGS84 LatLng to a local easting/northing offset in metres + * relative to the particle filter origin. Uses the same equirectangular + * approximation as ParticleFilter.latLngToLocal() to keep coordinate frames + * consistent between the two classes. */ private static float[] latLngToLocal(LatLng point, LatLng origin) { double latDiff = point.latitude - origin.latitude; @@ -270,10 +458,9 @@ private static float[] latLngToLocal(LatLng point, LatLng origin) { } /** - * Returns the cached building whose centre is geographically closest to origin. - * Used as a last-resort fallback when no polygon contains the origin. - - * Distance is computed as squared Euclidean in lat/lon degrees. + * Returns the cached building whose centre is closest to origin. + * Used as a last resort when no building polygon contains the origin point. + * Distance is compared as squared lat/lon degrees (no sqrt needed for ordering). */ private FloorplanApiClient.BuildingInfo closestBuilding(LatLng origin) { FloorplanApiClient.BuildingInfo best = null; @@ -290,4 +477,4 @@ private FloorplanApiClient.BuildingInfo closestBuilding(LatLng origin) { } return best; } -} +} \ No newline at end of file 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 3c98a543..c2e44fa6 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java @@ -46,7 +46,7 @@ public class ParticleFilter { private final Random random; private final SensorFusion sensorFusion; - // Map matcher for wall-constrained prediction (null-safe: if null, predict runs unchanged) + // MapMatcher used to check wall crossings during prediction. Null until set via setMapMatcher(). private MapMatcher mapMatcher; // Defines a single hypothesis about the user's position @@ -131,6 +131,17 @@ private void initialisePosition() { initialised = true; } + /** + * Sets the MapMatcher used for wall-crossing checks in predict(). + * Called once from SensorFusion after both objects are created. + * + * Bullet 1 (Map Matcher): wires the map data into the particle filter so position estimates + * are constrained by the building's floor geometry. + */ + public void setMapMatcher(MapMatcher mm) { + this.mapMatcher = mm; + } + /** * Function to retry initial position estimation if it was deferred at construction */ @@ -140,17 +151,6 @@ public void tryInitialise() { } } - /** - * Wires in a MapMatcher for wall-constrained particle prediction. - * When set, predict() will reject movements that enter or cross walls. - * Safe to call with null to disable map matching. - * - * @param mm the MapMatcher instance, or null - */ - public void setMapMatcher(MapMatcher mm) { - this.mapMatcher = mm; - } - /** * Distributes particles as an isotropic 2D gaussian distribution around * (centerX, centerY) and assigns equal weights to each particle (1/N). @@ -180,6 +180,16 @@ public void predict(float deltaEast, float deltaNorth, float PDRstd) { return; } + // Bullet 3 (Map Matcher): save each particle's position before it is displaced so we have + // a valid rollback point if the move crosses a wall. + // Save positions before the move so we can snap back any particle that crosses a wall + float[] oldX = new float[Num_Particles]; + float[] oldY = new float[Num_Particles]; + for (int i = 0; i < Num_Particles; i++) { + oldX[i] = particles[i].x; + oldY[i] = particles[i].y; + } + // Shift each particle with Gaussian noise considered for (int i = 0; i < Num_Particles; i++) { float noiseX = (float) (random.nextGaussian() * PDRstd); @@ -188,51 +198,29 @@ public void predict(float deltaEast, float deltaNorth, float PDRstd) { particles[i].y += deltaNorth + noiseY; } - // Wall rejection: zero weights of particles that land inside a wall polygon - Log.d("ParticleFilter", "predict() reached wall check — mapMatcher=" - + (mapMatcher == null ? "null" : (mapMatcher.isInitialised() ? "ready" : "not initialised"))); - if (mapMatcher == null) { - Log.w("ParticleFilter", "Wall rejection skipped: mapMatcher is null"); - } else if (!mapMatcher.isInitialised()) { - Log.w("ParticleFilter", "Wall rejection skipped: mapMatcher not initialised" - + " (building=" + mapMatcher.getLoadedBuildingId() + ")"); - } else { + // Bullet 3 (Map Matcher): use the movement (PDR displacement) and the wall geometry from the map + // to reject any particle whose move segment crossed a wall. That particle is snapped + // back to its pre-move position, keeping the estimate inside walkable space. + // If MapMatcher is loaded, check each particle's move segment against walls on the + // current floor. Any particle that crossed a wall is snapped back to its old position. + if (mapMatcher != null && mapMatcher.isInitialised()) { int floorIndex = mapMatcher.getLikelyFloorIndex(); List walls = mapMatcher.getWallsForFloor(floorIndex); - int zeroed = 0; + int snapped = 0; for (int i = 0; i < Num_Particles; i++) { for (MapMatcher.WallFeature wall : walls) { - if (!wall.isPolygon) continue; - if (MapGeometry.isPointInsidePolygon( - particles[i].x, particles[i].y, wall.localPoints)) { - particles[i].weight = 0f; - zeroed++; + if (MapGeometry.doesSegmentCrossPolygon( + oldX[i], oldY[i], particles[i].x, particles[i].y, + wall.localPoints)) { + particles[i].x = oldX[i]; + particles[i].y = oldY[i]; + snapped++; break; } } } Log.d("ParticleFilter", "Wall rejection (floor " + floorIndex + "): " - + zeroed + "/" + Num_Particles + " particles zeroed this step"); - - // Renormalise remaining weights - float weightSum = 0f; - for (int i = 0; i < Num_Particles; i++) { - weightSum += particles[i].weight; - } - if (weightSum == 0f) { - Log.w("ParticleFilter", "All particles rejected — recovering with uniform respread around estimate"); - float spread = 2.0f; - float uniform = 1.0f / Num_Particles; - for (int i = 0; i < Num_Particles; i++) { - particles[i].x = estimatedX + (float) (random.nextGaussian() * spread); - particles[i].y = estimatedY + (float) (random.nextGaussian() * spread); - particles[i].weight = uniform; - } - } else { - for (int i = 0; i < Num_Particles; i++) { - particles[i].weight /= weightSum; - } - } + + snapped + "/" + Num_Particles + " particles snapped this step"); } // Update weighted mean estimate 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 42c16ef3..43edc55d 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -34,19 +34,11 @@ import java.util.stream.Stream; /** - * The SensorFusion class is the main data gathering and processing class of the application. + * Main data gathering and processing class for the application. Singleton - all fragments + * share the same instance via SensorFusion.getInstance(). * - *

It follows the singleton design pattern to ensure that every fragment and process has access - * to the same data and sensor instances. Internally it delegates to specialised modules:

- *
    - *
  • {@link SensorState} – shared sensor data holder
    • - *
  • {@link SensorEventHandler} – sensor event dispatch (switch logic)
    • - *
  • {@link TrajectoryRecorder} – recording lifecycle & protobuf construction
    • - *
  • {@link WifiPositionManager} – WiFi scan processing & positioning
    • - *
- * - *

The public API is unchanged – all external callers continue to use - * {@code SensorFusion.getInstance().method()}.

+ * Delegates to: SensorState (data holder), SensorEventHandler (sensor dispatch), + * TrajectoryRecorder (recording and protobuf), WifiPositionManager (WiFi positioning). */ public class SensorFusion implements SensorEventListener { @@ -97,6 +89,14 @@ public class SensorFusion implements SensorEventListener { // Floorplan API cache (latest result from start-location step) private final Map floorplanBuildingCache = new HashMap<>(); + + // Max distances from a staircase / lift for a barometer floor change to be accepted + private static final float STAIR_PROXIMITY_THRESHOLD_M = 5.0f; + private static final float LIFT_PROXIMITY_THRESHOLD_M = 50.0f; + // Elevation recorded when the current floor was last confirmed; NaN before first barometer reading + private float lastFloorElevation = Float.NaN; + // WiFi floor number at the last confirmed floor level; MIN_VALUE until first WiFi fix + private int lastAcceptedWifiFloor = Integer.MIN_VALUE; //endregion //region Initialisation @@ -118,17 +118,10 @@ public static SensorFusion getInstance() { } /** - * Initialisation function for the SensorFusion instance. - * - *

Initialises all movement sensor instances, creates internal modules, and prepares - * the system for data collection.

+ * Sets up all sensors, internal modules, and data processors. + * Must be called once with an application context before anything else. * - * @param context application context for permissions and device access. - * - * @see MovementSensor handling all SensorManager based data collection devices. - * @see ServerCommunications handling communication with the server. - * @see GNSSDataProcessor for location data processing. - * @see WifiDataProcessor for network data processing. + * @param context application context */ public void setContext(Context context) { this.appContext = context.getApplicationContext(); @@ -166,7 +159,8 @@ public void setContext(Context context) { // Initialise particle filter this.particleFilter = new ParticleFilter(); - // Create map matcher and wire into particle filter + // Create the map matcher and give the particle filter a reference to it. + // MapMatcher handles floor detection and stair/lift lookups to help determine whether near stairs or lift. this.mapMatcher = new MapMatcher(this); particleFilter.setMapMatcher(mapMatcher); @@ -212,16 +206,19 @@ public void update(Object[] objList) { //region SensorEventListener /** - * {@inheritDoc} - * - *

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

+ * Forwards sensor events to SensorEventHandler for processing. + * On each barometer reading while recording, also checks whether enough elevation + * change has accumulated to trigger a floor step. */ @Override public void onSensorChanged(SensorEvent sensorEvent) { eventHandler.handleSensorEvent(sensorEvent); + // Check for floor changes on every pressure update + if (sensorEvent.sensor.getType() == Sensor.TYPE_PRESSURE && recorder.isRecording()) { + checkAndApplyFloorChange(state.elevation); + } } - /** {@inheritDoc} */ @Override public void onAccuracyChanged(Sensor sensor, int i) {} @@ -230,9 +227,7 @@ public void onAccuracyChanged(Sensor sensor, int i) {} //region Start/Stop listening /** - * Registers all device listeners and enables updates with the specified sampling rate. - * - *

Should be called from {@link MainActivity} when resuming the application.

+ * Registers all device listeners. Called from MainActivity on resume. */ public void resumeListening() { accelerometerSensor.sensorManager.registerListener(this, @@ -263,9 +258,8 @@ public void resumeListening() { } /** - * Un-registers all device listeners and pauses data collection. - * - *

Should be called from {@link MainActivity} when pausing the application.

+ * Unregisters all device listeners. Called from MainActivity on pause. + * Does nothing while a recording is in progress. */ public void stopListening() { if (!recorder.isRecording()) { @@ -341,8 +335,11 @@ public void startRecording() { recorder.startRecording(pdrProcessing); eventHandler.resetBootTime(recorder.getBootTime()); particleFilter.tryInitialise(); + // Reset the floor-change baselines for the new recording session + lastFloorElevation = Float.NaN; + lastAcceptedWifiFloor = Integer.MIN_VALUE; - // Try to load building map now that particles are initialised + // Attempt to load the building map now that the particle filter is ready tryTriggerMapMatcher(getSelectedBuildingId()); // Handover WiFi/BLE scan lifecycle from activity callbacks to foreground service. @@ -439,7 +436,7 @@ public void setFloorplanBuildings(List building floorplanBuildingCache.put(building.getName(), building); } - // Attempt to load building map now that the cache is populated + // Cache is now populated - try to load the building map tryTriggerMapMatcher(getSelectedBuildingId()); } @@ -466,14 +463,11 @@ public List getFloorplanBuildings() { } /** - * Attempts to load building map data into the map matcher when all required - * conditions are met: particle filter initialised, origin set, and floorplan - * cache non-empty. - * - *

Called from both {@link #setFloorplanBuildings} and {@link #startRecording} - * because either can fire first; all guards are checked each time.

+ * Loads the building map into MapMatcher once the particle filter is ready, + * the origin is set, and the floorplan cache is populated. Called from both + * startRecording() and setFloorplanBuildings() since either can arrive first. * - * @param preferredBuildingId building name hint; may be null for auto-detection + * @param preferredBuildingId building name hint, or null to auto-detect */ private void tryTriggerMapMatcher(String preferredBuildingId) { if (mapMatcher == null) return; @@ -668,6 +662,141 @@ public void logSensorFrequencies() { eventHandler.logSensorFrequencies(); } + /** + * Returns the current floor as a WiFi-scale integer (0=GF, 1=F1, -1=LG etc.) + * for use by the floor switch in TrajectoryMapFragment. + * + * When MapMatcher is active, getLikelyFloorIndex() returns an API index that + * already has the building bias baked in. We subtract it here so the caller's + * setCurrentFloor(x, autoFloor=true) can add it back without double-counting. + * Falls back to the raw WiFi floor if MapMatcher is not loaded yet. + */ + public int getMapMatcherFloor() { + if (mapMatcher != null && mapMatcher.isInitialised()) { + return mapMatcher.getLikelyFloorIndex() - mapMatcher.getAutoFloorBias(); + } + return getWifiFloor(); + } + + //endregion + + //region Floor change gating + + /** + * Called on every barometer reading while recording. + * + * Rule 1: if WiFi reports a new floor since last check, snap the map to match immediately if near stairs or lift. + * Rule 2: if accumulated elevation change exceeds one floor height (per building), step + * the floor up or down. Only fires when near a staircase or lift. Uses + * state.elevator to classify the transition as lift or stairs. + */ + private void checkAndApplyFloorChange(float elevation) { + if (mapMatcher == null || !mapMatcher.isInitialised()) return; + + // Seed reference on first call + if (Float.isNaN(lastFloorElevation)) { + lastFloorElevation = elevation; + lastAcceptedWifiFloor = getWifiFloor(); + return; + } + + int currentWifiFloor = getWifiFloor(); + + // Rule 1: WiFi floor changed - snap the map to match and reset the elevation baseline + if (lastAcceptedWifiFloor != Integer.MIN_VALUE + && currentWifiFloor != lastAcceptedWifiFloor) { + int wifiApiFloor = mapMatcher.physicalFloorToApiIndex(currentWifiFloor); + if (wifiApiFloor >= 0) { + mapMatcher.setCurrentFloor(wifiApiFloor); + Log.i("SensorFusion", "Floor snapped to WiFi floor " + wifiApiFloor + + " (physical=" + currentWifiFloor + ")"); + } else { + mapMatcher.setCurrentFloor(-1); // unknown floor - clear override and let WiFi drive + } + lastFloorElevation = elevation; + lastAcceptedWifiFloor = currentWifiFloor; + return; + } + + // Bullet 4 (Map Matcher): check accumulated barometer elevation change against the building's + // floor height. Only accept a floor change if the user is physically near a + // staircase or lift centroid (from the map). Floor changes in the middle of an + // open room are ignored. + // Rule 2: Barometer - divide elapsed elevation by the building's floor height + float floorHeight = mapMatcher.getFloorHeight(); + float elevationDelta = elevation - lastFloorElevation; + int floorsToMove = (int) (Math.abs(elevationDelta) / floorHeight); + if (floorsToMove == 0) return; + + // Only proceed if the user is physically near a staircase or lift + if (particleFilter == null || !particleFilter.isInitialised()) return; + int currentApiFloor = mapMatcher.getLikelyFloorIndex(); + float[] pos = particleFilter.getEstimatedLocalPosition(); + boolean nearLift = isNearAny(pos, + mapMatcher.getLiftCentersForFloor(currentApiFloor), LIFT_PROXIMITY_THRESHOLD_M); + boolean nearStairs = isNearAny(pos, + mapMatcher.getStairCentersForFloor(currentApiFloor), STAIR_PROXIMITY_THRESHOLD_M); + if (!nearLift && !nearStairs) { + Log.d("SensorFusion", "Barometer floor change skipped: not near a lift or stairs"); + return; + } + // Bullet 5 (Map Matcher): use state.elevator (the PDR movement model) together with proximity + // to decide whether the user is in a lift or on stairs. state.elevator is true + // when vertical acceleration dominates and step activity is low - the lift pattern. + // state.elevator is true when vertical acceleration dominates and step activity is low. + // Use it alongside proximity to decide lift vs stairs. + boolean movementModelSaysLift = state.elevator; + String transitionType; + if (nearLift && movementModelSaysLift) { + transitionType = "lift"; + } else if (nearStairs && !movementModelSaysLift) { + transitionType = "stairs"; + } else { + // Fallback: proximity wins when movement model and geometry disagree + transitionType = nearLift ? "lift" : "stairs"; + } + + int direction = elevationDelta > 0 ? 1 : -1; + int newApiFloor = currentApiFloor; + for (int i = 0; i < floorsToMove; i++) { + int next = mapMatcher.getAdjacentFloorIndex(newApiFloor, direction); + if (next == newApiFloor) break; // already at the top or bottom - stop + newApiFloor = next; + } + + if (newApiFloor != currentApiFloor) { + mapMatcher.setCurrentFloor(newApiFloor); + // Advance reference by the consumed distance, preserving the remainder + lastFloorElevation += floorsToMove * floorHeight * direction; + Log.i("SensorFusion", "Floor changed to " + newApiFloor + + " via " + transitionType + " (barometer, " + floorsToMove + + " floor(s), delta=" + elevationDelta + "m)"); + } + } + + /** + * Returns true if the estimated position is near a staircase or lift on the current floor. + * Called by TrajectoryMapFragment before applying an auto floor change. + */ + public boolean isNearTransition() { + if (mapMatcher == null || !mapMatcher.isInitialised()) return false; + if (particleFilter == null || !particleFilter.isInitialised()) return false; + int floor = mapMatcher.getLikelyFloorIndex(); + float[] pos = particleFilter.getEstimatedLocalPosition(); + return isNearAny(pos, mapMatcher.getStairCentersForFloor(floor), STAIR_PROXIMITY_THRESHOLD_M) + || isNearAny(pos, mapMatcher.getLiftCentersForFloor(floor), LIFT_PROXIMITY_THRESHOLD_M); + } + + /** Returns true if pos is within threshold meters of any centre in the list. */ + private boolean isNearAny(float[] pos, List centers, float threshold) { + if (centers == null || pos == null) return false; + for (float[] c : centers) { + float dx = pos[0] - c[0], dy = pos[1] - c[1]; + if (Math.sqrt(dx * dx + dy * dy) <= threshold) return true; + } + return false; + } + //endregion //region Location listener @@ -694,4 +823,4 @@ public void onLocationChanged(@NonNull Location location) { } //endregion -} +} \ No newline at end of file From 02eeac50f2ebda922845dcac3bbd3174cb7e4ffe Mon Sep 17 00:00:00 2001 From: Joshua Bray Date: Wed, 1 Apr 2026 20:52:42 +0100 Subject: [PATCH 11/14] Comment clarification --- .../PositionMe/sensors/ParticleFilter.java | 34 ++++++++++++------- 1 file changed, 22 insertions(+), 12 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 c2e44fa6..2e1045eb 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java @@ -8,7 +8,11 @@ import java.util.Random; /** - * The Particle Filter class represents a sensor fusion algorithm + * The Particle Filter class upholds a sensor fusion algorithm. + * + * Fuses PDR motion estimates with WiFi and GNSS position fixes through + * a predict, update and resample cycle to produce a fused position estimate + * of user location. */ public class ParticleFilter { @@ -19,7 +23,7 @@ public class ParticleFilter { // Meters conversion constant for WGS84 to easting, northing space private static final double Meters_Per_Degree = 111111.0; - // Initial standard deviation spread from GNSS/WiFi fix. + // Approximated standard deviation spread from GNSS/WiFi fix. private static final float GNSS_Init_STD = 15.0f; private static final float WiFi_Init_STD = 8.0f; @@ -40,7 +44,7 @@ public class ParticleFilter { private float estimatedX; private float estimatedY; - // True once initial position estimation has been established + // True once initial position estimate has been established private boolean initialised; private final Random random; @@ -54,7 +58,7 @@ private static class Particle{ // Easting offset from local origin float x; - //Northing offset from local origin + // Northing offset from local origin float y; // Normalised importance weight @@ -104,7 +108,7 @@ private void initialisePosition() { // Defer initialisation if neither GNSS or WiFi have a valid reading if (!hasGNSS && !hasWifi) { - return; // Initialisation retried via tryInitialise() on later call + return; } // Initialisation source and particle spread based on priority @@ -152,8 +156,8 @@ public void tryInitialise() { } /** - * Distributes particles as an isotropic 2D gaussian distribution around - * (centerX, centerY) and assigns equal weights to each particle (1/N). + * Distributes particles as an isotropic 2D Gaussian distribution around + * (centerX, centerY) and assigns equal weights to each particle (1/Num_Particles). */ private void spreadParticles(float centerX, float centerY, float stdM) { float initWeight = 1.0f / Num_Particles; @@ -219,6 +223,8 @@ public void predict(float deltaEast, float deltaNorth, float PDRstd) { } } } + + // Debug and terminal verification Log.d("ParticleFilter", "Wall rejection (floor " + floorIndex + "): " + snapped + "/" + Num_Particles + " particles snapped this step"); } @@ -231,13 +237,13 @@ public void predict(float deltaEast, float deltaNorth, float PDRstd) { estimatedY += particles[i].y * particles[i].weight; } - //Debug test + //Debug test and terminal verification Log.d("ParticleFilter", "Delta: (" + deltaEast + ", " + deltaNorth + " ; Estimate: ("+ estimatedX + ", " + estimatedY +")"); } /** - * Particle weight update + * */ public void updateWeights(LatLng measurementLatLng, float accuracy) { // Early return if not initialised @@ -307,7 +313,10 @@ public void updateWeights(LatLng measurementLatLng, float accuracy) { } /** - * Systematic resampling of particles for weight degeneration solution (SIR) + * Systematic resampling of particles for weight degeneration solution. + * + * Eliminates low weight particles and duplicates high weight particles when + * N_eff < Num_Particles / 2. */ private void resample() { // Calculate effective sample size as 1 / sum of particle weights squared @@ -331,7 +340,7 @@ private void resample() { cumulativeSum[i] = cumulativeSum[i-1] + particles[i].weight; } - // Set uniform staring point + // Set uniform starting point float overN = 1.0f / Num_Particles; float u1 = random.nextFloat() * overN; @@ -353,6 +362,7 @@ private void resample() { // Replace particle array with resampled particles System.arraycopy(resampledParticles, 0, particles, 0, Num_Particles); + // Debugging terminal output line Log.d("ParticleFilter", "Resampling complete: all weights reset to " + uniformWeighting); } @@ -389,7 +399,7 @@ public LatLng localToLatLng(float eastingM, float northingM) { } /** - * State accessors defined below. + * State accessors: */ public boolean isInitialised() { From bb3a20d332d0fab4d08b94c6551e19bd732a0485 Mon Sep 17 00:00:00 2001 From: Greg Date: Wed, 1 Apr 2026 21:55:35 +0100 Subject: [PATCH 12/14] Final Changes --- .../fragment/RecordingFragment.java | 16 +-- .../fragment/TrajectoryMapFragment.java | 42 +++---- .../PositionMe/sensors/MapGeometry.java | 42 ++----- .../PositionMe/sensors/MapMatcher.java | 113 +++++++----------- .../PositionMe/sensors/ParticleFilter.java | 11 +- .../PositionMe/sensors/SensorFusion.java | 16 +-- 6 files changed, 91 insertions(+), 149 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 427d69c6..2dfee84a 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 @@ -213,7 +213,7 @@ public void onViewCreated(@NonNull View view, // Start the 1-second fused position + trajectory update loop fusedTrajectoryHandler.postDelayed(fusedTrajectoryTask, 1000); - + // Start the timed or indefinite UI refresh if (this.settings.getBoolean("split_trajectory", false)) { // A maximum recording time is set @@ -249,7 +249,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; } @@ -349,7 +349,7 @@ private void updateUIandPosition() { } // --- Colour-coded observation markers --- - + // GNSS observation: add a blue marker whenever the raw GNSS fix changes float[] gnssRaw = sensorFusion.getSensorValueMap().get(SensorTypes.GNSSLATLONG); if (gnssRaw != null) { @@ -360,7 +360,7 @@ private void updateUIandPosition() { lastGnssObsPos = gnssObs; } } - + // WiFi observation: add an orange marker and correct the particle filter // whenever a new WiFi fix arrives LatLng wifiObs = sensorFusion.getLatLngWifiPositioning(); @@ -370,13 +370,13 @@ private void updateUIandPosition() { sensorFusion.correctWithWifiPosition(wifiObs); lastWifiObsPos = wifiObs; } - + // PDR observation: add a red marker whenever PDR position has moved ≥ 1 m LatLng currentLoc = trajectoryMapFragment.getCurrentLocation(); if (currentLoc != null) { double pdrDelta = Math.sqrt( Math.pow(pdrValues[0] - previousObsPosX, 2) - + Math.pow(pdrValues[1] - previousObsPosY, 2)); + + Math.pow(pdrValues[1] - previousObsPosY, 2)); if (pdrDelta >= 1.0) { trajectoryMapFragment.addObservationMarker(currentLoc, TrajectoryMapFragment.ObservationSource.PDR); @@ -384,7 +384,7 @@ private void updateUIandPosition() { previousObsPosY = pdrValues[1]; } } - + // Update previous previousPosX = pdrValues[0]; @@ -438,4 +438,4 @@ private static class TestPoint { private final List testPoints = new ArrayList<>(); -} +} \ No newline at end of file diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java index 9a472133..f5174feb 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 @@ -43,11 +43,11 @@ /** * A fragment responsible for displaying a trajectory map using Google Maps. - * + * * The TrajectoryMapFragment provides a map interface for visualizing movement trajectories, * GNSS tracking, and indoor mapping. It manages map settings, user interactions, and real-time * updates to user location and GNSS markers. - * + * * Key Features: * - Displays a Google Map with support for different map types (Hybrid, Normal, Satellite). * - Tracks and visualizes user movement using polylines. @@ -66,16 +66,16 @@ public class TrajectoryMapFragment extends Fragment { /** Sources for colour-coded position observations on the map. */ public enum ObservationSource { GNSS, WIFI, PDR } - + // Observation marker colours private static final int COLOR_GNSS_OBS = 0xFF2196F3; // blue (unused directly; hue used below) private static final int COLOR_WIFI_OBS = 0xFFFF9800; // orange private static final int COLOR_PDR_OBS = 0xFFF44336; // red private static final int COLOR_FUSED = 0xFF9C27B0; // purple – fused trajectory - + /** Weight applied to the newest sample in the EMA smoothing filter (0 < α ≤ 1). */ private static final float EMA_ALPHA = 0.3f; - + private GoogleMap gMap; // Google Maps instance private LatLng currentLocation; // Stores the user's current location private Marker orientationMarker; // Marker representing user's heading @@ -93,18 +93,18 @@ public enum ObservationSource { GNSS, WIFI, PDR } // Fused trajectory – updated every 1 s or on movement private Polyline fusedTrajectoryPolyline; private LatLng lastFusedTrajectoryPoint = null; - + // Last 3 observation circle-markers per source; index 0 = newest (label "1") private static final int OBS_HISTORY = 3; private final Deque gnssObsMarkers = new ArrayDeque<>(); private final Deque wifiObsMarkers = new ArrayDeque<>(); private final Deque pdrObsMarkers = new ArrayDeque<>(); - + // EMA smoothing state private boolean smoothingEnabled = false; private double smoothedLat = Double.NaN; private double smoothedLng = Double.NaN; - + private LatLng pendingCameraPosition = null; // Stores pending camera movement private boolean hasPendingCameraMove = false; // Tracks if camera needs to move @@ -246,7 +246,7 @@ public void onMapReady(@NonNull GoogleMap googleMap) { if (polyline != null) polyline.setVisible(isChecked); }); } - + // Auto-floor toggle: start/stop periodic floor evaluation sensorFusion = SensorFusion.getInstance(); autoFloorSwitch.setOnCheckedChangeListener((compoundButton, isChecked) -> { @@ -308,7 +308,7 @@ private void initMapSettings(GoogleMap map) { .width(5f) .add() // start empty ); - + // Fused best-estimate trajectory in purple fusedTrajectoryPolyline = map.addPolyline(new PolylineOptions() .color(COLOR_FUSED) @@ -476,7 +476,7 @@ public void updateGNSS(@NonNull LatLng gnssLocation) { */ public void updateFusedPosition(@NonNull LatLng pos, float orientation) { if (gMap == null) return; - + LatLng displayPos; if (smoothingEnabled) { if (Double.isNaN(smoothedLat)) { @@ -491,7 +491,7 @@ public void updateFusedPosition(@NonNull LatLng pos, float orientation) { } else { displayPos = pos; } - + if (orientationMarker == null) { orientationMarker = gMap.addMarker(new MarkerOptions() .position(displayPos) @@ -506,15 +506,15 @@ public void updateFusedPosition(@NonNull LatLng pos, float orientation) { orientationMarker.setRotation(orientation); gMap.moveCamera(CameraUpdateFactory.newLatLng(displayPos)); } - + currentLocation = displayPos; - + if (indoorMapManager != null) { indoorMapManager.setCurrentLocation(displayPos); setFloorControlsVisibility(indoorMapManager.getIsIndoorMapSet() ? View.VISIBLE : View.GONE); } } - + /** * Appends a point to the purple fused-trajectory polyline. * Only adds a point when the position has actually changed to avoid duplicate vertices. @@ -524,13 +524,13 @@ public void updateFusedPosition(@NonNull LatLng pos, float orientation) { public void updateFusedTrajectory(@NonNull LatLng pos) { if (gMap == null || fusedTrajectoryPolyline == null) return; if (pos.equals(lastFusedTrajectoryPoint)) return; - + List points = new ArrayList<>(fusedTrajectoryPolyline.getPoints()); points.add(pos); fusedTrajectoryPolyline.setPoints(points); lastFusedTrajectoryPoint = pos; } - + // Show a numbered circle marker for the given source, keeping last 3 positions. // Circle 1 = latest, 3 = oldest. public void addObservationMarker(@NonNull LatLng pos, @NonNull ObservationSource source) { @@ -599,7 +599,7 @@ private Bitmap makeObsCircleBitmap(int solidColor, int number) { return bmp; } - + /** * Enables or disables the EMA position smoothing filter. * Disabling resets the filter so the next call re-seeds from the raw position. @@ -613,7 +613,7 @@ public void setSmoothingEnabled(boolean enabled) { smoothedLng = Double.NaN; } } - + /** * Remove GNSS marker if user toggles it off */ @@ -687,14 +687,14 @@ public void clearMapAndReset() { wifiObsMarkers.clear(); for (Marker m : pdrObsMarkers) m.remove(); pdrObsMarkers.clear(); - + // Clear fused trajectory if (fusedTrajectoryPolyline != null) { fusedTrajectoryPolyline.remove(); fusedTrajectoryPolyline = null; } lastFusedTrajectoryPoint = null; - + // Reset EMA filter smoothedLat = Double.NaN; smoothedLng = Double.NaN; diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/MapGeometry.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/MapGeometry.java index a37dd243..6e1d29c2 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/MapGeometry.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/MapGeometry.java @@ -6,13 +6,10 @@ import java.util.List; /** - * Geometry utility class for the map matcher. All methods are static - no instances are created. - * - * Provides a point-in-polygon test used to detect which building the user is in, and a - * segment-crossing test used by ParticleFilter to check whether a particle's movement - * crosses a wall. Also runs a self-test on load to verify both work correctly. - * - * All coordinates are local ENU metres matching the frame used by ParticleFilter and MapMatcher. + * Seperated in a different class for modularity. + * Static geometry helpers used by MapMatcher and ParticleFilter. + * Handles point-in-polygon checks and wall-crossing detection. + * Coordinates are local metres, same frame as ParticleFilter. */ class MapGeometry { @@ -30,8 +27,8 @@ private MapGeometry() {} * edges it crosses. An odd count means inside, even means outside. * Returns false for null or degenerate polygons (fewer than 3 vertices). * - * @param x easting in local ENU metres - * @param y northing in local ENU metres + * @param x easting in local metres + * @param y northing in local metres * @param polygon list of float[]{eastingM, northingM} vertices in order * @return true if the point is inside the polygon */ @@ -60,10 +57,6 @@ static boolean isPointInsidePolygon(float x, float y, List polygon) { /** * Returns true if the segment from (x1,y1) to (x2,y2) crosses any edge of the polygon. * - * Bullet 3 (Map Matcher): this is the core wall-crossing test. ParticleFilter.predict() calls this - * for every particle after it is displaced. If the move segment crosses a wall edge, - * the particle is snapped back, correcting any estimate that would pass through a wall. - * * Called by ParticleFilter.predict() after each particle is displaced. If the move * segment crosses a wall edge, the particle is snapped back to its previous position. * Iterates all consecutive vertex pairs of the polygon and checks each edge with @@ -71,10 +64,10 @@ static boolean isPointInsidePolygon(float x, float y, List polygon) { * * Returns false for null or single-point polygons. * - * @param x1 start easting in local ENU metres - * @param y1 start northing in local ENU metres - * @param x2 end easting in local ENU metres - * @param y2 end northing in local ENU metres + * @param x1 start easting in local metres + * @param y1 start northing in local metres + * @param x2 end easting in local metres + * @param y2 end northing in local metres * @param polygon wall vertices as float[]{eastingM, northingM} */ static boolean doesSegmentCrossPolygon( @@ -93,14 +86,8 @@ static boolean doesSegmentCrossPolygon( } /** - * Tests whether segment AB and segment CD intersect. - * - * Uses the parametric cross-product method. Each segment is expressed as a - * start point plus a direction vector. The scalar parameters t and u represent - * how far along each segment the potential crossing point lies. If both t and u - * are in [0, 1], the crossing point is within both segment lengths, so they - * intersect. If the cross product of the two direction vectors is near zero, - * the segments are parallel and cannot intersect. + * Returns true if segment AB and segment CD intersect. + * Uses parametric cross-product; returns false for parallel/collinear segments. * * @param ax start of segment A - easting * @param ay start of segment A - northing @@ -123,9 +110,7 @@ private static boolean segmentsIntersect( return t >= 0f && t <= 1f && u >= 0f && u <= 1f; } - // ------------------------------------------------------------------------- - // Self-test (Step 2) - // ------------------------------------------------------------------------- + /** * Runs a set of known-answer checks against isPointInsidePolygon and @@ -148,7 +133,6 @@ static void selfTest() { isPointInsidePolygon(15f, 5f, square), false); // Segment crossing the left edge of the square (x=-2 to x=5, y=5): should cross - // Segment entering the square through the left edge - should cross check("doesSegmentCrossPolygon(-2,5 -> 5,5) == true", doesSegmentCrossPolygon(-2f, 5f, 5f, 5f, square), true); diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/MapMatcher.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/MapMatcher.java index 9b39162b..72cab52c 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/MapMatcher.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/MapMatcher.java @@ -12,13 +12,12 @@ import java.util.Map; /** - * Loads and stores the floorplan geometry for the current building. Used by the rest of the - * system for three purposes: supplying per-floor wall data to ParticleFilter so particles - * cannot pass through walls, storing stair and lift centroids so SensorFusion can check - * whether the user is near a transition point before accepting a floor change, and resolving - * the current floor index by combining the WiFi floor reading with any barometer-confirmed - * override. Handles buildings where the API floor list is stored in non-physical order - * (e.g. Nucleus stores GF at index 4 rather than index 0). + * Seperated in a different class for modularity. + * Loads and stores floorplan geometry for the current building. + * Provides wall data to ParticleFilter, stair/lift centroids to SensorFusion, + * and floor index resolution combining WiFi with barometer overrides. + * Handles buildings where the API floor list is not in physical order + * (e.g. Nucleus stores GF at index 4). * * Call tryLoadBuilding() once at the start of a recording to initialise. */ @@ -31,11 +30,11 @@ class MapMatcher { - // A single wall polygon or polyline from the floorplan API, converted to local ENU metres. + // A single wall polygon or polyline from the floorplan API, converted to local metres. static class WallFeature { // Vertices as {eastingM, northingM} relative to the particle filter origin final List localPoints; - // True for filled Polygon/MultiPolygon shapes; false for LineString wall segments + // True for filled Polygon/MultiPolygon shapes, false for LineString wall segments final boolean isPolygon; WallFeature(List pts, boolean isPolygon) { @@ -61,7 +60,7 @@ static class WallFeature { @SuppressWarnings("unchecked") private List[] wallsByFloor = null; - // Centroids of staircase and lift features on each floor, in local ENU metres. + // Centroids of staircase and lift features on each floor, in local metres. // Used by SensorFusion.isNearTransition() to gate barometer floor changes. @SuppressWarnings("unchecked") private List[] stairCentersByFloor = null; @@ -72,17 +71,15 @@ static class WallFeature { // store the confirmed API index here. -1 means no override, fall back to WiFi. private int currentFloorOverride = -1; - // Physical floor number (LG=-1, GF=0, F1=1 ...) -> API floorShapes list index. + // Physical floor number (LG=-1, GF=0, F1=1 etc...) -> API floorShapes list index. // Built at load time. Needed because some buildings store floors out of physical order - // (e.g. Nucleus: [LG, F1, F2, F3, GF] so GF is at index 4, not 0). + // (e.g. Nucleus: (LG, F1, F2, F3, GF) so GF is at index 4, not 0). private Map physicalToApiIndex = null; /** * Creates a MapMatcher. No geometry is loaded until tryLoadBuilding() is called. - * - * @param sf the SensorFusion singleton used to access the floorplan cache */ MapMatcher(SensorFusion sf) { this.sensorFusion = sf; @@ -92,17 +89,13 @@ static class WallFeature { /** * Loads wall, stair, and lift features from the floorplan cache for the current building. * - * Building selection works in order: preferred name first (if provided), then + * Building is selected in order: preferred name first (if provided), then * point-in-polygon check against all cached building outlines, then closest center - * as a last resort. Returns early without loading if origin is null or no building - * is found in the cache. + * as fallback. Returns early if origin is null or no building is found in the cache. * * On success, builds the displayName and physicalFloor index lookup maps, converts - * every floor's wall/stair/lift geometry to local ENU metres, and runs - * MapGeometry.selfTest() to verify the geometry helpers. + * all floor geometry to local metres, and runs MapGeometry.selfTest(). * - * @param preferredBuildingId building name from floorplan API (e.g. "nucleus_building"); null to auto-detect - * @param origin local frame origin from ParticleFilter */ @SuppressWarnings("unchecked") void tryLoadBuilding(String preferredBuildingId, LatLng origin) { @@ -111,7 +104,7 @@ void tryLoadBuilding(String preferredBuildingId, LatLng origin) { return; } - // 1. Preferred building by name + // Preferred building by name FloorplanApiClient.BuildingInfo building = null; if (preferredBuildingId != null && !preferredBuildingId.isEmpty()) { building = sensorFusion.getFloorplanBuilding(preferredBuildingId); @@ -120,7 +113,7 @@ void tryLoadBuilding(String preferredBuildingId, LatLng origin) { } } - // 2. Polygon detection against all cached buildings + // Polygon detection against all cached buildings if (building == null) { for (FloorplanApiClient.BuildingInfo b : sensorFusion.getFloorplanBuildings()) { List outline = b.getOutlinePolygon(); @@ -133,7 +126,7 @@ void tryLoadBuilding(String preferredBuildingId, LatLng origin) { } } - // 3. Closest center fallback + // Closest center fallback if (building == null) { building = closestBuilding(origin); if (building != null) { @@ -157,8 +150,8 @@ void tryLoadBuilding(String preferredBuildingId, LatLng origin) { mapOrigin = origin; loadedBuildingId = building.getName(); - // Build both lookup maps: displayName -> index (for adjacent-floor arithmetic) - // and physicalFloor -> index (for WiFi floor conversion) + // Build both lookup maps: displayName to index (for adjacent-floor arithmetic) + // and physicalFloor to index (for WiFi floor conversion) displayNameToIndex = new HashMap<>(); physicalToApiIndex = new HashMap<>(); for (int f = 0; f < numFloors; f++) { @@ -180,14 +173,8 @@ void tryLoadBuilding(String preferredBuildingId, LatLng origin) { liftCentersByFloor[f] = new ArrayList<>(); FloorplanApiClient.FloorShapes floor = floorShapes.get(f); - // Bullet 2 (Map Matcher): the floorplan API gives each feature an indoor_type. We sort every - // feature on this floor into one of the three required categories: - // "wall" -> stored as WallFeature for wall-crossing checks (Bullet 3, Map Matcher) - // "stairs"/"staircase" -> centroid stored for proximity gating (Bullets 4 & 5, Map Matcher) - // "lift"/"elevator" -> centroid stored for proximity gating (Bullets 4 & 5, Map Matcher) - // Walk every feature on this floor and sort it into walls, stairs, or lifts. - // Wall parts are stored as WallFeature objects (polygon or polyline). - // Stair and lift parts have their centroid computed and stored for proximity checks. + // Sort features into walls (for crossing checks) and + // stairs/lifts (centroid stored for proximity gating). for (FloorplanApiClient.MapShapeFeature feature : floor.getFeatures()) { String type = feature.getIndoorType(); @@ -234,7 +221,6 @@ void tryLoadBuilding(String preferredBuildingId, LatLng origin) { /** * Returns true once tryLoadBuilding() has successfully parsed at least one floor. - * All other public methods guard against being called before this returns true. */ boolean isInitialised() { return loadedBuildingId != null && mapOrigin != null && numFloors > 0; @@ -257,20 +243,12 @@ String getLoadedBuildingId() { } /** - * Returns the best guess for which API floorShapes index the user is currently on. - * Index 0 matches floorShapesList[0], index 1 matches floorShapesList[1], and so on. - * - * Bullet 1 (Map Matcher): combines WiFi and barometer data to give the rest of the system a single - * reliable floor number, accounting for buildings where the API list is in non-physical - * order (e.g. Nucleus stores GF at index 4). - * - * A barometer-confirmed override (set by SensorFusion when near lift) takes - * priority over the WiFi reading. When no override is set, the WiFi floor integer is - * converted to an API index via physicalToApiIndex. Falls back to the bias formula - * if the lookup map is not ready yet (Nucleus/Murchison: WiFi 0 maps to index 1; - * other buildings: WiFi 0 maps to index 0). + * Returns the current API floorShapes index. Index 0 = floorShapesList[0], etc. * - * Result is clamped to [0, numFloors-1]. + * A barometer-confirmed override set by SensorFusion takes priority over the WiFi + * reading. When no override is set, the WiFi floor integer is converted to an API + * index via physicalToApiIndex. Falls back to the bias formula if the lookup map + * is not ready yet (Nucleus/Murchison: WiFi 0 maps to index 1; others: index 0). */ int getLikelyFloorIndex() { // Barometer override beats WiFi when SensorFusion has confirmed a floor change @@ -280,7 +258,7 @@ int getLikelyFloorIndex() { } int wifiFloor = sensorFusion.getWifiFloor(); // physicalToApiIndex handles buildings where the API list is not in physical order - // (e.g. Nucleus: [LG, F1, F2, F3, GF] - WiFi 0 = GF = API index 4, not index 1). + // (e.g. Nucleus: (LG, F1, F2, F3, GF) - WiFi 0 = GF = API index 4, not index 1). // Falls back to the bias formula when the map has not loaded yet. Integer apiIdx = (physicalToApiIndex != null) ? physicalToApiIndex.get(wifiFloor) : null; int result = (apiIdx != null) @@ -293,9 +271,8 @@ int getLikelyFloorIndex() { /** * Returns the index offset between WiFi floor 0 (GF) and its position in the API floor list. - * Nucleus and Murchison store LG at index 0, so GF sits at index 1 (bias = 1). - * All other buildings place GF at index 0, so no offset is needed (bias = 0). - * Kept in sync with IndoorMapManager.getAutoFloorBias(). + * Nucleus and Murchison store LG at index 0, so GF is at index 1 (bias = 1). + * All other buildings place GF at index 0 (bias = 0). */ int getAutoFloorBias() { if ("nucleus_building".equals(loadedBuildingId) @@ -306,12 +283,12 @@ int getAutoFloorBias() { } /** - * Returns the approximate floor-to-floor height in metres for the loaded building. - * SensorFusion divides accumulated elevation change by this value to decide how many - * floors the user has moved. Kept in sync with IndoorMapManager's per-building constants. + * Returns the floor-to-floor height in metres for the loaded building. + * SensorFusion divides the accumulated barometer elevation change by this value + * to determine how many floors the user has moved. */ float getFloorHeight() { - if ("nucleus_building".equals(loadedBuildingId)) return 4.2f; + if ("nucleus_building".equals(loadedBuildingId)) return 5.0f; if ("murchison_house".equals(loadedBuildingId)) return 4.0f; if ("library".equals(loadedBuildingId)) return 3.6f; return 4.0f; // generic fallback @@ -326,20 +303,20 @@ void setCurrentFloor(int floor) { currentFloorOverride = floor; } - // Returns the staircase centroids in local ENU metres for floor f, or an empty list. + // Returns the staircase centroids in local metres for floor f, or an empty list. List getStairCentersForFloor(int f) { if (stairCentersByFloor == null || f < 0 || f >= numFloors) return new ArrayList<>(); return stairCentersByFloor[f]; } - // Returns the lift centroids in local ENU metres for floor f, or an empty list. + // Returns the lift centroids in local metres for floor f, or an empty list. List getLiftCentersForFloor(int f) { if (liftCentersByFloor == null || f < 0 || f >= numFloors) return new ArrayList<>(); return liftCentersByFloor[f]; } /** - * Converts a physical floor number (LG=-1, GF=0, F1=1, F2=2, ...) to its + * Converts a physical floor number (LG=-1, GF=0, F1=1, F2=2, etc ...) to its * position in the API floorShapes list. Returns -1 if not found. * * SensorFusion calls this when WiFi changes floor rather than going through @@ -380,17 +357,11 @@ private String wifiFloorToDisplayName(int wifiFloor) { /** - * Returns the API floorShapes index that is physicalDelta floors above (positive) or - * below (negative) currentApiIndex in physical building height (LG=-1, GF=0, F1=1, ...). - * - * A simple +/-1 on the index would be wrong for buildings with non-physical API ordering - * (e.g. Nucleus: GF is at index 4, so going up one floor from GF must land on index 3 for - * F1, not index 5 which does not exist). Instead, we reverse-lookup the current floor's - * display name, convert it to a physical number, add the delta, then look up the resulting - * physical floor in physicalToApiIndex. - * Issue with being on GF and F1 is showed in auto-floor feature so this fixes that issue. - * - * Returns currentApiIndex unchanged if the target floor does not exist in the list. + * Returns the API index that is physicalDelta floors above/below currentApiIndex. + * Can't just do +/-1 on the index for buildings with non-physical API ordering + * (e.g. Nucleus GF is at index 4, not 0), so we convert via display name instead. + * Fixes the GF/F1 off-by-one seen in the auto-floor feature. + * Returns currentApiIndex unchanged if the target floor doesn't exist. */ int getAdjacentFloorIndex(int currentApiIndex, int physicalDelta) { if (physicalToApiIndex == null || displayNameToIndex == null) return currentApiIndex; @@ -427,7 +398,7 @@ private static int displayNameToPhysical(String name) { } /** - * Computes the centroid of a set of LatLng points in local ENU metres. + * Computes the centroid of a set of LatLng points in local metres. * Used to get a single representative point for stairs/lift polygons. * Returns null if pts is null or empty. */ 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 2e1045eb..705d0e6c 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/ParticleFilter.java @@ -138,9 +138,6 @@ private void initialisePosition() { /** * Sets the MapMatcher used for wall-crossing checks in predict(). * Called once from SensorFusion after both objects are created. - * - * Bullet 1 (Map Matcher): wires the map data into the particle filter so position estimates - * are constrained by the building's floor geometry. */ public void setMapMatcher(MapMatcher mm) { this.mapMatcher = mm; @@ -184,8 +181,6 @@ public void predict(float deltaEast, float deltaNorth, float PDRstd) { return; } - // Bullet 3 (Map Matcher): save each particle's position before it is displaced so we have - // a valid rollback point if the move crosses a wall. // Save positions before the move so we can snap back any particle that crosses a wall float[] oldX = new float[Num_Particles]; float[] oldY = new float[Num_Particles]; @@ -202,11 +197,7 @@ public void predict(float deltaEast, float deltaNorth, float PDRstd) { particles[i].y += deltaNorth + noiseY; } - // Bullet 3 (Map Matcher): use the movement (PDR displacement) and the wall geometry from the map - // to reject any particle whose move segment crossed a wall. That particle is snapped - // back to its pre-move position, keeping the estimate inside walkable space. - // If MapMatcher is loaded, check each particle's move segment against walls on the - // current floor. Any particle that crossed a wall is snapped back to its old position. + // If MapMatcher is loaded, snap back any particle whose move crosses a wall on the current floor. if (mapMatcher != null && mapMatcher.isInitialised()) { int floorIndex = mapMatcher.getLikelyFloorIndex(); List walls = mapMatcher.getWallsForFloor(floorIndex); 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 6d2d5312..2acbc092 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -745,11 +745,9 @@ private void checkAndApplyFloorChange(float elevation) { return; } - // Bullet 4 (Map Matcher): check accumulated barometer elevation change against the building's - // floor height. Only accept a floor change if the user is physically near a - // staircase or lift centroid (from the map). Floor changes in the middle of an - // open room are ignored. - // Rule 2: Barometer - divide elapsed elevation by the building's floor height + // Rule 2: Barometer - checks accumulated elevation change against the building's floor + // height. Only accepts a floor change if the user is physically near a staircase or + // lift from the map. Floor changes in the middle of a room are ignored. float floorHeight = mapMatcher.getFloorHeight(); float elevationDelta = elevation - lastFloorElevation; int floorsToMove = (int) (Math.abs(elevationDelta) / floorHeight); @@ -767,11 +765,9 @@ private void checkAndApplyFloorChange(float elevation) { Log.d("SensorFusion", "Barometer floor change skipped: not near a lift or stairs"); return; } - // Bullet 5 (Map Matcher): use state.elevator (the PDR movement model) together with proximity - // to decide whether the user is in a lift or on stairs. state.elevator is true - // when vertical acceleration dominates and step activity is low - the lift pattern. - // state.elevator is true when vertical acceleration dominates and step activity is low. - // Use it alongside proximity to decide lift vs stairs. + // Uses state.elevator (the PDR movement model) together with proximity to decide + // whether the user is in a lift or on stairs. state.elevator is true when vertical + // acceleration dominates and step activity is low - the lift pattern. boolean movementModelSaysLift = state.elevator; String transitionType; if (nearLift && movementModelSaysLift) { From 93db7d71ff1487d6ec62d15c5f4b432e49362576 Mon Sep 17 00:00:00 2001 From: Joshua Bray Date: Thu, 2 Apr 2026 12:38:10 +0100 Subject: [PATCH 13/14] Key removal --- .gitignore | Bin 233 -> 289 bytes 1 file changed, 0 insertions(+), 0 deletions(-) diff --git a/.gitignore b/.gitignore index d4c3a57eee159cd72ac70f71a203d2756ab24459..86feb6f3024f96b8208def09fa76fcd271ebc05b 100644 GIT binary patch literal 289 zcmZ`!F%H5o4D=i+A7LUdz#?rT$Aya5lZp=Qz;b7w&ZqMZrBB}BT`4B^ zZl7|Xghg{f8r;GS-38nM_`M_!%%vlXrig@vZ_x0BvYc R0im=xH{`|!(zt>L2Vc$DS}Xtn literal 233 zcmZ{fu@1s83`F;Q3WNI(2v(L3jL1!_V&Nu{h=W-%2)RL$ zM}e#L`q From 1133476cc388f5db913ee3e01f391aaa86122fea Mon Sep 17 00:00:00 2001 From: Joshua Bray Date: Thu, 2 Apr 2026 13:05:45 +0100 Subject: [PATCH 14/14] Stop tracking secrets.properties --- secrets.properties | 6 ------ 1 file changed, 6 deletions(-) delete mode 100644 secrets.properties diff --git a/secrets.properties b/secrets.properties deleted file mode 100644 index 893161db..00000000 --- a/secrets.properties +++ /dev/null @@ -1,6 +0,0 @@ -# -# Modify the variables to set your keys -# -MAPS_API_KEY=AIzaSyC13ios2NevfsRVS6yIj1arAM29ecy0GxQ -OPENPOSITIONING_API_KEY=c64dIaytmbCb84kU1-uuXw -OPENPOSITIONING_MASTER_KEY=ewireless