Provides three levels of map-based correction on top of the + * {@link com.openpositioning.PositionMe.positioning.FusionManager} estimate:
+ * + *Also tracks barometric elevation for floor transition detection.
+ * + * @param stepLength PDR step length in metres + * @param headingRad compass heading in radians + * @param elevation barometric elevation in metres + */ + public void predict(float stepLength, float headingRad, float elevation) { + predictCallCount++; + if (predictCallCount <= 3 || predictCallCount % 20 == 0) { + Log.d(TAG, "predict() #" + predictCallCount + + " stepLen=" + String.format("%.2f", stepLength) + + " heading=" + String.format("%.1f", Math.toDegrees(headingRad)) + "deg" + + " elev=" + String.format("%.2f", elevation) + "m"); + } + if (!isActive()) return; + + this.currentElevation = elevation; + this.lastStepLength = stepLength; + this.stepsSinceLastFloorChange++; + + for (Particle p : particles) { + double noisyStep = stepLength + random.nextGaussian() * PDR_NOISE_STEP; + double noisyHeading = headingRad + random.nextGaussian() * PDR_NOISE_HEADING; + noisyStep = Math.max(0, Math.min(noisyStep, 2.0)); + + double dx = noisyStep * Math.sin(noisyHeading); + double dy = noisyStep * Math.cos(noisyHeading); + double oldX = p.getX(); + double oldY = p.getY(); + double newX = oldX + dx; + double newY = oldY + dy; + + p.setX(newX); + p.setY(newY); + + // §3.2: penalise particles that cross wall segments + wallCheckCount++; + if (doesCrossWall(oldX, oldY, newX, newY, p.getFloor())) { + p.setWeight(p.getWeight() * WALL_PENALTY); + wallHitCount++; + } + } + + if (wallCheckCount > 0 && wallCheckCount % 200 == 0) { + Log.d(TAG, "Wall stats — hits=" + wallHitCount + "/" + wallCheckCount + + " (" + String.format("%.1f", 100.0 * wallHitCount / wallCheckCount) + "%)"); + } + + checkFloorTransition(); + normaliseWeights(); + if (effectiveSampleSize() < RESAMPLE_THRESHOLD * NUM_PARTICLES) { + resample(); + } + } + + // ========================================================================= + // Observation updates + // ========================================================================= + + /** + * Updates particle weights from a WiFi observation. Includes lost-filter + * recovery: if the weighted mean drifts more than 12m from WiFi, the + * filter reinitialises around the WiFi fix. + * + * @param lat WiFi latitude + * @param lng WiFi longitude + * @param wifiFloor WiFi-reported floor number + * @param accuracy WiFi accuracy estimate + */ + public void updateWifi(double lat, double lng, int wifiFloor, float accuracy) { + if (!isActive()) return; + + double obsX = (lng - refLng) * METRES_PER_DEG_LNG; + double obsY = (lat - refLat) * METRES_PER_DEG_LAT; + double sigma = Math.max(WIFI_SIGMA, accuracy); + + for (Particle p : particles) { + double dist = Math.sqrt(Math.pow(p.getX() - obsX, 2) + Math.pow(p.getY() - obsY, 2)); + double likelihood = gaussianLikelihood(dist, sigma); + // Floor mismatch penalty + if (p.getFloor() != wifiFloor) likelihood *= 0.4; + p.setWeight(p.getWeight() * likelihood); + } + normaliseWeights(); + + // Lost-filter recovery + double weightedX = 0, weightedY = 0; + for (Particle p : particles) { + weightedX += p.getX() * p.getWeight(); + weightedY += p.getY() * p.getWeight(); + } + double meanDistFromWifi = Math.sqrt( + Math.pow(weightedX - obsX, 2) + Math.pow(weightedY - obsY, 2)); + + if (meanDistFromWifi > 12.0) { + Log.w(TAG, "Lost filter — reinitialising dist=" + String.format("%.1f", meanDistFromWifi) + "m"); + particles.clear(); + for (int i = 0; i < NUM_PARTICLES; i++) { + particles.add(new Particle( + obsX + random.nextGaussian() * WIFI_SIGMA, + obsY + random.nextGaussian() * WIFI_SIGMA, + wifiFloor, 1.0 / NUM_PARTICLES)); + } + normaliseWeights(); + double rx = 0, ry = 0; + for (Particle p : particles) { + rx += p.getX() * p.getWeight(); + ry += p.getY() * p.getWeight(); + } + smoothedOutputX = rx; + smoothedOutputY = ry; + this.lastValidX = rx; + this.lastValidY = ry; + return; + } + + // WiFi fixes reset the building-outline constraint tracker + this.lastValidX = obsX; + this.lastValidY = obsY; + + if (effectiveSampleSize() < RESAMPLE_THRESHOLD * NUM_PARTICLES) resample(); + Log.d(TAG, "updateWifi() — floor=" + wifiFloor); + } + + /** Updates particle weights from a GNSS observation. */ + public void updateGnss(double lat, double lng, float accuracy) { + if (!isActive()) return; + double obsX = (lng - refLng) * METRES_PER_DEG_LNG; + double obsY = (lat - refLat) * METRES_PER_DEG_LAT; + double sigma = Math.max(GNSS_SIGMA, accuracy); + for (Particle p : particles) { + double dist = Math.sqrt(Math.pow(p.getX() - obsX, 2) + Math.pow(p.getY() - obsY, 2)); + p.setWeight(p.getWeight() * gaussianLikelihood(dist, sigma)); + } + normaliseWeights(); + if (effectiveSampleSize() < RESAMPLE_THRESHOLD * NUM_PARTICLES) resample(); + } + + // ========================================================================= + // Floor transition logic (§3.2) + // ========================================================================= + + /** + * Checks whether the accumulated barometric elevation change indicates a + * floor transition. Implements all §3.2 floor requirements: + * + *Uses ray-casting point-in-polygon to test whether the position is + * inside the building. If outside, returns the last known valid inside + * position (hard snap). When FM re-centres via WiFi, the constraint + * tracker is reset through {@link #updateWifi}.
+ * + *Interior wall corrections are handled by the particle filter's + * {@code WALL_PENALTY} in {@link #predict}, not here — the building's + * 594 wall segments on F1 cause excessive false positives for any + * direct line-crossing check on the displayed position.
+ * + * @param lat FusionManager latitude + * @param lng FusionManager longitude + * @return constrained position (unchanged if inside, last-valid if outside) + */ + public LatLng constrainPosition(double lat, double lng) { + if (!isActive()) return new LatLng(lat, lng); + + double posX = (lng - refLng) * METRES_PER_DEG_LNG; + double posY = (lat - refLat) * METRES_PER_DEG_LAT; + + // First call — accept unconditionally + if (Double.isNaN(lastValidX)) { + lastValidX = posX; + lastValidY = posY; + return new LatLng(lat, lng); + } + + // Building outline — hard snap (position cannot leave the building) + if (outlineX != null && !isInsidePolygon(posX, posY, outlineX, outlineY)) { + Log.d(TAG, "constrainPosition: OUTSIDE BUILDING — hard snap"); + return new LatLng(refLat + lastValidY / METRES_PER_DEG_LAT, + refLng + lastValidX / METRES_PER_DEG_LNG); + } + + // Inside building — accept fully + lastValidX = posX; + lastValidY = posY; + return new LatLng(lat, lng); + } + + // ========================================================================= + // Point-in-polygon (ray casting algorithm) + // ========================================================================= + + /** + * Ray-casting algorithm for point-in-polygon testing. Counts the number + * of times a horizontal ray from (px, py) crosses polygon edges. + * Odd crossings = inside, even = outside. Works for convex and concave. + */ + private static boolean isInsidePolygon(double px, double py, double[] polyX, double[] polyY) { + int n = polyX.length; + boolean inside = false; + for (int i = 0, j = n - 1; i < n; j = i++) { + if ((polyY[i] > py) != (polyY[j] > py) + && px < (polyX[j] - polyX[i]) * (py - polyY[i]) / (polyY[j] - polyY[i]) + polyX[i]) { + inside = !inside; + } + } + return inside; + } + + // ========================================================================= + // Wall collision detection (§3.2) + // ========================================================================= + + /** + * Tests whether the line segment from (x1,y1) to (x2,y2) intersects + * any wall feature on the given floor. Used by {@link #predict} to + * penalise wall-crossing particles. + */ + private boolean doesCrossWall(double x1, double y1, double x2, double y2, int floor) { + if (floorShapes == null || floor < 0 || floor >= floorShapes.size()) return false; + FloorplanApiClient.FloorShapes floorData = floorShapes.get(floor); + for (FloorplanApiClient.MapShapeFeature feature : floorData.getFeatures()) { + if (!"wall".equals(feature.getIndoorType())) continue; + for (ListEach particle holds a 2D position in easting/northing (metres), a floor index, + * and a weight reflecting how well the particle agrees with observations and map + * constraints.
+ * + * @see MapMatchingEngine the particle filter that manages a population of particles + */ +public class Particle { + + /** Easting position in metres relative to the reference point. */ + private double x; + + /** Northing position in metres relative to the reference point. */ + private double y; + + /** Floor index (0-based, matching FloorShapes list order). */ + private int floor; + + /** Importance weight of this particle. */ + private double weight; + + /** + * Creates a new particle with the given position, floor, and weight. + * + * @param x easting in metres + * @param y northing in metres + * @param floor floor index + * @param weight importance weight + */ + public Particle(double x, double y, int floor, double weight) { + this.x = x; + this.y = y; + this.floor = floor; + this.weight = weight; + } + + /** + * Creates a deep copy of this particle. + * + * @return a new Particle with the same state + */ + public Particle copy() { + return new Particle(x, y, floor, weight); + } + + // --- Getters and setters --- + + public double getX() { return x; } + public void setX(double x) { this.x = x; } + + public double getY() { return y; } + public void setY(double y) { this.y = y; } + + public int getFloor() { return floor; } + public void setFloor(int floor) { this.floor = floor; } + + public double getWeight() { return weight; } + public void setWeight(double weight) { this.weight = weight; } +} diff --git a/app/src/main/java/com/openpositioning/PositionMe/positioning/CoordinateUtils.java b/app/src/main/java/com/openpositioning/PositionMe/positioning/CoordinateUtils.java new file mode 100644 index 00000000..aa85b0df --- /dev/null +++ b/app/src/main/java/com/openpositioning/PositionMe/positioning/CoordinateUtils.java @@ -0,0 +1,27 @@ +package com.openpositioning.PositionMe.positioning; + +public class CoordinateUtils { + private final double originLat; + private final double originLon; + private final double metersPerLat; + private final double metersPerLon; + + public CoordinateUtils(double originLat, double originLon) { + this.originLat = originLat; + this.originLon = originLon; + this.metersPerLat = 111320.0; + this.metersPerLon = 111320.0 * Math.cos(Math.toRadians(originLat)); + } + + public double[] toLocal(double lat, double lon) { + double east = (lon - originLon) * metersPerLon; + double north = (lat - originLat) * metersPerLat; + return new double[]{east, north}; + } + + public double[] toGlobal(double east, double north) { + double lon = originLon + east / metersPerLon; + double lat = originLat + north / metersPerLat; + return new double[]{lat, lon}; + } +} \ No newline at end of file diff --git a/app/src/main/java/com/openpositioning/PositionMe/positioning/FusionManager.java b/app/src/main/java/com/openpositioning/PositionMe/positioning/FusionManager.java new file mode 100644 index 00000000..69984bfc --- /dev/null +++ b/app/src/main/java/com/openpositioning/PositionMe/positioning/FusionManager.java @@ -0,0 +1,271 @@ +package com.openpositioning.PositionMe.positioning; + +import android.util.Log; + +/** + * Sensor fusion engine using a particle filter to combine PDR, GNSS, and WiFi + * observations into a single position estimate. + * + *Implements §3.1 (Positioning Fusion):
+ *Includes a WiFi death-spiral recovery mechanism: if the compass drifts + * the estimate beyond the WiFi outlier gate and consecutive fixes are rejected, + * the filter re-centres on WiFi to prevent permanent divergence.
+ * + * @see ParticleFilter for the underlying filter implementation + * @see com.openpositioning.PositionMe.mapmatching.MapMatchingEngine for map-based corrections + */ +public class FusionManager { + + private static final String TAG = "FusionManager"; + private static final int PARTICLE_COUNT = 250; + + private static final FusionManager instance = new FusionManager(); + + private ParticleFilter particleFilter; + private CoordinateUtils coords; + private boolean ready = false; + + /** Exponential moving average for display smoothing (higher = faster response). */ + private double[] smoothedLatLon = null; + private static final double DISPLAY_SMOOTHING_ALPHA = 0.5; + + /** Tracks whether FM was initialised from WiFi to avoid re-init from weaker GNSS. */ + private boolean initialisedFromWifi = false; + + /** + * WiFi death-spiral recovery counter. When compass error causes FM to drift + * past the outlier gate, every subsequent WiFi fix is also rejected, causing + * permanent divergence. After {@code MAX_WIFI_REJECTIONS_BEFORE_RECENTRE} + * consecutive rejections, FM force re-centres on the WiFi position. + */ + private int consecutiveWifiRejections = 0; + private static final int MAX_WIFI_REJECTIONS_BEFORE_RECENTRE = 2; + + /** WiFi outlier gate distance in metres — fixes beyond this are rejected. */ + private static final double WIFI_OUTLIER_GATE_M = 10.0; + + private FusionManager() {} + + public static FusionManager getInstance() { + return instance; + } + + /** Resets all state for a new recording session. */ + public synchronized void reset() { + particleFilter = null; + coords = null; + ready = false; + smoothedLatLon = null; + initialisedFromWifi = false; + consecutiveWifiRejections = 0; + } + + // ========================================================================= + // GNSS observation update (§3.1) + // ========================================================================= + + /** + * Processes a GNSS position fix. Initialises the filter on first valid fix, + * or updates the particle weights for subsequent fixes. + * + * @param lat WGS84 latitude + * @param lon WGS84 longitude + * @param accuracyMeters reported horizontal accuracy from the location provider + */ + public synchronized void onGnss(double lat, double lon, float accuracyMeters) { + float rawAccuracy = accuracyMeters > 0 ? accuracyMeters : 8.0f; + + // Indoor GNSS below 15m is rare — reject poor fixes early + if (rawAccuracy > 15.0f) { + Log.d(TAG, "Ignoring GNSS fix due to poor accuracy: " + rawAccuracy); + return; + } + + float sigma = Math.max(6.0f, rawAccuracy * 1.5f); + + if (!ready) { + coords = new CoordinateUtils(lat, lon); + particleFilter = new ParticleFilter(PARTICLE_COUNT); + particleFilter.initialise(0.0, 0.0, Math.max(sigma, 6.0f)); + ready = true; + Log.d(TAG, "Initialised from GNSS: " + lat + ", " + lon + " acc=" + sigma); + return; + } + + double[] en = coords.toLocal(lat, lon); + + // Outlier gate: reject fixes far from current estimate + double[] est = particleFilter.estimate(); + if (est != null) { + double de = en[0] - est[0]; + double dn = en[1] - est[1]; + double dist = Math.sqrt(de * de + dn * dn); + + if (dist > Math.max(18.0, sigma * 2.5)) { + Log.d(TAG, "Rejected GNSS outlier. Dist=" + dist + " sigma=" + sigma); + return; + } + } + + particleFilter.updateGnss(en[0], en[1], sigma); + } + + // ========================================================================= + // WiFi observation update (§3.1) + // ========================================================================= + + /** + * Processes a WiFi position fix from the openpositioning API. + * + *On first fix, initialises the filter. If GNSS-initialised and WiFi + * disagrees significantly, re-centres on WiFi (more reliable indoors). + * Includes death-spiral recovery: after consecutive rejections, force + * re-centres to prevent permanent compass-driven divergence.
+ * + * @param lat WGS84 latitude from WiFi positioning + * @param lon WGS84 longitude from WiFi positioning + */ + public synchronized void onWifi(double lat, double lon) { + if (!ready) { + // First fix — initialise from WiFi + coords = new CoordinateUtils(lat, lon); + particleFilter = new ParticleFilter(PARTICLE_COUNT); + particleFilter.initialise(0.0, 0.0, 8.0); + ready = true; + initialisedFromWifi = true; + consecutiveWifiRejections = 0; + Log.d(TAG, "Initialised from WiFi: " + lat + ", " + lon); + return; + } + + // If GNSS-initialised and WiFi is far away, re-centre once on WiFi + // since WiFi is more accurate indoors than GNSS + if (!initialisedFromWifi && coords != null) { + double[] en = coords.toLocal(lat, lon); + double[] est = particleFilter.estimate(); + if (est != null) { + double dist = Math.sqrt(Math.pow(en[0] - est[0], 2) + Math.pow(en[1] - est[1], 2)); + if (dist > 10.0) { + coords = new CoordinateUtils(lat, lon); + particleFilter = new ParticleFilter(PARTICLE_COUNT); + particleFilter.initialise(0.0, 0.0, 8.0); + initialisedFromWifi = true; + consecutiveWifiRejections = 0; + Log.d(TAG, "Re-centred from WiFi (GNSS was " + String.format("%.1f", dist) + "m off): " + lat + ", " + lon); + return; + } + } + initialisedFromWifi = true; + } + + double[] en = coords.toLocal(lat, lon); + + // Outlier gate with death-spiral recovery + double[] est = particleFilter.estimate(); + if (est != null) { + double de = en[0] - est[0]; + double dn = en[1] - est[1]; + double dist = Math.sqrt(de * de + dn * dn); + + if (dist > WIFI_OUTLIER_GATE_M) { + consecutiveWifiRejections++; + + if (consecutiveWifiRejections >= MAX_WIFI_REJECTIONS_BEFORE_RECENTRE) { + // Death-spiral detected: compass drift has pushed FM so far + // that every WiFi fix gets rejected. WiFi is the most reliable + // indoor source — re-centre unconditionally. + coords = new CoordinateUtils(lat, lon); + particleFilter = new ParticleFilter(PARTICLE_COUNT); + particleFilter.initialise(0.0, 0.0, 8.0); + smoothedLatLon = null; + consecutiveWifiRejections = 0; + Log.w(TAG, "Re-centred from WiFi after " + MAX_WIFI_REJECTIONS_BEFORE_RECENTRE + + " consecutive rejections. Dist was " + String.format("%.1f", dist) + "m"); + return; + } + + Log.d(TAG, "Rejected WiFi outlier. Dist=" + dist + + " (rejection " + consecutiveWifiRejections + + "/" + MAX_WIFI_REJECTIONS_BEFORE_RECENTRE + ")"); + return; + } + } + + // WiFi accepted — reset rejection counter and update particles + consecutiveWifiRejections = 0; + particleFilter.updateWifi(en[0], en[1]); + } + + // ========================================================================= + // PDR movement model (§3.1) + // ========================================================================= + + /** + * Propagates all particles forward using the PDR step detection. + * + * @param stepLen step length in metres from the step detector + * @param headingRad compass heading in radians from the rotation vector sensor + */ + public synchronized void onStep(double stepLen, double headingRad) { + if (!ready || particleFilter == null) return; + if (Double.isNaN(stepLen) || Double.isNaN(headingRad) || stepLen <= 0.05) return; + particleFilter.predict(stepLen, headingRad); + } + + // ========================================================================= + // Position output + // ========================================================================= + + /** + * Returns the best fused position estimate in WGS84, with exponential + * moving average display smoothing applied. + * + * @return [latitude, longitude] or null if not yet initialised + */ + public synchronized double[] getBestPosition() { + if (!ready || particleFilter == null || coords == null) return null; + + double[] en = particleFilter.estimate(); + if (en == null) return null; + + double[] latLon = coords.toGlobal(en[0], en[1]); + + if (smoothedLatLon == null) { + smoothedLatLon = latLon; + } else { + smoothedLatLon[0] = + DISPLAY_SMOOTHING_ALPHA * latLon[0] + + (1.0 - DISPLAY_SMOOTHING_ALPHA) * smoothedLatLon[0]; + smoothedLatLon[1] = + DISPLAY_SMOOTHING_ALPHA * latLon[1] + + (1.0 - DISPLAY_SMOOTHING_ALPHA) * smoothedLatLon[1]; + } + + return new double[]{smoothedLatLon[0], smoothedLatLon[1]}; + } + + /** Returns the raw particle filter estimate in local ENU coordinates. */ + public synchronized double[] getBestPositionLocal() { + if (!ready || particleFilter == null) return null; + return particleFilter.estimate(); + } + + /** Returns the effective sample size ratio as a confidence metric (0–1). */ + public synchronized double getConfidence() { + if (!ready || particleFilter == null) return 0.0; + return particleFilter.getConfidence(); + } + + /** Returns true once the filter has been initialised from any source. */ + public synchronized boolean isReady() { + return ready; + } +} \ No newline at end of file diff --git a/app/src/main/java/com/openpositioning/PositionMe/positioning/ParticleFilter.java b/app/src/main/java/com/openpositioning/PositionMe/positioning/ParticleFilter.java new file mode 100644 index 00000000..6997fb23 --- /dev/null +++ b/app/src/main/java/com/openpositioning/PositionMe/positioning/ParticleFilter.java @@ -0,0 +1,262 @@ +package com.openpositioning.PositionMe.positioning; + +import java.util.ArrayList; +import java.util.List; +import java.util.Random; + +/** + * Bootstrap particle filter for sensor fusion in easting/northing (ENU) space. + * + *Implements the predict–update–resample cycle used by {@link FusionManager} + * to fuse PDR step detections with absolute GNSS and WiFi observations. + * Operates entirely in local metres around a WGS84 reference point managed + * by {@link CoordinateUtils}.
+ * + *Key design choices:
+ *- * The RecordingFragment serves as the interface for users to initiate, monitor, and - * complete trajectory recording. It integrates sensor fusion data to track user movement - * and updates a map view in real time. Additionally, it provides UI controls to cancel, - * stop, and monitor recording progress. - *
- * Features: - * - Starts and stops trajectory recording. - * - Displays real-time sensor data such as elevation and distance traveled. - * - Provides UI controls to cancel or complete recording. - * - Uses {@link TrajectoryMapFragment} to visualize recorded paths. - * - Manages GNSS tracking and error display. + * Fragment responsible for the recording screen during a live positioning session. + * + *
Position display architecture (§3.1 + §3.2 + §3.3):
+ *Floor detection uses MapMatchingEngine's barometric particle filter + * with 30s warmup, sustained-step guards, and stairs/lift classification.
* - * @author Shu Gu + *Display updates at 5 Hz (200ms interval) with colour-coded observation + * markers for GNSS, WiFi, and PDR (§3.3).
*/ - public class RecordingFragment extends Fragment { - // UI elements + private static final String TAG = "RecordingFragment"; + private MaterialButton completeButton, cancelButton; private ImageView recIcon; private ProgressBar timeRemaining; private TextView elevation, distanceTravelled, gnssError; - // App settings private SharedPreferences settings; - - // Sensor & data logic private SensorFusion sensorFusion; private Handler refreshDataHandler; private CountDownTimer autoStop; - // Distance tracking private float distance = 0f; private float previousPosX = 0f; private float previousPosY = 0f; - // References to the child map fragment + private LatLng rawPdrPosition = null; + private TrajectoryMapFragment trajectoryMapFragment; + private int uiUpdateCount = 0; + /** Tracks last MME floor to detect and log floor changes. */ + private int lastMmFloor = -1; + + // Observation display throttling — prevents marker spam on the map + private LatLng lastDisplayedGnssObservation = null; + private long lastDisplayedGnssTimeMs = 0L; + private static final long GNSS_DISPLAY_MIN_INTERVAL_MS = 1500L; + private static final double GNSS_DISPLAY_MIN_DISTANCE_M = 3.0; + + // Display-only GNSS sanity gates: +// 1) If GNSS is >25 m away from the current displayed path, don't draw it. +// 2) If a new GNSS point jumps >20 m from the last drawn GNSS point, don't draw it. + private static final double GNSS_MAX_DISPLAY_ERROR_M = 25.0; + private static final double GNSS_MAX_JUMP_FROM_LAST_M = 20.0; + + private LatLng lastDisplayedWifiObservation = null; + private long lastDisplayedWifiTimeMs = 0L; + private static final long WIFI_DISPLAY_MIN_INTERVAL_MS = 1500L; + private static final double WIFI_DISPLAY_MIN_DISTANCE_M = 2.0; + + /** Heading smoothing to prevent arrow jitter on the map (§3.3). */ + private Float smoothedHeadingDeg = null; + private static final float HEADING_SMOOTHING_ALPHA = 0.20f; + + /** UI refresh task at 5 Hz. */ private final Runnable refreshDataTask = new Runnable() { @Override public void run() { updateUIandPosition(); - // Loop again refreshDataHandler.postDelayed(refreshDataTask, 200); } }; - public RecordingFragment() { - // Required empty public constructor - } + public RecordingFragment() {} @Override public void onCreate(Bundle savedInstanceState) { @@ -114,7 +130,6 @@ public void onCreate(Bundle savedInstanceState) { public View onCreateView(@NonNull LayoutInflater inflater, @Nullable ViewGroup container, @Nullable Bundle savedInstanceState) { - // Inflate only the "recording" UI parts (no map) return inflater.inflate(R.layout.fragment_recording, container, false); } @@ -123,12 +138,9 @@ public void onViewCreated(@NonNull View view, @Nullable Bundle savedInstanceState) { super.onViewCreated(view, savedInstanceState); - // Child Fragment: the container in fragment_recording.xml - // where TrajectoryMapFragment is placed trajectoryMapFragment = (TrajectoryMapFragment) getChildFragmentManager().findFragmentById(R.id.trajectoryMapFragmentContainer); - // If not present, create it if (trajectoryMapFragment == null) { trajectoryMapFragment = new TrajectoryMapFragment(); getChildFragmentManager() @@ -137,7 +149,6 @@ public void onViewCreated(@NonNull View view, .commit(); } - // Initialize UI references elevation = view.findViewById(R.id.currentElevation); distanceTravelled = view.findViewById(R.id.currentDistanceTraveled); gnssError = view.findViewById(R.id.gnssError); @@ -148,54 +159,39 @@ public void onViewCreated(@NonNull View view, timeRemaining = view.findViewById(R.id.timeRemainingBar); view.findViewById(R.id.btn_test_point).setOnClickListener(v -> onAddTestPoint()); - - // Hide or initialize default values gnssError.setVisibility(View.GONE); elevation.setText(getString(R.string.elevation, "0")); distanceTravelled.setText(getString(R.string.meter, "0")); - // Buttons completeButton.setOnClickListener(v -> { - // Stop recording & go to correction if (autoStop != null) autoStop.cancel(); sensorFusion.stopRecording(); - // Show Correction screen ((RecordingActivity) requireActivity()).showCorrectionScreen(); }); - - // Cancel button with confirmation dialog cancelButton.setOnClickListener(v -> { AlertDialog dialog = new AlertDialog.Builder(requireActivity()) .setTitle("Confirm Cancel") .setMessage("Are you sure you want to cancel the recording? Your progress will be lost permanently!") .setNegativeButton("Yes", (dialogInterface, which) -> { - // User confirmed cancellation sensorFusion.stopRecording(); if (autoStop != null) autoStop.cancel(); requireActivity().onBackPressed(); }) - .setPositiveButton("No", (dialogInterface, which) -> { - // User cancelled the dialog. Do nothing. - dialogInterface.dismiss(); - }) - .create(); // Create the dialog but do not show it yet + .setPositiveButton("No", (dialogInterface, which) -> dialogInterface.dismiss()) + .create(); - // Show the dialog and change the button color dialog.setOnShowListener(dialogInterface -> { Button negativeButton = dialog.getButton(AlertDialog.BUTTON_NEGATIVE); - negativeButton.setTextColor(Color.RED); // Set "Yes" button color to red + negativeButton.setTextColor(Color.RED); }); - dialog.show(); // Finally, show the dialog + dialog.show(); }); - // The blinking effect for recIcon blinkingRecordingIcon(); - // Start the timed or indefinite UI refresh if (this.settings.getBoolean("split_trajectory", false)) { - // A maximum recording time is set long limit = this.settings.getInt("split_duration", 30) * 60000L; timeRemaining.setMax((int) (limit / 1000)); timeRemaining.setProgress(0); @@ -215,101 +211,226 @@ public void onFinish() { } }.start(); } else { - // No set time limit, just keep refreshing refreshDataHandler.post(refreshDataTask); } } + /** Adds a user ground-truth test point marker at the current position. */ private void onAddTestPoint() { - // 1) Ensure the map fragment is ready if (trajectoryMapFragment == null) return; - // 2) Read current track position (must lie on the current path) LatLng cur = trajectoryMapFragment.getCurrentLocation(); if (cur == null) { - Toast.makeText(requireContext(), "" + + Toast.makeText(requireContext(), "I haven't gotten my current location yet, let me take a couple of steps/wait for the map to load.", Toast.LENGTH_SHORT).show(); return; } - // 3) Generate index + timestamp (satisfies "save timestamp") int idx = ++testPointIndex; long ts = System.currentTimeMillis(); - - // 4) Keep a local copy for in-session tracking testPoints.add(new TestPoint(idx, ts, cur.latitude, cur.longitude)); - - // Write test point into protobuf payload sensorFusion.addTestPointToProto(ts, cur.latitude, cur.longitude); - - // 5) Draw numbered marker on map (satisfies "leave numbered marker") trajectoryMapFragment.addTestPointMarker(idx, ts, cur); } - /** - * Update the UI with sensor data and pass map updates to TrajectoryMapFragment. + * Core UI update cycle. Called at 5 Hz. + * + *Retrieves the best position from FusionManager, applies MapMatchingEngine's + * building outline constraint, updates the map, and refreshes sensor readouts.
*/ private void updateUIandPosition() { float[] pdrValues = sensorFusion.getSensorValueMap().get(SensorTypes.PDR); if (pdrValues == null) return; - // Distance + uiUpdateCount++; + + // Update distance counter distance += Math.sqrt(Math.pow(pdrValues[0] - previousPosX, 2) + Math.pow(pdrValues[1] - previousPosY, 2)); distanceTravelled.setText(getString(R.string.meter, String.format("%.2f", distance))); - // Elevation + // Update elevation readout float elevationVal = sensorFusion.getElevation(); elevation.setText(getString(R.string.elevation, String.format("%.1f", elevationVal))); - // Current location - // Convert PDR coordinates to actual LatLng if you have a known starting lat/lon - // Or simply pass relative data for the TrajectoryMapFragment to handle - // For example: - float[] latLngArray = sensorFusion.getGNSSLatitude(true); - if (latLngArray != null) { - LatLng oldLocation = trajectoryMapFragment.getCurrentLocation(); // or store locally - LatLng newLocation = UtilFunctions.calculateNewPos( - oldLocation == null ? new LatLng(latLngArray[0], latLngArray[1]) : oldLocation, - new float[]{ pdrValues[0] - previousPosX, pdrValues[1] - previousPosY } - ); - - // Pass the location + orientation to the map - if (trajectoryMapFragment != null) { - trajectoryMapFragment.updateUserLocation(newLocation, - (float) Math.toDegrees(sensorFusion.passOrientation())); - } + com.openpositioning.PositionMe.mapmatching.MapMatchingEngine mmEngine = + sensorFusion.getMapMatchingEngine(); + + // Wait for MME initialisation before drawing — ensures traces + // start from the manual pin position, not from (0,0) + if (mmEngine == null || !mmEngine.isActive()) { + previousPosX = pdrValues[0]; + previousPosY = pdrValues[1]; + return; + } + + // Initialise raw PDR trace from the manual start pin (once) + if (rawPdrPosition == null) { + LatLng mmStart = mmEngine.getStartLatLng(); + if (mmStart == null) mmStart = mmEngine.getEstimatedPosition(); + if (mmStart != null) rawPdrPosition = mmStart; + } + + // Advance raw PDR by step delta (red polyline, §3.3) + if (rawPdrPosition != null) { + rawPdrPosition = UtilFunctions.calculateNewPos( + rawPdrPosition, + new float[]{ pdrValues[0] - previousPosX, pdrValues[1] - previousPosY }); } - // GNSS logic if you want to show GNSS error, etc. + if (rawPdrPosition != null && trajectoryMapFragment != null) { + trajectoryMapFragment.addRawPdrPoint(rawPdrPosition); + } + + // ── Position source selection (§3.1 + §3.2) ───────────────────────── + LatLng fusedLocation = null; + String positionSource = "none"; + + // Primary: FusionManager → building outline constraint + double[] fusedPos = FusionManager.getInstance().getBestPosition(); + if (fusedPos != null) { + fusedLocation = mmEngine.constrainPosition(fusedPos[0], fusedPos[1]); + positionSource = "FusionManager+MMConstraint"; + } + + // Fallback: MME particle filter (before FM initialises) + if (fusedLocation == null) { + fusedLocation = mmEngine.getEstimatedPosition(); + if (fusedLocation != null) positionSource = "MapMatchingEngine"; + } + // ───────────────────────────────────────────────────────────────────── + + // ── Floor detection from MME (§3.2) ────────────────────────────────── + int mmFloor = mmEngine.getEstimatedFloor(); + if (mmFloor != lastMmFloor && lastMmFloor != -1) { + Log.d(TAG, "MME floor change: " + lastMmFloor + " → " + mmFloor); + } + lastMmFloor = mmFloor; + // ───────────────────────────────────────────────────────────────────── + + if (uiUpdateCount % 5 == 0) { + Log.d(TAG, "Position source=" + positionSource + + " | floor=" + mmFloor + + " | fusedLoc=" + (fusedLocation == null ? "null" + : String.format("(%.5f, %.5f)", + fusedLocation.latitude, fusedLocation.longitude))); + } + + // Smooth heading for display arrow (§3.3) + float headingDeg = smoothHeadingDeg( + (float) Math.toDegrees(sensorFusion.passOrientation())); + + // Update map with best position + if (fusedLocation != null && trajectoryMapFragment != null) { + trajectoryMapFragment.updateUserLocation(fusedLocation, headingDeg); + trajectoryMapFragment.updatePdrObservation(fusedLocation); + } else if (rawPdrPosition != null && trajectoryMapFragment != null) { + trajectoryMapFragment.updateUserLocation(rawPdrPosition, headingDeg); + trajectoryMapFragment.updatePdrObservation(rawPdrPosition); + } + + long now = System.currentTimeMillis(); + + // ── GNSS observation markers (§3.3) ────────────────────────────────── float[] gnss = sensorFusion.getSensorValueMap().get(SensorTypes.GNSSLATLONG); if (gnss != null && trajectoryMapFragment != null) { - // If user toggles showing GNSS in the map, call e.g. if (trajectoryMapFragment.isGnssEnabled()) { LatLng gnssLocation = new LatLng(gnss[0], gnss[1]); LatLng currentLoc = trajectoryMapFragment.getCurrentLocation(); + + double errorDist = -1.0; if (currentLoc != null) { - double errorDist = UtilFunctions.distanceBetweenPoints(currentLoc, gnssLocation); + errorDist = UtilFunctions.distanceBetweenPoints(currentLoc, gnssLocation); gnssError.setVisibility(View.VISIBLE); - gnssError.setText(String.format(getString(R.string.gnss_error) + "%.2fm", errorDist)); + gnssError.setText(String.format( + getString(R.string.gnss_error) + "%.2fm", errorDist)); + } else { + gnssError.setVisibility(View.GONE); } - trajectoryMapFragment.updateGNSS(gnssLocation); + + boolean tooFarFromDisplayedPath = + (currentLoc != null && errorDist > GNSS_MAX_DISPLAY_ERROR_M); + + boolean hugeJumpFromLastDisplayed = + (lastDisplayedGnssObservation != null && + UtilFunctions.distanceBetweenPoints(lastDisplayedGnssObservation, gnssLocation) + > GNSS_MAX_JUMP_FROM_LAST_M); + + if (!tooFarFromDisplayedPath && !hugeJumpFromLastDisplayed && + shouldDisplayObservation( + gnssLocation, + lastDisplayedGnssObservation, + now - lastDisplayedGnssTimeMs, + GNSS_DISPLAY_MIN_INTERVAL_MS, + GNSS_DISPLAY_MIN_DISTANCE_M)) { + + trajectoryMapFragment.updateGNSS(gnssLocation); + lastDisplayedGnssObservation = gnssLocation; + lastDisplayedGnssTimeMs = now; + + } else { + if (tooFarFromDisplayedPath) { + Log.d(TAG, "Skipping GNSS draw: too far from displayed path (" + + String.format("%.2f", errorDist) + "m)"); + } else if (hugeJumpFromLastDisplayed) { + Log.d(TAG, "Skipping GNSS draw: huge jump from last GNSS point"); + } + } + } else { gnssError.setVisibility(View.GONE); trajectoryMapFragment.clearGNSS(); } } - // Update previous + // ── WiFi observation markers (§3.3) ────────────────────────────────── + if (trajectoryMapFragment != null) { + LatLng wifiLocation = sensorFusion.getLatLngWifiPositioning(); + if (wifiLocation != null && shouldDisplayObservation( + wifiLocation, lastDisplayedWifiObservation, + now - lastDisplayedWifiTimeMs, + WIFI_DISPLAY_MIN_INTERVAL_MS, WIFI_DISPLAY_MIN_DISTANCE_M)) { + trajectoryMapFragment.updateWifiObservation(wifiLocation); + lastDisplayedWifiObservation = wifiLocation; + lastDisplayedWifiTimeMs = now; + } + } + previousPosX = pdrValues[0]; previousPosY = pdrValues[1]; } /** - * Start the blinking effect for the recording icon. + * Throttles observation marker display to prevent map clutter. + * Only shows a new marker if enough time and distance have passed. */ + private boolean shouldDisplayObservation(LatLng candidate, LatLng previous, + long ageMs, long minIntervalMs, + double minDistanceMeters) { + if (candidate == null) return false; + if (previous == null) return true; + if (ageMs < minIntervalMs) return false; + return UtilFunctions.distanceBetweenPoints(previous, candidate) >= minDistanceMeters; + } + + /** Exponential heading smoother to prevent display arrow jitter. */ + private float smoothHeadingDeg(float newHeadingDeg) { + if (Float.isNaN(newHeadingDeg) || Float.isInfinite(newHeadingDeg)) { + return smoothedHeadingDeg != null ? smoothedHeadingDeg : 0f; + } + if (smoothedHeadingDeg == null) { + smoothedHeadingDeg = newHeadingDeg; + return newHeadingDeg; + } + float delta = newHeadingDeg - smoothedHeadingDeg; + while (delta > 180f) delta -= 360f; + while (delta < -180f) delta += 360f; + smoothedHeadingDeg = smoothedHeadingDeg + HEADING_SMOOTHING_ALPHA * delta; + return smoothedHeadingDeg; + } + private void blinkingRecordingIcon() { Animation blinking = new AlphaAnimation(1, 0); blinking.setDuration(800); @@ -328,11 +449,12 @@ public void onPause() { @Override public void onResume() { super.onResume(); - if(!this.settings.getBoolean("split_trajectory", false)) { + if (!this.settings.getBoolean("split_trajectory", false)) { refreshDataHandler.postDelayed(refreshDataTask, 500); } } + // ── Test point tracking ────────────────────────────────────────────────── private int testPointIndex = 0; private static class TestPoint { @@ -350,6 +472,4 @@ private static class TestPoint { } private final ListThe public API is unchanged – all external callers continue to use
@@ -88,6 +91,8 @@ public class SensorFusion implements SensorEventListener {
private PdrProcessing pdrProcessing;
private PathView pathView;
+ private final MapMatchingEngine mapMatchingEngine = new MapMatchingEngine();
+
// Sensor registration latency setting
long maxReportLatencyNs = 0;
@@ -162,7 +167,7 @@ public void setContext(Context context) {
long bootTime = SystemClock.uptimeMillis();
this.eventHandler = new SensorEventHandler(
- state, pdrProcessing, pathView, recorder, bootTime);
+ state, pdrProcessing, pathView, recorder, bootTime, mapMatchingEngine);
// Register WiFi observer on WifiPositionManager (not on SensorFusion)
this.wifiProcessor = new WifiDataProcessor(context);
@@ -326,6 +331,7 @@ private void stopWirelessCollectors() {
* @see SensorCollectionService
*/
public void startRecording() {
+ FusionManager.getInstance().reset();
recorder.startRecording(pdrProcessing);
eventHandler.resetBootTime(recorder.getBootTime());
@@ -482,6 +488,34 @@ public float[] getGNSSLatitude(boolean start) {
return latLong;
}
+ /**
+ * Returns the floor estimated by the MapMatchingEngine's barometric
+ * particle filter, or -1 if not available.
+ */
+ public int getMapMatchingFloor() {
+ if (mapMatchingEngine != null && mapMatchingEngine.isActive()) {
+ return mapMatchingEngine.getEstimatedFloor();
+ }
+ return -1;
+ }
+
+ /**
+ * Returns the map matching particle filter engine.
+ */
+ public MapMatchingEngine getMapMatchingEngine() {
+ return mapMatchingEngine;
+ }
+
+ /**
+ * Initialises map matching with current position and building data.
+ * Called when user starts recording inside a building.
+ */
+ public void initialiseMapMatching(double lat, double lng, int floor,
+ float floorHeight,
+ java.util.List Implements {@link Observer} to receive updates from {@link WifiDataProcessor},
* replacing the role previously held by {@link SensorFusion}. Receives updates from {@link WifiDataProcessor}. Converts the raw object array
* to a typed list, delegates fingerprint recording to {@link TrajectoryRecorder},
- * and triggers a WiFi positioning request.