diff --git a/.gitignore b/.gitignore
index d4c3a57e..4e82ab6f 100644
--- a/.gitignore
+++ b/.gitignore
@@ -14,3 +14,4 @@
 .cxx
 local.properties
 /.idea/
+secrets.properties
\ No newline at end of file
diff --git a/app/src/main/java/com/openpositioning/PositionMe/data/remote/FloorplanApiClient.java b/app/src/main/java/com/openpositioning/PositionMe/data/remote/FloorplanApiClient.java
index cadb6037..b50496a5 100644
--- a/app/src/main/java/com/openpositioning/PositionMe/data/remote/FloorplanApiClient.java
+++ b/app/src/main/java/com/openpositioning/PositionMe/data/remote/FloorplanApiClient.java
@@ -372,7 +372,19 @@ private List<FloorShapes> parseMapShapes(String mapShapesJson) {
             while (it.hasNext()) {
                 keys.add(it.next());
             }
-            Collections.sort(keys);
+            //Collections.sort(keys);
+            List<String> Keys = new ArrayList<>();
+            String[] physicalOrder = {"LG", "GF", "F1", "F2", "F3"};
+            for (String pKey : physicalOrder) {
+                if (root.has(pKey)) {
+                    Keys.add(pKey);
+                }
+            }
+            Iterator<String> allKeys = root.keys();
+            while (allKeys.hasNext()) {
+                String k = allKeys.next();
+                if (!Keys.contains(k)) Keys.add(k);
+            }
 
             for (String key : keys) {
                 JSONObject floorCollection = root.getJSONObject(key);
diff --git a/app/src/main/java/com/openpositioning/PositionMe/fusion/MapBounds.java b/app/src/main/java/com/openpositioning/PositionMe/fusion/MapBounds.java
new file mode 100644
index 00000000..b0758df4
--- /dev/null
+++ b/app/src/main/java/com/openpositioning/PositionMe/fusion/MapBounds.java
@@ -0,0 +1,15 @@
+package com.openpositioning.PositionMe.fusion;
+
+public class MapBounds {
+    public final float minX;
+    public final float maxX;
+    public final float minY;
+    public final float maxY;
+
+    public MapBounds(float minX, float maxX, float minY, float maxY) {
+        this.minX = minX;
+        this.maxX = maxX;
+        this.minY = minY;
+        this.maxY = maxY;
+    }
+}
diff --git a/app/src/main/java/com/openpositioning/PositionMe/fusion/Measurement.java b/app/src/main/java/com/openpositioning/PositionMe/fusion/Measurement.java
new file mode 100644
index 00000000..88f1d73e
--- /dev/null
+++ b/app/src/main/java/com/openpositioning/PositionMe/fusion/Measurement.java
@@ -0,0 +1,14 @@
+package com.openpositioning.PositionMe.fusion;
+
+/** Represents a position measurement with an accuracy estimate. */
+public class Measurement {
+    public final float x;
+    public final float y;
+    public final double accuracy;
+
+    public Measurement(float x, float y, double accuracy) {
+        this.x = x;
+        this.y = y;
+        this.accuracy = accuracy;
+    }
+}
diff --git a/app/src/main/java/com/openpositioning/PositionMe/fusion/PDRMovement.java b/app/src/main/java/com/openpositioning/PositionMe/fusion/PDRMovement.java
new file mode 100644
index 00000000..8e3e86e5
--- /dev/null
+++ b/app/src/main/java/com/openpositioning/PositionMe/fusion/PDRMovement.java
@@ -0,0 +1,12 @@
+package com.openpositioning.PositionMe.fusion;
+
+/** Represents a planar PDR displacement for a single update step. */
+public class PDRMovement {
+    public final float deltaX;
+    public final float deltaY;
+
+    public PDRMovement(float deltaX, float deltaY) {
+        this.deltaX = deltaX;
+        this.deltaY = deltaY;
+    }
+}
diff --git a/app/src/main/java/com/openpositioning/PositionMe/fusion/Particle.java b/app/src/main/java/com/openpositioning/PositionMe/fusion/Particle.java
new file mode 100644
index 00000000..8fb0c0d5
--- /dev/null
+++ b/app/src/main/java/com/openpositioning/PositionMe/fusion/Particle.java
@@ -0,0 +1,57 @@
+package com.openpositioning.PositionMe.fusion;
+
+import java.util.Random;
+
+/** Represents a single particle in the particle filter. */
+public class Particle {
+    float x;
+    float y;
+    public float oldX;
+    public float oldY;
+    int floor;
+    double weight;
+
+    /**
+     * Creates a particle with an initial position and floor.
+     *
+     * @param x initial x coordinate in meters
+     * @param y initial y coordinate in meters
+     * @param floor initial floor index
+     */
+    public Particle(float x, float y, int floor) {
+        this.x = x;
+        this.y = y;
+        this.oldX = x;
+        this.oldY = y;
+        this.floor = floor;
+        this.weight = 1.0;
+    }
+
+    /**
+     * Moves the particle using the default motion noise.
+     *
+     * @param deltaX movement along the x axis in meters
+     * @param deltaY movement along the y axis in meters
+     * @param random random source used to sample motion noise
+     */
+    public void move(float deltaX, float deltaY, Random random) {
+        move(deltaX, deltaY, random, 0.03f);
+    }
+
+    /**
+     * Moves the particle and adds Gaussian motion noise.
+     *
+     * @param deltaX movement along the x axis in meters
+     * @param deltaY movement along the y axis in meters
+     * @param random random source used to sample motion noise
+     * @param noiseStdDev standard deviation of the motion noise in meters
+     */
+    public void move(float deltaX, float deltaY, Random random, float noiseStdDev) {
+        this.oldX = this.x;
+        this.oldY = this.y;
+        this.x += deltaX;
+        this.y += deltaY;
+        this.x += random.nextGaussian() * noiseStdDev;
+        this.y += random.nextGaussian() * noiseStdDev;
+    }
+}
diff --git a/app/src/main/java/com/openpositioning/PositionMe/fusion/ParticleFilter.java b/app/src/main/java/com/openpositioning/PositionMe/fusion/ParticleFilter.java
new file mode 100644
index 00000000..118b7471
--- /dev/null
+++ b/app/src/main/java/com/openpositioning/PositionMe/fusion/ParticleFilter.java
@@ -0,0 +1,384 @@
+package com.openpositioning.PositionMe.fusion;
+
+import com.google.android.gms.maps.model.LatLng;
+import com.openpositioning.PositionMe.data.remote.FloorplanApiClient;
+import com.openpositioning.PositionMe.utils.GeometryUtils;
+
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Random;
+
+/** Particle filter used to fuse PDR motion with map and measurement constraints. */
+public class ParticleFilter {
+
+    private List<Particle> particles;
+    private final int numParticles;
+    private final Random random = new Random();
+
+    /**
+     * Creates a particle filter with a fixed number of particles.
+     *
+     * @param numParticles number of particles maintained by the filter
+     */
+    public ParticleFilter(int numParticles) {
+        this.numParticles = numParticles;
+        this.particles = new ArrayList<>(numParticles);
+    }
+
+    /**
+     * Initializes particles uniformly within the provided map bounds.
+     *
+     * @param mapBounds rectangular initialization bounds in local map coordinates
+     */
+    public void initialize(MapBounds mapBounds) {
+        particles.clear();
+        float width = mapBounds.maxX - mapBounds.minX;
+        float height = mapBounds.maxY - mapBounds.minY;
+
+        for (int i = 0; i < numParticles; i++) {
+            float randomX = mapBounds.minX + random.nextFloat() * width;
+            float randomY = mapBounds.minY + random.nextFloat() * height;
+            particles.add(new Particle(randomX, randomY, 0));
+        }
+    }
+
+    /**
+     * Applies a PDR motion update to all particles.
+     *
+     * @param movement PDR motion in local map coordinates
+     */
+    public void predict(PDRMovement movement) {
+        predict(movement, 1);
+    }
+
+    /**
+     * Applies a subdivided PDR motion update to all particles.
+     *
+     * @param movement PDR motion in local map coordinates
+     * @param subdivisions number of substeps used for this update
+     */
+    public void predict(PDRMovement movement, int subdivisions) {
+        int safeSubdivisions = Math.max(1, subdivisions);
+        float dx = movement.deltaX / safeSubdivisions;
+        float dy = movement.deltaY / safeSubdivisions;
+        float noiseStdDev = 0.03f / (float) Math.sqrt(safeSubdivisions);
+
+        for (Particle particle : particles) {
+            particle.move(dx, dy, this.random, noiseStdDev);
+        }
+    }
+
+    /**
+     * Updates particle weights using an external position measurement.
+     *
+     * @param measurement measured position and uncertainty
+     * @param walls unused, kept to match the current call sites
+     * @param startLocation unused, kept to match the current call sites
+     */
+    public void updateWeights(
+            Measurement measurement,
+            List<FloorplanApiClient.MapShapeFeature> walls,
+            LatLng startLocation) {
+        double totalWeight = 0;
+
+        for (Particle particle : particles) {
+            double distanceSquared =
+                    Math.pow(particle.x - measurement.x, 2)
+                            + Math.pow(particle.y - measurement.y, 2);
+            double sigma = measurement.accuracy;
+            double likelihood = Math.exp(-distanceSquared / (2 * Math.pow(sigma, 2)));
+            particle.weight *= likelihood;
+            totalWeight += particle.weight;
+        }
+
+        for (Particle particle : particles) {
+            if (totalWeight > 0) {
+                particle.weight /= totalWeight;
+            } else {
+                particle.weight = 1.0 / numParticles;
+            }
+        }
+    }
+
+    /** Resamples particles using roulette-wheel sampling. */
+    public void resample() {
+        List<Particle> newParticles = new ArrayList<>(numParticles);
+
+        double maxWeight = 0;
+        for (Particle particle : particles) {
+            if (particle.weight > maxWeight) {
+                maxWeight = particle.weight;
+            }
+        }
+
+        if (maxWeight == 0) {
+            return;
+        }
+
+        double beta = 0.0;
+        int index = random.nextInt(numParticles);
+
+        for (int i = 0; i < numParticles; i++) {
+            beta += random.nextDouble() * 2.0 * maxWeight;
+
+            while (beta > particles.get(index).weight) {
+                beta -= particles.get(index).weight;
+                index = (index + 1) % numParticles;
+            }
+
+            Particle selected = particles.get(index);
+            newParticles.add(new Particle(selected.x, selected.y, selected.floor));
+        }
+
+        this.particles = newParticles;
+    }
+
+    /**
+     * Returns the weighted mean particle position.
+     *
+     * @return estimated position in local map coordinates
+     */
+    public Position getEstimatedPosition() {
+        if (particles == null || particles.isEmpty()) {
+            return new Position(0, 0);
+        }
+
+        float expectedX = 0;
+        float expectedY = 0;
+        double totalWeight = 0;
+
+        for (Particle particle : particles) {
+            expectedX += particle.x * particle.weight;
+            expectedY += particle.y * particle.weight;
+            totalWeight += particle.weight;
+        }
+
+        if (totalWeight == 0) {
+            float sumX = 0;
+            float sumY = 0;
+            for (Particle particle : particles) {
+                sumX += particle.x;
+                sumY += particle.y;
+            }
+            return new Position(sumX / particles.size(), sumY / particles.size());
+        }
+
+        return new Position((float) (expectedX / totalWeight), (float) (expectedY / totalWeight));
+    }
+
+    /**
+     * Returns a map-aware estimated position.
+     *
+     * <p>If the weighted mean falls inside a blocked polygon, the filter falls back to the
+     * highest-weight valid particle.
+     *
+     * @param walls blocking map features
+     * @param startLocation origin used to convert local coordinates to latitude and longitude
+     * @return estimated position in local map coordinates
+     */
+    public Position getEstimatedPosition(
+            List<FloorplanApiClient.MapShapeFeature> walls, LatLng startLocation) {
+        Position estimate = getEstimatedPosition();
+        if (walls == null || startLocation == null || walls.isEmpty()) {
+            return estimate;
+        }
+
+        LatLng estimatedLatLng = toLatLng(estimate.x, estimate.y, startLocation);
+        if (!isBlockedPosition(estimatedLatLng, walls)) {
+            return estimate;
+        }
+
+        Particle bestParticle = null;
+        for (Particle particle : particles) {
+            LatLng particleLatLng = toLatLng(particle.x, particle.y, startLocation);
+            if (isBlockedPosition(particleLatLng, walls)) {
+                continue;
+            }
+            if (bestParticle == null || particle.weight > bestParticle.weight) {
+                bestParticle = particle;
+            }
+        }
+
+        if (bestParticle != null) {
+            return new Position(bestParticle.x, bestParticle.y);
+        }
+
+        return estimate;
+    }
+
+    /**
+     * Returns the current estimated position converted to geographic coordinates.
+     *
+     * @param walls blocking map features
+     * @param startLocation origin used to convert local coordinates to latitude and longitude
+     * @return estimated latitude and longitude, or {@code null} if the start location is unavailable
+     */
+    public LatLng getEstimatedLatLng(
+            List<FloorplanApiClient.MapShapeFeature> walls, LatLng startLocation) {
+        if (startLocation == null) {
+            return null;
+        }
+        Position estimate = getEstimatedPosition(walls, startLocation);
+        return toLatLng(estimate.x, estimate.y, startLocation);
+    }
+
+    /**
+     * Returns a horizontal motion scale for special indoor areas.
+     *
+     * @param features current floor map features
+     * @param startLocation origin used to convert local coordinates to latitude and longitude
+     * @param elevatorDetected whether elevator motion is currently detected
+     * @return horizontal motion scale applied before prediction
+     */
+    public float getHorizontalMovementScale(
+            List<FloorplanApiClient.MapShapeFeature> features,
+            LatLng startLocation,
+            boolean elevatorDetected) {
+        if (features == null || startLocation == null || features.isEmpty()) {
+            return 1.0f;
+        }
+
+        Position estimate = getEstimatedPosition(features, startLocation);
+        LatLng estimatedLatLng = toLatLng(estimate.x, estimate.y, startLocation);
+
+        if (isInsideIndoorType(estimatedLatLng, features, "elevator")) {
+            return elevatorDetected ? 0.05f : 0.15f;
+        }
+        if (isInsideIndoorType(estimatedLatLng, features, "stairs")) {
+            return 0.18f;
+        }
+        return 1.0f;
+    }
+
+    /**
+     * Suppresses particles that cross blocking geometry or end inside blocked polygons.
+     *
+     * @param walls blocking map features
+     * @param startLocation origin used to convert local coordinates to latitude and longitude
+     */
+    public void applyMapMatching(
+            List<FloorplanApiClient.MapShapeFeature> walls, LatLng startLocation) {
+        if (walls == null || startLocation == null || particles.isEmpty()) {
+            return;
+        }
+
+        boolean allDead = true;
+
+        for (Particle particle : particles) {
+            LatLng oldPos = toLatLng(particle.oldX, particle.oldY, startLocation);
+            LatLng newPos = toLatLng(particle.x, particle.y, startLocation);
+            boolean hitWall = isPathBlocked(oldPos, newPos, walls)
+                    || isBlockedPosition(newPos, walls);
+
+            if (hitWall) {
+                particle.weight = 0.0;
+            } else {
+                allDead = false;
+            }
+        }
+
+        if (allDead) {
+            for (Particle particle : particles) {
+                particle.x = particle.oldX + (float) (random.nextGaussian() * 0.1);
+                particle.y = particle.oldY + (float) (random.nextGaussian() * 0.1);
+                particle.weight = 1.0;
+            }
+            return;
+        }
+
+        double totalWeight = 0;
+        for (Particle particle : particles) {
+            totalWeight += particle.weight;
+        }
+
+        if (totalWeight > 0 && totalWeight < particles.size()) {
+            for (Particle particle : particles) {
+                particle.weight /= totalWeight;
+            }
+            resample();
+            android.util.Log.d("MapMatch", "Blocked particles were removed and resampled.");
+        }
+    }
+
+    private LatLng toLatLng(float x, float y, LatLng startLocation) {
+        double radius = 6378137.0;
+        double lat0Rad = Math.toRadians(startLocation.latitude);
+        double dLat = y / radius;
+        double dLng = x / (radius * Math.cos(lat0Rad));
+        return new LatLng(
+                startLocation.latitude + Math.toDegrees(dLat),
+                startLocation.longitude + Math.toDegrees(dLng));
+    }
+
+    private boolean isPathBlocked(
+            LatLng oldPos, LatLng newPos, List<FloorplanApiClient.MapShapeFeature> walls) {
+        for (FloorplanApiClient.MapShapeFeature wall : walls) {
+            if (!isBlockingFeature(wall)) {
+                continue;
+            }
+            for (List<LatLng> part : wall.getParts()) {
+                if (part == null || part.size() < 2) {
+                    continue;
+                }
+                for (int i = 0; i < part.size() - 1; i++) {
+                    if (GeometryUtils.doSegmentsIntersect(oldPos, newPos, part.get(i), part.get(i + 1))) {
+                        return true;
+                    }
+                }
+                if (part.size() > 2
+                        && GeometryUtils.doSegmentsIntersect(
+                                oldPos, newPos, part.get(part.size() - 1), part.get(0))) {
+                    return true;
+                }
+            }
+        }
+        return false;
+    }
+
+    private boolean isBlockedPosition(
+            LatLng position, List<FloorplanApiClient.MapShapeFeature> walls) {
+        for (FloorplanApiClient.MapShapeFeature wall : walls) {
+            if (!isBlockingFeature(wall)) {
+                continue;
+            }
+            if (!"Polygon".equals(wall.getGeometryType())
+                    && !"MultiPolygon".equals(wall.getGeometryType())) {
+                continue;
+            }
+            for (List<LatLng> part : wall.getParts()) {
+                if (part != null && part.size() >= 3
+                        && GeometryUtils.isPointInPolygon(position, part)) {
+                    return true;
+                }
+            }
+        }
+        return false;
+    }
+
+    private boolean isInsideIndoorType(
+            LatLng position,
+            List<FloorplanApiClient.MapShapeFeature> features,
+            String indoorType) {
+        for (FloorplanApiClient.MapShapeFeature feature : features) {
+            if (!indoorType.equals(feature.getIndoorType())) {
+                continue;
+            }
+            if (!"Polygon".equals(feature.getGeometryType())
+                    && !"MultiPolygon".equals(feature.getGeometryType())) {
+                continue;
+            }
+            for (List<LatLng> part : feature.getParts()) {
+                if (part != null && part.size() >= 3
+                        && GeometryUtils.isPointInPolygon(position, part)) {
+                    return true;
+                }
+            }
+        }
+        return false;
+    }
+
+    private boolean isBlockingFeature(FloorplanApiClient.MapShapeFeature feature) {
+        return "wall".equals(feature.getIndoorType())
+                || "unaccessible".equals(feature.getIndoorType());
+    }
+}
diff --git a/app/src/main/java/com/openpositioning/PositionMe/fusion/Position.java b/app/src/main/java/com/openpositioning/PositionMe/fusion/Position.java
new file mode 100644
index 00000000..51291953
--- /dev/null
+++ b/app/src/main/java/com/openpositioning/PositionMe/fusion/Position.java
@@ -0,0 +1,11 @@
+package com.openpositioning.PositionMe.fusion;
+
+public class Position {
+    public final float x;
+    public final float y;
+
+    public Position(float x, float y) {
+        this.x = x;
+        this.y = y;
+    }
+}
\ No newline at end of file
diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/CorrectionFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/CorrectionFragment.java
index 165b9e16..f2dcbe80 100644
--- a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/CorrectionFragment.java
+++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/CorrectionFragment.java
@@ -31,16 +31,17 @@
 import com.google.android.gms.maps.model.MarkerOptions;
 
 /**
- * A simple {@link Fragment} subclass. Corrections Fragment is displayed after a recording session
- * is finished to enable manual adjustments to the PDR. The adjustments are not saved as of now.
+ * Lets the user review and manually rescale the recorded PDR trajectory after a session.
+ *
+ * <p>Corrections made in this screen are applied to the preview only and are not persisted.
  */
 public class CorrectionFragment extends Fragment {
 
-    //Map variable
+    /** Google Map used to preview the corrected trajectory. */
     public GoogleMap mMap;
-    //Button to go to next
+    /** Advances to the next screen after reviewing the correction. */
     private Button button;
-    //Singleton SensorFusion class
+    /** Shared sensor fusion pipeline instance. */
     private SensorFusion sensorFusion = SensorFusion.getInstance();
     private TextView averageStepLengthText;
     private EditText stepLengthInput;
@@ -53,7 +54,7 @@ public class CorrectionFragment extends Fragment {
     private PathView pathView;
 
     public CorrectionFragment() {
-        // Required empty public constructor
+        // Required empty public constructor.
     }
 
     @Override
@@ -65,13 +66,13 @@ public View onCreateView(LayoutInflater inflater, ViewGroup container,
         }
         View rootView = inflater.inflate(R.layout.fragment_correction, container, false);
 
-        // Validate trajectory quality before uploading
+        // Validate trajectory quality before uploading.
         validateAndUpload();
 
-        //Obtain start position
+        // Obtain the start position.
         float[] startPosition = sensorFusion.getGNSSLatitude(true);
 
-        // Initialize map fragment
+        // Initialize the embedded map fragment.
         SupportMapFragment supportMapFragment=(SupportMapFragment)
                 getChildFragmentManager().findFragmentById(R.id.map);
 
@@ -85,11 +86,11 @@ public void onMapReady(GoogleMap map) {
                 mMap.getUiSettings().setRotateGesturesEnabled(true);
                 mMap.getUiSettings().setScrollGesturesEnabled(true);
 
-                // Add a marker at the start position
+                // Add a marker at the start position.
                 start = new LatLng(startPosition[0], startPosition[1]);
                 mMap.addMarker(new MarkerOptions().position(start).title("Start Position"));
 
-                // Calculate zoom for demonstration
+                // Calculate a zoom level that matches the rendered path scale.
                 double zoom = Math.log(156543.03392f * Math.cos(startPosition[0] * Math.PI / 180)
                         * scalingRatio) / Math.log(2);
                 mMap.moveCamera(CameraUpdateFactory.newLatLngZoom(start, (float) zoom));
@@ -111,11 +112,11 @@ public void onViewCreated(@NonNull View view, @Nullable Bundle savedInstanceStat
         averageStepLengthText.setText(getString(R.string.averageStepLgn) + ": "
                 + String.format("%.2f", averageStepLength));
 
-        // Listen for ENTER key
+        // Listen for the Enter key to apply a new step length.
         this.stepLengthInput.setOnKeyListener((v, keyCode, event) -> {
             if (keyCode == KeyEvent.KEYCODE_ENTER) {
                 newStepLength = Float.parseFloat(changedText.toString());
-                // Rescale path
+                // Rescale the previewed path.
                 sensorFusion.redrawPath(newStepLength / averageStepLength);
                 averageStepLengthText.setText(getString(R.string.averageStepLgn)
                         + ": " + String.format("%.2f", newStepLength));
@@ -170,7 +171,6 @@ private void validateAndUpload() {
         TrajectoryValidator.ValidationResult result = sensorFusion.validateTrajectory();
 
         if (result.isClean()) {
-            // All checks passed — upload immediately
             Log.i("CorrectionFragment", "Trajectory validation passed, uploading");
             sensorFusion.sendTrajectoryToCloud();
             return;
@@ -180,7 +180,6 @@ private void validateAndUpload() {
         Log.w("CorrectionFragment", "Trajectory quality issues:\n" + summary);
 
         if (!result.isPassed()) {
-            // Blocking errors exist — warn strongly but still allow upload
             new AlertDialog.Builder(requireContext())
                     .setTitle(R.string.validation_error_title)
                     .setMessage(getString(R.string.validation_error_message, summary))
@@ -193,7 +192,6 @@ private void validateAndUpload() {
                     .setCancelable(false)
                     .show();
         } else {
-            // Only warnings — show lighter dialog
             new AlertDialog.Builder(requireContext())
                     .setTitle(R.string.validation_warning_title)
                     .setMessage(getString(R.string.validation_warning_message, summary))
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..955a087f 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
@@ -302,6 +302,18 @@ private void updateUIandPosition() {
             }
         }
 
+        if (trajectoryMapFragment != null) {
+            LatLng newWifi = sensorFusion.popLatestWifiLocation();
+            if (newWifi != null) {
+                trajectoryMapFragment.drawWifiUpdatePoint(newWifi);
+            }
+
+            LatLng newGnss = sensorFusion.popLatestGnssLocation();
+            if (newGnss != null) {
+                trajectoryMapFragment.drawGnssUpdatePoint(newGnss);
+            }
+        }
+        // ===================================
         // Update previous
         previousPosX = pdrValues[0];
         previousPosY = pdrValues[1];
diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java
index 479ea51b..ff6771bc 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
@@ -81,6 +81,8 @@ public class TrajectoryMapFragment extends Fragment {
     private static final String TAG = "TrajectoryMapFragment";
     private static final long AUTO_FLOOR_DEBOUNCE_MS = 3000;
     private static final long AUTO_FLOOR_CHECK_INTERVAL_MS = 1000;
+    private static final int MAX_AUTO_FLOOR_STEP_WITHOUT_ELEVATOR = 1;
+    private static final int MAX_WIFI_BARO_FLOOR_DISAGREEMENT = 1;
     private Handler autoFloorHandler;
     private Runnable autoFloorTask;
     private int lastCandidateFloor = Integer.MIN_VALUE;
@@ -96,6 +98,8 @@ public class TrajectoryMapFragment extends Fragment {
     private TextView floorLabel;
     private Button switchColorButton;
     private Polygon buildingPolygon;
+    private long lastAuthoritativeTime = 0;
+    private boolean hasAuthoritativeSource = false;
 
 
     public TrajectoryMapFragment() {
@@ -305,7 +309,6 @@ public void onNothingSelected(AdapterView<?> parent) {}
      * and append to polyline if the user actually moved.
      *
      * @param newLocation The new location to plot.
-     * @param orientation The user’s heading (e.g. from sensor fusion).
      */
     public void updateUserLocation(@NonNull LatLng newLocation, float orientation) {
         if (gMap == null) return;
@@ -652,15 +655,8 @@ private void applyImmediateFloor() {
         if (sensorFusion == null || indoorMapManager == null) return;
         if (!indoorMapManager.getIsIndoorMapSet()) return;
 
-        int candidateFloor;
-        if (sensorFusion.getLatLngWifiPositioning() != null) {
-            candidateFloor = sensorFusion.getWifiFloor();
-        } else {
-            float elevation = sensorFusion.getElevation();
-            float floorHeight = indoorMapManager.getFloorHeight();
-            if (floorHeight <= 0) return;
-            candidateFloor = Math.round(elevation / floorHeight);
-        }
+        Integer candidateFloor = resolveAutoFloorCandidate();
+        if (candidateFloor == null) return;
 
         indoorMapManager.setCurrentFloor(candidateFloor, true);
         updateFloorLabel();
@@ -690,20 +686,10 @@ private void evaluateAutoFloor() {
         if (sensorFusion == null || indoorMapManager == null) return;
         if (!indoorMapManager.getIsIndoorMapSet()) return;
 
-        int candidateFloor;
-
-        // Priority 1: WiFi-based floor (only if WiFi positioning has returned data)
-        if (sensorFusion.getLatLngWifiPositioning() != null) {
-            candidateFloor = sensorFusion.getWifiFloor();
-        } else {
-            // Fallback: barometric elevation estimate
-            float elevation = sensorFusion.getElevation();
-            float floorHeight = indoorMapManager.getFloorHeight();
-            if (floorHeight <= 0) return;
-            candidateFloor = Math.round(elevation / floorHeight);
-        }
+        Integer candidateFloor = resolveAutoFloorCandidate();
+        if (candidateFloor == null) return;
+        boolean hasAuthoritativeSource = sensorFusion.getLatLngWifiPositioning() != null;
 
-        // Debounce: require the same floor reading for AUTO_FLOOR_DEBOUNCE_MS
         long now = SystemClock.elapsedRealtime();
         if (candidateFloor != lastCandidateFloor) {
             lastCandidateFloor = candidateFloor;
@@ -712,12 +698,85 @@ 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
+            if (hasAuthoritativeSource || (now - lastAuthoritativeTime > 15000)) {
+                indoorMapManager.setCurrentFloor(candidateFloor, true);
+                updateFloorLabel();
+
+                if (hasAuthoritativeSource) {
+                    lastAuthoritativeTime = now;
+                }
+            }
             lastCandidateTime = now;
         }
     }
 
+    private Integer resolveAutoFloorCandidate() {
+        boolean hasWifiFloor = sensorFusion.getLatLngWifiPositioning() != null;
+        Integer wifiFloor = hasWifiFloor ? sensorFusion.getWifiFloor() : null;
+        Integer barometricFloor = getBarometricFloorEstimate();
+        boolean elevatorDetected = sensorFusion.getElevator();
+
+        if (hasWifiFloor) {
+            if (barometricFloor != null
+                    && Math.abs(wifiFloor - barometricFloor) > MAX_WIFI_BARO_FLOOR_DISAGREEMENT
+                    && !elevatorDetected) {
+                Log.d(TAG, "Skipping WiFi auto-floor candidate due to large barometric disagreement: wifi="
+                        + wifiFloor + ", barometric=" + barometricFloor);
+            } else if (isPlausibleAutoFloorCandidate(wifiFloor, elevatorDetected)) {
+                Log.d(TAG, "Auto-floor candidate from WiFi: " + wifiFloor);
+                return wifiFloor;
+            } else {
+                Log.d(TAG, "Skipping implausible WiFi auto-floor candidate: " + wifiFloor);
+            }
+        }
+
+        if (barometricFloor != null && isPlausibleAutoFloorCandidate(barometricFloor, elevatorDetected)) {
+            Log.d(TAG, "Auto-floor candidate from barometer: " + barometricFloor);
+            return barometricFloor;
+        }
+
+        if (barometricFloor != null) {
+            Log.d(TAG, "Skipping implausible barometric auto-floor candidate: " + barometricFloor);
+        }
+        return null;
+    }
+
+    private Integer getBarometricFloorEstimate() {
+        float floorHeight = indoorMapManager.getFloorHeight();
+        if (floorHeight <= 0) return null;
+        return Math.round(sensorFusion.getElevation() / floorHeight);
+    }
+
+    private boolean isPlausibleAutoFloorCandidate(int candidateFloor, boolean elevatorDetected) {
+        int currentLogicalFloor = indoorMapManager.getCurrentFloor() - indoorMapManager.getAutoFloorBias();
+        if (candidateFloor == currentLogicalFloor) return true;
+        if (elevatorDetected) return Math.abs(candidateFloor - currentLogicalFloor) <= 2;
+        return Math.abs(candidateFloor - currentLogicalFloor) <= MAX_AUTO_FLOOR_STEP_WITHOUT_ELEVATOR;
+    }
+
+    /**
+     */
+    public void drawGnssUpdatePoint(LatLng location) {
+        if (gMap == null || location == null) return;
+        gMap.addCircle(new CircleOptions()
+                .center(location)
+                .radius(0.75)
+                .fillColor(Color.BLUE)
+                .strokeWidth(0)
+                .zIndex(2));
+    }
+
+    /**
+     */
+    public void drawWifiUpdatePoint(LatLng location) {
+        if (gMap == null || location == null) return;
+        gMap.addCircle(new CircleOptions()
+                .center(location)
+                .radius(0.75)
+                .fillColor(Color.GREEN)
+                .strokeWidth(0)
+                .zIndex(3));
+    }
+
     //endregion
 }
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..15d55775 100644
--- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java
+++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java
@@ -6,12 +6,14 @@
 import android.os.SystemClock;
 import android.util.Log;
 
+import com.openpositioning.PositionMe.data.remote.FloorplanApiClient;
 import com.openpositioning.PositionMe.utils.PathView;
 import com.openpositioning.PositionMe.utils.PdrProcessing;
 
 import java.util.ArrayList;
 import java.util.HashMap;
 import java.util.List;
+import com.google.android.gms.maps.model.LatLng;
 
 /**
  * Handles sensor event dispatching for all registered movement sensors.
@@ -24,7 +26,10 @@ public class SensorEventHandler {
 
     private static final float ALPHA = 0.8f;
     private static final long LARGE_GAP_THRESHOLD_MS = 500;
-
+    private static final float MAP_MATCH_SUBSTEP_METERS = 0.75f;
+    private static final int MAX_MAP_MATCH_SUBDIVISIONS = 2;
+    private static final long TURN_CONFIRM_WINDOW_MS = 850;
+    private static final float TURN_CONFIRM_GYRO_THRESHOLD_RAD_S = 0.6f;
     private final SensorState state;
     private final PdrProcessing pdrProcessing;
     private final PathView pathView;
@@ -34,10 +39,14 @@ public class SensorEventHandler {
     private final HashMap<Integer, Long> lastEventTimestamps = new HashMap<>();
     private final HashMap<Integer, Integer> eventCounts = new HashMap<>();
     private long lastStepTime = 0;
+    private long lastHeadingMotionTimestamp = 0;
     private long bootTime;
 
     // Acceleration magnitude buffer between steps
     private final List<Double> accelMagnitude = new ArrayList<>();
+    // Track the previous raw PDR position to compute per-step deltas.
+    private float lastPdrX = 0f;
+    private float lastPdrY = 0f;
 
     /**
      * Creates a new SensorEventHandler.
@@ -98,7 +107,9 @@ public void handleSensorEvent(SensorEvent sensorEvent) {
                 state.angularVelocity[0] = sensorEvent.values[0];
                 state.angularVelocity[1] = sensorEvent.values[1];
                 state.angularVelocity[2] = sensorEvent.values[2];
-
+                if (Math.abs(sensorEvent.values[2]) >= TURN_CONFIRM_GYRO_THRESHOLD_RAD_S) {
+                    lastHeadingMotionTimestamp = currentTime;
+                }
             case Sensor.TYPE_LINEAR_ACCELERATION:
                 state.filteredAcc[0] = sensorEvent.values[0];
                 state.filteredAcc[1] = sensorEvent.values[1];
@@ -164,21 +175,63 @@ public void handleSensorEvent(SensorEvent sensorEvent) {
                                 "stepDetection triggered, accelMagnitude size = "
                                         + accelMagnitude.size());
                     }
-
-                    float[] newCords = this.pdrProcessing.updatePdr(
+                    float[] rawPdrCords = this.pdrProcessing.updatePdr(
                             stepTime,
                             this.accelMagnitude,
-                            state.orientation[0]
+                            state.orientation[0],
+                            state.angularVelocity[2],
+                            currentTime - lastHeadingMotionTimestamp <= TURN_CONFIRM_WINDOW_MS
                     );
-
                     this.accelMagnitude.clear();
+                    // Derive the raw PDR delta for this detected step.
+                    float deltaX = rawPdrCords[0] - lastPdrX;
+                    float deltaY = rawPdrCords[1] - lastPdrY;
+
+                    // Persist the raw PDR position for the next step.
+                    lastPdrX = rawPdrCords[0];
+                    lastPdrY = rawPdrCords[1];
+
+                    // Run prediction and map matching before drawing the fused position.
+                    com.openpositioning.PositionMe.fusion.ParticleFilter pf = SensorFusion.getInstance().getParticleFilter();
+                    float[] finalCordsToDraw = rawPdrCords;
+                    if (pf != null && (deltaX != 0 || deltaY != 0)) {
+                        // Split larger moves into a small number of map-matching substeps.
+                        int subdivisions = Math.max(1, Math.min(MAX_MAP_MATCH_SUBDIVISIONS,
+                                (int) Math.ceil(Math.hypot(deltaX, deltaY) / MAP_MATCH_SUBSTEP_METERS)));
+
+                        // Apply area-based horizontal motion scaling and wall constraints.
+                        List<FloorplanApiClient.MapShapeFeature> walls = SensorFusion.getInstance().getCurrentWalls();
+                        LatLng startLocation = new LatLng(state.startLocation[0], state.startLocation[1]);
+                        float horizontalMovementScale = pf.getHorizontalMovementScale(
+                                walls, startLocation, state.elevator);
+                        float adjustedDeltaX = deltaX * horizontalMovementScale;
+                        float adjustedDeltaY = deltaY * horizontalMovementScale;
+                        for (int i = 0; i < subdivisions; i++) {
+                            pf.predict(new com.openpositioning.PositionMe.fusion.PDRMovement(adjustedDeltaX, adjustedDeltaY), subdivisions);
+                            pf.applyMapMatching(walls, startLocation);
+                        }
+
+                        // Draw the fused position instead of the raw PDR position.
+                        com.openpositioning.PositionMe.fusion.Position fusedPosition =
+                                pf.getEstimatedPosition(walls, startLocation);
+
+                        // Debug logging for comparing raw PDR and particle-filter output.
+                        finalCordsToDraw = new float[]{fusedPosition.x, fusedPosition.y};
+
+                        // Debug logging for comparing raw PDR and particle-filter output.
+                        Log.d("ParticleFilter", "X: " + rawPdrCords[0] + " Y: " + rawPdrCords[1]);
+                        Log.d("ParticleFilter", "X: " + fusedPosition.x + " Y: " + fusedPosition.y);
+                    }
 
                     if (recorder.isRecording()) {
-                        this.pathView.drawTrajectory(newCords);
+                        // Draw the fused trajectory on screen.
+                        this.pathView.drawTrajectory(finalCordsToDraw);
+
                         state.stepCounter++;
+                        // Keep the recorded PDR trace in the raw local coordinate frame.
                         recorder.addPdrData(
                                 SystemClock.uptimeMillis() - bootTime,
-                                newCords[0], newCords[1]);
+                                rawPdrCords[0], rawPdrCords[1]);
                     }
                     break;
                 }
@@ -204,4 +257,5 @@ public void logSensorFrequencies() {
     void resetBootTime(long newBootTime) {
         this.bootTime = newBootTime;
     }
+
 }
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..6a5a7339 100644
--- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java
+++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java
@@ -12,6 +12,7 @@
 import android.os.Looper;
 import android.os.SystemClock;
 import android.widget.Toast;
+import android.util.Log;
 
 import androidx.annotation.NonNull;
 import androidx.preference.PreferenceManager;
@@ -52,6 +53,9 @@ public class SensorFusion implements SensorEventListener {
     //region Static variables
     private static final SensorFusion sensorFusion = new SensorFusion();
     //endregion
+    private static final double START_WIFI_WEIGHT = 0.7;
+    private static final double START_GNSS_WEIGHT = 0.3;
+    private static final double MAX_START_BLEND_DISTANCE_METERS = 25.0;
 
     //region Instance variables
     private Context appContext;
@@ -87,6 +91,7 @@ public class SensorFusion implements SensorEventListener {
     // PDR and path
     private PdrProcessing pdrProcessing;
     private PathView pathView;
+    private com.openpositioning.PositionMe.fusion.ParticleFilter particleFilter;
 
     // Sensor registration latency setting
     long maxReportLatencyNs = 0;
@@ -94,6 +99,9 @@ public class SensorFusion implements SensorEventListener {
     // Floorplan API cache (latest result from start-location step)
     private final Map<String, FloorplanApiClient.BuildingInfo> floorplanBuildingCache =
             new HashMap<>();
+    private List<FloorplanApiClient.MapShapeFeature> currentWalls = null;
+    private LatLng latestWifiLocation = null;
+    private LatLng latestGnssLocation = null;
     //endregion
 
     //region Initialisation
@@ -121,7 +129,6 @@ public static SensorFusion getInstance() {
      * the system for data collection.</p>
      *
      * @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.
@@ -193,6 +200,12 @@ public void update(Object[] objList) {
                             "WiFi RTT is not supported on this device",
                             Toast.LENGTH_LONG).show());
         }
+        // Initialise the particle filter once. Particle placement still happens later
+        // after map bounds become available.
+        this.particleFilter = new com.openpositioning.PositionMe.fusion.ParticleFilter(1000);
+        // ------------------------------
+        this.particleFilter = new com.openpositioning.PositionMe.fusion.ParticleFilter(1000);
+        // ------------------------------
     }
 
     //endregion
@@ -209,9 +222,12 @@ public void onSensorChanged(SensorEvent sensorEvent) {
         eventHandler.handleSensorEvent(sensorEvent);
     }
 
-    /** {@inheritDoc} */
+    /**
+     * {@inheritDoc}
+     */
     @Override
-    public void onAccuracyChanged(Sensor sensor, int i) {}
+    public void onAccuracyChanged(Sensor sensor, int i) {
+    }
 
     //endregion
 
@@ -488,8 +504,64 @@ public float[] getGNSSLatitude(boolean start) {
      * @param startPosition contains the initial location set by the user
      */
     public void setStartGNSSLatitude(float[] startPosition) {
-        state.startLocation[0] = startPosition[0];
-        state.startLocation[1] = startPosition[1];
+        LatLng blendedStart = resolveBlendedStartLocation(startPosition);
+        state.startLocation[0] = (float) blendedStart.latitude;
+        state.startLocation[1] = (float) blendedStart.longitude;
+
+        if (this.particleFilter != null) {
+            // The local coordinate origin is the user-selected start point.
+            // Initialise particles within a symmetric uncertainty box around that point.
+            float initialUncertainty = 10.0f; // 10-meter half-width
+
+            com.openpositioning.PositionMe.fusion.MapBounds bounds =
+                    new com.openpositioning.PositionMe.fusion.MapBounds(
+                            -initialUncertainty,  // minX
+                            initialUncertainty,  // maxX
+                            -initialUncertainty,  // minY
+                            initialUncertainty   // maxY
+                    );
+
+            // Initialise particles around the selected start point.
+            this.particleFilter.initialize(bounds);
+
+            Log.d("ParticleFilter", "Particle filter initialised around the selected start point.");
+        }
+        // Particle filter start-up is complete.
+    }
+
+    private LatLng resolveBlendedStartLocation(float[] startPosition) {
+        LatLng gnssStart = new LatLng(startPosition[0], startPosition[1]);
+        LatLng wifiStart = getLatLngWifiPositioning();
+
+        if (wifiStart == null) {
+            Log.d("StartLocation", "Using GNSS-only start location");
+            return gnssStart;
+        }
+
+        double distanceMeters = distanceMeters(gnssStart, wifiStart);
+        if (distanceMeters > MAX_START_BLEND_DISTANCE_METERS) {
+            Log.d("StartLocation", "Skipping WiFi/GNSS start blending, distance too large: "
+                    + distanceMeters);
+            return gnssStart;
+        }
+
+        double blendedLat = wifiStart.latitude * START_WIFI_WEIGHT + gnssStart.latitude * START_GNSS_WEIGHT;
+        double blendedLng = wifiStart.longitude * START_WIFI_WEIGHT + gnssStart.longitude * START_GNSS_WEIGHT;
+        Log.d("StartLocation", "Using blended start location, wifiWeight="
+                + START_WIFI_WEIGHT + ", gnssWeight=" + START_GNSS_WEIGHT
+                + ", distance=" + distanceMeters);
+        return new LatLng(blendedLat, blendedLng);
+    }
+
+    private double distanceMeters(LatLng a, LatLng b) {
+        double radius = 6378137.0;
+        double lat1 = Math.toRadians(a.latitude);
+        double lat2 = Math.toRadians(b.latitude);
+        double dLat = lat2 - lat1;
+        double dLng = Math.toRadians(b.longitude - a.longitude);
+        double x = dLng * Math.cos((lat1 + lat2) / 2.0);
+        double y = dLat;
+        return Math.sqrt(x * x + y * y) * radius;
     }
 
     /**
@@ -645,8 +717,102 @@ public void onLocationChanged(@NonNull Location location) {
             state.latitude = (float) location.getLatitude();
             state.longitude = (float) location.getLongitude();
             recorder.addGnssData(location);
+
+            // Apply GNSS corrections only when the reported accuracy is acceptable.
+            com.openpositioning.PositionMe.fusion.ParticleFilter pf = getParticleFilter();
+
+            if (pf != null && state.startLocation != null && state.startLocation[0] != 0.0f) {
+
+                float realAccuracy = location.getAccuracy();
+
+                // Ignore poor indoor GNSS fixes to avoid dragging the trajectory away.
+                // Indoor GPS errors often become large enough to be harmful instead of helpful.
+                // When that happens, skip this update entirely.
+                // The threshold is currently set to 20 meters.
+                if (realAccuracy > 20.0f) {
+                    android.util.Log.d("ParticleFilter", "GNSS fix rejected due to poor accuracy: " + realAccuracy + "m");
+                    return; // Skip this GNSS correction.
+                }
+
+                // Convert the current GNSS fix into the local map coordinate system.
+                // Convert the current GNSS fix into the local map coordinate system.
+                double lat0 = state.startLocation[0];
+                double lng0 = state.startLocation[1];
+
+                double lat = location.getLatitude();
+                double lng = location.getLongitude();
+
+                double R = 6378137.0;
+                double dLat = Math.toRadians(lat - lat0);
+                double dLng = Math.toRadians(lng - lng0);
+                float x = (float) (R * dLng * Math.cos(Math.toRadians(lat0)));
+                float y = (float) (R * dLat);
+
+                // Use the reported accuracy, but keep a lower bound for numerical stability.
+                // This avoids overly confident updates from unrealistic near-zero values.
+                float usedSigma = Math.max(realAccuracy, 10.0f);
+
+                com.openpositioning.PositionMe.fusion.Measurement gnssMeasurement =
+                        new com.openpositioning.PositionMe.fusion.Measurement(x, y, usedSigma);
+
+                // Feed the accepted GNSS measurement into the particle filter.
+                // Reuse the current map state so blocked particles can be down-weighted.
+                LatLng startLocation = new LatLng(state.startLocation[0], state.startLocation[1]);
+                LatLng gnssLoc = new LatLng(location.getLatitude(), location.getLongitude());
+                setLatestGnssLocation(gnssLoc);
+                pf.updateWeights(gnssMeasurement, currentWalls, startLocation);
+                pf.resample();
+
+                android.util.Log.d("ParticleFilter", "GNSS correction applied with sigma=" + usedSigma + "m");
+
+                android.util.Log.d("ParticleFilter", "GNSS correction applied with sigma=" + usedSigma + "m");
+            }
         }
     }
 
+    // Returns the shared particle filter instance.
+    // Returns the shared particle filter instance.
+    public com.openpositioning.PositionMe.fusion.ParticleFilter getParticleFilter() {
+        return this.particleFilter;
+    }
+
+    // Accessors for the current map state used by map matching and drawing.
+    public void setCurrentWalls(List<FloorplanApiClient.MapShapeFeature> walls) {
+        this.currentWalls = walls;
+    }
+
+    public List<FloorplanApiClient.MapShapeFeature> getCurrentWalls() {
+        return this.currentWalls;
+    }
+
+    public LatLng getStartLocationLatLng() {
+        if (state.startLocation == null || state.startLocation.length < 2) {
+            return null;
+        }
+        if (state.startLocation[0] == 0.0f && state.startLocation[1] == 0.0f) {
+            return null;
+        }
+        return new LatLng(state.startLocation[0], state.startLocation[1]);
+    }
+
+    public void setLatestWifiLocation(LatLng loc) {
+        this.latestWifiLocation = loc;
+    }
+
+    public LatLng popLatestWifiLocation() {
+        LatLng loc = this.latestWifiLocation;
+        this.latestWifiLocation = null; // Clear after consumption so the marker is drawn once.
+        return loc;
+    }
+
+    public void setLatestGnssLocation(LatLng loc) {
+        this.latestGnssLocation = loc;
+    }
+
+    public LatLng popLatestGnssLocation() {
+        LatLng loc = this.latestGnssLocation;
+        this.latestGnssLocation = null; // Clear after consumption so the marker is drawn once.
+        return loc;
+    }
     //endregion
-}
+}
\ No newline at end of file
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..e7359e6a 100644
--- a/app/src/main/java/com/openpositioning/PositionMe/sensors/WiFiPositioning.java
+++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/WiFiPositioning.java
@@ -118,55 +118,62 @@ public void request(JSONObject jsonWifiFeatures) {
 
 
     /**
-     * Creates a POST request using the WiFi fingerprint to obtain user's location
-     * The POST request is issued to https://openpositioning.org/api/position/fine
-     * (the openpositioning API) with the WiFI fingerprint passed as the parameter.
+     * Creates a POST request using the WiFi fingerprint to obtain the user's location.
      *
-     * The response of the post request returns the coordinates of the WiFi position
-     * along with the floor of the building the user is at though a callback.
+     * <p>The POST request is issued to {@code https://openpositioning.org/api/position/fine}
+     * (the openpositioning API) with the WiFi fingerprint passed as the parameter.
      *
-     * A try and catch block along with error Logs have been added to keep a record of error's
-     * obtained while handling POST requests (for better maintainability and secure programming)
+     * <p>The response of the POST request returns the coordinates of the WiFi position
+     * along with the floor of the building the user is at through a callback.
      *
-     * @param jsonWifiFeatures WiFi Fingerprint from device
-     * @param callback callback function to allow user to use location when ready
+     * <p>A try-catch block along with error logs have been added to keep a record of errors
+     * obtained while handling POST requests for better maintainability and secure programming.
+     *
+     * @param jsonWifiFeatures WiFi fingerprint from the device
+     * @param callback callback function to handle the location response or errors
      */
-    public void request( JSONObject jsonWifiFeatures, final VolleyCallback callback) {
-        // Creating the POST request using WiFi fingerprint (a JSON object)
+    public void request(JSONObject jsonWifiFeatures, final VolleyCallback callback) {
+        // Create the POST request using the WiFi fingerprint (a JSON object).
         JsonObjectRequest jsonObjectRequest = new JsonObjectRequest(
                 Request.Method.POST, url, jsonWifiFeatures,
                 response -> {
                     try {
-                        Log.d("jsonObject",response.toString());
-                        wifiLocation = new LatLng(response.getDouble("lat"),response.getDouble("lon"));
+                        Log.d("WifiSuccess", "Success! Server returned coordinates.");
+                        Log.d("jsonObject", response.toString());
+
+                        wifiLocation = new LatLng(response.getDouble("lat"), response.getDouble("lon"));
                         floor = response.getInt("floor");
-                        callback.onSuccess(wifiLocation,floor);
+                        callback.onSuccess(wifiLocation, floor);
                     } catch (JSONException e) {
-                        Log.e("jsonErrors","Error parsing response: "+e.getMessage()+" "+ response);
+                        Log.e("jsonErrors", "Error parsing response: " + e.getMessage() + " " + response);
                         callback.onError("Error parsing response: " + e.getMessage());
                     }
                 },
                 error -> {
-                    // Validation Error
-                    if (error.networkResponse!=null && error.networkResponse.statusCode==422){
-                        Log.e("WiFiPositioning", "Validation Error "+ error.getMessage());
-                        callback.onError( "Validation Error (422): "+ error.getMessage());
-                    }
-                    // Other Errors
-                    else{
-                        // When Response code is available
-                        if (error.networkResponse!=null) {
-                            Log.e("WiFiPositioning","Response Code: " + error.networkResponse.statusCode + ", " + error.getMessage());
-                            callback.onError("Response Code: " + error.networkResponse.statusCode + ", " + error.getMessage());
-                        }
-                        else{
-                            Log.e("WiFiPositioning","Error message: " + error.getMessage());
-                            callback.onError("Error message: " + error.getMessage());
+                    if (error.networkResponse != null) {
+                        int statusCode = error.networkResponse.statusCode;
+                        String responseBody = "";
+
+                        if (error.networkResponse.data != null) {
+                            try {
+                                responseBody = new String(error.networkResponse.data, "UTF-8");
+                            } catch (Exception e) {
+                                responseBody = "";
+                            }
                         }
+
+                        Log.e("WifiProbe", "HTTP Status Code: " + statusCode + ", Server Response: " + responseBody);
+                        callback.onError("Status: " + statusCode + " Body: " + responseBody);
+                    } else {
+                        Log.e("WifiProbe", "Network layer error: " + error.toString());
+                        callback.onError("Network Error: " + error.toString());
                     }
                 }
         );
-        // Adds the request to the request queue
+
+        Log.d("WifiProbe", "==== WiFi Fingerprint to be sent: ==== \n" + jsonWifiFeatures.toString());
+
+        // Add the request to the request queue.
         requestQueue.add(jsonObjectRequest);
     }
 
@@ -178,4 +185,4 @@ public interface VolleyCallback {
         void onError(String message);
     }
 
-}
\ No newline at end of file
+}
diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java
index 1edfd68a..9b3f25ff 100644
--- a/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java
+++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java
@@ -3,6 +3,7 @@
 import android.util.Log;
 
 import com.google.android.gms.maps.model.LatLng;
+import com.openpositioning.PositionMe.data.remote.FloorplanApiClient;
 
 import org.json.JSONException;
 import org.json.JSONObject;
@@ -18,11 +19,13 @@
  * replacing the role previously held by {@link SensorFusion}.</p>
  *
  * @see WifiDataProcessor the observable that triggers WiFi scan updates
- * @see WiFiPositioning   the API client for WiFi-based positioning
+ * @see WiFiPositioning the API client for WiFi-based positioning
  */
 public class WifiPositionManager implements Observer {
 
     private static final String WIFI_FINGERPRINT = "wf";
+    private static final float WIFI_FUSION_MAX_DISTANCE_METERS = 10.0f;
+    private static final double WIFI_FUSION_SIGMA_METERS = 12.0;
 
     private final WiFiPositioning wiFiPositioning;
     private final TrajectoryRecorder recorder;
@@ -32,10 +35,9 @@ public class WifiPositionManager implements Observer {
      * Creates a new WifiPositionManager.
      *
      * @param wiFiPositioning WiFi positioning API client
-     * @param recorder        trajectory recorder for writing WiFi fingerprints
+     * @param recorder trajectory recorder for writing WiFi fingerprints
      */
-    public WifiPositionManager(WiFiPositioning wiFiPositioning,
-                               TrajectoryRecorder recorder) {
+    public WifiPositionManager(WiFiPositioning wiFiPositioning, TrajectoryRecorder recorder) {
         this.wiFiPositioning = wiFiPositioning;
         this.recorder = recorder;
     }
@@ -49,9 +51,14 @@ public WifiPositionManager(WiFiPositioning wiFiPositioning,
      */
     @Override
     public void update(Object[] wifiList) {
-        this.wifiList = Stream.of(wifiList).map(o -> (Wifi) o).collect(Collectors.toList());
+        Log.d("WifiProbe", "Received WiFi scan results.");
+        this.wifiList = Stream.of(wifiList)
+                .map(o -> (Wifi) o)
+                .filter(w -> w.getLevel() < -40)
+                .collect(Collectors.toList());
         recorder.addWifiFingerprint(this.wifiList);
-        createWifiPositioningRequest();
+        // Use the callback-based request path so the fusion layer can react to the result.
+        createWifiPositionRequestCallback();
     }
 
     /**
@@ -67,7 +74,8 @@ private void createWifiPositioningRequest() {
             wifiFingerPrint.put(WIFI_FINGERPRINT, wifiAccessPoints);
             this.wiFiPositioning.request(wifiFingerPrint);
         } catch (JSONException e) {
-            Log.e("jsonErrors", "Error creating json object" + e.toString());
+            Log.e("WifiProbe", "Failed to build WiFi positioning JSON: " + e);
+            Log.e("jsonErrors", "Error creating json object" + e);
         }
     }
 
@@ -82,19 +90,68 @@ private void createWifiPositionRequestCallback() {
             }
             JSONObject wifiFingerPrint = new JSONObject();
             wifiFingerPrint.put(WIFI_FINGERPRINT, wifiAccessPoints);
+            Log.d("WifiProbe", "Sending WiFi positioning request.");
             this.wiFiPositioning.request(wifiFingerPrint, new WiFiPositioning.VolleyCallback() {
                 @Override
                 public void onSuccess(LatLng wifiLocation, int floor) {
-                    // Handle the success response
+                    Log.d("WifiSuccess", "WiFi location received, updating particle filter.");
+
+                    com.openpositioning.PositionMe.fusion.ParticleFilter pf =
+                            SensorFusion.getInstance().getParticleFilter();
+
+                    if (pf != null && wifiLocation != null) {
+                        // Use the start location as the origin of the local coordinate frame.
+                        float[] startCoords = SensorFusion.getInstance().getGNSSLatitude(true);
+                        double lat0 = startCoords[0];
+                        double lng0 = startCoords[1];
+
+                        // Convert the WiFi location from WGS84 to local x/y coordinates in meters.
+                        double radius = 6378137.0;
+                        double lat = wifiLocation.latitude;
+                        double lng = wifiLocation.longitude;
+                        double dLat = Math.toRadians(lat - lat0);
+                        double dLng = Math.toRadians(lng - lng0);
+                        float x = (float) (radius * dLng * Math.cos(Math.toRadians(lat0)));
+                        float y = (float) (radius * dLat);
+
+                        // Build a measurement with the tuned WiFi uncertainty.
+                        com.openpositioning.PositionMe.fusion.Measurement measurement =
+                                new com.openpositioning.PositionMe.fusion.Measurement(
+                                        x, y, WIFI_FUSION_SIGMA_METERS);
+
+                        // Reuse the active map state so WiFi fusion respects map constraints.
+                        LatLng startLocation = new LatLng(startCoords[0], startCoords[1]);
+                        List<FloorplanApiClient.MapShapeFeature> walls =
+                                SensorFusion.getInstance().getCurrentWalls();
+
+                        SensorFusion.getInstance().setLatestWifiLocation(wifiLocation);
+                        com.openpositioning.PositionMe.fusion.Position estimatedPosition =
+                                pf.getEstimatedPosition(walls, startLocation);
+                        double wifiDistance = Math.hypot(
+                                estimatedPosition.x - x, estimatedPosition.y - y);
+
+                        if (wifiDistance <= WIFI_FUSION_MAX_DISTANCE_METERS) {
+                            pf.updateWeights(measurement, walls, startLocation);
+                            pf.resample();
+                        } else {
+                            Log.d("WifiSuccess",
+                                    "Skipping WiFi fusion, distance too large: " + wifiDistance);
+                        }
+
+                        com.openpositioning.PositionMe.fusion.Position pos =
+                                pf.getEstimatedPosition();
+                        Log.d("ParticleFilter",
+                                "Fused WiFi position X: " + pos.x + " Y: " + pos.y);
+                    }
                 }
 
                 @Override
                 public void onError(String message) {
-                    // Handle the error response
+                    Log.e("WifiProbe", "WiFi positioning request failed: " + message);
                 }
             });
         } catch (JSONException e) {
-            Log.e("jsonErrors", "Error creating json object" + e.toString());
+            Log.e("jsonErrors", "Error creating json object" + e);
         }
     }
 
diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/GeometryUtils.java b/app/src/main/java/com/openpositioning/PositionMe/utils/GeometryUtils.java
new file mode 100644
index 00000000..6a8c5e62
--- /dev/null
+++ b/app/src/main/java/com/openpositioning/PositionMe/utils/GeometryUtils.java
@@ -0,0 +1,160 @@
+package com.openpositioning.PositionMe.utils;
+
+import com.google.android.gms.maps.model.LatLng;
+
+import java.util.List;
+
+/** Geometry helpers used by map matching and indoor polygon checks. */
+public class GeometryUtils {
+
+    private static final double EPSILON = 1e-9;
+
+    /**
+     * Returns whether a point lies inside a polygon using ray casting.
+     *
+     * @param point point to test
+     * @param polygon polygon vertices
+     * @return {@code true} if the point lies inside the polygon
+     */
+    public static boolean isPointInPolygon(LatLng point, List<LatLng> polygon) {
+        if (polygon == null || polygon.isEmpty()) {
+            return false;
+        }
+
+        int intersections = 0;
+        for (int i = 0, j = polygon.size() - 1; i < polygon.size(); j = i++) {
+            LatLng p1 = polygon.get(i);
+            LatLng p2 = polygon.get(j);
+
+            if (point.longitude == p1.longitude && point.latitude == p1.latitude) {
+                return true;
+            }
+
+            if (((p1.latitude > point.latitude) != (p2.latitude > point.latitude))
+                    && (point.longitude
+                    < (p2.longitude - p1.longitude) * (point.latitude - p1.latitude)
+                    / (p2.latitude - p1.latitude) + p1.longitude)) {
+                intersections++;
+            }
+        }
+        return intersections % 2 == 1;
+    }
+
+    /**
+     * Returns whether two line segments intersect.
+     *
+     * @param a first endpoint of the first segment
+     * @param b second endpoint of the first segment
+     * @param c first endpoint of the second segment
+     * @param d second endpoint of the second segment
+     * @return {@code true} if the segments intersect
+     */
+    public static boolean doSegmentsIntersect(LatLng a, LatLng b, LatLng c, LatLng d) {
+        double d1 = direction(c, d, a);
+        double d2 = direction(c, d, b);
+        double d3 = direction(a, b, c);
+        double d4 = direction(a, b, d);
+
+        if (((d1 > EPSILON && d2 < -EPSILON) || (d1 < -EPSILON && d2 > EPSILON))
+                && ((d3 > EPSILON && d4 < -EPSILON) || (d3 < -EPSILON && d4 > EPSILON))) {
+            return true;
+        }
+
+        if (Math.abs(d1) <= EPSILON && onSegment(c, d, a)) {
+            return true;
+        }
+        if (Math.abs(d2) <= EPSILON && onSegment(c, d, b)) {
+            return true;
+        }
+        if (Math.abs(d3) <= EPSILON && onSegment(a, b, c)) {
+            return true;
+        }
+        if (Math.abs(d4) <= EPSILON && onSegment(a, b, d)) {
+            return true;
+        }
+
+        return false;
+    }
+
+    private static double direction(LatLng a, LatLng b, LatLng c) {
+        return (c.longitude - a.longitude) * (b.latitude - a.latitude)
+                - (b.longitude - a.longitude) * (c.latitude - a.latitude);
+    }
+
+    private static boolean onSegment(LatLng a, LatLng b, LatLng p) {
+        return p.longitude <= Math.max(a.longitude, b.longitude) + EPSILON
+                && p.longitude >= Math.min(a.longitude, b.longitude) - EPSILON
+                && p.latitude <= Math.max(a.latitude, b.latitude) + EPSILON
+                && p.latitude >= Math.min(a.latitude, b.latitude) - EPSILON;
+    }
+
+    /**
+     * Returns the planar distance between two geographic points in meters.
+     *
+     * @param a first point
+     * @param b second point
+     * @return approximate distance in meters
+     */
+    public static double distanceMeters(LatLng a, LatLng b) {
+        double radius = 6378137.0;
+        double lat1 = Math.toRadians(a.latitude);
+        double lat2 = Math.toRadians(b.latitude);
+        double dLat = lat2 - lat1;
+        double dLng = Math.toRadians(b.longitude - a.longitude);
+        double x = dLng * Math.cos((lat1 + lat2) / 2.0);
+        double y = dLat;
+        return Math.sqrt(x * x + y * y) * radius;
+    }
+
+    /**
+     * Returns the minimum distance between a point and a polygon in meters.
+     *
+     * @param point point to test
+     * @param polygon polygon vertices
+     * @return distance in meters, or {@link Double#POSITIVE_INFINITY} for an empty polygon
+     */
+    public static double distancePointToPolygonMeters(LatLng point, List<LatLng> polygon) {
+        if (polygon == null || polygon.isEmpty()) {
+            return Double.POSITIVE_INFINITY;
+        }
+        if (polygon.size() >= 3 && isPointInPolygon(point, polygon)) {
+            return 0.0;
+        }
+
+        double minDistance = Double.POSITIVE_INFINITY;
+        for (int i = 0; i < polygon.size(); i++) {
+            LatLng a = polygon.get(i);
+            LatLng b = polygon.get((i + 1) % polygon.size());
+            minDistance = Math.min(minDistance, distancePointToSegmentMeters(point, a, b));
+        }
+        return minDistance;
+    }
+
+    private static double distancePointToSegmentMeters(LatLng point, LatLng a, LatLng b) {
+        double latScale = 111320.0;
+        double lngScale =
+                Math.cos(Math.toRadians((a.latitude + b.latitude + point.latitude) / 3.0)) * 111320.0;
+
+        double px = point.longitude * lngScale;
+        double py = point.latitude * latScale;
+        double ax = a.longitude * lngScale;
+        double ay = a.latitude * latScale;
+        double bx = b.longitude * lngScale;
+        double by = b.latitude * latScale;
+
+        double abx = bx - ax;
+        double aby = by - ay;
+        double apx = px - ax;
+        double apy = py - ay;
+        double lengthSquared = abx * abx + aby * aby;
+
+        double t = lengthSquared <= EPSILON ? 0.0 : (apx * abx + apy * aby) / lengthSquared;
+        t = Math.max(0.0, Math.min(1.0, t));
+
+        double closestX = ax + t * abx;
+        double closestY = ay + t * aby;
+        double dx = px - closestX;
+        double dy = py - closestY;
+        return Math.sqrt(dx * dx + dy * dy);
+    }
+}
diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java b/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java
index f8058603..93032390 100644
--- a/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java
+++ b/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java
@@ -161,15 +161,22 @@ public int getAutoFloorBias() {
     public void setCurrentFloor(int newFloor, boolean autoFloor) {
         if (currentFloorShapes == null || currentFloorShapes.isEmpty()) return;
 
+        int targetIndex = -1;
+
         if (autoFloor) {
-            newFloor += getAutoFloorBias();
+            targetIndex = newFloor + 1;
+        } else {
+            targetIndex = newFloor;
         }
 
-        if (newFloor >= 0 && newFloor < currentFloorShapes.size()
-                && newFloor != this.currentFloor) {
-            this.currentFloor = newFloor;
-            drawFloorShapes(newFloor);
+        if (targetIndex >= 0 && targetIndex < currentFloorShapes.size()) {
+            this.currentFloor = targetIndex;
+            drawFloorShapes(targetIndex);
         }
+        List<FloorplanApiClient.MapShapeFeature> currentFeatures = currentFloorShapes.get(targetIndex).getFeatures();
+        SensorFusion.getInstance().setCurrentWalls(currentFeatures);
+
+        android.util.Log.d("FloorFix", "鎴愬姛鍒囨崲鍒版ゼ灞傜储寮? " + targetIndex + "锛屽苟鏇存柊浜嗗澹佹暟鎹紒");
     }
 
     /**
@@ -235,6 +242,8 @@ private void setBuildingOverlay() {
                 if (currentFloorShapes != null && !currentFloorShapes.isEmpty()) {
                     drawFloorShapes(currentFloor);
                     isIndoorMapSet = true;
+                    List<FloorplanApiClient.MapShapeFeature> currentFeatures = currentFloorShapes.get(currentFloor).getFeatures();
+                    SensorFusion.getInstance().setCurrentWalls(currentFeatures);
                 }
 
             } else if (!inAnyBuilding && isIndoorMapSet) {
@@ -411,4 +420,12 @@ public void setIndicationOfIndoorMap() {
             gMap.addPolyline(new PolylineOptions().color(Color.GREEN).addAll(pts));
         }
     }
+    /**
+     */
+    public List<FloorplanApiClient.MapShapeFeature> getCurrentFloorMapShapes() {
+        if (currentFloorShapes != null && currentFloor >= 0 && currentFloor < currentFloorShapes.size()) {
+            return currentFloorShapes.get(currentFloor).getFeatures();
+        }
+        return null;
+    }
 }
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..39811ba3 100644
--- a/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java
+++ b/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java
@@ -38,6 +38,12 @@ public class PdrProcessing {
     // Threshold under which movement is considered non-existent
     private static final float epsilon = 0.18f;
     private static final int MIN_REQUIRED_SAMPLES = 2;
+    private static final float HEADING_MEDIUM_JUMP_THRESHOLD_RAD = (float) Math.toRadians(32.0);
+    private static final float HEADING_JUMP_THRESHOLD_RAD = (float) Math.toRadians(50.0);
+    private static final float HEADING_SMOOTH_ALPHA_VERY_STABLE = 0.08f;
+    private static final float HEADING_SMOOTH_ALPHA_STABLE = 0.25f;
+    private static final float HEADING_SMOOTH_ALPHA_TURNING = 0.80f;
+    private static final float GYRO_TURN_CONFIRM_THRESHOLD_RAD_S = 0.6f;
     //endregion
 
     //region Instance variables
@@ -71,6 +77,7 @@ public class PdrProcessing {
     // Step sum and length aggregation variables
     private float sumStepLength = 0;
     private int stepCount = 0;
+    private float filteredHeadingRad = Float.NaN;
     //endregion
 
     /**
@@ -82,8 +89,10 @@ public class PdrProcessing {
     public PdrProcessing(Context context) {
         // Initialise settings
         this.settings = PreferenceManager.getDefaultSharedPreferences(context);
-        // Check if estimate or manual values should be used
-        this.useManualStep = this.settings.getBoolean("manual_step_values", false);
+//        // Check if estimate or manual values should be used
+//        this.useManualStep = this.settings.getBoolean("manual_step_values", false);
+        // Keep manual step length enabled because it is more stable than the auto estimate here.
+        this.useManualStep = true;
         if(useManualStep) {
             try {
                 // Retrieve manual step  length
@@ -91,7 +100,7 @@ public PdrProcessing(Context context) {
             } catch (Exception e) {
                 // Invalid values - reset to defaults
                 this.stepLength = 0.75f;
-                this.settings.edit().putInt("user_step_length", 75).apply();
+                this.settings.edit().putInt("user_step_length", 20).apply();
             }
         }
         else {
@@ -140,20 +149,30 @@ public PdrProcessing(Context context) {
      * @param headingRad                heading relative to magnetic north in radians.
      */
     public float[] updatePdr(long currentStepEnd, List<Double> accelMagnitudeOvertime, float headingRad) {
+        return updatePdr(currentStepEnd, accelMagnitudeOvertime, headingRad, 0f, false);
+    }
+
+    public float[] updatePdr(long currentStepEnd,
+                             List<Double> accelMagnitudeOvertime,
+                             float headingRad,
+                             float gyroZRadPerSec,
+                             boolean recentTurnDetected) {
         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
         }
 
+        float stableHeading = stabiliseHeading(headingRad, gyroZRadPerSec, recentTurnDetected);
+
         // Change angle so zero rad is east
-        float adaptedHeading = (float) (Math.PI/2 - headingRad);
+        float adaptedHeading = (float) (Math.PI/2 - stableHeading);
 
         // check if accelMagnitudeOvertime is empty
         if (accelMagnitudeOvertime == null || accelMagnitudeOvertime.isEmpty()) {
             // return current position, do not update
             return new float[]{this.positionX, this.positionY};
         }
-        
+
         // Calculate step length
         if(!useManualStep) {
             //ArrayList<Double> accelMagnitudeFiltered = filter(accelMagnitudeOvertime);
@@ -161,6 +180,8 @@ public float[] updatePdr(long currentStepEnd, List<Double> accelMagnitudeOvertim
             this.stepLength = weibergMinMax(accelMagnitudeOvertime);
             // System.err.println("Step Length" + stepLength);
         }
+        // Use the manually tuned constant step length after testing.
+        stepLength = 0.75f;
 
         // Increment aggregate variables
         sumStepLength += stepLength;
@@ -178,6 +199,54 @@ public float[] updatePdr(long currentStepEnd, List<Double> accelMagnitudeOvertim
         return new float[]{this.positionX, this.positionY};
     }
 
+    private float stabiliseHeading(float rawHeadingRad, float gyroZRadPerSec, boolean recentTurnDetected) {
+        if (Float.isNaN(filteredHeadingRad)) {
+            filteredHeadingRad = normalizeAngle(rawHeadingRad);
+            return filteredHeadingRad;
+        }
+
+        float normalizedHeading = unwrapAngle(rawHeadingRad, filteredHeadingRad);
+        float delta = normalizedHeading - filteredHeadingRad;
+        float absDelta = Math.abs(delta);
+        boolean mediumJump = absDelta > HEADING_MEDIUM_JUMP_THRESHOLD_RAD;
+        boolean largeJump = Math.abs(delta) > HEADING_JUMP_THRESHOLD_RAD;
+        boolean activeTurn = recentTurnDetected || Math.abs(gyroZRadPerSec) >= GYRO_TURN_CONFIRM_THRESHOLD_RAD_S;
+
+        if (largeJump && !activeTurn) {
+            return filteredHeadingRad;
+        }
+
+        if (mediumJump && !activeTurn) {
+            filteredHeadingRad = normalizeAngle(filteredHeadingRad + HEADING_SMOOTH_ALPHA_VERY_STABLE * delta);
+            return filteredHeadingRad;
+        }
+
+        float alpha = activeTurn ? HEADING_SMOOTH_ALPHA_TURNING : HEADING_SMOOTH_ALPHA_STABLE;
+        filteredHeadingRad = normalizeAngle(filteredHeadingRad + alpha * delta);
+        return filteredHeadingRad;
+    }
+
+    private float unwrapAngle(float angle, float referenceAngle) {
+        float normalized = normalizeAngle(angle);
+        while (normalized - referenceAngle > Math.PI) {
+            normalized -= (float) (2 * Math.PI);
+        }
+        while (normalized - referenceAngle < -Math.PI) {
+            normalized += (float) (2 * Math.PI);
+        }
+        return normalized;
+    }
+
+    private float normalizeAngle(float angle) {
+        while (angle > Math.PI) {
+            angle -= (float) (2 * Math.PI);
+        }
+        while (angle < -Math.PI) {
+            angle += (float) (2 * Math.PI);
+        }
+        return angle;
+    }
+
     /**
      * Calculates the relative elevation compared to the start position.
      * The start elevation is the median of the first three seconds of data to give the sensor time
@@ -395,6 +464,7 @@ public void resetPDR() {
         this.startElevationBuffer = new Float[3];
         // Start floor - assumed to be zero
         this.currentFloor = 0;
+        this.filteredHeadingRad = Float.NaN;
     }
 
     /**
diff --git a/secrets.properties b/secrets.properties
deleted file mode 100644
index f0dc54fd..00000000
--- a/secrets.properties
+++ /dev/null
@@ -1,6 +0,0 @@
-#
-# Modify the variables to set your keys
-#
-MAPS_API_KEY=<your Google maps API key>
-OPENPOSITIONING_API_KEY=<your openpositioning user API key>
-OPENPOSITIONING_MASTER_KEY=<The openpositioning master key>