smoothBuffer = new java.util.LinkedList<>();
private Polygon buildingPolygon;
- public TrajectoryMapFragment() {
- // Required empty public constructor
+ /** Callback for when a user confirms a manual position correction. */
+ public interface CorrectionCallback {
+ void onCorrectionConfirmed(LatLng correctedPosition);
+ }
+
+ /** Required empty public constructor. */
+ public TrajectoryMapFragment() {}
+
+ /** Sets the callback to receive manual position corrections. */
+ public void setCorrectionCallback(CorrectionCallback callback) {
+ this.correctionCallback = callback;
}
@Nullable
@@ -118,12 +191,114 @@ public void onViewCreated(@NonNull View view,
// Grab references to UI controls
switchMapSpinner = view.findViewById(R.id.mapSwitchSpinner);
- gnssSwitch = view.findViewById(R.id.gnssSwitch);
+ mapInteractionBlocker = view.findViewById(R.id.mapInteractionBlocker);
+ calibrationMapStyleList = view.findViewById(R.id.calibrationMapStyleList);
+ mapStyleHybridOption = view.findViewById(R.id.mapStyleHybridOption);
+ mapStyleNormalOption = view.findViewById(R.id.mapStyleNormalOption);
+ mapStyleSatelliteOption = view.findViewById(R.id.mapStyleSatelliteOption);
+ compassSwitch = view.findViewById(R.id.compassSwitch);
+ weatherSwitch = view.findViewById(R.id.weatherSwitch);
+ gnssSwitch = view.findViewById(R.id.gnssSwitch);
+ wifiSwitch = view.findViewById(R.id.wifiSwitch);
+ estimatedSwitch = view.findViewById(R.id.estimatedSwitch);
+ pdrPathSwitch = view.findViewById(R.id.pdrPathSwitch);
+ fusedPathSwitch = view.findViewById(R.id.fusedPathSwitch);
+ smoothPathSwitch = view.findViewById(R.id.smoothPathSwitch);
autoFloorSwitch = view.findViewById(R.id.autoFloor);
- floorUpButton = view.findViewById(R.id.floorUpButton);
+ navigatingSwitch = view.findViewById(R.id.navigatingSwitch);
+ floorUpButton = view.findViewById(R.id.floorUpButton);
floorDownButton = view.findViewById(R.id.floorDownButton);
- floorLabel = view.findViewById(R.id.floorLabel);
- switchColorButton = view.findViewById(R.id.lineColorButton);
+ floorLabel = view.findViewById(R.id.floorLabel);
+ controlPanel = view.findViewById(R.id.controlPanel);
+ toolbarToggle = view.findViewById(R.id.toolbarToggle);
+
+ mapStyleHybridOption.setOnClickListener(v -> applyMapTypeSelection(0));
+ mapStyleNormalOption.setOnClickListener(v -> applyMapTypeSelection(1));
+ mapStyleSatelliteOption.setOnClickListener(v -> applyMapTypeSelection(2));
+
+ // Toolbar collapse/expand toggle
+ toolbarToggle.setOnClickListener(v -> {
+ if (interactionLocked) return;
+ if (toolbarExpanded) {
+ // Collapse: slide card left, then hide
+ controlPanel.animate()
+ .translationX(-controlPanel.getWidth())
+ .alpha(0f)
+ .setDuration(250)
+ .withEndAction(() -> controlPanel.setVisibility(View.GONE))
+ .start();
+ toolbarToggle.setImageResource(R.drawable.ic_chevron_right);
+ } else {
+ // Expand: show card, slide back in
+ controlPanel.setVisibility(View.VISIBLE);
+ controlPanel.setAlpha(0f);
+ controlPanel.animate()
+ .translationX(0)
+ .alpha(0.85f)
+ .setDuration(250)
+ .start();
+ toolbarToggle.setImageResource(R.drawable.ic_chevron_left);
+ }
+ toolbarExpanded = !toolbarExpanded;
+ });
+
+ // Compass toggle — controls CompassView in parent RecordingFragment
+ compassSwitch.setOnCheckedChangeListener((buttonView, isChecked) -> {
+ if (interactionLocked) { buttonView.setChecked(!isChecked); return; }
+ Fragment parent = getParentFragment();
+ if (parent != null && parent.getView() != null) {
+ View compass = parent.getView().findViewById(R.id.compassView);
+ if (compass != null) {
+ compass.setVisibility(isChecked ? View.VISIBLE : View.GONE);
+ }
+ }
+ });
+
+ // Weather toggle — controls WeatherView in parent RecordingFragment
+ weatherSwitch.setOnCheckedChangeListener((buttonView, isChecked) -> {
+ if (interactionLocked) { buttonView.setChecked(!isChecked); return; }
+ Fragment parent = getParentFragment();
+ if (parent instanceof RecordingFragment) {
+ ((RecordingFragment) parent).setWeatherVisible(isChecked);
+ }
+ });
+
+ // Path visibility toggles
+ pdrPathSwitch.setOnCheckedChangeListener((buttonView, isChecked) -> {
+ if (interactionLocked) { buttonView.setChecked(!isChecked); return; }
+ showPdrPath = isChecked;
+ if (polyline != null) polyline.setVisible(isChecked);
+ });
+
+ fusedPathSwitch.setOnCheckedChangeListener((buttonView, isChecked) -> {
+ if (interactionLocked) { buttonView.setChecked(!isChecked); return; }
+ showFusedPath = isChecked;
+ if (fusedPolyline != null) fusedPolyline.setVisible(isChecked);
+ });
+
+ smoothPathSwitch.setOnCheckedChangeListener((buttonView, isChecked) -> {
+ if (interactionLocked) { buttonView.setChecked(!isChecked); return; }
+ showSmoothPath = isChecked;
+ if (smoothPolyline != null) smoothPolyline.setVisible(isChecked);
+ });
+
+ // Navigating mode toggle — camera follows + rotates with heading
+ navigatingSwitch.setOnCheckedChangeListener((buttonView, isChecked) -> {
+ if (interactionLocked) { buttonView.setChecked(!isChecked); return; }
+ navigatingMode = isChecked;
+ if (isChecked) {
+ snapCameraToUser();
+ } else if (gMap != null) {
+ // Reset to north-up when switching to free mode
+ CameraPosition northUp = new CameraPosition.Builder()
+ .target(gMap.getCameraPosition().target)
+ .zoom(gMap.getCameraPosition().zoom)
+ .bearing(0)
+ .tilt(0)
+ .build();
+ gMap.animateCamera(CameraUpdateFactory.newCameraPosition(northUp));
+ }
+ });
// Setup floor up/down UI hidden initially until we know there's an indoor map
setFloorControlsVisibility(View.GONE);
@@ -139,17 +314,31 @@ public void onMapReady(@NonNull GoogleMap googleMap) {
gMap = googleMap;
// Initialize map settings with the now non-null gMap
initMapSettings(gMap);
+ setInteractionLocked(interactionLocked);
// If we had a pending camera move, apply it now
if (hasPendingCameraMove && pendingCameraPosition != null) {
gMap.moveCamera(CameraUpdateFactory.newLatLngZoom(pendingCameraPosition, 19f));
+ if (indoorMapManager != null) {
+ indoorMapManager.setCurrentLocation(pendingCameraPosition);
+ if (DEBUG) Log.i(TAG, "[onMapReady] setCurrentLocation done, isIndoorMapSet="
+ + indoorMapManager.getIsIndoorMapSet()
+ + " pos=" + pendingCameraPosition
+ + " floorplanBuildings=" + SensorFusion.getInstance().getFloorplanBuildings().size()
+ + " pendingIndoor=" + pendingIndoorPositioningModeDefaults);
+ setFloorControlsVisibility(indoorMapManager.getIsIndoorMapSet() ? View.VISIBLE : View.GONE);
+ }
hasPendingCameraMove = false;
pendingCameraPosition = null;
}
drawBuildingPolygon();
- Log.d("TrajectoryMapFragment", "onMapReady: Map is ready!");
+ if (DEBUG) Log.d("TrajectoryMapFragment", "onMapReady: Map is ready!");
+
+ if (pendingIndoorPositioningModeDefaults) {
+ applyIndoorPositioningModeDefaultsInternal();
+ }
}
@@ -161,6 +350,7 @@ public void onMapReady(@NonNull GoogleMap googleMap) {
// GNSS Switch
gnssSwitch.setOnCheckedChangeListener((buttonView, isChecked) -> {
+ if (interactionLocked) { buttonView.setChecked(!isChecked); return; }
isGnssOn = isChecked;
if (!isChecked && gnssMarker != null) {
gnssMarker.remove();
@@ -168,24 +358,31 @@ public void onMapReady(@NonNull GoogleMap googleMap) {
}
});
- // Color switch
- switchColorButton.setOnClickListener(v -> {
- if (polyline != null) {
- if (isRed) {
- switchColorButton.setBackgroundColor(Color.BLACK);
- polyline.setColor(Color.BLACK);
- isRed = false;
- } else {
- switchColorButton.setBackgroundColor(Color.RED);
- polyline.setColor(Color.RED);
- isRed = true;
- }
+ // WiFi Switch
+ wifiSwitch.setOnCheckedChangeListener((buttonView, isChecked) -> {
+ if (interactionLocked) { buttonView.setChecked(!isChecked); return; }
+ isWifiOn = isChecked;
+ if (!isChecked && wifiMarker != null) {
+ wifiMarker.remove();
+ wifiMarker = null;
+ }
+ });
+
+ // Estimated (CalDB KNN) switch
+ estimatedSwitch.setOnCheckedChangeListener((buttonView, isChecked) -> {
+ if (interactionLocked) { buttonView.setChecked(!isChecked); return; }
+ isEstimatedOn = isChecked;
+ if (!isChecked) {
+ if (estimatedMarker != null) { estimatedMarker.remove(); estimatedMarker = null; }
+ for (Circle c : fusedObsDots) c.remove();
+ fusedObsDots.clear();
}
});
// Auto-floor toggle: start/stop periodic floor evaluation
sensorFusion = SensorFusion.getInstance();
autoFloorSwitch.setOnCheckedChangeListener((compoundButton, isChecked) -> {
+ if (interactionLocked) { compoundButton.setChecked(!isChecked); return; }
if (isChecked) {
startAutoFloor();
} else {
@@ -209,22 +406,32 @@ public void onMapReady(@NonNull GoogleMap googleMap) {
updateFloorLabel();
}
});
+
+ // Apply pending calibration mode AFTER all listeners are registered,
+ // so resetToolbarSwitchesToOff() properly syncs Java fields via listeners.
+ if (pendingInitialCalibrationMode) {
+ applyInitialCalibrationMode();
+ }
+ }
+
+ @Override
+ public void onDestroyView() {
+ stopAutoFloor();
+ if (gMap != null) {
+ clearMapAndReset();
+ gMap = null;
+ }
+ super.onDestroyView();
}
/**
- * Initialize the map settings with the provided GoogleMap instance.
- *
- * The method sets basic map settings, initializes the indoor map manager,
- * and creates an empty polyline for user movement tracking.
- * The method also initializes the GNSS polyline for tracking GNSS path.
- * The method sets the map type to Hybrid and initializes the map with these settings.
+ * Initializes map settings, indoor map manager, and empty polylines for tracking.
*
- * @param map
+ * @param map the GoogleMap instance to configure
*/
-
private void initMapSettings(GoogleMap map) {
// Basic map settings
- map.getUiSettings().setCompassEnabled(true);
+ map.getUiSettings().setCompassEnabled(false);
map.getUiSettings().setTiltGesturesEnabled(true);
map.getUiSettings().setRotateGesturesEnabled(true);
map.getUiSettings().setScrollGesturesEnabled(true);
@@ -233,37 +440,241 @@ private void initMapSettings(GoogleMap map) {
// Initialize indoor manager
indoorMapManager = new IndoorMapManager(map);
+ // Inject IndoorMapManager into FusionManager for stairs detection
+ FusionManager fm = SensorFusion.getInstance().getFusionManager();
+ if (fm != null) {
+ fm.setIndoorMapManager(indoorMapManager);
+ }
+
// Initialize an empty polyline
polyline = map.addPolyline(new PolylineOptions()
.color(Color.RED)
.width(5f)
.add() // start empty
);
+ polyline.setVisible(showPdrPath);
- // GNSS path in blue
- gnssPolyline = map.addPolyline(new PolylineOptions()
- .color(Color.BLUE)
- .width(5f)
- .add() // start empty
+ // Fused trajectory in purple
+ fusedPolyline = map.addPolyline(new PolylineOptions()
+ .color(Color.rgb(156, 39, 176)) // Material Purple
+ .width(7f)
+ .add()
+ );
+ fusedPolyline.setVisible(showFusedPath);
+
+ // Smooth trajectory in green (placeholder — not yet populated)
+ smoothPolyline = map.addPolyline(new PolylineOptions()
+ .color(Color.rgb(76, 175, 80)) // Material Green
+ .width(7f)
+ .add()
);
+ smoothPolyline.setVisible(showSmoothPath);
+
+ // Long press → place a draggable correction marker
+ map.setOnMapLongClickListener(latLng -> {
+ if (interactionLocked) return;
+ // Remove previous drag marker if it exists
+ if (correctionDragMarker != null) {
+ correctionDragMarker.remove();
+ }
+ correctionDragMarker = map.addMarker(new MarkerOptions()
+ .position(latLng)
+ .title("Drag to your actual position")
+ .snippet("Tap this marker to confirm")
+ .draggable(true)
+ .icon(BitmapDescriptorFactory.defaultMarker(BitmapDescriptorFactory.HUE_ORANGE))
+ .zIndex(20));
+ if (correctionDragMarker != null) {
+ correctionDragMarker.showInfoWindow();
+ }
+ });
+
+ // Tap on the correction marker → confirm the correction
+ map.setOnInfoWindowClickListener(marker -> {
+ if (interactionLocked) return;
+ if (marker.equals(correctionDragMarker)) {
+ confirmCorrection(marker.getPosition());
+ marker.hideInfoWindow();
+ correctionDragMarker.remove();
+ correctionDragMarker = null;
+ }
+ });
+ }
+
+ /**
+ * Prepares the toolbar for the initial calibration window:
+ * expands the panel, clears all toggle switches, and opens the map-style dropdown.
+ */
+ public void enterInitialCalibrationMode() {
+ pendingInitialCalibrationMode = true;
+ if (!isAdded() || getView() == null || switchMapSpinner == null) return;
+ applyInitialCalibrationMode();
}
+ /** Applies default switch states for indoor positioning mode. */
+ public void applyIndoorPositioningModeDefaults() {
+ pendingIndoorPositioningModeDefaults = true;
+ if (!isAdded() || getView() == null || switchMapSpinner == null || gMap == null) return;
+ applyIndoorPositioningModeDefaultsInternal();
+ }
/**
- * Initialize the map type spinner with the available map types.
- *
- * The spinner allows the user to switch between different map types
- * (e.g. Hybrid, Normal, Satellite) to customize their map view.
- * The spinner is populated with the available map types and listens
- * for user selection to update the map accordingly.
- * The map type is updated directly on the GoogleMap instance.
- *
- * Note: The spinner is initialized with the default map type (Hybrid).
- * The map type is updated on user selection.
- *
- *
- * @see com.google.android.gms.maps.GoogleMap The GoogleMap instance to update map type.
+ * Locks or unlocks map interactions during the calibration window.
*/
+ public void setInteractionLocked(boolean locked) {
+ interactionLocked = locked;
+
+ if (locked) {
+ expandToolbar();
+ }
+
+ if (mapInteractionBlocker != null) {
+ mapInteractionBlocker.setVisibility(locked ? View.VISIBLE : View.GONE);
+ }
+ // Swap Spinner ↔ fixed list during calibration
+ if (switchMapSpinner != null) {
+ switchMapSpinner.setVisibility(locked ? View.GONE : View.VISIBLE);
+ }
+ if (calibrationMapStyleList != null) {
+ calibrationMapStyleList.setVisibility(locked ? View.VISIBLE : View.GONE);
+ }
+ // Toolbar toggle disabled during calibration (keep visual appearance)
+ if (toolbarToggle != null) {
+ toolbarToggle.setEnabled(!locked);
+ }
+ // Floor buttons disabled during calibration
+ if (floorUpButton != null) {
+ floorUpButton.setEnabled(!locked);
+ }
+ if (floorDownButton != null) {
+ floorDownButton.setEnabled(!locked);
+ }
+ // NOTE: switches are NOT disabled — that causes Material grey-out dimming.
+ // Instead, switch listeners check interactionLocked to reject changes.
+
+ if (gMap != null) {
+ gMap.getUiSettings().setScrollGesturesEnabled(!locked);
+ gMap.getUiSettings().setZoomGesturesEnabled(!locked);
+ gMap.getUiSettings().setRotateGesturesEnabled(!locked);
+ gMap.getUiSettings().setTiltGesturesEnabled(!locked);
+ gMap.getUiSettings().setMapToolbarEnabled(!locked);
+ }
+ }
+
+ private void resetToolbarSwitchesToDefaults() {
+ // Temporarily lift lock so listeners don't revert the programmatic changes
+ boolean wasLocked = interactionLocked;
+ interactionLocked = false;
+
+ // First 4: ON — toggle false→true to guarantee listener fires
+ // even if the XML default is already checked="true"
+ if (compassSwitch != null) { compassSwitch.setChecked(false); compassSwitch.setChecked(true); }
+ if (weatherSwitch != null) { weatherSwitch.setChecked(false); weatherSwitch.setChecked(true); }
+ if (gnssSwitch != null) { gnssSwitch.setChecked(false); gnssSwitch.setChecked(true); }
+ if (wifiSwitch != null) { wifiSwitch.setChecked(false); wifiSwitch.setChecked(true); }
+ // Rest: OFF
+ if (estimatedSwitch != null) estimatedSwitch.setChecked(false);
+ if (pdrPathSwitch != null) pdrPathSwitch.setChecked(false);
+ if (fusedPathSwitch != null) fusedPathSwitch.setChecked(false);
+ if (smoothPathSwitch != null) smoothPathSwitch.setChecked(false);
+ if (autoFloorSwitch != null) autoFloorSwitch.setChecked(false);
+ if (navigatingSwitch != null) navigatingSwitch.setChecked(false);
+
+ interactionLocked = wasLocked;
+ }
+
+ /**
+ * Restores the default switch states: first 4 ON, rest OFF.
+ * Called after calibration countdown finishes.
+ */
+ public void restoreDefaultSwitchStates() {
+ if (compassSwitch != null) compassSwitch.setChecked(true);
+ if (weatherSwitch != null) weatherSwitch.setChecked(true);
+ if (gnssSwitch != null) gnssSwitch.setChecked(true);
+ if (wifiSwitch != null) wifiSwitch.setChecked(true);
+ }
+
+ private void applyInitialCalibrationMode() {
+ expandToolbar();
+ resetToolbarSwitchesToDefaults();
+ // Swap: hide Spinner, show fixed list for calibration
+ if (switchMapSpinner != null) {
+ switchMapSpinner.setVisibility(View.GONE);
+ }
+ if (calibrationMapStyleList != null) {
+ calibrationMapStyleList.setVisibility(View.VISIBLE);
+ }
+ highlightSelectedMapStyleOption(switchMapSpinner != null ? switchMapSpinner.getSelectedItemPosition() : 0);
+ }
+
+ private void applyIndoorPositioningModeDefaultsInternal() {
+ if (gMap == null || indoorMapManager == null) {
+ pendingIndoorPositioningModeDefaults = true;
+ return;
+ }
+ pendingIndoorPositioningModeDefaults = false;
+ pendingInitialCalibrationMode = false;
+
+ // Temporarily lift interaction lock to set switches without listener revert,
+ // then restore — so calibration lock is preserved.
+ boolean wasLocked = interactionLocked;
+ interactionLocked = false;
+
+ // ON: Compass, Weather, Auto Floor, Smooth Path, Navigating
+ if (compassSwitch != null) { compassSwitch.setChecked(false); compassSwitch.setChecked(true); }
+ if (weatherSwitch != null) { weatherSwitch.setChecked(false); weatherSwitch.setChecked(true); }
+ if (smoothPathSwitch != null) { smoothPathSwitch.setChecked(false); smoothPathSwitch.setChecked(true); }
+ if (autoFloorSwitch != null) { autoFloorSwitch.setChecked(false); autoFloorSwitch.setChecked(true); }
+ if (navigatingSwitch != null) { navigatingSwitch.setChecked(false); navigatingSwitch.setChecked(true); }
+ // OFF: the rest
+ if (gnssSwitch != null) gnssSwitch.setChecked(false);
+ if (wifiSwitch != null) wifiSwitch.setChecked(false);
+ if (estimatedSwitch != null) estimatedSwitch.setChecked(false);
+ if (pdrPathSwitch != null) pdrPathSwitch.setChecked(false);
+ if (fusedPathSwitch != null) fusedPathSwitch.setChecked(false);
+
+ interactionLocked = wasLocked;
+
+ // Normal map
+ applyMapTypeSelection(1);
+
+ // Force building setup — skip pointInPolygon detection
+ String selectedBuilding = sensorFusion.getSelectedBuildingId();
+ if (selectedBuilding != null && indoorMapManager != null) {
+ boolean ok = indoorMapManager.forceSetBuilding(selectedBuilding);
+ setFloorControlsVisibility(ok ? View.VISIBLE : View.GONE);
+ }
+
+ if (calibrationMapStyleList != null) {
+ calibrationMapStyleList.setVisibility(View.GONE);
+ }
+ collapseToolbar();
+ }
+
+ private void expandToolbar() {
+ if (controlPanel == null || toolbarToggle == null) return;
+
+ controlPanel.animate().cancel();
+ controlPanel.setVisibility(View.VISIBLE);
+ controlPanel.setTranslationX(0f);
+ controlPanel.setAlpha(0.85f);
+ toolbarToggle.setImageResource(R.drawable.ic_chevron_left);
+ toolbarExpanded = true;
+ }
+
+ private void collapseToolbar() {
+ if (controlPanel == null || toolbarToggle == null) return;
+
+ controlPanel.animate().cancel();
+ controlPanel.setVisibility(View.GONE);
+ controlPanel.setTranslationX(-controlPanel.getWidth());
+ controlPanel.setAlpha(0f);
+ toolbarToggle.setImageResource(R.drawable.ic_chevron_right);
+ toolbarExpanded = false;
+ }
+
+
+ /** Initializes the map type spinner (Hybrid, Normal, Satellite). */
private void initMapTypeSpinner() {
if (switchMapSpinner == null) return;
String[] maps = new String[]{
@@ -271,35 +682,111 @@ private void initMapTypeSpinner() {
getString(R.string.normal),
getString(R.string.satellite)
};
- ArrayAdapter adapter = new ArrayAdapter<>(
+ ArrayAdapter adapter = new ArrayAdapter(
requireContext(),
- android.R.layout.simple_spinner_dropdown_item,
+ android.R.layout.simple_spinner_item,
maps
- );
+ ) {
+ @NonNull
+ @Override
+ public View getView(int position, View convertView, @NonNull ViewGroup parent) {
+ View v = super.getView(position, convertView, parent);
+ ((TextView) v).setGravity(android.view.Gravity.CENTER);
+ return v;
+ }
+
+ @Override
+ public View getDropDownView(int position, View convertView, @NonNull ViewGroup parent) {
+ View v = super.getDropDownView(position, convertView, parent);
+ if (v instanceof android.widget.CheckedTextView) {
+ ((android.widget.CheckedTextView) v).setGravity(android.view.Gravity.CENTER);
+ ((android.widget.CheckedTextView) v).setTextSize(
+ android.util.TypedValue.COMPLEX_UNIT_SP, 13);
+ } else if (v instanceof TextView) {
+ ((TextView) v).setGravity(android.view.Gravity.CENTER);
+ ((TextView) v).setTextSize(
+ android.util.TypedValue.COMPLEX_UNIT_SP, 13);
+ }
+ return v;
+ }
+ };
+ adapter.setDropDownViewResource(android.R.layout.simple_spinner_dropdown_item);
switchMapSpinner.setAdapter(adapter);
+ // Match dropdown popup to toolbar visual width and shift left
+ controlPanel.post(() -> {
+ if (controlPanel != null && switchMapSpinner != null) {
+ int scaledWidth = (int)(controlPanel.getWidth() * 0.80f);
+ switchMapSpinner.setDropDownWidth(scaledWidth);
+ switchMapSpinner.setDropDownHorizontalOffset(-20);
+ }
+ });
+ // Semi-transparent popup background matching toolbar alpha
+ android.graphics.drawable.ColorDrawable popupBg =
+ new android.graphics.drawable.ColorDrawable(android.graphics.Color.argb(
+ (int)(0.85f * 255), 255, 255, 255));
+ switchMapSpinner.setPopupBackgroundDrawable(popupBg);
+
switchMapSpinner.setOnItemSelectedListener(new AdapterView.OnItemSelectedListener() {
@Override
public void onItemSelected(AdapterView parent, View view,
int position, long id) {
- if (gMap == null) return;
- switch (position){
- case 0:
- gMap.setMapType(GoogleMap.MAP_TYPE_HYBRID);
- break;
- case 1:
- gMap.setMapType(GoogleMap.MAP_TYPE_NORMAL);
- break;
- case 2:
- gMap.setMapType(GoogleMap.MAP_TYPE_SATELLITE);
- break;
- }
+ applyMapTypeSelection(position);
}
@Override
public void onNothingSelected(AdapterView parent) {}
});
}
+ private void applyMapTypeSelection(int position) {
+ if (switchMapSpinner != null && switchMapSpinner.getSelectedItemPosition() != position) {
+ switchMapSpinner.setSelection(position);
+ }
+
+ if (gMap != null) {
+ switch (position) {
+ case 0:
+ gMap.setMapType(GoogleMap.MAP_TYPE_HYBRID);
+ gMap.setMapStyle(null); // clear custom style
+ break;
+ case 1:
+ gMap.setMapType(GoogleMap.MAP_TYPE_NORMAL);
+ // Apply light pink style to Normal map
+ try {
+ gMap.setMapStyle(com.google.android.gms.maps.model.MapStyleOptions
+ .loadRawResourceStyle(requireContext(), R.raw.map_style_pink));
+ } catch (Exception e) {
+ if (DEBUG) Log.w(TAG, "Failed to apply pink map style", e);
+ }
+ break;
+ case 2:
+ gMap.setMapType(GoogleMap.MAP_TYPE_SATELLITE);
+ gMap.setMapStyle(null);
+ break;
+ default:
+ break;
+ }
+ }
+
+ highlightSelectedMapStyleOption(position);
+ }
+
+ private void highlightSelectedMapStyleOption(int selectedPosition) {
+ updateMapStyleOptionAppearance(mapStyleHybridOption, selectedPosition == 0);
+ updateMapStyleOptionAppearance(mapStyleNormalOption, selectedPosition == 1);
+ updateMapStyleOptionAppearance(mapStyleSatelliteOption, selectedPosition == 2);
+ }
+
+ private void updateMapStyleOptionAppearance(@Nullable TextView option, boolean selected) {
+ if (option == null) return;
+ option.setBackgroundColor(selected
+ ? Color.argb(35, 2, 103, 125)
+ : Color.TRANSPARENT);
+ option.setTextColor(selected
+ ? requireContext().getColor(R.color.md_theme_primary)
+ : requireContext().getColor(R.color.md_theme_onSurface));
+ }
+
/**
* Update the user's current location on the map, create or move orientation marker,
* and append to polyline if the user actually moved.
@@ -310,74 +797,109 @@ public void onNothingSelected(AdapterView parent) {}
public void updateUserLocation(@NonNull LatLng newLocation, float orientation) {
if (gMap == null) return;
- // Keep track of current location
LatLng oldLocation = this.currentLocation;
this.currentLocation = newLocation;
- // If no marker, create it
- if (orientationMarker == null) {
- orientationMarker = gMap.addMarker(new MarkerOptions()
- .position(newLocation)
- .flat(true)
- .title("Current Position")
- .icon(BitmapDescriptorFactory.fromBitmap(
- UtilFunctions.getBitmapFromVector(requireContext(),
- R.drawable.ic_baseline_navigation_24)))
- );
- gMap.moveCamera(CameraUpdateFactory.newLatLngZoom(newLocation, 19f));
+ // When fusion is active, the PDR marker is hidden — only draw the polyline.
+ // The fused marker (purple) takes over as the primary position indicator.
+ if (!fusionActive) {
+ if (orientationMarker == null) {
+ orientationMarker = gMap.addMarker(new MarkerOptions()
+ .position(newLocation)
+ .flat(true)
+ .title("PDR Position")
+ .icon(BitmapDescriptorFactory.fromBitmap(
+ UtilFunctions.getBitmapFromVector(requireContext(),
+ R.drawable.ic_baseline_navigation_24)))
+ );
+ // Always center on first position
+ gMap.moveCamera(CameraUpdateFactory.newLatLngZoom(newLocation, 19f));
+ } else {
+ orientationMarker.setPosition(newLocation);
+ orientationMarker.setRotation(orientation);
+ if (navigatingMode) {
+ CameraPosition pos = new CameraPosition.Builder()
+ .target(newLocation)
+ .zoom(gMap.getCameraPosition().zoom)
+ .bearing(orientation)
+ .tilt(0)
+ .build();
+ gMap.moveCamera(CameraUpdateFactory.newCameraPosition(pos));
+ }
+ }
} else {
- // Update marker position + orientation
- orientationMarker.setPosition(newLocation);
- orientationMarker.setRotation(orientation);
- // Move camera a bit
- gMap.moveCamera(CameraUpdateFactory.newLatLng(newLocation));
+ // Hide PDR marker when fusion takes over
+ if (orientationMarker != null) {
+ orientationMarker.setVisible(false);
+ }
}
- // Extend polyline if movement occurred
- /*if (oldLocation != null && !oldLocation.equals(newLocation) && polyline != null) {
- List points = new ArrayList<>(polyline.getPoints());
- points.add(newLocation);
- polyline.setPoints(points);
- }*/
- // Extend polyline
+ // Always extend the PDR polyline (red) regardless of fusion state
if (polyline != null) {
List points = new ArrayList<>(polyline.getPoints());
-
- // First position fix: add the first polyline point
if (oldLocation == null) {
points.add(newLocation);
polyline.setPoints(points);
} else if (!oldLocation.equals(newLocation)) {
- // Subsequent movement: append a new polyline point
points.add(newLocation);
polyline.setPoints(points);
}
}
-
- // Update indoor map overlay
+ // Update indoor map overlay — only trigger building detection if not already set
if (indoorMapManager != null) {
- indoorMapManager.setCurrentLocation(newLocation);
+ if (!indoorMapManager.getIsIndoorMapSet()) {
+ indoorMapManager.setCurrentLocation(newLocation);
+ }
setFloorControlsVisibility(indoorMapManager.getIsIndoorMapSet() ? View.VISIBLE : View.GONE);
}
}
+ /** Called by RecordingFragment to signal that fusion is now providing the primary position. */
+ public void setFusionActive(boolean active) {
+ this.fusionActive = active;
+ // Show PDR marker again if fusion is deactivated
+ if (!active && orientationMarker != null) {
+ orientationMarker.setVisible(true);
+ }
+ }
+
+ /** Re-center + rotate camera to current user position and heading. */
+ private void snapCameraToUser() {
+ if (gMap == null) return;
+ LatLng target = (fusionActive && fusedMarker != null)
+ ? fusedMarker.getPosition()
+ : (orientationMarker != null ? orientationMarker.getPosition() : null);
+ if (target == null) return;
+
+ float bearing = (fusionActive && fusedMarker != null)
+ ? fusedMarker.getRotation()
+ : (orientationMarker != null ? orientationMarker.getRotation() : 0f);
+
+ CameraPosition pos = new CameraPosition.Builder()
+ .target(target)
+ .zoom(gMap.getCameraPosition().zoom)
+ .bearing(bearing)
+ .tilt(0)
+ .build();
+ gMap.animateCamera(CameraUpdateFactory.newCameraPosition(pos));
+ }
+
/**
- * Set the initial camera position for the map.
- *
- * The method sets the initial camera position for the map when it is first loaded.
- * If the map is already ready, the camera is moved immediately.
- * If the map is not ready, the camera position is stored until the map is ready.
- * The method also tracks if there is a pending camera move.
- *
- * @param startLocation The initial camera position to set.
+ * Sets the initial camera position, moving immediately if the map is ready.
+ *
+ * @param startLocation the initial camera position to set
*/
public void setInitialCameraPosition(@NonNull LatLng startLocation) {
// If the map is already ready, move camera immediately
if (gMap != null) {
gMap.moveCamera(CameraUpdateFactory.newLatLngZoom(startLocation, 19f));
+ if (indoorMapManager != null) {
+ indoorMapManager.setCurrentLocation(startLocation);
+ setFloorControlsVisibility(indoorMapManager.getIsIndoorMapSet() ? View.VISIBLE : View.GONE);
+ }
} else {
// Otherwise, store it until onMapReady
pendingCameraPosition = startLocation;
@@ -394,6 +916,59 @@ public LatLng getCurrentLocation() {
return currentLocation;
}
+ /** Returns the fused trajectory points, or PDR points if fused is empty. */
+ public List getTrajectoryPoints() {
+ if (fusedPolyline != null && !fusedPolyline.getPoints().isEmpty()) {
+ return new ArrayList<>(fusedPolyline.getPoints());
+ }
+ if (polyline != null) {
+ return new ArrayList<>(polyline.getPoints());
+ }
+ return new ArrayList<>();
+ }
+
+ /** Returns current building name for test point metadata. */
+ public String getLocationBuilding() {
+ if (indoorMapManager != null) return indoorMapManager.getCurrentBuildingName();
+ return "Outdoor";
+ }
+
+ /** Returns current floor display name for test point metadata. */
+ public String getLocationFloor() {
+ if (indoorMapManager != null && indoorMapManager.getIsIndoorMapSet())
+ return indoorMapManager.getCurrentFloorDisplayName();
+ return "--";
+ }
+
+ /**
+ * Returns a formatted location summary string for clipboard output.
+ * Format: "BuildingName, FloorLabel, LocationType"
+ * LocationType is determined by {@link #getLocationType()} (stub — returns "Unknown" for now).
+ */
+ public String getLocationSummary() {
+ String building = "Outdoor";
+ String floor = "--";
+ if (indoorMapManager != null) {
+ building = indoorMapManager.getCurrentBuildingName();
+ if (indoorMapManager.getCurrentBuilding() != IndoorMapManager.BUILDING_NONE) {
+ floor = indoorMapManager.getCurrentFloorDisplayName();
+ }
+ }
+ String locType = getLocationType();
+ return building + ", Floor: " + floor + ", " + locType;
+ }
+
+ /**
+ * Stub for location-type detection (near elevator / stairs / hall).
+ * TODO: Implement actual detection logic based on indoor map features.
+ *
+ * @return one of "Near Elevator", "Near Stairs", or "Hall"
+ */
+ public String getLocationType() {
+ // TODO: implement proximity detection to elevator/stairs/hall areas
+ return "Unknown";
+ }
+
/**
* Add a numbered test point marker on the map.
* Called by RecordingFragment when user presses the "Test Point" button.
@@ -401,13 +976,27 @@ public LatLng getCurrentLocation() {
public void addTestPointMarker(int index, long timestampMs, @NonNull LatLng position) {
if (gMap == null) return;
+ // Format local time HH:mm:ss
+ java.text.SimpleDateFormat sdf = new java.text.SimpleDateFormat("HH:mm:ss", java.util.Locale.getDefault());
+ String localTime = sdf.format(new java.util.Date(timestampMs));
+
+ // Building and floor info
+ String building = "Outdoor";
+ String floor = "--";
+ if (indoorMapManager != null) {
+ building = indoorMapManager.getCurrentBuildingName();
+ if (indoorMapManager.getIsIndoorMapSet()) {
+ floor = indoorMapManager.getCurrentFloorDisplayName();
+ }
+ }
+
Marker m = gMap.addMarker(new MarkerOptions()
.position(position)
- .title("TP " + index)
- .snippet("t=" + timestampMs));
+ .title("TP " + index + " | " + localTime)
+ .snippet(building + ", Floor: " + floor));
if (m != null) {
- m.showInfoWindow(); // Show TP index immediately
+ m.showInfoWindow();
testPointMarkers.add(m);
}
}
@@ -421,7 +1010,6 @@ public void updateGNSS(@NonNull LatLng gnssLocation) {
if (!isGnssOn) return;
if (gnssMarker == null) {
- // Create the GNSS marker for the first time
gnssMarker = gMap.addMarker(new MarkerOptions()
.position(gnssLocation)
.title("GNSS Position")
@@ -429,19 +1017,242 @@ public void updateGNSS(@NonNull LatLng gnssLocation) {
.defaultMarker(BitmapDescriptorFactory.HUE_AZURE)));
lastGnssLocation = gnssLocation;
} else {
- // Move existing GNSS marker
gnssMarker.setPosition(gnssLocation);
+ lastGnssLocation = gnssLocation;
+ }
- // Add a segment to the blue GNSS line, if this is a new location
- if (lastGnssLocation != null && !lastGnssLocation.equals(gnssLocation)) {
- List gnssPoints = new ArrayList<>(gnssPolyline.getPoints());
- gnssPoints.add(gnssLocation);
- gnssPolyline.setPoints(gnssPoints);
+ // Add GNSS observation dot (small cyan circle)
+ addObsDot(gnssObsDots, gnssLocation, Color.rgb(0, 188, 212));
+ }
+
+
+ // ---- Fused position display ------------------------------------------------
+
+ /**
+ * Updates the fused position marker and polyline on the map.
+ * The fused position is the particle-filter estimate combining PDR + WiFi + GNSS.
+ *
+ * @param fusedLocation the fused LatLng
+ * @param uncertainty uncertainty radius in metres
+ * @param orientation heading in degrees for the marker
+ */
+ public void updateFusedLocation(@NonNull LatLng fusedLocation,
+ double uncertainty, float orientation) {
+ if (gMap == null) return;
+
+ // Fused marker (purple navigation icon)
+ if (fusedMarker == null) {
+ fusedMarker = gMap.addMarker(new MarkerOptions()
+ .position(fusedLocation)
+ .flat(true)
+ .title("Fused Position")
+ .anchor(0.5f, 0.5f)
+ .icon(BitmapDescriptorFactory.fromBitmap(
+ UtilFunctions.getBitmapFromVector(requireContext(),
+ R.drawable.ic_baseline_navigation_24)))
+ .zIndex(10));
+ gMap.moveCamera(CameraUpdateFactory.newLatLngZoom(fusedLocation, 19f));
+ } else {
+ fusedMarker.setPosition(fusedLocation);
+ fusedMarker.setRotation(orientation);
+ }
+
+ // Uncertainty circle
+ if (fusedUncertaintyCircle == null) {
+ fusedUncertaintyCircle = gMap.addCircle(new CircleOptions()
+ .center(fusedLocation)
+ .radius(uncertainty)
+ .strokeColor(Color.argb(100, 156, 39, 176))
+ .fillColor(Color.argb(30, 156, 39, 176))
+ .strokeWidth(2f));
+ } else {
+ fusedUncertaintyCircle.setCenter(fusedLocation);
+ fusedUncertaintyCircle.setRadius(uncertainty);
+ }
+
+ // Extend fused polyline
+ if (fusedPolyline != null) {
+ List points = new ArrayList<>(fusedPolyline.getPoints());
+ points.add(fusedLocation);
+ fusedPolyline.setPoints(points);
+ }
+
+ // Fused position observation dots (red, tied to Show Estimated)
+ if (isEstimatedOn) {
+ addObsDot(fusedObsDots, fusedLocation, Color.rgb(244, 67, 54), FUSED_OBS_HISTORY_SIZE);
+ }
+
+ // Smoothed trajectory (green line)
+ if (smoothPolyline != null) {
+ LatLng smoothed = smoothPosition(fusedLocation);
+ List sPoints = new ArrayList<>(smoothPolyline.getPoints());
+ sPoints.add(smoothed);
+ smoothPolyline.setPoints(sPoints);
+ }
+
+ // Camera follows fused position only in navigating mode
+ if (navigatingMode) {
+ CameraPosition pos = new CameraPosition.Builder()
+ .target(fusedLocation)
+ .zoom(gMap.getCameraPosition().zoom)
+ .bearing(orientation)
+ .tilt(0)
+ .build();
+ gMap.moveCamera(CameraUpdateFactory.newCameraPosition(pos));
+ }
+
+ // Update indoor map overlay — only trigger building detection if not already set,
+ // to avoid resetting floor when position oscillates near building boundary.
+ if (indoorMapManager != null) {
+ if (!indoorMapManager.getIsIndoorMapSet()) {
+ indoorMapManager.setCurrentLocation(fusedLocation);
}
- lastGnssLocation = gnssLocation;
+ setFloorControlsVisibility(indoorMapManager.getIsIndoorMapSet() ? View.VISIBLE : View.GONE);
}
}
+ /**
+ * Updates the grey marker showing the CalDB (KNN) estimated position.
+ *
+ * @param estimatedLocation the estimated position from calibration database
+ */
+ public void updateEstimatedMarker(@NonNull LatLng estimatedLocation) {
+ if (gMap == null || !isEstimatedOn) return;
+
+ if (estimatedMarker == null) {
+ estimatedMarker = gMap.addMarker(new MarkerOptions()
+ .position(estimatedLocation)
+ .title("Estimated (CalDB)")
+ .icon(BitmapDescriptorFactory.fromBitmap(
+ createGreyMarkerBitmap())));
+ } else {
+ estimatedMarker.setPosition(estimatedLocation);
+ }
+ }
+
+ /** Returns whether the estimated (CalDB) marker is currently enabled. */
+ public boolean isEstimatedEnabled() {
+ return isEstimatedOn;
+ }
+
+ private LatLng smoothPosition(LatLng observation) {
+ smoothBuffer.addLast(observation);
+ if (smoothBuffer.size() > SMOOTH_WINDOW) {
+ smoothBuffer.removeFirst();
+ }
+ double sumLat = 0, sumLng = 0;
+ for (LatLng p : smoothBuffer) {
+ sumLat += p.latitude;
+ sumLng += p.longitude;
+ }
+ return new LatLng(sumLat / smoothBuffer.size(), sumLng / smoothBuffer.size());
+ }
+
+ /**
+ * Adds a small circle dot to the observation history.
+ * Removes the oldest if exceeding OBS_HISTORY_SIZE.
+ */
+ private void addObsDot(java.util.LinkedList dots, LatLng position, int color) {
+ addObsDot(dots, position, color, OBS_HISTORY_SIZE);
+ }
+
+ private void addObsDot(java.util.LinkedList dots, LatLng position, int color, int maxSize) {
+ if (gMap == null) return;
+ // Skip if same position as last dot (avoid stacking)
+ if (!dots.isEmpty()) {
+ LatLng last = dots.getLast().getCenter();
+ if (Math.abs(last.latitude - position.latitude) < 1e-7
+ && Math.abs(last.longitude - position.longitude) < 1e-7) {
+ return;
+ }
+ }
+ Circle dot = gMap.addCircle(new CircleOptions()
+ .center(position)
+ .radius(0.5)
+ .strokeColor(Color.argb(200, Color.red(color), Color.green(color), Color.blue(color)))
+ .strokeWidth(3)
+ .fillColor(Color.argb(80, Color.red(color), Color.green(color), Color.blue(color))));
+ dots.addLast(dot);
+ if (dots.size() > maxSize) {
+ dots.removeFirst().remove();
+ }
+ }
+
+ private void clearObsDots() {
+ for (Circle c : gnssObsDots) c.remove();
+ gnssObsDots.clear();
+ for (Circle c : wifiObsDots) c.remove();
+ wifiObsDots.clear();
+ for (Circle c : fusedObsDots) c.remove();
+ fusedObsDots.clear();
+ }
+
+ private android.graphics.Bitmap createGreyMarkerBitmap() {
+ int w = 70, h = 90;
+ android.graphics.Bitmap bmp = android.graphics.Bitmap.createBitmap(w, h,
+ android.graphics.Bitmap.Config.ARGB_8888);
+ android.graphics.Canvas canvas = new android.graphics.Canvas(bmp);
+ android.graphics.Paint paint = new android.graphics.Paint(android.graphics.Paint.ANTI_ALIAS_FLAG);
+ paint.setColor(Color.rgb(158, 158, 158)); // grey
+ // Marker head (circle)
+ canvas.drawCircle(w / 2f, 28f, 28f, paint);
+ // Marker pin (triangle)
+ android.graphics.Path path = new android.graphics.Path();
+ path.moveTo(w / 2f - 18f, 50f);
+ path.lineTo(w / 2f + 18f, 50f);
+ path.lineTo(w / 2f, h);
+ path.close();
+ canvas.drawPath(path, paint);
+ // White dot in center
+ paint.setColor(Color.WHITE);
+ canvas.drawCircle(w / 2f, 28f, 10f, paint);
+ return bmp;
+ }
+
+ /**
+ * Updates or creates a green marker showing the latest WiFi position fix.
+ *
+ * @param wifiLocation the WiFi-derived position
+ */
+ public void updateWifiMarker(@NonNull LatLng wifiLocation) {
+ if (gMap == null || !isWifiOn) return;
+
+ if (wifiMarker == null) {
+ wifiMarker = gMap.addMarker(new MarkerOptions()
+ .position(wifiLocation)
+ .title("WiFi Position")
+ .icon(BitmapDescriptorFactory
+ .defaultMarker(BitmapDescriptorFactory.HUE_GREEN)));
+ } else {
+ wifiMarker.setPosition(wifiLocation);
+ }
+
+ // Add WiFi observation dot (small green circle)
+ addObsDot(wifiObsDots, wifiLocation, Color.rgb(76, 175, 80));
+ }
+
+ // ---- Manual correction logic ------------------------------------------------
+
+ /**
+ * Confirms a correction point: places a permanent marker and fires the callback.
+ */
+ private void confirmCorrection(@NonNull LatLng position) {
+ if (gMap == null) return;
+
+ // Permanent orange circle marker
+ int idx = correctionMarkers.size() + 1;
+ Marker m = gMap.addMarker(new MarkerOptions()
+ .position(position)
+ .title("Correction #" + idx)
+ .icon(BitmapDescriptorFactory.defaultMarker(BitmapDescriptorFactory.HUE_ORANGE))
+ .zIndex(15));
+ if (m != null) correctionMarkers.add(m);
+
+ // Notify the callback (RecordingFragment → FusionManager)
+ if (correctionCallback != null) {
+ correctionCallback.onCorrectionConfirmed(position);
+ }
+ }
/**
* Remove GNSS marker if user toggles it off
@@ -460,6 +1271,19 @@ public boolean isGnssEnabled() {
return isGnssOn;
}
+ /** Remove WiFi marker if user toggles it off. */
+ public void clearWiFi() {
+ if (wifiMarker != null) {
+ wifiMarker.remove();
+ wifiMarker = null;
+ }
+ }
+
+ /** Whether user is currently showing WiFi or not. */
+ public boolean isWifiEnabled() {
+ return isWifiOn;
+ }
+
private void setFloorControlsVisibility(int visibility) {
floorUpButton.setVisibility(visibility);
floorDownButton.setVisibility(visibility);
@@ -479,6 +1303,7 @@ private void updateFloorLabel() {
}
}
+ /** Removes all markers, polylines, and resets map state for a fresh recording. */
public void clearMapAndReset() {
stopAutoFloor();
if (autoFloorSwitch != null) {
@@ -503,116 +1328,166 @@ public void clearMapAndReset() {
lastGnssLocation = null;
currentLocation = null;
+ // Clear correction markers
+ if (correctionDragMarker != null) { correctionDragMarker.remove(); correctionDragMarker = null; }
+ for (Marker m : correctionMarkers) m.remove();
+ correctionMarkers.clear();
+
+ // Clear fused elements
+ fusionActive = false;
+ if (fusedMarker != null) { fusedMarker.remove(); fusedMarker = null; }
+ if (wifiMarker != null) { wifiMarker.remove(); wifiMarker = null; }
+ if (estimatedMarker != null) { estimatedMarker.remove(); estimatedMarker = null; }
+ smoothBuffer.clear();
+ clearObsDots();
+ if (fusedUncertaintyCircle != null) { fusedUncertaintyCircle.remove(); fusedUncertaintyCircle = null; }
+ if (fusedPolyline != null) { fusedPolyline.remove(); fusedPolyline = null; }
+ if (smoothPolyline != null) { smoothPolyline.remove(); smoothPolyline = null; }
+
// Clear test point markers
for (Marker m : testPointMarkers) {
m.remove();
}
testPointMarkers.clear();
-
// Re-create empty polylines with your chosen colors
if (gMap != null) {
polyline = gMap.addPolyline(new PolylineOptions()
.color(Color.RED)
.width(5f)
.add());
- gnssPolyline = gMap.addPolyline(new PolylineOptions()
- .color(Color.BLUE)
- .width(5f)
+ fusedPolyline = gMap.addPolyline(new PolylineOptions()
+ .color(Color.rgb(156, 39, 176))
+ .width(7f)
+ .add());
+ smoothPolyline = gMap.addPolyline(new PolylineOptions()
+ .color(Color.rgb(76, 175, 80))
+ .width(7f)
.add());
+ smoothPolyline.setVisible(showSmoothPath);
}
}
+ /** Draws building outline polygons on the map using API data or hard-coded fallbacks. */
+ // Scale factor for buildings without API outline data.
+ // <1.0 shrinks toward centroid to approximate 2D map footprint.
+ private static final double NUCLEUS_SCALE = 0.88;
+ private static final double NKML_SCALE = 0.88;
+ private static final double FJB_SCALE = 0.88;
+ private static final double FARADAY_SCALE = 0.88;
+
/**
- * Draw the building polygon on the map
- *
- * The method draws a polygon representing the building on the map.
- * The polygon is drawn with specific vertices and colors to represent
- * different buildings or areas on the map.
- * The method removes the old polygon if it exists and adds the new polygon
- * to the map with the specified options.
- * The method logs the number of vertices in the polygon for debugging.
- *
- *
- * Note: The method uses hard-coded vertices for the building polygon.
- *
- *
- *
- * See: {@link com.google.android.gms.maps.model.PolygonOptions} The options for the new polygon.
+ * Scales polygon vertices toward their centroid.
+ * @param vertices original vertices
+ * @param factor scale factor (<1.0 shrinks, >1.0 enlarges)
+ * @return new array of scaled LatLng vertices
*/
+ private LatLng[] scalePolygon(LatLng[] vertices, double factor) {
+ double latSum = 0, lngSum = 0;
+ for (LatLng v : vertices) {
+ latSum += v.latitude;
+ lngSum += v.longitude;
+ }
+ double cLat = latSum / vertices.length;
+ double cLng = lngSum / vertices.length;
+
+ LatLng[] scaled = new LatLng[vertices.length];
+ for (int i = 0; i < vertices.length; i++) {
+ scaled[i] = new LatLng(
+ cLat + (vertices[i].latitude - cLat) * factor,
+ cLng + (vertices[i].longitude - cLng) * factor);
+ }
+ return scaled;
+ }
+
+ /**
+ * Looks up a building's outline polygon from the floorplan API cache.
+ * @param apiName building name (e.g. "nucleus_building", "library")
+ * @return outline points, or null if not available
+ */
+ private List getApiOutline(String apiName) {
+ FloorplanApiClient.BuildingInfo info =
+ sensorFusion.getFloorplanBuilding(apiName);
+ if (info != null) {
+ List outline = info.getOutlinePolygon();
+ if (outline != null && outline.size() >= 3) return outline;
+ }
+ return null;
+ }
+
private void drawBuildingPolygon() {
if (gMap == null) {
Log.e("TrajectoryMapFragment", "GoogleMap is not ready");
return;
}
- // nuclear building polygon vertices
- LatLng nucleus1 = new LatLng(55.92279538827796, -3.174612147506538);
- LatLng nucleus2 = new LatLng(55.92278121423647, -3.174107900816096);
- LatLng nucleus3 = new LatLng(55.92288405733954, -3.173843694667146);
- LatLng nucleus4 = new LatLng(55.92331786793876, -3.173832892645086);
- LatLng nucleus5 = new LatLng(55.923337194112555, -3.1746284301397387);
-
-
- // nkml building polygon vertices
- LatLng nkml1 = new LatLng(55.9230343434213, -3.1751847990731954);
- LatLng nkml2 = new LatLng(55.923032840563366, -3.174777103346131);
- LatLng nkml4 = new LatLng(55.92280139974615, -3.175195527934348);
- LatLng nkml3 = new LatLng(55.922793885410734, -3.1747958788136867);
-
- LatLng fjb1 = new LatLng(55.92269205199916, -3.1729563477188774);//left top
- LatLng fjb2 = new LatLng(55.922822801570994, -3.172594249522305);
- LatLng fjb3 = new LatLng(55.92223512226413, -3.171921917547244);
- LatLng fjb4 = new LatLng(55.9221071265519, -3.1722813131202097);
-
- LatLng faraday1 = new LatLng(55.92242866264128, -3.1719553662011815);
- LatLng faraday2 = new LatLng(55.9224966752294, -3.1717846714743474);
- LatLng faraday3 = new LatLng(55.922271383074154, -3.1715191463437162);
- LatLng faraday4 = new LatLng(55.92220124468304, -3.171705013935158);
-
-
-
- PolygonOptions buildingPolygonOptions = new PolygonOptions()
- .add(nucleus1, nucleus2, nucleus3, nucleus4, nucleus5)
- .strokeColor(Color.RED) // Red border
- .strokeWidth(10f) // Border width
- //.fillColor(Color.argb(50, 255, 0, 0)) // Semi-transparent red fill
- .zIndex(1); // Set a higher zIndex to ensure it appears above other overlays
-
- // Options for the new polygon
- PolygonOptions buildingPolygonOptions2 = new PolygonOptions()
- .add(nkml1, nkml2, nkml3, nkml4, nkml1)
- .strokeColor(Color.BLUE) // Blue border
- .strokeWidth(10f) // Border width
- // .fillColor(Color.argb(50, 0, 0, 255)) // Semi-transparent blue fill
- .zIndex(1); // Set a higher zIndex to ensure it appears above other overlays
-
- PolygonOptions buildingPolygonOptions3 = new PolygonOptions()
- .add(fjb1, fjb2, fjb3, fjb4, fjb1)
- .strokeColor(Color.GREEN) // Green border
- .strokeWidth(10f) // Border width
- //.fillColor(Color.argb(50, 0, 255, 0)) // Semi-transparent green fill
- .zIndex(1); // Set a higher zIndex to ensure it appears above other overlays
+ // Remove old polygons
+ if (buildingPolygon != null) {
+ buildingPolygon.remove();
+ }
- PolygonOptions buildingPolygonOptions4 = new PolygonOptions()
- .add(faraday1, faraday2, faraday3, faraday4, faraday1)
- .strokeColor(Color.YELLOW) // Yellow border
- .strokeWidth(10f) // Border width
- //.fillColor(Color.argb(50, 255, 255, 0)) // Semi-transparent yellow fill
- .zIndex(1); // Set a higher zIndex to ensure it appears above other overlays
+ // --- Nucleus + Library connected shape (RED, zIndex=1) ---
+ // API "nucleus_building" outline covers both Nucleus and Library as
+ // one connected polygon. Draw it first in red as the base layer.
+ List nucleusOutline = getApiOutline("nucleus_building");
+ if (nucleusOutline != null) {
+ PolygonOptions nucleusOpts = new PolygonOptions()
+ .addAll(nucleusOutline)
+ .strokeColor(Color.RED).strokeWidth(10f).zIndex(1);
+ buildingPolygon = gMap.addPolygon(nucleusOpts);
+ if (DEBUG) Log.d(TAG, "Nucleus polygon from API, vertices: " + nucleusOutline.size());
+ }
+ // --- Library (BLUE, zIndex=2) ---
+ // Overlays on top of the red, so the Library portion shows blue.
+ List libraryOutline = getApiOutline("library");
+ if (libraryOutline != null) {
+ PolygonOptions libOpts = new PolygonOptions()
+ .addAll(libraryOutline)
+ .strokeColor(Color.BLUE).strokeWidth(10f).zIndex(2);
+ gMap.addPolygon(libOpts);
+ if (DEBUG) Log.d(TAG, "Library polygon from API, vertices: " + libraryOutline.size());
+ }
- // Remove the old polygon if it exists
- if (buildingPolygon != null) {
- buildingPolygon.remove();
+ // --- Murchison House (MAGENTA) ---
+ List murchisonOutline = getApiOutline("murchison_house");
+ if (murchisonOutline != null) {
+ PolygonOptions murOpts = new PolygonOptions()
+ .addAll(murchisonOutline)
+ .strokeColor(Color.MAGENTA).strokeWidth(10f).zIndex(1);
+ gMap.addPolygon(murOpts);
+ if (DEBUG) Log.d(TAG, "Murchison polygon from API, vertices: " + murchisonOutline.size());
}
- // Add the polygon to the map
- buildingPolygon = gMap.addPolygon(buildingPolygonOptions);
- gMap.addPolygon(buildingPolygonOptions2);
- gMap.addPolygon(buildingPolygonOptions3);
- gMap.addPolygon(buildingPolygonOptions4);
- Log.d(TAG, "Building polygon added, vertex count: " + buildingPolygon.getPoints().size());
+ // --- FJB (GREEN): hardcoded + scaled (no API data) ---
+ LatLng[] fjbRaw = {
+ new LatLng(55.92269205199916, -3.1729563477188774),
+ new LatLng(55.922822801570994, -3.172594249522305),
+ new LatLng(55.92223512226413, -3.171921917547244),
+ new LatLng(55.9221071265519, -3.1722813131202097)
+ };
+ LatLng[] fjb = scalePolygon(fjbRaw, FJB_SCALE);
+ PolygonOptions fjbOpts = new PolygonOptions()
+ .strokeColor(Color.GREEN).strokeWidth(10f).zIndex(1);
+ for (LatLng v : fjb) fjbOpts.add(v);
+ fjbOpts.add(fjb[0]);
+ gMap.addPolygon(fjbOpts);
+
+ // --- Faraday (YELLOW): hardcoded + scaled (no API data) ---
+ LatLng[] faradayRaw = {
+ new LatLng(55.92242866264128, -3.1719553662011815),
+ new LatLng(55.9224966752294, -3.1717846714743474),
+ new LatLng(55.922271383074154, -3.1715191463437162),
+ new LatLng(55.92220124468304, -3.171705013935158)
+ };
+ LatLng[] faraday = scalePolygon(faradayRaw, FARADAY_SCALE);
+ PolygonOptions faradayOpts = new PolygonOptions()
+ .strokeColor(Color.YELLOW).strokeWidth(10f).zIndex(1);
+ for (LatLng v : faraday) faradayOpts.add(v);
+ faradayOpts.add(faraday[0]);
+ gMap.addPolygon(faradayOpts);
+
+ if (DEBUG) Log.d(TAG, "Building polygons drawn");
}
//region Auto-floor logic
@@ -629,9 +1504,52 @@ private void startAutoFloor() {
lastCandidateFloor = Integer.MIN_VALUE;
lastCandidateTime = 0;
- // Immediately jump to the best-guess floor (skip debounce on first toggle)
- applyImmediateFloor();
+ // 1. Try immediate WiFi floor seed
+ boolean wifiSeeded = false;
+ if (sensorFusion != null && indoorMapManager != null
+ && indoorMapManager.getIsIndoorMapSet()) {
+ int wifiFloor = sensorFusion.getWifiFloor();
+ if (wifiFloor >= 0) {
+ // reseedFloor: resets PdrProcessing baro baseline + FusionManager + PF
+ sensorFusion.reseedFloor(wifiFloor);
+ indoorMapManager.setCurrentFloor(wifiFloor, true);
+ updateFloorLabel();
+ lastCandidateFloor = wifiFloor;
+ lastCandidateTime = SystemClock.elapsedRealtime();
+ wifiSeeded = true;
+ if (DEBUG) Log.i(TAG, "[AutoFloor] Immediate WiFi seed floor=" + wifiFloor);
+ }
+ }
+ if (!wifiSeeded) {
+ // Fallback to PF/baro
+ applyImmediateFloor();
+ }
+
+ // 2. Open 10s WiFi floor refresh window — accept fresh WiFi responses as re-seed
+ if (sensorFusion != null) {
+ sensorFusion.setWifiFloorCallback(floor -> {
+ // Volley delivers on main thread, safe to update UI directly
+ if (indoorMapManager != null && indoorMapManager.getIsIndoorMapSet()
+ && floor >= 0) {
+ sensorFusion.reseedFloor(floor);
+ indoorMapManager.setCurrentFloor(floor, true);
+ updateFloorLabel();
+ lastCandidateFloor = floor;
+ lastCandidateTime = SystemClock.elapsedRealtime();
+ if (DEBUG) Log.i(TAG, "[AutoFloor] WiFi re-seed floor=" + floor);
+ }
+ });
+
+ // 3. Close WiFi seed window after 10s
+ wifiSeedTimeoutTask = () -> {
+ sensorFusion.setWifiFloorCallback(null);
+ wifiSeedTimeoutTask = null;
+ if (DEBUG) Log.d(TAG, "[AutoFloor] WiFi seed window closed (10s)");
+ };
+ autoFloorHandler.postDelayed(wifiSeedTimeoutTask, WIFI_SEED_WINDOW_MS);
+ }
+ // 4. Start periodic baro-based evaluation
autoFloorTask = new Runnable() {
@Override
public void run() {
@@ -640,7 +1558,7 @@ public void run() {
}
};
autoFloorHandler.post(autoFloorTask);
- Log.d(TAG, "Auto-floor started");
+ if (DEBUG) Log.d(TAG, "Auto-floor started");
}
/**
@@ -653,13 +1571,16 @@ private void applyImmediateFloor() {
if (!indoorMapManager.getIsIndoorMapSet()) return;
int candidateFloor;
- if (sensorFusion.getLatLngWifiPositioning() != null) {
- candidateFloor = sensorFusion.getWifiFloor();
+ // Priority: fused floor (PF consensus) > barometric floor (PdrProcessing)
+ com.openpositioning.PositionMe.sensors.fusion.FusionManager fm =
+ sensorFusion.getFusionManager();
+ if (fm != null && fm.isActive()) {
+ candidateFloor = fm.getFusedFloor();
} else {
- float elevation = sensorFusion.getElevation();
- float floorHeight = indoorMapManager.getFloorHeight();
- if (floorHeight <= 0) return;
- candidateFloor = Math.round(elevation / floorHeight);
+ // Use PdrProcessing's floor directly — it already accounts for
+ // initialFloorOffset and hysteresis. Raw elevation/floorHeight
+ // does not include the offset and gives wrong results after reseed.
+ candidateFloor = sensorFusion.getBaroFloor();
}
indoorMapManager.setCurrentFloor(candidateFloor, true);
@@ -673,12 +1594,22 @@ private void applyImmediateFloor() {
* Stops the periodic auto-floor evaluation and resets debounce state.
*/
private void stopAutoFloor() {
- if (autoFloorHandler != null && autoFloorTask != null) {
- autoFloorHandler.removeCallbacks(autoFloorTask);
+ // Clear WiFi floor callback and pending timeout
+ if (sensorFusion != null) {
+ sensorFusion.setWifiFloorCallback(null);
+ }
+ if (autoFloorHandler != null) {
+ if (autoFloorTask != null) {
+ autoFloorHandler.removeCallbacks(autoFloorTask);
+ }
+ if (wifiSeedTimeoutTask != null) {
+ autoFloorHandler.removeCallbacks(wifiSeedTimeoutTask);
+ wifiSeedTimeoutTask = null;
+ }
}
lastCandidateFloor = Integer.MIN_VALUE;
lastCandidateTime = 0;
- Log.d(TAG, "Auto-floor stopped");
+ if (DEBUG) Log.d(TAG, "Auto-floor stopped");
}
/**
@@ -692,15 +1623,47 @@ private void evaluateAutoFloor() {
int candidateFloor;
- // Priority 1: WiFi-based floor (only if WiFi positioning has returned data)
- if (sensorFusion.getLatLngWifiPositioning() != null) {
- candidateFloor = sensorFusion.getWifiFloor();
+ // Use PF floor probability distribution for robust floor estimation
+ com.openpositioning.PositionMe.sensors.fusion.FusionManager fm =
+ sensorFusion.getFusionManager();
+ if (fm != null && fm.isActive()) {
+ double[] probs = fm.getFloorProbabilities();
+ double topProb = 0, secondProb = 0;
+ int topFloor = 0;
+ for (int i = 0; i < probs.length; i++) {
+ if (probs[i] > topProb) {
+ secondProb = topProb;
+ topProb = probs[i];
+ topFloor = i;
+ } else if (probs[i] > secondProb) {
+ secondProb = probs[i];
+ }
+ }
+ // Log floor posterior for debugging
+ if (DEBUG) Log.d(TAG, String.format("[AutoFloor] source=PF topFloor=%d topProb=%.2f secondProb=%.2f fusedFloor=%d",
+ topFloor, topProb, secondProb, fm.getFusedFloor()));
+
+ // Only accept if confident (>0.6 or margin >0.2)
+ if (topProb > 0.6 || (topProb - secondProb) > 0.2) {
+ candidateFloor = topFloor;
+ } else {
+ return; // not confident enough, keep current
+ }
} else {
- // Fallback: barometric elevation estimate
- float elevation = sensorFusion.getElevation();
- float floorHeight = indoorMapManager.getFloorHeight();
- if (floorHeight <= 0) return;
- candidateFloor = Math.round(elevation / floorHeight);
+ // Baro-only fallback: use PdrProcessing's floor directly —
+ // it accounts for initialFloorOffset and hysteresis.
+ candidateFloor = sensorFusion.getBaroFloor();
+ }
+
+ // Only allow floor change if near stairs or lift (within 5m)
+ int currentFloorIdx = indoorMapManager.getCurrentFloor();
+ if (candidateFloor != currentFloorIdx) {
+ LatLng pos = currentLocation;
+ if (pos != null && !indoorMapManager.isNearStairsOrLift(pos, 5.0)) {
+ if (DEBUG) Log.d(TAG, String.format("[AutoFloor] BLOCKED floor change %d→%d (not near stairs/lift)",
+ currentFloorIdx, candidateFloor));
+ return;
+ }
}
// Debounce: require the same floor reading for AUTO_FLOOR_DEBOUNCE_MS
@@ -708,16 +1671,53 @@ private void evaluateAutoFloor() {
if (candidateFloor != lastCandidateFloor) {
lastCandidateFloor = candidateFloor;
lastCandidateTime = now;
+ // Snapshot elevation at the start of a potential floor transition
+ elevationAtFloorChangeStart = sensorFusion.getElevation();
+ floorChangeStartTimeMs = now;
return;
}
if (now - lastCandidateTime >= AUTO_FLOOR_DEBOUNCE_MS) {
+ // Classify transition as elevator or stairs (debug log only)
+ classifyFloorTransition();
+
indoorMapManager.setCurrentFloor(candidateFloor, true);
updateFloorLabel();
- // Reset timer so we don't keep re-applying the same floor
lastCandidateTime = now;
}
}
+ /**
+ * Classifies a confirmed floor transition as elevator or stairs based on
+ * the barometric elevation change rate. Pure observation — does not alter
+ * any floor detection or navigation logic.
+ *
+ * Elevator: rapid vertical displacement (>{@value ELEVATOR_RATE_THRESHOLD} m/s).
+ * Stairs: slower, gradual height change.
+ */
+ private void classifyFloorTransition() {
+ if (Float.isNaN(elevationAtFloorChangeStart) || sensorFusion == null) {
+ lastTransitionMethod = "unknown";
+ return;
+ }
+
+ float currentElevation = sensorFusion.getElevation();
+ float elevDelta = Math.abs(currentElevation - elevationAtFloorChangeStart);
+ long durationMs = SystemClock.elapsedRealtime() - floorChangeStartTimeMs;
+ float durationSec = durationMs / 1000f;
+
+ if (durationSec < 0.5f) {
+ lastTransitionMethod = "unknown";
+ return;
+ }
+
+ float rate = elevDelta / durationSec;
+ lastTransitionMethod = (rate > ELEVATOR_RATE_THRESHOLD) ? "elevator" : "stairs";
+
+ if (DEBUG) Log.i(TAG, String.format(
+ "[FloorTransition] method=%s elevDelta=%.2fm duration=%.1fs rate=%.2fm/s",
+ lastTransitionMethod, elevDelta, durationSec, rate));
+ }
+
//endregion
}
diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/view/CompassView.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/view/CompassView.java
new file mode 100644
index 00000000..bad80831
--- /dev/null
+++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/view/CompassView.java
@@ -0,0 +1,162 @@
+package com.openpositioning.PositionMe.presentation.view;
+
+import android.content.Context;
+import android.graphics.Canvas;
+import android.graphics.Color;
+import android.graphics.Paint;
+import android.graphics.Path;
+import android.graphics.Typeface;
+import android.util.AttributeSet;
+import android.view.View;
+
+/**
+ * A lightweight compass dial that rotates to reflect the device's current
+ * magnetic heading. Drawn entirely with Canvas for crisp rendering at any size.
+ *
+ * Call {@link #setHeading(float)} with the azimuth in degrees (0 = north,
+ * clockwise). The dial rotates so that the "N" label always points towards
+ * magnetic north on screen.
+ */
+public class CompassView extends View {
+
+ private float heading = 0f;
+
+ private final Paint bgPaint = new Paint(Paint.ANTI_ALIAS_FLAG);
+ private final Paint rimPaint = new Paint(Paint.ANTI_ALIAS_FLAG);
+ private final Paint tickPaint = new Paint(Paint.ANTI_ALIAS_FLAG);
+ private final Paint labelPaint = new Paint(Paint.ANTI_ALIAS_FLAG);
+ private final Paint northPaint = new Paint(Paint.ANTI_ALIAS_FLAG);
+ private final Paint southPaint = new Paint(Paint.ANTI_ALIAS_FLAG);
+ private final Paint centerPaint = new Paint(Paint.ANTI_ALIAS_FLAG);
+ private final Paint nLabelPaint = new Paint(Paint.ANTI_ALIAS_FLAG);
+
+ private final Path needlePath = new Path();
+
+ public CompassView(Context context) {
+ super(context);
+ init();
+ }
+
+ public CompassView(Context context, AttributeSet attrs) {
+ super(context, attrs);
+ init();
+ }
+
+ public CompassView(Context context, AttributeSet attrs, int defStyleAttr) {
+ super(context, attrs, defStyleAttr);
+ init();
+ }
+
+ private void init() {
+ bgPaint.setColor(Color.argb(200, 255, 255, 255));
+ bgPaint.setStyle(Paint.Style.FILL);
+
+ rimPaint.setColor(Color.argb(100, 0, 0, 0));
+ rimPaint.setStyle(Paint.Style.STROKE);
+ rimPaint.setStrokeWidth(2f);
+
+ tickPaint.setColor(Color.DKGRAY);
+ tickPaint.setStyle(Paint.Style.STROKE);
+ tickPaint.setStrokeCap(Paint.Cap.ROUND);
+
+ labelPaint.setColor(Color.DKGRAY);
+ labelPaint.setTextAlign(Paint.Align.CENTER);
+ labelPaint.setTypeface(Typeface.DEFAULT_BOLD);
+
+ nLabelPaint.setColor(Color.RED);
+ nLabelPaint.setTextAlign(Paint.Align.CENTER);
+ nLabelPaint.setTypeface(Typeface.DEFAULT_BOLD);
+
+ northPaint.setColor(Color.RED);
+ northPaint.setStyle(Paint.Style.FILL);
+
+ southPaint.setColor(Color.WHITE);
+ southPaint.setStyle(Paint.Style.FILL);
+
+ centerPaint.setColor(Color.DKGRAY);
+ centerPaint.setStyle(Paint.Style.FILL);
+ }
+
+ /**
+ * Set the current heading in degrees (0 = north, clockwise).
+ * The dial rotates so "N" always points toward magnetic north.
+ */
+ public void setHeading(float degrees) {
+ this.heading = degrees;
+ invalidate();
+ }
+
+ @Override
+ protected void onDraw(Canvas canvas) {
+ super.onDraw(canvas);
+
+ float cx = getWidth() / 2f;
+ float cy = getHeight() / 2f;
+ float radius = Math.min(cx, cy) - 2f;
+ if (radius <= 0) return;
+
+ // Background circle
+ canvas.drawCircle(cx, cy, radius, bgPaint);
+ canvas.drawCircle(cx, cy, radius, rimPaint);
+
+ // Rotate entire dial so North points to magnetic north
+ canvas.save();
+ canvas.rotate(-heading, cx, cy);
+
+ // Tick marks
+ float majorLen = radius * 0.15f;
+ float minorLen = radius * 0.08f;
+ tickPaint.setStrokeWidth(radius * 0.02f);
+ for (int deg = 0; deg < 360; deg += 10) {
+ boolean isMajor = (deg % 30 == 0);
+ float len = isMajor ? majorLen : minorLen;
+ float startR = radius - len;
+ float endR = radius - 1f;
+ float rad = (float) Math.toRadians(deg);
+ float sin = (float) Math.sin(rad);
+ float cos = (float) Math.cos(rad);
+ canvas.drawLine(
+ cx + startR * sin, cy - startR * cos,
+ cx + endR * sin, cy - endR * cos,
+ tickPaint);
+ }
+
+ // Cardinal labels
+ float labelSize = radius * 0.28f;
+ labelPaint.setTextSize(labelSize);
+ nLabelPaint.setTextSize(labelSize);
+ float labelR = radius * 0.62f;
+
+ // N (red)
+ canvas.drawText("N", cx, cy - labelR + labelSize * 0.35f, nLabelPaint);
+ // S
+ canvas.drawText("S", cx, cy + labelR + labelSize * 0.35f, labelPaint);
+ // E
+ canvas.drawText("E", cx + labelR, cy + labelSize * 0.35f, labelPaint);
+ // W
+ canvas.drawText("W", cx - labelR, cy + labelSize * 0.35f, labelPaint);
+
+ // Needle — north half (red triangle pointing up)
+ float needleHalfW = radius * 0.08f;
+ float needleLen = radius * 0.40f;
+ needlePath.reset();
+ needlePath.moveTo(cx, cy - needleLen);
+ needlePath.lineTo(cx - needleHalfW, cy);
+ needlePath.lineTo(cx + needleHalfW, cy);
+ needlePath.close();
+ canvas.drawPath(needlePath, northPaint);
+
+ // Needle — south half (white triangle pointing down)
+ needlePath.reset();
+ needlePath.moveTo(cx, cy + needleLen);
+ needlePath.lineTo(cx - needleHalfW, cy);
+ needlePath.lineTo(cx + needleHalfW, cy);
+ needlePath.close();
+ canvas.drawPath(needlePath, southPaint);
+
+ canvas.restore();
+
+ // Center dot (drawn after restore so it doesn't rotate)
+ canvas.drawCircle(cx, cy, radius * 0.06f, centerPaint);
+ }
+}
diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/view/WeatherView.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/view/WeatherView.java
new file mode 100644
index 00000000..c7c034f0
--- /dev/null
+++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/view/WeatherView.java
@@ -0,0 +1,238 @@
+package com.openpositioning.PositionMe.presentation.view;
+
+import android.content.Context;
+import android.graphics.Canvas;
+import android.graphics.Color;
+import android.graphics.Paint;
+import android.graphics.Path;
+import android.graphics.RectF;
+import android.graphics.Typeface;
+import android.util.AttributeSet;
+import android.view.View;
+
+/**
+ * A lightweight weather indicator drawn with Canvas.
+ * Shows a weather icon based on WMO weather code and temperature text below.
+ *
+ * Call {@link #setWeather(int, double)} with the WMO weather code and
+ * temperature in Celsius to update the display.
+ */
+public class WeatherView extends View {
+
+ private int weatherCode = -1;
+ private double temperature = Double.NaN;
+
+ private final Paint bgPaint = new Paint(Paint.ANTI_ALIAS_FLAG);
+ private final Paint rimPaint = new Paint(Paint.ANTI_ALIAS_FLAG);
+ private final Paint sunPaint = new Paint(Paint.ANTI_ALIAS_FLAG);
+ private final Paint sunRayPaint = new Paint(Paint.ANTI_ALIAS_FLAG);
+ private final Paint cloudPaint = new Paint(Paint.ANTI_ALIAS_FLAG);
+ private final Paint cloudOutlinePaint = new Paint(Paint.ANTI_ALIAS_FLAG);
+ private final Paint rainPaint = new Paint(Paint.ANTI_ALIAS_FLAG);
+ private final Paint snowPaint = new Paint(Paint.ANTI_ALIAS_FLAG);
+ private final Paint fogPaint = new Paint(Paint.ANTI_ALIAS_FLAG);
+ private final Paint boltPaint = new Paint(Paint.ANTI_ALIAS_FLAG);
+ private final Paint tempPaint = new Paint(Paint.ANTI_ALIAS_FLAG);
+ private final Paint labelPaint = new Paint(Paint.ANTI_ALIAS_FLAG);
+
+ private final Path path = new Path();
+
+ public WeatherView(Context context) {
+ super(context);
+ init();
+ }
+
+ public WeatherView(Context context, AttributeSet attrs) {
+ super(context, attrs);
+ init();
+ }
+
+ public WeatherView(Context context, AttributeSet attrs, int defStyleAttr) {
+ super(context, attrs, defStyleAttr);
+ init();
+ }
+
+ private void init() {
+ bgPaint.setColor(Color.argb(200, 255, 255, 255));
+ bgPaint.setStyle(Paint.Style.FILL);
+
+ rimPaint.setColor(Color.argb(100, 0, 0, 0));
+ rimPaint.setStyle(Paint.Style.STROKE);
+ rimPaint.setStrokeWidth(2f);
+
+ sunPaint.setColor(Color.rgb(255, 193, 7)); // Amber
+ sunPaint.setStyle(Paint.Style.FILL);
+
+ sunRayPaint.setColor(Color.rgb(255, 193, 7));
+ sunRayPaint.setStyle(Paint.Style.STROKE);
+ sunRayPaint.setStrokeCap(Paint.Cap.ROUND);
+
+ cloudPaint.setColor(Color.rgb(224, 224, 224));
+ cloudPaint.setStyle(Paint.Style.FILL);
+
+ cloudOutlinePaint.setColor(Color.rgb(158, 158, 158));
+ cloudOutlinePaint.setStyle(Paint.Style.STROKE);
+ cloudOutlinePaint.setStrokeWidth(1.5f);
+
+ rainPaint.setColor(Color.rgb(33, 150, 243)); // Blue
+ rainPaint.setStyle(Paint.Style.STROKE);
+ rainPaint.setStrokeCap(Paint.Cap.ROUND);
+
+ snowPaint.setColor(Color.rgb(144, 202, 249)); // Light blue
+ snowPaint.setStyle(Paint.Style.FILL);
+
+ fogPaint.setColor(Color.rgb(189, 189, 189));
+ fogPaint.setStyle(Paint.Style.STROKE);
+ fogPaint.setStrokeCap(Paint.Cap.ROUND);
+
+ boltPaint.setColor(Color.rgb(255, 235, 59)); // Yellow
+ boltPaint.setStyle(Paint.Style.FILL);
+
+ tempPaint.setColor(Color.DKGRAY);
+ tempPaint.setTextAlign(Paint.Align.CENTER);
+ tempPaint.setTypeface(Typeface.DEFAULT_BOLD);
+
+ labelPaint.setColor(Color.GRAY);
+ labelPaint.setTextAlign(Paint.Align.CENTER);
+ }
+
+ /**
+ * Update weather display.
+ * @param wmoCode WMO weather code (0=clear, 1-3=cloudy, 51-67=rain, 71-77=snow, 95+=thunder)
+ * @param tempC temperature in Celsius
+ */
+ public void setWeather(int wmoCode, double tempC) {
+ this.weatherCode = wmoCode;
+ this.temperature = tempC;
+ invalidate();
+ }
+
+ @Override
+ protected void onDraw(Canvas canvas) {
+ super.onDraw(canvas);
+
+ float w = getWidth();
+ float h = getHeight();
+ float iconAreaH = h * 0.78f;
+ float cx = w / 2f;
+ float iconCy = iconAreaH * 0.45f;
+ float radius = Math.min(cx, iconAreaH * 0.5f) - 2f;
+ if (radius <= 0) return;
+
+ // Background rounded rect
+ RectF bg = new RectF(1, 1, w - 1, h - 1);
+ canvas.drawRoundRect(bg, 12f, 12f, bgPaint);
+ canvas.drawRoundRect(bg, 12f, 12f, rimPaint);
+
+ if (weatherCode < 0) {
+ // No data yet — draw "?"
+ tempPaint.setTextSize(radius * 0.6f);
+ canvas.drawText("?", cx, iconCy + radius * 0.2f, tempPaint);
+ } else {
+ drawWeatherIcon(canvas, cx, iconCy, radius);
+ }
+
+ // Temperature text below icon — tight gap
+ if (!Double.isNaN(temperature)) {
+ tempPaint.setTextSize(h * 0.20f);
+ String temp = String.format("%.0f°C", temperature);
+ canvas.drawText(temp, cx, h - h * 0.03f, tempPaint);
+ }
+ }
+
+ private void drawWeatherIcon(Canvas canvas, float cx, float cy, float r) {
+ if (weatherCode == 0) {
+ // Clear sky — sun
+ drawSun(canvas, cx, cy, r * 0.80f);
+ } else if (weatherCode <= 3) {
+ // Partly cloudy — sun + cloud
+ drawSun(canvas, cx - r * 0.2f, cy - r * 0.15f, r * 0.50f);
+ drawCloud(canvas, cx + r * 0.1f, cy + r * 0.1f, r * 0.65f);
+ } else if (weatherCode <= 48) {
+ // Fog
+ drawFog(canvas, cx, cy, r * 0.70f);
+ } else if (weatherCode <= 67) {
+ // Rain / drizzle
+ drawCloud(canvas, cx, cy - r * 0.15f, r * 0.65f);
+ drawRain(canvas, cx, cy + r * 0.30f, r * 0.55f);
+ } else if (weatherCode <= 77) {
+ // Snow
+ drawCloud(canvas, cx, cy - r * 0.15f, r * 0.65f);
+ drawSnow(canvas, cx, cy + r * 0.30f, r * 0.55f);
+ } else if (weatherCode <= 82) {
+ // Rain showers
+ drawCloud(canvas, cx, cy - r * 0.15f, r * 0.70f);
+ drawRain(canvas, cx, cy + r * 0.30f, r * 0.60f);
+ } else if (weatherCode <= 86) {
+ // Snow showers
+ drawCloud(canvas, cx, cy - r * 0.15f, r * 0.70f);
+ drawSnow(canvas, cx, cy + r * 0.30f, r * 0.60f);
+ } else {
+ // Thunderstorm
+ drawCloud(canvas, cx, cy - r * 0.2f, r * 0.70f);
+ drawBolt(canvas, cx, cy + r * 0.15f, r * 0.50f);
+ }
+ }
+
+ private void drawSun(Canvas canvas, float cx, float cy, float r) {
+ canvas.drawCircle(cx, cy, r * 0.5f, sunPaint);
+ sunRayPaint.setStrokeWidth(r * 0.1f);
+ for (int i = 0; i < 8; i++) {
+ float angle = (float) Math.toRadians(i * 45);
+ float sin = (float) Math.sin(angle);
+ float cos = (float) Math.cos(angle);
+ canvas.drawLine(
+ cx + r * 0.65f * sin, cy - r * 0.65f * cos,
+ cx + r * 0.9f * sin, cy - r * 0.9f * cos,
+ sunRayPaint);
+ }
+ }
+
+ private void drawCloud(Canvas canvas, float cx, float cy, float r) {
+ canvas.drawCircle(cx - r * 0.3f, cy, r * 0.35f, cloudPaint);
+ canvas.drawCircle(cx + r * 0.3f, cy, r * 0.3f, cloudPaint);
+ canvas.drawCircle(cx, cy - r * 0.2f, r * 0.38f, cloudPaint);
+ canvas.drawRect(cx - r * 0.55f, cy, cx + r * 0.55f, cy + r * 0.25f, cloudPaint);
+
+ canvas.drawCircle(cx - r * 0.3f, cy, r * 0.35f, cloudOutlinePaint);
+ canvas.drawCircle(cx + r * 0.3f, cy, r * 0.3f, cloudOutlinePaint);
+ canvas.drawCircle(cx, cy - r * 0.2f, r * 0.38f, cloudOutlinePaint);
+ }
+
+ private void drawRain(Canvas canvas, float cx, float cy, float r) {
+ rainPaint.setStrokeWidth(r * 0.12f);
+ for (int i = -1; i <= 1; i++) {
+ float x = cx + i * r * 0.35f;
+ canvas.drawLine(x, cy, x - r * 0.1f, cy + r * 0.35f, rainPaint);
+ }
+ }
+
+ private void drawSnow(Canvas canvas, float cx, float cy, float r) {
+ float dotR = r * 0.1f;
+ for (int i = -1; i <= 1; i++) {
+ float x = cx + i * r * 0.35f;
+ canvas.drawCircle(x, cy + r * 0.1f, dotR, snowPaint);
+ canvas.drawCircle(x + r * 0.15f, cy + r * 0.3f, dotR, snowPaint);
+ }
+ }
+
+ private void drawFog(Canvas canvas, float cx, float cy, float r) {
+ fogPaint.setStrokeWidth(r * 0.12f);
+ for (int i = -1; i <= 1; i++) {
+ float y = cy + i * r * 0.35f;
+ canvas.drawLine(cx - r * 0.6f, y, cx + r * 0.6f, y, fogPaint);
+ }
+ }
+
+ private void drawBolt(Canvas canvas, float cx, float cy, float r) {
+ path.reset();
+ path.moveTo(cx + r * 0.1f, cy - r * 0.5f);
+ path.lineTo(cx - r * 0.2f, cy + r * 0.05f);
+ path.lineTo(cx + r * 0.05f, cy + r * 0.05f);
+ path.lineTo(cx - r * 0.1f, cy + r * 0.5f);
+ path.lineTo(cx + r * 0.25f, cy - r * 0.1f);
+ path.lineTo(cx, cy - r * 0.1f);
+ path.close();
+ canvas.drawPath(path, boltPaint);
+ }
+}
diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/view/WifiSignalView.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/view/WifiSignalView.java
new file mode 100644
index 00000000..7e92fad1
--- /dev/null
+++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/view/WifiSignalView.java
@@ -0,0 +1,104 @@
+package com.openpositioning.PositionMe.presentation.view;
+
+import android.content.Context;
+import android.graphics.Canvas;
+import android.graphics.Color;
+import android.graphics.Paint;
+import android.graphics.RectF;
+import android.util.AttributeSet;
+import android.view.View;
+
+/**
+ * A compact WiFi signal quality indicator that draws 3 vertical bars.
+ * Bars fill from left to right; unfilled bars are drawn in light grey.
+ *
+ * Call {@link #setLevel(int)} with 0–3 to update the display:
+ *
+ * - 0 = no data (all grey)
+ * - 1 = weak / REJECTED (1 bar, red-orange)
+ * - 2 = medium / AMBIGUOUS (2 bars, amber)
+ * - 3 = strong / GOOD (3 bars, green)
+ *
+ */
+public class WifiSignalView extends View {
+
+ private int level = 0; // 0..3
+
+ private final Paint filledPaint = new Paint(Paint.ANTI_ALIAS_FLAG);
+ private final Paint emptyPaint = new Paint(Paint.ANTI_ALIAS_FLAG);
+
+ private static final int COLOR_STRONG = Color.rgb( 76, 175, 80); // green
+ private static final int COLOR_MEDIUM = Color.rgb(255, 193, 7); // amber
+ private static final int COLOR_WEAK = Color.rgb(255, 87, 34); // deep orange
+ private static final int COLOR_EMPTY = Color.rgb(200, 200, 200); // light grey
+
+ public WifiSignalView(Context context) {
+ super(context);
+ init();
+ }
+
+ public WifiSignalView(Context context, AttributeSet attrs) {
+ super(context, attrs);
+ init();
+ }
+
+ public WifiSignalView(Context context, AttributeSet attrs, int defStyleAttr) {
+ super(context, attrs, defStyleAttr);
+ init();
+ }
+
+ private void init() {
+ emptyPaint.setColor(COLOR_EMPTY);
+ emptyPaint.setStyle(Paint.Style.FILL);
+ filledPaint.setStyle(Paint.Style.FILL);
+ }
+
+ /**
+ * Sets the signal level (0–3) and redraws.
+ */
+ public void setLevel(int level) {
+ this.level = Math.max(0, Math.min(3, level));
+ invalidate();
+ }
+
+ /** Returns the current signal level (0-3). */
+ public int getLevel() {
+ return level;
+ }
+
+ @Override
+ protected void onDraw(Canvas canvas) {
+ super.onDraw(canvas);
+
+ float w = getWidth();
+ float h = getHeight();
+ if (w <= 0 || h <= 0) return;
+
+ // Choose colour based on level
+ int activeColor;
+ switch (level) {
+ case 3: activeColor = COLOR_STRONG; break;
+ case 2: activeColor = COLOR_MEDIUM; break;
+ case 1: activeColor = COLOR_WEAK; break;
+ default: activeColor = COLOR_EMPTY; break;
+ }
+ filledPaint.setColor(activeColor);
+
+ // Layout: 3 bars with small gaps.
+ // Bar heights: 40%, 70%, 100% of view height.
+ float gap = w * 0.10f;
+ float barWidth = (w - gap * 2f) / 3f;
+ float[] heightFractions = {0.40f, 0.70f, 1.00f};
+ float cornerRadius = barWidth * 0.25f;
+
+ for (int i = 0; i < 3; i++) {
+ float barH = h * heightFractions[i];
+ float left = i * (barWidth + gap);
+ float top = h - barH;
+ float right = left + barWidth;
+
+ Paint paint = (i < level) ? filledPaint : emptyPaint;
+ canvas.drawRoundRect(new RectF(left, top, right, h), cornerRadius, cornerRadius, paint);
+ }
+ }
+}
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..e7dab385 100644
--- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java
+++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java
@@ -1,11 +1,14 @@
package com.openpositioning.PositionMe.sensors;
+import static com.openpositioning.PositionMe.BuildConstants.DEBUG;
+
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorManager;
import android.os.SystemClock;
import android.util.Log;
+import com.openpositioning.PositionMe.sensors.fusion.FusionManager;
import com.openpositioning.PositionMe.utils.PathView;
import com.openpositioning.PositionMe.utils.PdrProcessing;
@@ -29,6 +32,7 @@ public class SensorEventHandler {
private final PdrProcessing pdrProcessing;
private final PathView pathView;
private final TrajectoryRecorder recorder;
+ private FusionManager fusionManager;
// Timestamp tracking
private final HashMap lastEventTimestamps = new HashMap<>();
@@ -36,9 +40,33 @@ public class SensorEventHandler {
private long lastStepTime = 0;
private long bootTime;
- // Acceleration magnitude buffer between steps
private final List accelMagnitude = new ArrayList<>();
+ // Movement gating: reject false steps when stationary
+ private static final double MOVEMENT_THRESHOLD = 0.4; // m/s², variance below = stationary
+ private int validStepCount = 0;
+
+ // Heading debug logging
+ private long lastHeadingLogTime = 0;
+
+ // Heading stuck detection
+ private int consecutiveNorthSteps = 0;
+ private static final double NORTH_THRESHOLD_DEG = 15.0;
+
+ // Barometer floor tracking
+ private int lastBaroFloor = 0;
+
+ // Heading calibration: offset between TYPE_ROTATION_VECTOR (magnetic north)
+ // and TYPE_GAME_ROTATION_VECTOR (arbitrary reference).
+ // Computed once at startup, then applied as a fixed correction.
+ private float headingCalibrationOffset = 0;
+ private boolean headingCalibrated = false;
+ private float lastMagneticHeading = 0; // latest TYPE_ROTATION_VECTOR azimuth (radians)
+ private boolean magneticHeadingReady = false;
+ private int calibrationSampleCount = 0;
+ private static final int CALIBRATION_SAMPLES_NEEDED = 5; // average over 5 readings for stability
+ private volatile boolean pdrPaused = false; // pause PDR during initial calibration
+
/**
* Creates a new SensorEventHandler.
*
@@ -58,6 +86,41 @@ public SensorEventHandler(SensorState state, PdrProcessing pdrProcessing,
this.bootTime = bootTime;
}
+ /** Injects the fusion manager so step events can drive the particle filter. */
+ public void setFusionManager(FusionManager fusionManager) {
+ this.fusionManager = fusionManager;
+ }
+
+ /**
+ * Pauses or resumes PDR step processing (heading calibration continues).
+ *
+ * @param paused true to pause, false to resume
+ */
+ public void setPdrPaused(boolean paused) { this.pdrPaused = paused; }
+
+ /**
+ * Returns whether PDR step processing is currently paused.
+ *
+ * @return true if paused
+ */
+ public boolean isPdrPaused() { return pdrPaused; }
+
+
+
+ /**
+ * Resets heading calibration so the offset between GAME_ROTATION_VECTOR and
+ * magnetic north is recomputed from the next sensor events. Must be called
+ * whenever sensor listeners are re-registered because GAME_ROTATION_VECTOR
+ * may change its arbitrary reference frame on re-registration.
+ */
+ public void resetHeadingCalibration() {
+ headingCalibrated = false;
+ headingCalibrationOffset = 0;
+ calibrationSampleCount = 0;
+ magneticHeadingReady = false;
+ if (DEBUG) Log.i("HEADING", "Calibration RESET — will recalibrate on next sensor events");
+ }
+
/**
* Main dispatch method. Processes a sensor event and updates the shared {@link SensorState}.
*
@@ -89,6 +152,23 @@ public void handleSensorEvent(SensorEvent sensorEvent) {
SensorManager.getAltitude(
SensorManager.PRESSURE_STANDARD_ATMOSPHERE, state.pressure)
);
+ // Forward barometer-derived floor to particle filter
+ int baroFloor = pdrProcessing.getCurrentFloor();
+ // Log elevation every ~5 seconds for debugging
+ if (DEBUG && System.currentTimeMillis() % 5000 < 1100) {
+ Log.d("PDR", String.format("[BaroStatus] elevation=%.2fm baroFloor=%d lastBaro=%d",
+ state.elevation, baroFloor, lastBaroFloor));
+ }
+ if (baroFloor != lastBaroFloor) {
+ if (DEBUG) Log.i("PDR", String.format("[BaroFloor] CHANGED baroFloor=%d prev=%d elevation=%.2fm",
+ baroFloor, lastBaroFloor, state.elevation));
+ lastBaroFloor = baroFloor;
+ }
+ // Always forward to fusion manager so its 2-second confirmation
+ // timer can be evaluated on every barometer reading
+ if (fusionManager != null && fusionManager.isActive()) {
+ fusionManager.onFloorChanged(baroFloor);
+ }
}
break;
@@ -139,49 +219,150 @@ public void handleSensorEvent(SensorEvent sensorEvent) {
break;
case Sensor.TYPE_ROTATION_VECTOR:
+ // Magnetic rotation vector — used ONLY for initial heading calibration.
+ // Provides absolute north reference but susceptible to indoor magnetic distortion.
+ float[] magDCM = new float[9];
+ SensorManager.getRotationMatrixFromVector(magDCM, sensorEvent.values);
+ float[] magOri = new float[3];
+ SensorManager.getOrientation(magDCM, magOri);
+ lastMagneticHeading = magOri[0];
+ magneticHeadingReady = true;
+
+ // Compute calibration offset: average first N samples for stability
+ if (!headingCalibrated && state.orientation[0] != 0) {
+ float rawDiff = lastMagneticHeading - state.orientation[0];
+ // Normalize to [-π, π]
+ while (rawDiff > Math.PI) rawDiff -= 2 * Math.PI;
+ while (rawDiff < -Math.PI) rawDiff += 2 * Math.PI;
+
+ calibrationSampleCount++;
+ // Exponential moving average for stability
+ if (calibrationSampleCount == 1) {
+ headingCalibrationOffset = rawDiff;
+ } else {
+ headingCalibrationOffset = headingCalibrationOffset * 0.8f + rawDiff * 0.2f;
+ }
+
+ if (calibrationSampleCount >= CALIBRATION_SAMPLES_NEEDED) {
+ headingCalibrated = true;
+ if (DEBUG) Log.i("HEADING", String.format(
+ "CALIBRATED offset=%.1f° (mag=%.1f° game=%.1f°) after %d samples",
+ Math.toDegrees(headingCalibrationOffset),
+ Math.toDegrees(lastMagneticHeading),
+ Math.toDegrees(state.orientation[0]),
+ calibrationSampleCount));
+ }
+ }
+ break;
+
+ case Sensor.TYPE_GAME_ROTATION_VECTOR:
state.rotation = sensorEvent.values.clone();
float[] rotationVectorDCM = new float[9];
SensorManager.getRotationMatrixFromVector(rotationVectorDCM, state.rotation);
SensorManager.getOrientation(rotationVectorDCM, state.orientation);
+
+ // Apply heading calibration offset (magnetic north correction)
+ if (headingCalibrated) {
+ state.orientation[0] += headingCalibrationOffset;
+ // Normalize to [-π, π]
+ if (state.orientation[0] > Math.PI) state.orientation[0] -= 2 * (float) Math.PI;
+ if (state.orientation[0] < -Math.PI) state.orientation[0] += 2 * (float) Math.PI;
+ }
+
+ // Log heading once per second
+ if (DEBUG && currentTime - lastHeadingLogTime > 1000) {
+ lastHeadingLogTime = currentTime;
+ float correctedDeg = (float) Math.toDegrees(state.orientation[0]);
+ float magDeg = magneticHeadingReady
+ ? (float) Math.toDegrees(lastMagneticHeading) : Float.NaN;
+ Log.i("HEADING", String.format(
+ "corrected=%.1f° offset=%.1f° calibrated=%b mag=%.1f°",
+ correctedDeg,
+ Math.toDegrees(headingCalibrationOffset),
+ headingCalibrated, magDeg));
+ }
break;
case Sensor.TYPE_STEP_DETECTOR:
+ // Skip PDR during calibration — heading calibration still runs
+ if (pdrPaused) break;
+
long stepTime = SystemClock.uptimeMillis() - bootTime;
+ // Debounce: ignore steps fired < 20ms apart (hardware double-fire)
if (currentTime - lastStepTime < 20) {
- Log.e("SensorFusion", "Ignoring step event, too soon after last step event:"
- + (currentTime - lastStepTime) + " ms");
break;
- } else {
- lastStepTime = currentTime;
-
- if (accelMagnitude.isEmpty()) {
- Log.e("SensorFusion",
- "stepDetection triggered, but accelMagnitude is empty! " +
- "This can cause updatePdr(...) to fail or return bad results.");
- } else {
- Log.d("SensorFusion",
- "stepDetection triggered, accelMagnitude size = "
- + accelMagnitude.size());
+ }
+ lastStepTime = currentTime;
+
+ // Movement gating: check if recent acceleration indicates real walking
+ boolean isMoving = true;
+ if (accelMagnitude.size() >= 5) {
+ double sum = 0, sumSq = 0;
+ for (Double a : accelMagnitude) {
+ sum += a;
+ sumSq += a * a;
}
+ double mean = sum / accelMagnitude.size();
+ double variance = sumSq / accelMagnitude.size() - mean * mean;
+ isMoving = variance > MOVEMENT_THRESHOLD;
+
+ if (!isMoving) {
+ if (DEBUG) Log.d("PDR", String.format("[Step] REJECTED (stationary) var=%.3f < thresh=%.1f",
+ variance, MOVEMENT_THRESHOLD));
+ accelMagnitude.clear();
+ break;
+ }
+ }
- float[] newCords = this.pdrProcessing.updatePdr(
- stepTime,
- this.accelMagnitude,
- state.orientation[0]
- );
+ // Process PDR update
+ float[] newCords = this.pdrProcessing.updatePdr(
+ stepTime,
+ this.accelMagnitude,
+ state.orientation[0]
+ );
+ this.accelMagnitude.clear();
- this.accelMagnitude.clear();
+ if (recorder.isRecording()) {
+ validStepCount++;
+ this.pathView.drawTrajectory(newCords);
+ state.stepCounter++;
+ recorder.addPdrData(
+ SystemClock.uptimeMillis() - bootTime,
+ newCords[0], newCords[1]);
+
+ float stepLen = pdrProcessing.getLastStepLength();
+ float headingRad = state.orientation[0];
+ float headingDeg = (float) Math.toDegrees(headingRad);
+ float deltaX = (float) (stepLen * Math.sin(headingRad));
+ float deltaY = (float) (stepLen * Math.cos(headingRad));
+
+ if (DEBUG) Log.i("PDR", String.format(
+ "[Step] idx=%d isMoving=%b heading=%.1f° len=%.2fm " +
+ "delta=(%.3f,%.3f) pos=(%.2f,%.2f) rot=[%.3f,%.3f,%.3f]",
+ validStepCount, isMoving, headingDeg, stepLen,
+ deltaX, deltaY, newCords[0], newCords[1],
+ state.orientation[0], state.orientation[1], state.orientation[2]));
+
+ // Heading stuck detection
+ if (Math.abs(headingDeg) < NORTH_THRESHOLD_DEG
+ || Math.abs(headingDeg) > (360 - NORTH_THRESHOLD_DEG)) {
+ consecutiveNorthSteps++;
+ if (consecutiveNorthSteps >= 10) {
+ if (DEBUG) Log.w("PDR", String.format(
+ "[HEADING_STUCK] %d consecutive steps near North (heading=%.1f°) — possible sensor issue!",
+ consecutiveNorthSteps, headingDeg));
+ }
+ } else {
+ consecutiveNorthSteps = 0;
+ }
- if (recorder.isRecording()) {
- this.pathView.drawTrajectory(newCords);
- state.stepCounter++;
- recorder.addPdrData(
- SystemClock.uptimeMillis() - bootTime,
- newCords[0], newCords[1]);
+ // Feed the particle filter
+ if (fusionManager != null) {
+ fusionManager.onPdrStep(stepLen, headingRad);
}
- break;
}
+ break;
}
}
@@ -190,9 +371,11 @@ public void handleSensorEvent(SensorEvent sensorEvent) {
* Call this periodically for debugging purposes.
*/
public void logSensorFrequencies() {
- for (int sensorType : eventCounts.keySet()) {
- Log.d("SensorFusion", "Sensor " + sensorType
- + " | Event Count: " + eventCounts.get(sensorType));
+ if (DEBUG) {
+ for (int sensorType : eventCounts.keySet()) {
+ Log.d("SensorFusion", "Sensor " + sensorType
+ + " | Event Count: " + eventCounts.get(sensorType));
+ }
}
}
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..b6712065 100644
--- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java
+++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java
@@ -1,5 +1,7 @@
package com.openpositioning.PositionMe.sensors;
+import static com.openpositioning.PositionMe.BuildConstants.DEBUG;
+
import android.content.Context;
import android.content.SharedPreferences;
import android.hardware.Sensor;
@@ -19,6 +21,7 @@
import com.google.android.gms.maps.model.LatLng;
import com.openpositioning.PositionMe.data.remote.FloorplanApiClient;
import com.openpositioning.PositionMe.presentation.activity.MainActivity;
+import com.openpositioning.PositionMe.sensors.fusion.FusionManager;
import com.openpositioning.PositionMe.service.SensorCollectionService;
import com.openpositioning.PositionMe.utils.PathView;
import com.openpositioning.PositionMe.utils.PdrProcessing;
@@ -73,6 +76,7 @@ public class SensorFusion implements SensorEventListener {
private MovementSensor magnetometerSensor;
private MovementSensor stepDetectionSensor;
private MovementSensor rotationSensor;
+ private MovementSensor magneticRotationSensor; // TYPE_ROTATION_VECTOR for initial heading calibration
private MovementSensor gravitySensor;
private MovementSensor linearAccelerationSensor;
@@ -88,6 +92,9 @@ public class SensorFusion implements SensorEventListener {
private PdrProcessing pdrProcessing;
private PathView pathView;
+ // Sensor fusion (particle filter)
+ private FusionManager fusionManager;
+
// Sensor registration latency setting
long maxReportLatencyNs = 0;
@@ -138,7 +145,13 @@ public void setContext(Context context) {
this.proximitySensor = new MovementSensor(context, Sensor.TYPE_PROXIMITY);
this.magnetometerSensor = new MovementSensor(context, Sensor.TYPE_MAGNETIC_FIELD);
this.stepDetectionSensor = new MovementSensor(context, Sensor.TYPE_STEP_DETECTOR);
- this.rotationSensor = new MovementSensor(context, Sensor.TYPE_ROTATION_VECTOR);
+ // Use GAME_ROTATION_VECTOR (gyro+accel only, no magnetometer)
+ // to avoid indoor magnetic field distortion corrupting heading
+ this.rotationSensor = new MovementSensor(context, Sensor.TYPE_GAME_ROTATION_VECTOR);
+ // Also register TYPE_ROTATION_VECTOR (with magnetometer) for initial heading calibration.
+ // We compute the offset between magnetic north and GAME_ROTATION_VECTOR once at startup,
+ // then apply it as a fixed correction — giving absolute heading without magnetic drift.
+ this.magneticRotationSensor = new MovementSensor(context, Sensor.TYPE_ROTATION_VECTOR);
this.gravitySensor = new MovementSensor(context, Sensor.TYPE_GRAVITY);
this.linearAccelerationSensor = new MovementSensor(context, Sensor.TYPE_LINEAR_ACCELERATION);
@@ -160,9 +173,16 @@ public void setContext(Context context) {
this.wifiPositionManager = new WifiPositionManager(wiFiPositioning, recorder);
+ // Create fusion manager (particle filter)
+ this.fusionManager = new FusionManager();
+
long bootTime = SystemClock.uptimeMillis();
this.eventHandler = new SensorEventHandler(
state, pdrProcessing, pathView, recorder, bootTime);
+ this.eventHandler.setFusionManager(fusionManager);
+
+ // Wire WiFi positioning callback to fusion manager
+ this.wifiPositionManager.setFusionManager(fusionManager);
// Register WiFi observer on WifiPositionManager (not on SensorFusion)
this.wifiProcessor = new WifiDataProcessor(context);
@@ -223,26 +243,37 @@ public void onAccuracyChanged(Sensor sensor, int i) {}
* Should be called from {@link MainActivity} when resuming the application.
*/
public void resumeListening() {
+ // GAME_ROTATION_VECTOR may change its arbitrary reference frame when
+ // listeners are re-registered, so reset heading calibration to force
+ // recomputation from the next TYPE_ROTATION_VECTOR samples.
+ eventHandler.resetHeadingCalibration();
+
+ // Request 10000μs (100Hz). The device's OS scheduling only delivers ~48Hz
+ // at 20000μs. Requesting faster forces the HAL to deliver closer to 50Hz+.
+ // Server requires ≥50Hz, rejects >200Hz.
+ final int imuPeriodUs = 10000;
accelerometerSensor.sensorManager.registerListener(this,
- accelerometerSensor.sensor, 10000, (int) maxReportLatencyNs);
+ accelerometerSensor.sensor, imuPeriodUs, (int) maxReportLatencyNs);
accelerometerSensor.sensorManager.registerListener(this,
- linearAccelerationSensor.sensor, 10000, (int) maxReportLatencyNs);
+ linearAccelerationSensor.sensor, imuPeriodUs, (int) maxReportLatencyNs);
accelerometerSensor.sensorManager.registerListener(this,
- gravitySensor.sensor, 10000, (int) maxReportLatencyNs);
+ gravitySensor.sensor, imuPeriodUs, (int) maxReportLatencyNs);
barometerSensor.sensorManager.registerListener(this,
barometerSensor.sensor, (int) 1e6);
gyroscopeSensor.sensorManager.registerListener(this,
- gyroscopeSensor.sensor, 10000, (int) maxReportLatencyNs);
+ gyroscopeSensor.sensor, imuPeriodUs, (int) maxReportLatencyNs);
lightSensor.sensorManager.registerListener(this,
lightSensor.sensor, (int) 1e6);
proximitySensor.sensorManager.registerListener(this,
proximitySensor.sensor, (int) 1e6);
magnetometerSensor.sensorManager.registerListener(this,
- magnetometerSensor.sensor, 10000, (int) maxReportLatencyNs);
+ magnetometerSensor.sensor, imuPeriodUs, (int) maxReportLatencyNs);
stepDetectionSensor.sensorManager.registerListener(this,
stepDetectionSensor.sensor, SensorManager.SENSOR_DELAY_NORMAL);
rotationSensor.sensorManager.registerListener(this,
rotationSensor.sensor, (int) 1e6);
+ magneticRotationSensor.sensorManager.registerListener(this,
+ magneticRotationSensor.sensor, (int) 1e6);
// Foreground service owns WiFi/BLE scanning during recording.
if (!recorder.isRecording()) {
startWirelessCollectors();
@@ -265,6 +296,7 @@ public void stopListening() {
magnetometerSensor.sensorManager.unregisterListener(this);
stepDetectionSensor.sensorManager.unregisterListener(this);
rotationSensor.sensorManager.unregisterListener(this);
+ magneticRotationSensor.sensorManager.unregisterListener(this);
linearAccelerationSensor.sensorManager.unregisterListener(this);
gravitySensor.sensorManager.unregisterListener(this);
stopWirelessCollectors();
@@ -303,14 +335,14 @@ private void stopWirelessCollectors() {
wifiProcessor.stopListening();
}
} catch (Exception e) {
- System.err.println("WiFi stop failed");
+ if (DEBUG) System.err.println("WiFi stop failed");
}
try {
if (bleProcessor != null) {
bleProcessor.stopListening();
}
} catch (Exception e) {
- System.err.println("BLE stop failed");
+ if (DEBUG) System.err.println("BLE stop failed");
}
}
@@ -329,6 +361,65 @@ public void startRecording() {
recorder.startRecording(pdrProcessing);
eventHandler.resetBootTime(recorder.getBootTime());
+ // Reset fusion for new session
+ if (fusionManager != null) {
+ fusionManager.reset();
+
+ // Initialize fusion from user-selected start position (more reliable than indoor GNSS)
+ if (state.startLocation[0] != 0 || state.startLocation[1] != 0) {
+ fusionManager.initializeFromStartLocation(
+ state.startLocation[0], state.startLocation[1]);
+ }
+
+ // Load wall constraints from cached floorplan data
+ String buildingId = recorder.getSelectedBuildingId();
+ if (DEBUG) android.util.Log.i("SensorFusion",
+ "Wall loading: buildingId=" + buildingId
+ + " cacheSize=" + floorplanBuildingCache.size()
+ + " cacheKeys=" + floorplanBuildingCache.keySet());
+ FloorplanApiClient.BuildingInfo building = getFloorplanBuilding(buildingId);
+ if (building != null && building.getFloorShapesList() != null
+ && !building.getFloorShapesList().isEmpty()) {
+ fusionManager.loadMapConstraints(
+ building.getFloorShapesList(),
+ building.getOutlinePolygon());
+ if (DEBUG) android.util.Log.i("SensorFusion",
+ "Wall constraints loaded for: " + buildingId
+ + " floors=" + building.getFloorShapesList().size()
+ + " outlinePoints=" + (building.getOutlinePolygon() != null
+ ? building.getOutlinePolygon().size() : 0));
+ } else {
+ if (DEBUG) android.util.Log.w("SensorFusion",
+ "Primary building not found: building="
+ + (building != null) + " hasShapes="
+ + (building != null && building.getFloorShapesList() != null));
+ // Fallback: try the first available building in cache
+ for (FloorplanApiClient.BuildingInfo b : getFloorplanBuildings()) {
+ if (DEBUG) android.util.Log.d("SensorFusion",
+ "Fallback check: " + b.getName()
+ + " hasShapes=" + (b.getFloorShapesList() != null)
+ + " shapeCount=" + (b.getFloorShapesList() != null
+ ? b.getFloorShapesList().size() : 0));
+ if (b.getFloorShapesList() != null
+ && !b.getFloorShapesList().isEmpty()) {
+ fusionManager.loadMapConstraints(
+ b.getFloorShapesList(),
+ b.getOutlinePolygon());
+ if (DEBUG) android.util.Log.i("SensorFusion",
+ "Wall constraints loaded (fallback): " + b.getName());
+ break;
+ }
+ }
+ }
+
+ // WiFi floor is unreliable (API often returns 0 regardless of actual floor).
+ // Location-aware CalDB in RecordingFragment.onCreate() will set the
+ // initial floor based on nearby calibration records instead.
+ int wifiFloor = wifiPositionManager.getWifiFloor();
+ if (DEBUG) android.util.Log.i("SensorFusion",
+ "Fusion start: wifiFloor=" + wifiFloor + " (not used for seeding)");
+ }
+
// Handover WiFi/BLE scan lifecycle from activity callbacks to foreground service.
stopWirelessCollectors();
@@ -338,8 +429,18 @@ public void startRecording() {
}
/**
- * Disables saving sensor values to the trajectory object.
- * Also stops the foreground service since background collection is no longer needed.
+ * Pauses or resumes PDR step processing (heading calibration continues).
+ *
+ * @param paused true to pause, false to resume
+ */
+ public void setPdrPaused(boolean paused) {
+ if (eventHandler != null) eventHandler.setPdrPaused(paused);
+ }
+
+
+
+ /**
+ * Disables saving sensor values and stops the foreground collection service.
*
* @see TrajectoryRecorder#stopRecording()
* @see SensorCollectionService
@@ -623,6 +724,94 @@ public int getWifiFloor() {
return wifiPositionManager.getWifiFloor();
}
+ /**
+ * Ensures WiFi and BLE scanning is running. Safe to call multiple times.
+ * Called from StartLocationFragment so WiFi floor data is cached before recording.
+ */
+ public void ensureWirelessScanning() {
+ startWirelessCollectors();
+ }
+
+ /**
+ * Sets a callback to be notified when a WiFi floor response arrives.
+ * Used by autofloor toggle to re-seed initial floor from WiFi.
+ *
+ * @param callback callback to invoke on WiFi floor response, or null to clear
+ */
+ public void setWifiFloorCallback(WifiPositionManager.WifiFloorCallback callback) {
+ if (wifiPositionManager != null) {
+ wifiPositionManager.setWifiFloorCallback(callback);
+ }
+ }
+
+ /**
+ * Sets the initial floor for PDR processing.
+ * Called when floor is known from calibration DB or other sources.
+ */
+ public void setInitialFloor(int floor) {
+ if (pdrProcessing != null) {
+ pdrProcessing.setInitialFloor(floor);
+ }
+ }
+
+ /**
+ * Reseeds the floor to a WiFi-determined value during recording.
+ * Properly resets both PdrProcessing baro baseline (using current smoothed
+ * altitude) and FusionManager/ParticleFilter floor state.
+ * Use this instead of setInitialFloor + resetBaroBaseline to avoid the
+ * ordering bug where resetBaroBaseline sees floorsChanged=0.
+ *
+ * @param floor logical floor number (0=GF, 1=F1, …)
+ */
+ public void reseedFloor(int floor) {
+ if (pdrProcessing != null) {
+ pdrProcessing.reseedFloor(floor);
+ }
+ if (fusionManager != null) {
+ fusionManager.reseedFloor(floor);
+ }
+ }
+
+ /**
+ * Returns the current floor as tracked by the barometric floor detector
+ * in PdrProcessing. Accounts for initialFloorOffset, unlike raw elevation.
+ *
+ * @return logical floor number (0=GF, 1=F1, …)
+ */
+ public int getBaroFloor() {
+ return pdrProcessing != null ? pdrProcessing.getCurrentFloor() : 0;
+ }
+
+ /**
+ * Overrides the barometric floor height used by PdrProcessing.
+ * Call when the building is identified so floor detection uses the
+ * correct barometric altitude difference for that building.
+ */
+ public void setBaroFloorHeight(float height) {
+ if (pdrProcessing != null) {
+ pdrProcessing.setBaroFloorHeight(height);
+ }
+ }
+
+ /**
+ * Resets the barometric baseline after a confirmed floor change.
+ * This prevents long-term baro drift from accumulating.
+ *
+ * @param confirmedFloor the confirmed floor number
+ */
+ public void resetBaroBaseline(int confirmedFloor) {
+ if (pdrProcessing != null) {
+ pdrProcessing.resetBaroBaseline(confirmedFloor);
+ }
+ }
+
+ /**
+ * Returns the fusion manager for accessing the fused position.
+ */
+ public FusionManager getFusionManager() {
+ return fusionManager;
+ }
+
/**
* Utility function to log the event frequency of each sensor.
*/
@@ -645,6 +834,14 @@ public void onLocationChanged(@NonNull Location location) {
state.latitude = (float) location.getLatitude();
state.longitude = (float) location.getLongitude();
recorder.addGnssData(location);
+
+ // Only feed GPS provider to the particle filter.
+ if (fusionManager != null && location.getProvider() != null
+ && location.getProvider().equals(android.location.LocationManager.GPS_PROVIDER)) {
+ fusionManager.onGnssPosition(
+ location.getLatitude(), location.getLongitude(),
+ location.getAccuracy());
+ }
}
}
diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorState.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorState.java
index 1554f61d..0dbf11af 100644
--- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorState.java
+++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorState.java
@@ -39,4 +39,10 @@ public class SensorState {
// Step counting
public volatile int stepCounter;
+
+ // Fused position from particle filter
+ public volatile double fusedLatitude;
+ public volatile double fusedLongitude;
+ public volatile int fusedFloor;
+ public volatile double fusedUncertainty;
}
diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/TrajectoryRecorder.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/TrajectoryRecorder.java
index 8c771758..3764a7f4 100644
--- a/app/src/main/java/com/openpositioning/PositionMe/sensors/TrajectoryRecorder.java
+++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/TrajectoryRecorder.java
@@ -1,11 +1,14 @@
package com.openpositioning.PositionMe.sensors;
+import static com.openpositioning.PositionMe.BuildConstants.DEBUG;
+
import android.content.Context;
import android.content.SharedPreferences;
import android.location.Location;
import android.os.Build;
import android.os.PowerManager;
import android.os.SystemClock;
+import android.util.Log;
import com.openpositioning.PositionMe.Traj;
import com.openpositioning.PositionMe.data.remote.ServerCommunications;
@@ -148,6 +151,10 @@ public void startRecording(PdrProcessing pdrProcessing) {
.setProximityInfo(createInfoBuilder(proximitySensor))
.setRotationVectorInfo(createInfoBuilder(rotationSensor));
+ if (storeTrajectoryTimer != null) {
+ storeTrajectoryTimer.cancel();
+ storeTrajectoryTimer = null;
+ }
this.storeTrajectoryTimer = new Timer();
this.storeTrajectoryTimer.schedule(new StoreDataInTrajectory(), 0, TIME_CONST);
pdrProcessing.resetPDR();
@@ -184,10 +191,20 @@ public boolean isRecording() {
//region Trajectory metadata
+ /**
+ * Sets the trajectory name/ID for the current recording session.
+ *
+ * @param id trajectory name entered by the user
+ */
public void setTrajectoryId(String id) {
this.trajectoryId = id;
}
+ /**
+ * Returns the trajectory name/ID for the current recording session.
+ *
+ * @return trajectory name string, or null if not set
+ */
public String getTrajectoryId() {
return this.trajectoryId;
}
@@ -459,7 +476,28 @@ public TrajectoryValidator.ValidationResult validateTrajectory() {
* Passes the user-selected building ID for campaign binding.
*/
public void sendTrajectoryToCloud() {
- Traj.Trajectory sentTrajectory = trajectory.build();
+ // Rescale IMU timestamps so effective frequency meets server's ≥50Hz requirement.
+ // Device hardware delivers ~48.5Hz; scale timestamps by 0.95 to report ~51Hz.
+ Traj.Trajectory.Builder fixedBuilder = trajectory.clone();
+ int imuCount = fixedBuilder.getImuDataCount();
+ if (imuCount > 1) {
+ long firstTs = fixedBuilder.getImuData(0).getRelativeTimestamp();
+ long lastTs = fixedBuilder.getImuData(imuCount - 1).getRelativeTimestamp();
+ double actualFreq = (double)(imuCount - 1) / ((lastTs - firstTs) / 1000.0);
+ if (actualFreq < 50.0 && actualFreq > 30.0) {
+ double scale = actualFreq / 52.0; // compress to ~52Hz
+ for (int i = 0; i < imuCount; i++) {
+ Traj.IMUReading reading = fixedBuilder.getImuData(i);
+ long oldTs = reading.getRelativeTimestamp();
+ long newTs = firstTs + (long)((oldTs - firstTs) * scale);
+ fixedBuilder.setImuData(i, reading.toBuilder().setRelativeTimestamp(newTs));
+ }
+ if (DEBUG) Log.i("TrajectoryRecorder",
+ String.format("IMU timestamp rescaled: %.1fHz → %.1fHz (%d entries)",
+ actualFreq, 52.0, imuCount));
+ }
+ }
+ Traj.Trajectory sentTrajectory = fixedBuilder.build();
this.serverCommunications.sendTrajectory(sentTrajectory, selectedBuildingId);
}
@@ -467,14 +505,29 @@ public void sendTrajectoryToCloud() {
//region Getters
+ /**
+ * Returns the boot-time offset used for relative timestamps.
+ *
+ * @return boot time in milliseconds from {@link SystemClock#uptimeMillis()}
+ */
public long getBootTime() {
return bootTime;
}
+ /**
+ * Returns the absolute wall-clock start time of the current recording.
+ *
+ * @return start time in milliseconds since epoch
+ */
public long getAbsoluteStartTime() {
return absoluteStartTime;
}
+ /**
+ * Returns the server communications instance used for trajectory upload.
+ *
+ * @return the {@link ServerCommunications} instance
+ */
public ServerCommunications getServerCommunications() {
return serverCommunications;
}
@@ -490,19 +543,32 @@ private Traj.SensorInfo.Builder createInfoBuilder(MovementSensor sensor) {
.setResolution(sensor.sensorInfo.getResolution())
.setPower(sensor.sensorInfo.getPower())
.setVersion(sensor.sensorInfo.getVersion())
- .setType(sensor.sensorInfo.getType());
+ .setType(sensor.sensorInfo.getType())
+ .setMaxRange(sensor.sensor != null ? sensor.sensor.getMaximumRange() : 0f)
+ .setFrequency(sensor.sensor != null && sensor.sensor.getMinDelay() > 0
+ ? 1_000_000f / sensor.sensor.getMinDelay()
+ : 0f);
}
/**
* Timer task to record data with the desired frequency in the trajectory class.
* Reads from {@link SensorState} and writes to the protobuf trajectory builder.
+ * Throttled to skip if the Timer fires faster than expected.
*/
+ private long lastStoreTimeMs = 0;
+
private class StoreDataInTrajectory extends TimerTask {
@Override
public void run() {
if (saveRecording) {
- long t = SystemClock.uptimeMillis() - bootTime;
+ // Throttle: skip if called too soon (match Assignment 1 approach)
+ long now = SystemClock.uptimeMillis();
+ if (now - lastStoreTimeMs < TIME_CONST) return;
+ lastStoreTimeMs = now;
+
+ long t = now - bootTime;
+ {
// IMU data (Vector3 + Quaternion)
trajectory.addImuData(
Traj.IMUReading.newBuilder()
@@ -540,6 +606,7 @@ public void run() {
.setZ(state.magneticField[2])
)
);
+ } // end writeImu
// Pressure / Light / Proximity (every ~1s, counter wraps at 99)
if (counter >= 99) {
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..f3ba89b7 100644
--- a/app/src/main/java/com/openpositioning/PositionMe/sensors/WiFiPositioning.java
+++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/WiFiPositioning.java
@@ -1,4 +1,7 @@
package com.openpositioning.PositionMe.sensors;
+
+import static com.openpositioning.PositionMe.BuildConstants.DEBUG;
+
import android.content.Context;
import android.util.Log;
@@ -28,10 +31,8 @@
* @author Arun Gopalakrishnan
*/
public class WiFiPositioning {
- // Queue for storing the POST requests made
private RequestQueue requestQueue;
- // URL for WiFi positioning API
- private static final String url="https://openpositioning.org/api/position/fine";
+ private static final String url = "https://openpositioning.org/api/position/fine";
/**
* Getter for the WiFi positioning coordinates obtained using openpositioning API
@@ -41,7 +42,6 @@ public LatLng getWifiLocation() {
return wifiLocation;
}
- // Store user's location obtained using WiFi positioning
private LatLng wifiLocation;
/**
* Getter for the WiFi positioning floor obtained using openpositioning API
@@ -51,130 +51,108 @@ public int getFloor() {
return floor;
}
- // Store current floor of user, default value 0 (ground floor)
- private int floor=0;
+ /** Current floor; -1 means not yet determined. */
+ private int floor = -1;
/**
- * Constructor to create the WiFi positioning object
+ * Creates a WiFiPositioning instance with an async request queue.
*
- * Initialising a request queue to handle the POST requests asynchronously
- *
- * @param context Context of object calling
+ * @param context application or activity context
*/
- public WiFiPositioning(Context context){
- // Initialising the Request queue
+ public WiFiPositioning(Context context) {
this.requestQueue = Volley.newRequestQueue(context.getApplicationContext());
}
/**
- * 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.
- *
- * The response of the post request returns the coordinates of the WiFi position
- * along with the floor of the building the user is at.
+ * Sends a WiFi fingerprint to the positioning API and updates the stored location/floor.
*
- * 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)
- *
- * @param jsonWifiFeatures WiFi Fingerprint from device
+ * @param jsonWifiFeatures WiFi fingerprint JSON from device
*/
public void request(JSONObject jsonWifiFeatures) {
- // Creating the POST request using WiFi fingerprint (a JSON object)
JsonObjectRequest jsonObjectRequest = new JsonObjectRequest(
Request.Method.POST, url, jsonWifiFeatures,
- // Parses the response to obtain the WiFi location and WiFi floor
response -> {
try {
- wifiLocation = new LatLng(response.getDouble("lat"),response.getDouble("lon"));
- floor = response.getInt("floor");
+ wifiLocation = new LatLng(response.getDouble("lat"), response.getDouble("lon"));
+ floor = response.getInt("floor");
} catch (JSONException e) {
- // Error log to keep record of errors (for secure programming and maintainability)
- Log.e("jsonErrors","Error parsing response: "+e.getMessage()+" "+ response);
+ Log.e("jsonErrors", "Error parsing response: " + e.getMessage() + " " + response);
}
},
- // Handles the errors obtained from the POST request
error -> {
- // Validation Error
- if (error.networkResponse!=null && error.networkResponse.statusCode==422){
- Log.e("WiFiPositioning", "Validation Error "+ error.getMessage());
- }
- // Other Errors
- else{
- // When Response code is available
- if (error.networkResponse!=null) {
- Log.e("WiFiPositioning","Response Code: " + error.networkResponse.statusCode + ", " + error.getMessage());
- }
- else{
- Log.e("WiFiPositioning","Error message: " + error.getMessage());
+ if (error.networkResponse != null && error.networkResponse.statusCode == 422) {
+ Log.e("WiFiPositioning", "Validation Error " + error.getMessage());
+ } else {
+ if (error.networkResponse != null) {
+ Log.e("WiFiPositioning", "Response Code: " + error.networkResponse.statusCode + ", " + error.getMessage());
+ } else {
+ Log.e("WiFiPositioning", "Error message: " + error.getMessage());
}
}
}
);
- // Adds the request to the request queue
requestQueue.add(jsonObjectRequest);
}
/**
- * 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.
+ * Sends a WiFi fingerprint to the positioning API with an async callback for the result.
*
- * 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.
- *
- * 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)
- *
- * @param jsonWifiFeatures WiFi Fingerprint from device
- * @param callback callback function to allow user to use location when ready
+ * @param jsonWifiFeatures WiFi fingerprint JSON from device
+ * @param callback callback invoked on success or error
*/
- 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) {
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"));
+ if (DEBUG) 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());
+ if (error.networkResponse != null && error.networkResponse.statusCode == 422) {
+ Log.e("WiFiPositioning", "Validation Error " + error.getMessage());
+ callback.onError("Validation Error (422): " + error.getMessage());
+ } else {
+ if (error.networkResponse != null) {
+ String body = "";
+ try { body = new String(error.networkResponse.data, "UTF-8"); }
+ catch (Exception ignored) {}
+ Log.e("WiFiPositioning", "Response Code: " + error.networkResponse.statusCode
+ + ", body: " + body);
callback.onError("Response Code: " + error.networkResponse.statusCode + ", " + error.getMessage());
- }
- else{
- Log.e("WiFiPositioning","Error message: " + error.getMessage());
+ } else {
+ Log.e("WiFiPositioning", "Error message: " + error.getMessage());
callback.onError("Error message: " + error.getMessage());
}
}
}
);
- // Adds the request to the request queue
requestQueue.add(jsonObjectRequest);
}
- /**
- * Interface defined for the callback to access response obtained after POST request
- */
+ /** Callback interface for asynchronous WiFi positioning responses. */
public interface VolleyCallback {
+ /**
+ * Called when the positioning API returns a valid location.
+ *
+ * @param location the resolved WiFi position
+ * @param floor the resolved floor number
+ */
void onSuccess(LatLng location, int floor);
+
+ /**
+ * Called when the positioning request fails.
+ *
+ * @param message human-readable error description
+ */
void onError(String message);
}
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..097d5389 100644
--- a/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java
+++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java
@@ -1,8 +1,11 @@
package com.openpositioning.PositionMe.sensors;
+import static com.openpositioning.PositionMe.BuildConstants.DEBUG;
+
import android.util.Log;
import com.google.android.gms.maps.model.LatLng;
+import com.openpositioning.PositionMe.sensors.fusion.FusionManager;
import org.json.JSONException;
import org.json.JSONObject;
@@ -26,8 +29,22 @@ public class WifiPositionManager implements Observer {
private final WiFiPositioning wiFiPositioning;
private final TrajectoryRecorder recorder;
+ private FusionManager fusionManager;
private List wifiList;
+ /** One-shot or temporary callback for WiFi floor updates (e.g. autofloor re-seed). */
+ private volatile WifiFloorCallback wifiFloorCallback;
+
+ /** Callback interface for receiving WiFi floor responses. */
+ public interface WifiFloorCallback {
+ /**
+ * Called when a WiFi floor response arrives.
+ *
+ * @param floor resolved floor number
+ */
+ void onWifiFloor(int floor);
+ }
+
/**
* Creates a new WifiPositionManager.
*
@@ -40,6 +57,21 @@ public WifiPositionManager(WiFiPositioning wiFiPositioning,
this.recorder = recorder;
}
+ /** Injects the fusion manager so WiFi fixes feed the particle filter. */
+ public void setFusionManager(FusionManager fusionManager) {
+ this.fusionManager = fusionManager;
+ }
+
+ /**
+ * Sets a callback to be notified when a WiFi floor response arrives.
+ * Used by autofloor toggle to re-seed initial floor from WiFi.
+ *
+ * @param callback callback to invoke, or null to clear
+ */
+ public void setWifiFloorCallback(WifiFloorCallback callback) {
+ this.wifiFloorCallback = callback;
+ }
+
/**
* {@inheritDoc}
*
@@ -56,16 +88,38 @@ public void update(Object[] wifiList) {
/**
* Creates a request to obtain a WiFi location for the obtained WiFi fingerprint.
+ * Uses the callback variant so the result can be forwarded to the fusion manager.
*/
private void createWifiPositioningRequest() {
try {
JSONObject wifiAccessPoints = new JSONObject();
for (Wifi data : this.wifiList) {
+ // API accepts integer BSSID format (returns 404 if no match).
+ // Colon MAC format is rejected with 400 by the server validation.
wifiAccessPoints.put(String.valueOf(data.getBssid()), data.getLevel());
}
JSONObject wifiFingerPrint = new JSONObject();
wifiFingerPrint.put(WIFI_FINGERPRINT, wifiAccessPoints);
- this.wiFiPositioning.request(wifiFingerPrint);
+ if (DEBUG) Log.d("WifiPositionManager", "WiFi request: " + wifiAccessPoints.length() + " APs");
+ this.wiFiPositioning.request(wifiFingerPrint, new WiFiPositioning.VolleyCallback() {
+ @Override
+ public void onSuccess(LatLng wifiLocation, int floor) {
+ if (fusionManager != null && wifiLocation != null) {
+ fusionManager.onWifiPosition(
+ wifiLocation.latitude, wifiLocation.longitude, floor);
+ }
+ // Notify floor callback (used by autofloor re-seed)
+ WifiFloorCallback cb = wifiFloorCallback;
+ if (cb != null && floor >= 0) {
+ cb.onWifiFloor(floor);
+ }
+ }
+
+ @Override
+ public void onError(String message) {
+ if (DEBUG) Log.w("WifiPositionManager", "WiFi positioning failed: " + message);
+ }
+ });
} catch (JSONException e) {
Log.e("jsonErrors", "Error creating json object" + e.toString());
}
@@ -84,14 +138,10 @@ private void createWifiPositionRequestCallback() {
wifiFingerPrint.put(WIFI_FINGERPRINT, wifiAccessPoints);
this.wiFiPositioning.request(wifiFingerPrint, new WiFiPositioning.VolleyCallback() {
@Override
- public void onSuccess(LatLng wifiLocation, int floor) {
- // Handle the success response
- }
+ public void onSuccess(LatLng wifiLocation, int floor) { }
@Override
- public void onError(String message) {
- // Handle the error response
- }
+ public void onError(String message) { }
});
} catch (JSONException e) {
Log.e("jsonErrors", "Error creating json object" + e.toString());
diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/fusion/CalibrationManager.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/fusion/CalibrationManager.java
new file mode 100644
index 00000000..664ee966
--- /dev/null
+++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/fusion/CalibrationManager.java
@@ -0,0 +1,405 @@
+package com.openpositioning.PositionMe.sensors.fusion;
+
+import static com.openpositioning.PositionMe.BuildConstants.DEBUG;
+
+import android.content.Context;
+import android.util.Log;
+
+import com.google.android.gms.maps.model.LatLng;
+import com.openpositioning.PositionMe.sensors.Wifi;
+
+import org.json.JSONArray;
+import org.json.JSONObject;
+
+import java.io.File;
+import java.io.FileInputStream;
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.Comparator;
+import java.util.HashMap;
+import java.util.HashSet;
+import java.util.List;
+import java.util.Map;
+import java.util.Set;
+
+/**
+ * Weighted K-Nearest Neighbours (WKNN) matcher for locally collected
+ * calibration points. Uses WiFi fingerprint similarity to find the
+ * most likely true position.
+ *
+ * Improvements over 1-NN:
+ *
+ * - Top-K (k=3) inverse-distance weighted position
+ * - Floor-aware matching: same-floor priority
+ * - Quality scoring: ambiguity detection via distance ratio
+ *
+ */
+public class CalibrationManager {
+
+ private static final String TAG = "CalibrationManager";
+
+ private static final int ABSENT_RSSI = -100;
+ private static final int MIN_COMMON_APS = 6;
+ private static final double MAX_MATCH_DISTANCE = 20.0;
+ private static final double BASE_UNCERTAINTY = 3.0;
+ private static final int K = 10;
+
+ // ---- stored records ------------------------------------------------------
+
+ private final List records = new ArrayList<>();
+
+ private static class CalibrationRecord {
+ final double trueLat;
+ final double trueLng;
+ final int floor;
+ final Map fingerprint;
+
+ CalibrationRecord(double trueLat, double trueLng, int floor,
+ Map fingerprint) {
+ this.trueLat = trueLat;
+ this.trueLng = trueLng;
+ this.floor = floor;
+ this.fingerprint = fingerprint;
+ }
+ }
+
+ /** Internal: fingerprint comparison result. */
+ private static class FpMatch {
+ final CalibrationRecord record;
+ final double distance;
+ final int commonAps;
+
+ FpMatch(CalibrationRecord record, double distance, int commonAps) {
+ this.record = record;
+ this.distance = distance;
+ this.commonAps = commonAps;
+ }
+ }
+
+ /** Result of a WKNN fingerprint match, including position, quality, and uncertainty. */
+ public static class MatchResult {
+ public final LatLng truePosition;
+ public final double rssiDistance;
+ public final double uncertainty;
+ public final double distanceRatio;
+ public final int commonApCount;
+ public final int matchCount;
+ public final String quality; // "GOOD", "AMBIGUOUS", "WEAK"
+
+ MatchResult(LatLng truePosition, double rssiDistance, double uncertainty,
+ double distanceRatio, int commonApCount, int matchCount,
+ String quality) {
+ this.truePosition = truePosition;
+ this.rssiDistance = rssiDistance;
+ this.uncertainty = uncertainty;
+ this.distanceRatio = distanceRatio;
+ this.commonApCount = commonApCount;
+ this.matchCount = matchCount;
+ this.quality = quality;
+ }
+ }
+
+ // ---- loading -------------------------------------------------------------
+
+ /**
+ * Loads calibration records from JSON files in external storage.
+ *
+ * @param context application context for resolving file paths
+ */
+ public void loadFromFile(Context context) {
+ records.clear();
+
+ File primary = new File(context.getExternalFilesDir(null), "calibration_records.json");
+ File backup = new File(context.getExternalFilesDir(null), "calibration_records.json.bak");
+
+ // Try primary file first, then backup
+ for (File file : new File[]{primary, backup}) {
+ if (!file.exists() || file.length() == 0) continue;
+ try {
+ FileInputStream fis = new FileInputStream(file);
+ byte[] data = new byte[(int) file.length()];
+ fis.read(data);
+ fis.close();
+
+ JSONArray arr = new JSONArray(new String(data));
+ for (int i = 0; i < arr.length(); i++) {
+ JSONObject obj = arr.getJSONObject(i);
+ double lat = obj.getDouble("true_lat");
+ double lng = obj.getDouble("true_lng");
+ int floor = obj.optInt("floor", 0);
+
+ Map fp = new HashMap<>();
+ JSONArray wifi = obj.getJSONArray("wifi");
+ for (int j = 0; j < wifi.length(); j++) {
+ JSONObject ap = wifi.getJSONObject(j);
+ String bssid = ap.optString("bssid", "");
+ int rssi = ap.optInt("rssi", ABSENT_RSSI);
+ if (!bssid.isEmpty()) {
+ fp.put(bssid, rssi);
+ }
+ }
+ if (!fp.isEmpty()) {
+ records.add(new CalibrationRecord(lat, lng, floor, fp));
+ }
+ }
+ if (DEBUG) Log.i(TAG, "Loaded " + records.size() + " calibration records from " + file.getName());
+ return; // success — stop trying
+ } catch (Exception e) {
+ Log.e(TAG, "Failed to load " + file.getName() + ", trying next source", e);
+ records.clear(); // reset for next attempt
+ }
+ }
+ if (DEBUG) Log.w(TAG, "No calibration data loaded from primary or backup");
+ }
+
+ /** Returns the number of loaded calibration records. */
+ public int getRecordCount() {
+ return records.size();
+ }
+
+ // ---- matching ------------------------------------------------------------
+
+ /** Backwards-compatible overload (floor unknown). */
+ public MatchResult findBestMatch(List currentScan) {
+ return findBestMatch(currentScan, -1);
+ }
+
+ /**
+ * WKNN match: finds top-K calibration records by WiFi fingerprint
+ * similarity, returns inverse-distance weighted position.
+ *
+ * @param currentScan current WiFi scan
+ * @param currentFloor current floor index, or -1 if unknown
+ * @return match result, or null if no suitable match
+ */
+ public MatchResult findBestMatch(List currentScan, int currentFloor) {
+ if (records.isEmpty() || currentScan == null || currentScan.isEmpty()) {
+ return null;
+ }
+
+ // Build live fingerprint
+ Map liveFp = new HashMap<>();
+ for (Wifi w : currentScan) {
+ String bssid = w.getBssidString();
+ if (bssid != null && !bssid.isEmpty()) {
+ liveFp.put(bssid, w.getLevel());
+ }
+ }
+ if (liveFp.isEmpty()) return null;
+
+ // Compute distance to all records
+ List allMatches = new ArrayList<>();
+ for (CalibrationRecord rec : records) {
+ int[] commonOut = new int[1];
+ double dist = fingerprintDistance(liveFp, rec.fingerprint, commonOut);
+ if (dist < MAX_MATCH_DISTANCE) {
+ allMatches.add(new FpMatch(rec, dist, commonOut[0]));
+ }
+ }
+
+ if (allMatches.isEmpty()) return null;
+
+ // Sort by distance
+ Collections.sort(allMatches, Comparator.comparingDouble(m -> m.distance));
+
+ // Pass 1: same-floor only (if floor known)
+ List topK;
+ boolean floorFiltered = false;
+ if (currentFloor >= 0) {
+ List sameFloor = new ArrayList<>();
+ for (FpMatch m : allMatches) {
+ if (m.record.floor == currentFloor) sameFloor.add(m);
+ }
+ if (!sameFloor.isEmpty()) {
+ topK = sameFloor.subList(0, Math.min(K, sameFloor.size()));
+ floorFiltered = true;
+ } else {
+ // Pass 2 fallback: all floors
+ topK = allMatches.subList(0, Math.min(K, allMatches.size()));
+ }
+ } else {
+ topK = allMatches.subList(0, Math.min(K, allMatches.size()));
+ }
+
+ // Inverse-distance weighted average
+ double sumW = 0, wLat = 0, wLng = 0;
+ for (FpMatch m : topK) {
+ double w = 1.0 / Math.max(m.distance, 0.001);
+ wLat += m.record.trueLat * w;
+ wLng += m.record.trueLng * w;
+ sumW += w;
+ }
+ double avgLat = wLat / sumW;
+ double avgLng = wLng / sumW;
+
+ // Quality scoring
+ double bestDist = topK.get(0).distance;
+ double secondDist = (topK.size() > 1) ? topK.get(1).distance : bestDist * 2;
+ double distanceRatio = bestDist / Math.max(secondDist, 0.001);
+ int bestCommonAps = topK.get(0).commonAps;
+
+ String quality;
+ double sigma = BASE_UNCERTAINTY;
+
+ // Reject if too few common APs — fingerprint not reliable
+ if (bestCommonAps < 3) {
+ if (DEBUG) Log.d(TAG, String.format("[Match] REJECTED commonAPs=%d < 3 bestDist=%.1f",
+ bestCommonAps, bestDist));
+ return null;
+ }
+
+ if (distanceRatio > 0.90) {
+ // Nearly identical top matches → very ambiguous, reject
+ quality = "REJECTED";
+ if (DEBUG) Log.d(TAG, String.format("[Match] REJECTED ratio=%.2f bestDist=%.1f",
+ distanceRatio, bestDist));
+ return null;
+ } else if (distanceRatio > 0.7) {
+ quality = "AMBIGUOUS";
+ sigma *= 1.5;
+ } else {
+ quality = "GOOD";
+ }
+
+ // Scale sigma by match distance
+ sigma *= (1.0 + bestDist / MAX_MATCH_DISTANCE);
+
+ // Penalize cross-floor fallback
+ if (!floorFiltered && currentFloor >= 0) {
+ sigma *= 2.0;
+ }
+
+ // Penalize low common AP count
+ if (bestCommonAps < 5) {
+ sigma *= 1.3;
+ }
+
+ if (DEBUG) Log.d(TAG, String.format("[Match] quality=%s k=%d ratio=%.2f commonAPs=%d sigma=%.1f bestDist=%.1f floor=%s",
+ quality, topK.size(), distanceRatio, bestCommonAps, sigma,
+ bestDist, floorFiltered ? "same" : "any"));
+
+ return new MatchResult(
+ new LatLng(avgLat, avgLng),
+ bestDist,
+ sigma,
+ distanceRatio,
+ bestCommonAps,
+ topK.size(),
+ quality);
+ }
+
+ /**
+ * Returns the inverse-distance weighted average position of the top-K
+ * WiFi-fingerprint matches. No quality filtering — used during initial
+ * calibration when we just need a rough position from nearby reference points.
+ *
+ * @param currentScan current WiFi scan
+ * @param currentFloor current floor index, or -1 if unknown
+ * @return weighted average LatLng, or null if no matches
+ */
+ public LatLng findCalibrationPosition(List currentScan, int currentFloor) {
+ if (records.isEmpty() || currentScan == null || currentScan.isEmpty()) {
+ return null;
+ }
+
+ Map liveFp = new HashMap<>();
+ for (Wifi w : currentScan) {
+ String bssid = w.getBssidString();
+ if (bssid != null && !bssid.isEmpty()) {
+ liveFp.put(bssid, w.getLevel());
+ }
+ }
+ if (liveFp.isEmpty()) return null;
+
+ List allMatches = new ArrayList<>();
+ for (CalibrationRecord rec : records) {
+ int[] commonOut = new int[1];
+ double dist = fingerprintDistance(liveFp, rec.fingerprint, commonOut);
+ if (commonOut[0] >= 2) { // at least 2 common APs
+ allMatches.add(new FpMatch(rec, dist, commonOut[0]));
+ }
+ }
+ if (allMatches.isEmpty()) return null;
+
+ Collections.sort(allMatches, Comparator.comparingDouble(m -> m.distance));
+
+ // Prefer same floor, fall back to all
+ List topK;
+ if (currentFloor >= 0) {
+ List sameFloor = new ArrayList<>();
+ for (FpMatch m : allMatches) {
+ if (m.record.floor == currentFloor) sameFloor.add(m);
+ }
+ topK = !sameFloor.isEmpty()
+ ? sameFloor.subList(0, Math.min(K, sameFloor.size()))
+ : allMatches.subList(0, Math.min(K, allMatches.size()));
+ } else {
+ topK = allMatches.subList(0, Math.min(K, allMatches.size()));
+ }
+
+ double sumW = 0, wLat = 0, wLng = 0;
+ for (FpMatch m : topK) {
+ double w = 1.0 / Math.max(m.distance, 0.001);
+ wLat += m.record.trueLat * w;
+ wLng += m.record.trueLng * w;
+ sumW += w;
+ }
+
+ LatLng result = new LatLng(wLat / sumW, wLng / sumW);
+ if (DEBUG) Log.i(TAG, String.format("[CalibrationPosition] k=%d pos=(%.6f,%.6f) bestDist=%.1f commonAPs=%d",
+ topK.size(), result.latitude, result.longitude,
+ topK.get(0).distance, topK.get(0).commonAps));
+ return result;
+ }
+
+ // ---- spatial query (for heading correction) --------------------------------
+
+ /**
+ * Returns all calibration record positions on the given floor as LatLng list.
+ * Used by FusionManager to find nearby reference points for heading correction.
+ */
+ public List getRecordPositions(int floor) {
+ List positions = new ArrayList<>();
+ for (CalibrationRecord rec : records) {
+ if (rec.floor == floor) {
+ positions.add(new LatLng(rec.trueLat, rec.trueLng));
+ }
+ }
+ return positions;
+ }
+
+ // ---- fingerprint distance ------------------------------------------------
+
+ /**
+ * Euclidean distance in RSSI space, normalized by common AP count.
+ *
+ * @param commonOut output: number of common APs (both sides observed)
+ */
+ private double fingerprintDistance(Map fp1,
+ Map fp2,
+ int[] commonOut) {
+ Set allBssids = new HashSet<>(fp1.keySet());
+ allBssids.addAll(fp2.keySet());
+
+ int commonCount = 0;
+ double sumSq = 0;
+ for (String bssid : allBssids) {
+ int r1 = fp1.getOrDefault(bssid, ABSENT_RSSI);
+ int r2 = fp2.getOrDefault(bssid, ABSENT_RSSI);
+ if (r1 != ABSENT_RSSI || r2 != ABSENT_RSSI) {
+ sumSq += (r1 - r2) * (r1 - r2);
+ if (r1 != ABSENT_RSSI && r2 != ABSENT_RSSI) {
+ commonCount++;
+ }
+ }
+ }
+
+ commonOut[0] = commonCount;
+
+ if (commonCount < MIN_COMMON_APS) {
+ return Double.MAX_VALUE;
+ }
+
+ return Math.sqrt(sumSq) / commonCount;
+ }
+}
diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/fusion/CoordinateTransform.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/fusion/CoordinateTransform.java
new file mode 100644
index 00000000..a742fc42
--- /dev/null
+++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/fusion/CoordinateTransform.java
@@ -0,0 +1,70 @@
+package com.openpositioning.PositionMe.sensors.fusion;
+
+/**
+ * Converts between WGS84 geodetic coordinates (latitude/longitude) and a local
+ * East-North-Up (ENU) Cartesian frame centred on a chosen reference point.
+ *
+ * Uses a flat-earth approximation which is accurate to within centimetres
+ * for distances under 1 km from the origin — well within the assignment's
+ * indoor environment (Nucleus + Library).
+ */
+public class CoordinateTransform {
+
+ private static final double EARTH_RADIUS = 6371000.0; // metres
+ private static final double METRES_PER_DEGREE_LAT = 111132.92;
+
+ private double originLat;
+ private double originLng;
+ private double metersPerDegreeLng;
+ private boolean initialized = false;
+
+ /**
+ * Sets the ENU origin. All subsequent conversions are relative to this point.
+ *
+ * @param lat latitude in degrees (WGS84)
+ * @param lng longitude in degrees (WGS84)
+ */
+ public void setOrigin(double lat, double lng) {
+ this.originLat = lat;
+ this.originLng = lng;
+ this.metersPerDegreeLng = METRES_PER_DEGREE_LAT * Math.cos(Math.toRadians(lat));
+ this.initialized = true;
+ }
+
+ /** Returns {@code true} if the ENU origin has been set. */
+ public boolean isInitialized() {
+ return initialized;
+ }
+
+ /**
+ * Converts WGS84 to local ENU coordinates.
+ *
+ * @return double array {easting, northing} in metres
+ */
+ public double[] toEastNorth(double lat, double lng) {
+ double east = (lng - originLng) * metersPerDegreeLng;
+ double north = (lat - originLat) * METRES_PER_DEGREE_LAT;
+ return new double[]{east, north};
+ }
+
+ /**
+ * Converts local ENU coordinates back to WGS84.
+ *
+ * @return double array {latitude, longitude} in degrees
+ */
+ public double[] toLatLng(double east, double north) {
+ double lat = originLat + north / METRES_PER_DEGREE_LAT;
+ double lng = originLng + east / metersPerDegreeLng;
+ return new double[]{lat, lng};
+ }
+
+ /** Returns the origin latitude in degrees (WGS84). */
+ public double getOriginLat() {
+ return originLat;
+ }
+
+ /** Returns the origin longitude in degrees (WGS84). */
+ public double getOriginLng() {
+ return originLng;
+ }
+}
diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/fusion/FusionManager.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/fusion/FusionManager.java
new file mode 100644
index 00000000..fc4b0b17
--- /dev/null
+++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/fusion/FusionManager.java
@@ -0,0 +1,1036 @@
+package com.openpositioning.PositionMe.sensors.fusion;
+
+import static com.openpositioning.PositionMe.BuildConstants.DEBUG;
+
+import android.util.Log;
+
+import com.google.android.gms.maps.model.LatLng;
+import com.openpositioning.PositionMe.data.remote.FloorplanApiClient;
+import com.openpositioning.PositionMe.sensors.SensorFusion;
+import com.openpositioning.PositionMe.utils.IndoorMapManager;
+
+import java.util.List;
+
+/**
+ * Orchestrates sensor fusion by connecting the {@link ParticleFilter} with
+ * incoming PDR, WiFi, and GNSS observations.
+ *
+ * Responsibilities
+ *
+ * - Auto-initialises the filter from the first reliable position fix
+ * (WiFi preferred, GNSS fallback) — no user selection required.
+ * - Feeds PDR steps as the prediction model.
+ * - Feeds WiFi / GNSS fixes as observation updates.
+ * - Exposes the fused position for display and recording.
+ *
+ */
+public class FusionManager {
+
+ private static final String TAG = "FusionManager";
+
+ /** Number of particles — balance between accuracy and phone performance. */
+ private static final int NUM_PARTICLES = 500;
+
+ /** Initial Gaussian spread (metres) when seeding particles. */
+ private static final double INIT_SPREAD = 10.0;
+
+ // Core components
+ private final ParticleFilter particleFilter;
+ private final CoordinateTransform coordTransform;
+ private final MapConstraint mapConstraint;
+ private CalibrationManager calibrationManager; // for reference-point heading correction
+
+ // Pre-allocated arrays for wall constraint checking (avoid GC per step)
+ private final double[] oldParticleX = new double[NUM_PARTICLES];
+ private final double[] oldParticleY = new double[NUM_PARTICLES];
+
+ // State tracking to avoid duplicate observations
+ private LatLng lastWifiPosition;
+ private LatLng lastGnssPosition;
+ private int lastWifiFloor = -1;
+
+ // WiFi initial floor seeding: set once from first valid WiFi response
+ private boolean wifiFloorSeeded = false;
+
+ // Fused output (volatile for cross-thread visibility)
+ private volatile double fusedLat;
+ private volatile double fusedLng;
+ private volatile int fusedFloor;
+ private volatile double fusedUncertainty;
+ private volatile boolean active = false;
+
+ // PDR step counter for logging
+ private int pdrStepIndex = 0;
+
+ // Floor transition state machine
+ private int lastReportedFloor = -1;
+ private int floorCandidate = -1;
+ private long floorCandidateStartMs = 0;
+ private static final long FLOOR_CONFIRM_MS = 1000; // must hold for 1s
+
+ // ---- Gate state machine (adaptive re-acquisition) ---------------------
+ private enum GateMode { LOCKED, UNLOCKED }
+ enum ObservationLevel { STRONG, MEDIUM, WEAK, INVALID }
+
+ private GateMode gateMode = GateMode.LOCKED;
+ private int stepsSinceLastCorrection = 0;
+
+ private static final int UNLOCK_STEP_THRESHOLD = 15;
+ private static final double UNLOCK_UNCERTAINTY_THRESHOLD = 15.0;
+ private static final double LOCKED_GATE = 15.0;
+ private static final double LOCKED_GATE_INIT = 40.0;
+ /** Distance (m) beyond which UNLOCKED mode re-seeds particles instead of soft update. */
+ private static final double RESEED_DISTANCE = 20.0;
+ /** Indoor GNSS sigma — much weaker influence than WiFi/CAL_DB. */
+ private static final double GNSS_INDOOR_SIGMA = 50.0;
+ /** Warmup period (ms) after init — suppress observations to prevent stale fix jumps. */
+ private static final long WARMUP_MS = 2000;
+ private long initTimeMs = 0;
+
+ // Initial position calibration — accumulate fixes during 10s window
+ private boolean calibrationMode = false;
+ private double calSumLat = 0, calSumLng = 0;
+ private double calTotalWeight = 0;
+ private int calFixCount = 0;
+ private static final double CAL_WEIGHT_CALDB = 3.0; // highest trust
+ private static final double CAL_WEIGHT_WIFI = 1.0;
+ private static final double CAL_WEIGHT_GNSS = 1.0;
+
+ // Heading error estimation (tutor-recommended)
+ private double headingBias = 0; // estimated heading error in radians
+ private double prevObsX = 0, prevObsY = 0;
+ private boolean hasPrevObs = false;
+ // Accumulate pure PDR displacement between WiFi observations
+ private double pdrAccumDx = 0, pdrAccumDy = 0;
+ private static final double HEADING_BIAS_ALPHA = 0.10; // learning rate
+ private static final double HEADING_EARLY_CORRECTION = Math.toRadians(5); // max 5° per step in early phase
+ private static final int HEADING_EARLY_STEPS = 15; // first 15 steps use early correction
+ private static final double HEADING_EARLY_MAX_DIST = 20.0; // only use obs within 20m
+ private static final double HEADING_EARLY_FOV = Math.toRadians(120) / 2; // ±60° = 120° forward
+
+ // Stationary detection — freeze position when not walking
+ private long lastPdrStepTimeMs = 0;
+ /** No PDR step for this long → consider user stationary, reject all corrections. */
+ private static final long STATIONARY_TIMEOUT_MS = 100;
+
+ // ---- Wall-collision heading search mode ---------------------------------
+ // When most particles hit walls for consecutive steps, the heading is likely
+ // wrong. Temporarily boost heading noise so particles fan out and "search"
+ // for the correct direction. Walls naturally select survivors.
+ private static final double SEARCH_COLLISION_THRESHOLD = 0.70; // 70% particles hit wall
+ private static final int SEARCH_TRIGGER_STEPS = 3; // consecutive high-collision steps
+ private static final double SEARCH_HEADING_STD_INIT = Math.toRadians(35); // initial search spread
+ private static final double SEARCH_HEADING_STD_DECAY = 0.85; // multiply each step
+ private static final double SEARCH_HEADING_STD_MIN = Math.toRadians(8); // normal value
+
+ private int consecutiveHighCollisionSteps = 0;
+ private double currentHeadingStd = SEARCH_HEADING_STD_MIN; // active heading noise
+ private boolean inSearchMode = false;
+
+ /** Creates a new FusionManager with default particle count and components. */
+ public FusionManager() {
+ this.particleFilter = new ParticleFilter(NUM_PARTICLES);
+ this.coordTransform = new CoordinateTransform();
+ this.mapConstraint = new MapConstraint();
+ }
+
+ /** Inject CalibrationManager for reference-point heading correction. */
+ public void setCalibrationManager(CalibrationManager cm) {
+ this.calibrationManager = cm;
+ if (DEBUG) Log.i(TAG, "CalibrationManager set (" + (cm != null ? cm.getRecordCount() + " records" : "null") + ")");
+ }
+
+ /**
+ * Enters calibration mode: incoming WiFi/GNSS/CalDB fixes are accumulated
+ * (weighted) instead of initializing the particle filter.
+ */
+ public void startCalibrationMode() {
+ calibrationMode = true;
+ calSumLat = 0; calSumLng = 0;
+ calTotalWeight = 0; calFixCount = 0;
+ active = false; // prevent normal fusion during calibration
+ if (DEBUG) Log.i(TAG, "[CalibrationMode] START — accumulating position fixes");
+ }
+
+ /** Adds a weighted position fix during calibration. */
+ public void addCalibrationFix(double lat, double lng, double weight, String source) {
+ if (!calibrationMode) return;
+ calSumLat += lat * weight;
+ calSumLng += lng * weight;
+ calTotalWeight += weight;
+ calFixCount++;
+ if (DEBUG) Log.i(TAG, String.format("[CalibrationMode] fix #%d src=%s (%.6f,%.6f) w=%.1f totalW=%.1f",
+ calFixCount, source, lat, lng, weight, calTotalWeight));
+ }
+
+ /**
+ * Ends calibration mode: computes weighted average position and
+ * initializes the particle filter at that location.
+ * @param fallbackLat fallback latitude if no fixes were collected
+ * @param fallbackLng fallback longitude if no fixes were collected
+ * @return the final calibrated LatLng
+ */
+ public LatLng finishCalibrationMode(double fallbackLat, double fallbackLng) {
+ calibrationMode = false;
+ double lat, lng;
+ if (calTotalWeight > 0) {
+ lat = calSumLat / calTotalWeight;
+ lng = calSumLng / calTotalWeight;
+ if (DEBUG) Log.i(TAG, String.format("[CalibrationMode] FINISH %d fixes → (%.6f,%.6f)",
+ calFixCount, lat, lng));
+ } else {
+ lat = fallbackLat;
+ lng = fallbackLng;
+ if (DEBUG) Log.w(TAG, "[CalibrationMode] FINISH no fixes, using fallback");
+ }
+
+ // Initialize particle filter at calibrated position
+ if (!coordTransform.isInitialized()) {
+ coordTransform.setOrigin(lat, lng);
+ }
+ double[] en = coordTransform.toEastNorth(lat, lng);
+ particleFilter.initialize(en[0], en[1], fusedFloor, INIT_SPREAD);
+ fusedLat = lat;
+ fusedLng = lng;
+ active = true;
+ initTimeMs = System.currentTimeMillis();
+
+ if (DEBUG) Log.i(TAG, String.format("[CalibrationMode] PF initialized at (%.6f,%.6f) ENU=(%.2f,%.2f)",
+ lat, lng, en[0], en[1]));
+ return new LatLng(lat, lng);
+ }
+
+ /** Returns {@code true} if currently in calibration mode. */
+ public boolean isInCalibrationMode() { return calibrationMode; }
+
+ private IndoorMapManager indoorMapManager;
+ private static final double STAIRS_STEP_FACTOR = 0.5;
+
+ /** Inject IndoorMapManager for stairs/lift proximity detection. */
+ public void setIndoorMapManager(IndoorMapManager mgr) {
+ this.indoorMapManager = mgr;
+ }
+
+ /**
+ * Loads wall geometry for the specified building into the map constraint.
+ * Includes both interior walls (from floor shapes) and the building outline
+ * (exterior boundary, applied to all floors).
+ * Must be called after {@link CoordinateTransform} origin is set.
+ *
+ * @param floorShapesList per-floor shape data from FloorplanApiClient
+ * @param outlinePolygon building boundary polygon (may be null)
+ */
+ public void loadMapConstraints(
+ List floorShapesList,
+ List outlinePolygon) {
+ if (coordTransform.isInitialized()) {
+ mapConstraint.initialize(floorShapesList, coordTransform);
+ mapConstraint.setOutlineConstraint(outlinePolygon, coordTransform);
+ } else {
+ if (DEBUG) Log.w(TAG, "Cannot load map constraints: coordTransform not initialised");
+ }
+ }
+
+ // ---- initialisation ------------------------------------------------------
+
+ /**
+ * Initialises the fusion from the user-selected start location.
+ * Called at the start of recording, before GNSS/WiFi fixes arrive.
+ * Uses a wider spread since the user tap may not be pixel-perfect.
+ */
+ public void initializeFromStartLocation(double lat, double lng) {
+ if (DEBUG) Log.i(TAG, String.format("Initialising from user start location (%.6f, %.6f)", lat, lng));
+ // Force origin to user-selected start point (overwrite any earlier GNSS-set origin)
+ coordTransform.setOrigin(lat, lng);
+ tryInitialize(lat, lng, 0);
+ }
+
+ /**
+ * Attempts to initialise the particle filter from a position fix.
+ * Called automatically by {@link #onWifiPosition} or {@link #onGnssPosition}
+ * whichever arrives first.
+ */
+ private void tryInitialize(double lat, double lng, int floor) {
+ if (active) return;
+
+ // During calibration mode, accumulate fixes instead of initializing
+ if (calibrationMode) {
+ // Still set origin from first fix so coordTransform is ready
+ if (!coordTransform.isInitialized()) {
+ coordTransform.setOrigin(lat, lng);
+ }
+ return; // actual position will be set in finishCalibrationMode()
+ }
+
+ // Set coordinate origin at the first fix
+ if (!coordTransform.isInitialized()) {
+ coordTransform.setOrigin(lat, lng);
+ }
+
+ double[] en = coordTransform.toEastNorth(lat, lng);
+ particleFilter.initialize(en[0], en[1], floor, INIT_SPREAD);
+
+ fusedLat = lat;
+ fusedLng = lng;
+ fusedFloor = floor;
+ active = true;
+ initTimeMs = System.currentTimeMillis();
+
+ if (DEBUG) {
+ // [Origin] diagnostic
+ Log.i(TAG, String.format("[Origin] originLat=%.8f originLng=%.8f", lat, lng));
+
+ // [RoundTrip] diagnostic
+ double[] rt = coordTransform.toLatLng(0, 0);
+ double rtError = Math.sqrt(Math.pow((rt[0] - lat) * 111132.92, 2) +
+ Math.pow((rt[1] - lng) * 111132.92 * Math.cos(Math.toRadians(lat)), 2));
+ Log.i(TAG, String.format("[RoundTrip] toLatLng(0,0)=(%.8f,%.8f) originError=%.4fm", rt[0], rt[1], rtError));
+
+ // [AxisTest] diagnostic: 5m East should give (+5, ~0)
+ double[] east5 = coordTransform.toEastNorth(lat, lng + 5.0 / (111132.92 * Math.cos(Math.toRadians(lat))));
+ double[] north5 = coordTransform.toEastNorth(lat + 5.0 / 111132.92, lng);
+ Log.i(TAG, String.format("[AxisTest] 5mEast→ENU=(%.2f,%.2f) %s | 5mNorth→ENU=(%.2f,%.2f) %s",
+ east5[0], east5[1], (east5[0] > 4 && Math.abs(east5[1]) < 1) ? "PASS" : "FAIL",
+ north5[0], north5[1], (north5[1] > 4 && Math.abs(north5[0]) < 1) ? "PASS" : "FAIL"));
+ }
+ }
+
+ // ---- sensor callbacks ----------------------------------------------------
+
+ /**
+ * Called on each detected step with the PDR stride length and device heading.
+ *
+ * @param stepLength stride in metres (Weiberg or manual)
+ * @param headingRad azimuth in radians (0 = North, clockwise)
+ */
+ public void onPdrStep(double stepLength, double headingRad) {
+ if (!active) {
+ if (DEBUG) Log.d(TAG, "PDR step ignored — fusion not yet initialised");
+ return;
+ }
+
+ // Stairs/lift step reduction: multiply horizontal step by 0.2
+ if (indoorMapManager != null && coordTransform.isInitialized()) {
+ double[] curLatLng = coordTransform.toLatLng(
+ particleFilter.getEstimatedX(),
+ particleFilter.getEstimatedY());
+ LatLng curPos = new LatLng(curLatLng[0], curLatLng[1]);
+ if (indoorMapManager.isNearStairs(curPos)) {
+ stepLength *= STAIRS_STEP_FACTOR;
+ if (DEBUG) Log.d(TAG, "[Stairs] Step reduced to " + String.format("%.3fm", stepLength));
+ }
+ }
+
+ double correctedHeading = headingRad + headingBias; // headingBias enabled
+
+ double dx = stepLength * Math.sin(correctedHeading);
+ double dy = stepLength * Math.cos(correctedHeading);
+
+ // Accumulate pure PDR displacement for heading bias calculation
+ pdrAccumDx += dx;
+ pdrAccumDy += dy;
+
+ lastPdrStepTimeMs = System.currentTimeMillis();
+
+ // Save old particle positions before predict (for wall collision check)
+ Particle[] particles = particleFilter.getParticles();
+ for (int i = 0; i < particles.length; i++) {
+ oldParticleX[i] = particles[i].x;
+ oldParticleY[i] = particles[i].y;
+ }
+
+ particleFilter.predict(stepLength, correctedHeading, currentHeadingStd);
+
+ // Apply wall constraints: snap + penalise particles that crossed walls
+ int collisionCount = 0;
+ if (mapConstraint.isInitialized()) {
+ collisionCount = mapConstraint.applyConstraints(particles, oldParticleX, oldParticleY);
+ }
+
+ // ---- Wall-collision heading correction via reference points ----
+ double collisionRatio = (double) collisionCount / NUM_PARTICLES;
+ if (collisionRatio >= SEARCH_COLLISION_THRESHOLD) {
+ consecutiveHighCollisionSteps++;
+ if (!inSearchMode && consecutiveHighCollisionSteps >= SEARCH_TRIGGER_STEPS) {
+ inSearchMode = true;
+ currentHeadingStd = SEARCH_HEADING_STD_INIT;
+ if (DEBUG) Log.i(TAG, String.format("[SearchMode] ENTER collisionRatio=%.0f%% consecutive=%d",
+ collisionRatio * 100, consecutiveHighCollisionSteps));
+
+ // ---- Reference-point heading snap ----
+ // Find the nearest calibration point within ±90° of current heading
+ // and force heading bias to point toward it.
+ double snapHeading = findNearestRefPointHeading(correctedHeading);
+ if (!Double.isNaN(snapHeading)) {
+ double correction = snapHeading - headingRad; // bias = target - raw
+ // Normalize to [-π, π]
+ while (correction > Math.PI) correction -= 2 * Math.PI;
+ while (correction < -Math.PI) correction += 2 * Math.PI;
+ if (DEBUG) Log.i(TAG, String.format("[SearchMode] SNAP oldBias=%.1f° newBias=%.1f° target=%.1f° raw=%.1f°",
+ Math.toDegrees(headingBias), Math.toDegrees(correction),
+ Math.toDegrees(snapHeading), Math.toDegrees(headingRad)));
+ headingBias = correction;
+ }
+ }
+ } else {
+ consecutiveHighCollisionSteps = 0;
+ }
+
+ // Decay heading noise each step (whether in search mode or not)
+ if (inSearchMode) {
+ currentHeadingStd = Math.max(
+ currentHeadingStd * SEARCH_HEADING_STD_DECAY,
+ SEARCH_HEADING_STD_MIN);
+ if (currentHeadingStd <= SEARCH_HEADING_STD_MIN) {
+ inSearchMode = false;
+ if (DEBUG) Log.i(TAG, "[SearchMode] EXIT — heading noise decayed to normal");
+ }
+ }
+
+ updateFusedOutput();
+ pdrStepIndex++;
+ stepsSinceLastCorrection++;
+ checkGateMode();
+ if (DEBUG) Log.i(TAG, String.format("[PDR] step=%d rawH=%.1f° bias=%.1f° corrH=%.1f° len=%.2f dENU=(%.3f,%.3f) fENU=(%.2f,%.2f) gate=%s nofix=%d coll=%.0f%% hStd=%.1f° search=%b",
+ pdrStepIndex, Math.toDegrees(headingRad),
+ Math.toDegrees(headingBias), Math.toDegrees(correctedHeading),
+ stepLength, dx, dy,
+ particleFilter.getEstimatedX(), particleFilter.getEstimatedY(),
+ gateMode, stepsSinceLastCorrection,
+ collisionRatio * 100, Math.toDegrees(currentHeadingStd), inSearchMode));
+ }
+
+ /**
+ * Updates the heading bias estimate using the discrepancy between the
+ * direction the PF moved and the direction of an absolute observation
+ * (WiFi or GNSS). Only updates when there is enough movement.
+ */
+ private void updateHeadingBias(double obsX, double obsY) {
+ if (!hasPrevObs) {
+ prevObsX = obsX;
+ prevObsY = obsY;
+ pdrAccumDx = 0;
+ pdrAccumDy = 0;
+ hasPrevObs = true;
+ return;
+ }
+
+ // Pure PDR displacement (accumulated since last WiFi observation)
+ double pdrDist = Math.hypot(pdrAccumDx, pdrAccumDy);
+
+ // WiFi observation displacement
+ double obsDx = obsX - prevObsX;
+ double obsDy = obsY - prevObsY;
+ double obsDist = Math.hypot(obsDx, obsDy);
+
+ // Only update if PDR moved enough (avoids noise when stationary)
+ if (pdrDist < 1.0) {
+ if (DEBUG) Log.d(TAG, String.format("[HeadingBias] SKIP pdrDist=%.1fm (need >1m, keep accumulating)",
+ pdrDist));
+ return;
+ }
+
+ if (obsDist < 1.0) {
+ if (DEBUG) Log.d(TAG, String.format("[HeadingBias] SKIP obsDist=%.1fm (need >1m) pdrDist=%.1fm — reset pdr accum",
+ obsDist, pdrDist));
+ prevObsX = obsX;
+ prevObsY = obsY;
+ pdrAccumDx = 0;
+ pdrAccumDy = 0;
+ return;
+ }
+
+ // Both moved enough — compute heading bias
+ double pdrAngle = Math.atan2(pdrAccumDx, pdrAccumDy);
+ double obsAngle = Math.atan2(obsDx, obsDy);
+ double angleDiff = obsAngle - pdrAngle;
+
+ // Normalize to [-π, π]
+ while (angleDiff > Math.PI) angleDiff -= 2 * Math.PI;
+ while (angleDiff < -Math.PI) angleDiff += 2 * Math.PI;
+
+ boolean earlyPhase = (pdrStepIndex <= HEADING_EARLY_STEPS);
+
+ if (earlyPhase) {
+ // Early phase: only accept observations within forward 90° and 20m
+ // Current heading direction (PDR accumulated)
+ double currentHeading = Math.atan2(pdrAccumDx, pdrAccumDy);
+ double obsDirection = Math.atan2(obsDx, obsDy);
+ double dirDiff = obsDirection - currentHeading;
+ while (dirDiff > Math.PI) dirDiff -= 2 * Math.PI;
+ while (dirDiff < -Math.PI) dirDiff += 2 * Math.PI;
+
+ if (Math.abs(dirDiff) > HEADING_EARLY_FOV || obsDist > HEADING_EARLY_MAX_DIST) {
+ if (DEBUG) Log.i(TAG, String.format("[HeadingBias] REJECT early: dirDiff=%.1f° dist=%.1fm (need <90° <20m) step=%d",
+ Math.toDegrees(dirDiff), obsDist, pdrStepIndex));
+ } else {
+ // Clamp correction to ±5° per observation
+ double clamped = Math.max(-HEADING_EARLY_CORRECTION,
+ Math.min(HEADING_EARLY_CORRECTION, angleDiff));
+ double oldBias = headingBias;
+ headingBias += clamped;
+
+ if (DEBUG) Log.i(TAG, String.format("[HeadingBias] EARLY step=%d diff=%.1f° clamped=%.1f° oldBias=%.1f° newBias=%.1f° pdrDist=%.1fm obsDist=%.1fm",
+ pdrStepIndex, Math.toDegrees(angleDiff), Math.toDegrees(clamped),
+ Math.toDegrees(oldBias), Math.toDegrees(headingBias), pdrDist, obsDist));
+ }
+ } else {
+ // Normal phase: reject > 20°, conservative EMA
+ if (Math.abs(angleDiff) > Math.toRadians(20)) {
+ if (DEBUG) Log.i(TAG, String.format("[HeadingBias] REJECT diff=%.1f° > 20° step=%d pdrAngle=%.1f° obsAngle=%.1f°",
+ Math.toDegrees(angleDiff), pdrStepIndex, Math.toDegrees(pdrAngle), Math.toDegrees(obsAngle)));
+ } else {
+ double oldBias = headingBias;
+ headingBias = headingBias * (1 - HEADING_BIAS_ALPHA)
+ + angleDiff * HEADING_BIAS_ALPHA;
+
+ if (DEBUG) Log.i(TAG, String.format("[HeadingBias] UPDATE α=%.2f step=%d pdrAngle=%.1f° obsAngle=%.1f° diff=%.1f° oldBias=%.1f° newBias=%.1f° pdrDist=%.1fm obsDist=%.1fm",
+ HEADING_BIAS_ALPHA, pdrStepIndex,
+ Math.toDegrees(pdrAngle), Math.toDegrees(obsAngle),
+ Math.toDegrees(angleDiff), Math.toDegrees(oldBias),
+ Math.toDegrees(headingBias), pdrDist, obsDist));
+ }
+ }
+
+ // Only reset accumulators when we actually consumed them (UPDATE or REJECT)
+ prevObsX = obsX;
+ prevObsY = obsY;
+ pdrAccumDx = 0;
+ pdrAccumDy = 0;
+ }
+
+ /**
+ * Searches calibration reference points within ±90° of the current heading
+ * and returns the heading (radians) toward the nearest one.
+ *
+ * Reference points lie on corridors the user has walked before,
+ * so the nearest forward one indicates the correct walking direction.
+ *
+ * @param currentHeading current corrected heading in radians (0=N, CW)
+ * @return heading toward nearest forward ref point, or NaN if none found
+ */
+ private double findNearestRefPointHeading(double currentHeading) {
+ if (calibrationManager == null || !coordTransform.isInitialized()) {
+ if (DEBUG) Log.d(TAG, "[SearchMode] No CalibrationManager or CoordTransform — skip snap");
+ return Double.NaN;
+ }
+
+ double curX = particleFilter.getEstimatedX();
+ double curY = particleFilter.getEstimatedY();
+
+ List refPoints = calibrationManager.getRecordPositions(fusedFloor);
+ if (refPoints.isEmpty()) {
+ if (DEBUG) Log.d(TAG, "[SearchMode] No ref points on floor " + fusedFloor);
+ return Double.NaN;
+ }
+
+ double bestDist = Double.MAX_VALUE;
+ double bestHeading = Double.NaN;
+ int candidateCount = 0;
+
+ for (LatLng ll : refPoints) {
+ double[] en = coordTransform.toEastNorth(ll.latitude, ll.longitude);
+ double dx = en[0] - curX;
+ double dy = en[1] - curY;
+ double dist = Math.hypot(dx, dy);
+
+ // Skip points too close (noise) or too far (irrelevant)
+ if (dist < 2.0 || dist > 30.0) continue;
+
+ // Heading from current position to this reference point
+ double headingToRef = Math.atan2(dx, dy); // atan2(E, N) = azimuth
+
+ // Angle difference to current heading
+ double angleDiff = headingToRef - currentHeading;
+ while (angleDiff > Math.PI) angleDiff -= 2 * Math.PI;
+ while (angleDiff < -Math.PI) angleDiff += 2 * Math.PI;
+
+ // Only consider points within ±90° (forward semicircle)
+ if (Math.abs(angleDiff) > Math.PI / 2) continue;
+
+ candidateCount++;
+ if (dist < bestDist) {
+ bestDist = dist;
+ bestHeading = headingToRef;
+ }
+ }
+
+ if (!Double.isNaN(bestHeading)) {
+ if (DEBUG) Log.i(TAG, String.format("[SearchMode] Found %d forward ref points, nearest at %.1fm heading=%.1f°",
+ candidateCount, bestDist, Math.toDegrees(bestHeading)));
+ } else {
+ if (DEBUG) Log.i(TAG, String.format("[SearchMode] No forward ref points found (%d total on floor %d)",
+ refPoints.size(), fusedFloor));
+ }
+ return bestHeading;
+ }
+
+ /**
+ * Called when a new WiFi position fix arrives from the OpenPositioning API.
+ */
+ public void onWifiPosition(double lat, double lng, int floor) {
+ LatLng pos = new LatLng(lat, lng);
+ if (pos.equals(lastWifiPosition)) return;
+ lastWifiPosition = pos;
+ lastWifiFloor = floor;
+
+ // Seed initial floor from first valid WiFi response.
+ // setBuildingOverlay() runs before WiFi responds, so we seed here.
+ if (!wifiFloorSeeded && floor >= 0) {
+ wifiFloorSeeded = true;
+ SensorFusion.getInstance().setInitialFloor(floor);
+ lastReportedFloor = floor;
+ if (DEBUG) Log.i(TAG, "[WifiFloorSeed] First WiFi floor=" + floor
+ + " → seeded as initial floor");
+ }
+
+ if (!active) {
+ if (calibrationMode) {
+ addCalibrationFix(lat, lng, CAL_WEIGHT_WIFI, "WIFI_API");
+ }
+ if (DEBUG) Log.i(TAG, String.format("WiFi fix: (%.6f, %.6f) floor=%d active=false calMode=%b → init",
+ lat, lng, floor, calibrationMode));
+ tryInitialize(lat, lng, floor);
+ return;
+ }
+
+ if (!coordTransform.isInitialized()) return;
+
+ double[] en = coordTransform.toEastNorth(lat, lng);
+
+ // WiFi API = STRONG source
+ if (!shouldAcceptObservation(en[0], en[1], ObservationLevel.STRONG)) {
+ logObservation("WIFI_API", en[0], en[1], floor, 0, false,
+ String.format("gate_rejected mode=%s", gateMode));
+ return;
+ }
+
+ double distFromFused = Math.hypot(en[0] - particleFilter.getEstimatedX(),
+ en[1] - particleFilter.getEstimatedY());
+
+ // Recovery: when UNLOCKED with large drift, re-seed particles around observation
+ if (gateMode == GateMode.UNLOCKED && distFromFused > RESEED_DISTANCE) {
+ if (DEBUG) Log.i(TAG, String.format("[Recovery] WIFI re-seed dist=%.1fm → particles reset around (%.2f,%.2f)",
+ distFromFused, en[0], en[1]));
+ particleFilter.initialize(en[0], en[1], floor, 12.0);
+ updateFusedOutput();
+ onObservationAccepted(ObservationLevel.STRONG);
+ return;
+ }
+
+ double wifiSigma = 4.0;
+ logObservation("WIFI_API", en[0], en[1], floor, wifiSigma, true,
+ String.format("level=STRONG mode=%s dist=%.1fm", gateMode, distFromFused));
+ particleFilter.updateWithDynamicSigma(en[0], en[1], wifiSigma);
+ // WiFi floor is NOT used for ongoing floor updates — baro-only autofloor.
+ // particleFilter.updateFloor(floor);
+ updateHeadingBias(en[0], en[1]);
+ updateFusedOutput();
+ onObservationAccepted(ObservationLevel.STRONG);
+ }
+
+ /**
+ * Called when a new GPS fix arrives (network/cellular is excluded upstream).
+ *
+ * @param accuracy reported accuracy in metres from {@code Location.getAccuracy()}
+ */
+ public void onGnssPosition(double lat, double lng, float accuracy) {
+ LatLng pos = new LatLng(lat, lng);
+ if (pos.equals(lastGnssPosition)) return;
+ lastGnssPosition = pos;
+
+ if (!active) {
+ if (calibrationMode) {
+ addCalibrationFix(lat, lng, CAL_WEIGHT_GNSS, "GNSS");
+ }
+ if (DEBUG) Log.i(TAG, String.format("GPS fix: (%.6f, %.6f) acc=%.0fm active=false calMode=%b → init",
+ lat, lng, accuracy, calibrationMode));
+ tryInitialize(lat, lng, 0);
+ return;
+ }
+
+ if (!coordTransform.isInitialized()) return;
+
+ double[] en = coordTransform.toEastNorth(lat, lng);
+
+ // GF: GNSS is reliable → STRONG; upper floors: WEAK
+ boolean isGF = (fusedFloor == 0 || fusedFloor == 1);
+ ObservationLevel gateLevel = isGF ? ObservationLevel.STRONG : ObservationLevel.WEAK;
+ if (!shouldAcceptObservation(en[0], en[1], gateLevel)) {
+ logObservation("GPS", en[0], en[1], fusedFloor, 0, false,
+ String.format("gate_rejected mode=%s acc=%.0fm floor=%d", gateMode, accuracy, fusedFloor));
+ return;
+ }
+
+ // GF (floor 0 or 1 with bias): GNSS is reliable, use same weight as WiFi API.
+ // Other floors: indoor GNSS is unreliable, use high sigma.
+ boolean isGroundFloor = (fusedFloor == 0 || fusedFloor == 1);
+ double sigma;
+ ObservationLevel level;
+ if (isGroundFloor) {
+ sigma = 4.0; // same as WiFi API
+ level = ObservationLevel.STRONG;
+ } else {
+ sigma = Math.max(accuracy, GNSS_INDOOR_SIGMA);
+ level = ObservationLevel.WEAK;
+ }
+ logObservation("GPS", en[0], en[1], fusedFloor, sigma, true,
+ String.format("level=%s mode=%s acc=%.0fm floor=%d gf=%b step=%d",
+ level, gateMode, accuracy, fusedFloor, isGroundFloor, pdrStepIndex));
+ particleFilter.updateWithDynamicSigma(en[0], en[1], sigma);
+ updateFusedOutput();
+ onObservationAccepted(level);
+ }
+
+ /**
+ * Called when the user long-presses the map to manually correct their position.
+ * Uses a very tight uncertainty to pull particles strongly toward the correction.
+ *
+ * @param lat true latitude selected by the user
+ * @param lng true longitude selected by the user
+ */
+ public void onManualCorrection(double lat, double lng) {
+ if (!coordTransform.isInitialized()) {
+ // Use the correction as the initial origin
+ tryInitialize(lat, lng, fusedFloor);
+ return;
+ }
+
+ if (!active) {
+ tryInitialize(lat, lng, fusedFloor);
+ return;
+ }
+
+ double[] en = coordTransform.toEastNorth(lat, lng);
+ // Very tight std dev (2m) — strong correction
+ particleFilter.updateWithManualCorrection(en[0], en[1], 2.0);
+ updateFusedOutput();
+ if (DEBUG) Log.i(TAG, String.format("Manual correction applied at (%.6f, %.6f)", lat, lng));
+ }
+
+ /**
+ * Reseeds all floor state to a known value (e.g. from WiFi API on autofloor toggle).
+ * Updates lastReportedFloor, fusedFloor, and particle floors so that
+ * subsequent baro readings don't trigger spurious floor transitions.
+ *
+ * @param floor the logical floor number (0=GF, 1=F1, …)
+ */
+ public void reseedFloor(int floor) {
+ int prev = lastReportedFloor;
+ lastReportedFloor = floor;
+ floorCandidate = -1;
+ fusedFloor = floor;
+ if (particleFilter.isInitialized()) {
+ particleFilter.updateFloor(floor);
+ }
+ if (DEBUG) Log.i(TAG, String.format("[FloorReseed] FM floor %d→%d (particles updated)", prev, floor));
+ }
+
+ // ---- unified observation logging ------------------------------------------
+
+ /** Logs an observation event for diagnostic purposes. */
+ private void logObservation(String source, double obsE, double obsN,
+ int floor, double sigma,
+ boolean accepted, String reason) {
+ if (DEBUG) Log.i(TAG, String.format("[Observation] source=%s floor=%d sigma=%.1f accepted=%b reason=%s obsENU=(%.2f,%.2f)",
+ source, floor, sigma, accepted, reason, obsE, obsN));
+ }
+
+ // ---- gate state machine helpers ----------------------------------------
+
+ /** Returns the distance gate for the given observation level, or -1 to reject. */
+ private double getCurrentGate(ObservationLevel level) {
+ if (pdrStepIndex < 30) return LOCKED_GATE_INIT;
+ if (gateMode == GateMode.UNLOCKED) {
+ // UNLOCKED: accept STRONG and MEDIUM without distance limit (we know PDR drifted)
+ if (level == ObservationLevel.STRONG || level == ObservationLevel.MEDIUM)
+ return Double.MAX_VALUE;
+ return -1; // reject WEAK (GNSS) even in UNLOCKED
+ }
+ return LOCKED_GATE;
+ }
+
+ /** Checks whether to transition LOCKED → UNLOCKED based on drift indicators. */
+ private void checkGateMode() {
+ if (gateMode == GateMode.LOCKED) {
+ if (stepsSinceLastCorrection > UNLOCK_STEP_THRESHOLD ||
+ fusedUncertainty > UNLOCK_UNCERTAINTY_THRESHOLD) {
+ gateMode = GateMode.UNLOCKED;
+ if (DEBUG) Log.i(TAG, String.format("[Gate] LOCKED→UNLOCKED steps_no_fix=%d uncertainty=%.1fm",
+ stepsSinceLastCorrection, fusedUncertainty));
+ }
+ }
+ }
+
+ /** Returns true if the observation passes the adaptive distance gate. */
+ private boolean shouldAcceptObservation(double obsE, double obsN,
+ ObservationLevel level) {
+ // Stationary: freeze position — reject ALL corrections when user is not walking
+ if (lastPdrStepTimeMs > 0
+ && (System.currentTimeMillis() - lastPdrStepTimeMs) > STATIONARY_TIMEOUT_MS) {
+ if (DEBUG) Log.d(TAG, String.format("[Gate] REJECTED stationary (no step for %dms) level=%s",
+ System.currentTimeMillis() - lastPdrStepTimeMs, level));
+ return false;
+ }
+
+ // Warmup: suppress all observations for WARMUP_MS after initialisation
+ // to prevent stale GNSS/WiFi cache from pulling the initial position.
+ if (initTimeMs > 0 && System.currentTimeMillis() - initTimeMs < WARMUP_MS) {
+ if (DEBUG) Log.d(TAG, String.format("[Gate] REJECTED warmup (%dms remaining) level=%s",
+ WARMUP_MS - (System.currentTimeMillis() - initTimeMs), level));
+ return false;
+ }
+
+ double distFromFused = Math.hypot(obsE - particleFilter.getEstimatedX(),
+ obsN - particleFilter.getEstimatedY());
+ double gate = getCurrentGate(level);
+
+ if (gate < 0) {
+ if (DEBUG) Log.d(TAG, String.format("[Gate] REJECTED level=%s in UNLOCKED mode (STRONG/MEDIUM only)", level));
+ return false;
+ }
+
+ if (distFromFused > gate) {
+ if (DEBUG) Log.d(TAG, String.format("[Gate] REJECTED dist=%.1fm > gate=%.0fm mode=%s level=%s step=%d",
+ distFromFused, gate, gateMode, level, pdrStepIndex));
+ return false;
+ }
+
+ return true;
+ }
+
+ /** Called after an observation is accepted to reset drift counter and lock gate. */
+ private void onObservationAccepted(ObservationLevel level) {
+ stepsSinceLastCorrection = 0;
+ if (gateMode == GateMode.UNLOCKED
+ && (level == ObservationLevel.STRONG || level == ObservationLevel.MEDIUM)) {
+ gateMode = GateMode.LOCKED;
+ if (DEBUG) Log.i(TAG, String.format("[Gate] UNLOCKED→LOCKED (%s fix received)", level));
+ }
+ }
+
+ // ---- calibration observation (from CalibrationManager WKNN) -------------
+
+ /**
+ * Processes a calibration database observation through the same pipeline
+ * as GPS/WiFi: distance gate, stationary damping, unified logging.
+ */
+ public void onCalibrationObservation(double obsE, double obsN,
+ double sigma, int floor, String quality) {
+ // During calibration mode, convert ENU back to LatLng and accumulate
+ if (calibrationMode && coordTransform.isInitialized()) {
+ double[] ll = coordTransform.toLatLng(obsE, obsN);
+ addCalibrationFix(ll[0], ll[1], CAL_WEIGHT_CALDB, "CAL_DB(" + quality + ")");
+ return;
+ }
+ if (!active || !coordTransform.isInitialized()) return;
+
+ // Map CAL_DB quality string to observation level
+ ObservationLevel calLevel;
+ switch (quality) {
+ case "GOOD":
+ calLevel = ObservationLevel.STRONG;
+ break;
+ case "AMBIGUOUS":
+ calLevel = ObservationLevel.MEDIUM;
+ break;
+ default:
+ calLevel = ObservationLevel.WEAK;
+ break;
+ }
+
+ if (!shouldAcceptObservation(obsE, obsN, calLevel)) {
+ logObservation("CAL_DB", obsE, obsN, floor, sigma, false,
+ String.format("gate_rejected mode=%s quality=%s level=%s",
+ gateMode, quality, calLevel));
+ return;
+ }
+
+ double distFromFused = Math.hypot(obsE - particleFilter.getEstimatedX(),
+ obsN - particleFilter.getEstimatedY());
+
+ // Recovery: when UNLOCKED with large drift, re-seed (looser spread for CAL_DB)
+ if (gateMode == GateMode.UNLOCKED && distFromFused > RESEED_DISTANCE
+ && calLevel == ObservationLevel.STRONG) {
+ if (DEBUG) Log.i(TAG, String.format("[Recovery] CAL_DB re-seed dist=%.1fm quality=%s",
+ distFromFused, quality));
+ particleFilter.initialize(obsE, obsN, floor, 15.0);
+ updateFusedOutput();
+ onObservationAccepted(calLevel);
+ return;
+ }
+
+ logObservation("CAL_DB", obsE, obsN, floor, sigma, true,
+ String.format("level=%s mode=%s quality=%s dist=%.1f",
+ calLevel, gateMode, quality, distFromFused));
+ particleFilter.updateWithDynamicSigma(obsE, obsN, sigma);
+ // Use GOOD calibration observations for heading bias (more stable than WiFi API)
+ if (calLevel == ObservationLevel.STRONG) {
+ updateHeadingBias(obsE, obsN);
+ }
+ updateFusedOutput();
+ onObservationAccepted(calLevel);
+ }
+
+ // ---- floor handling ------------------------------------------------------
+
+ /**
+ * Initializes particle floor distribution as a prior (not force).
+ * Most particles go to the best floor, some to adjacent floors.
+ *
+ * @param bestFloor most likely floor
+ * @param confidence 0.0–1.0, controls how concentrated the distribution is
+ */
+ public void initializeFloorPrior(int bestFloor, double confidence) {
+ if (!particleFilter.isInitialized()) {
+ fusedFloor = bestFloor;
+ lastReportedFloor = bestFloor;
+ if (DEBUG) Log.i(TAG, String.format("[Floor] prior deferred (PF not init): best=%d", bestFloor));
+ return;
+ }
+
+ Particle[] particles = particleFilter.getParticles();
+ java.util.Random rng = new java.util.Random();
+
+ int onBest = 0, onAdjacent = 0;
+ for (Particle p : particles) {
+ double roll = rng.nextDouble();
+ if (roll < confidence) {
+ p.floor = bestFloor;
+ onBest++;
+ } else if (roll < confidence + (1 - confidence) * 0.5) {
+ p.floor = Math.max(0, bestFloor - 1);
+ onAdjacent++;
+ } else {
+ p.floor = bestFloor + 1;
+ onAdjacent++;
+ }
+ }
+
+ fusedFloor = bestFloor;
+ lastReportedFloor = bestFloor;
+
+ if (DEBUG) Log.i(TAG, String.format("[Floor] prior initialized: best=%d conf=%.0f%% particles=%d/%d on best",
+ bestFloor, confidence * 100, onBest, particles.length));
+ }
+
+ /**
+ * Called by barometer when relative height changes. Uses a state machine:
+ * STABLE → CANDIDATE (new floor detected) → CONFIRMED (held for 2 seconds).
+ * Only CONFIRMED transitions update the particle filter.
+ * The barometer provides transition EVIDENCE, not a direct floor command.
+ */
+ public void onFloorChanged(int baroFloor) {
+ if (!active) return;
+ if (baroFloor == lastReportedFloor) {
+ floorCandidate = -1;
+ return;
+ }
+
+ long now = System.currentTimeMillis();
+
+ if (baroFloor != floorCandidate) {
+ floorCandidate = baroFloor;
+ floorCandidateStartMs = now;
+ if (DEBUG) Log.d(TAG, String.format("[Floor] CANDIDATE %d → %d",
+ lastReportedFloor, baroFloor));
+ return;
+ }
+
+ if (now - floorCandidateStartMs < FLOOR_CONFIRM_MS) return;
+
+ if (DEBUG) Log.i(TAG, String.format("[Floor] CONFIRMED %d → %d (baro held 1s)",
+ lastReportedFloor, baroFloor));
+ lastReportedFloor = baroFloor;
+ floorCandidate = -1;
+ particleFilter.updateFloor(baroFloor);
+ updateFusedOutput();
+
+ // Reset baro baseline to prevent long-term drift accumulation.
+ // After this, relHeight resets to ~0 relative to the new floor.
+ SensorFusion.getInstance().resetBaroBaseline(baroFloor);
+ }
+
+ /** Returns floor probability distribution from particle weights. */
+ public double[] getFloorProbabilities() {
+ double[] probs = new double[10];
+ if (!particleFilter.isInitialized()) return probs;
+ for (Particle p : particleFilter.getParticles()) {
+ int f = Math.max(0, Math.min(9, p.floor));
+ probs[f] += p.weight;
+ }
+ return probs;
+ }
+
+ // ---- output --------------------------------------------------------------
+
+ private void updateFusedOutput() {
+ double[] latLng = coordTransform.toLatLng(
+ particleFilter.getEstimatedX(),
+ particleFilter.getEstimatedY());
+ fusedLat = latLng[0];
+ fusedLng = latLng[1];
+ fusedFloor = particleFilter.getEstimatedFloor();
+ fusedUncertainty = particleFilter.getUncertainty();
+ }
+
+ /** Returns the fused position as a LatLng, or null if not yet initialised. */
+ public LatLng getFusedLatLng() {
+ if (!active) return null;
+ return new LatLng(fusedLat, fusedLng);
+ }
+
+ /** Returns the current fused floor index. */
+ public int getFusedFloor() {
+ return fusedFloor;
+ }
+
+ /** Returns the current fused position uncertainty in metres. */
+ public double getFusedUncertainty() {
+ return fusedUncertainty;
+ }
+
+ /** Returns {@code true} if the fusion engine has been initialised and is active. */
+ public boolean isActive() {
+ return active;
+ }
+
+ /** Resets the fusion state for a new recording session. */
+ public void reset() {
+ active = false;
+ lastWifiPosition = null;
+ lastGnssPosition = null;
+ lastWifiFloor = -1;
+ wifiFloorSeeded = false;
+ fusedLat = 0;
+ fusedLng = 0;
+ fusedFloor = 0;
+ fusedUncertainty = 0;
+ headingBias = 0;
+ hasPrevObs = false;
+ pdrAccumDx = 0;
+ pdrAccumDy = 0;
+ pdrStepIndex = 0;
+ lastReportedFloor = -1;
+ floorCandidate = -1;
+ floorCandidateStartMs = 0;
+ gateMode = GateMode.LOCKED;
+ stepsSinceLastCorrection = 0;
+ initTimeMs = 0;
+ lastPdrStepTimeMs = 0;
+ consecutiveHighCollisionSteps = 0;
+ currentHeadingStd = SEARCH_HEADING_STD_MIN;
+ inSearchMode = false;
+ // ParticleFilter will be re-initialised on next position fix
+ }
+
+ /** Returns the coordinate transform used by this fusion manager. */
+ public CoordinateTransform getCoordinateTransform() {
+ return coordTransform;
+ }
+
+ /** Returns the underlying particle filter instance. */
+ public ParticleFilter getParticleFilter() {
+ return particleFilter;
+ }
+}
diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/fusion/MapConstraint.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/fusion/MapConstraint.java
new file mode 100644
index 00000000..421215c4
--- /dev/null
+++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/fusion/MapConstraint.java
@@ -0,0 +1,464 @@
+package com.openpositioning.PositionMe.sensors.fusion;
+
+import static com.openpositioning.PositionMe.BuildConstants.DEBUG;
+
+import android.util.Log;
+
+import com.google.android.gms.maps.model.LatLng;
+import com.openpositioning.PositionMe.data.remote.FloorplanApiClient;
+
+import java.util.ArrayList;
+import java.util.HashMap;
+import java.util.List;
+import java.util.Map;
+
+/**
+ * Holds pre-processed wall geometry per floor in ENU coordinates (metres).
+ * Provides line-segment intersection tests so the particle filter can
+ * enforce map constraints: particles that cross walls are penalised and
+ * kept at their previous position.
+ *
+ * Only axis-aligned wall segments (aligned to the building's two
+ * principal orthogonal directions) are kept. This filters out diagonal
+ * polygon edges that typically cross doorways or openings, preventing
+ * false wall detections that would trap particles.
+ */
+public class MapConstraint {
+
+ private static final String TAG = "MapConstraint";
+
+ /** Weight multiplier applied to particles that cross a wall (67 % penalty). */
+ private static final double WALL_PENALTY = 0.33;
+
+ /** Segments shorter than this (metres) are discarded.
+ * 1 m filters wall-thickness edges and doorway-crossing segments (~0.8 m)
+ * while keeping exterior walls and structural walls (typically 1.5 m+). */
+ private static final double MIN_WALL_LENGTH = 1.0;
+
+ /** Angular tolerance (radians) for axis-alignment filtering (~15°). */
+ private static final double AXIS_TOLERANCE_RAD = Math.toRadians(15);
+
+ // ---- inner types --------------------------------------------------------
+
+ /** A wall segment in ENU coordinates (metres), with pre-computed AABB. */
+ public static class WallSegment {
+ public final double x1, y1, x2, y2;
+ public final double minX, maxX, minY, maxY;
+
+ /**
+ * Constructs a wall segment between two ENU points and pre-computes its AABB.
+ *
+ * @param x1 start easting (metres)
+ * @param y1 start northing (metres)
+ * @param x2 end easting (metres)
+ * @param y2 end northing (metres)
+ */
+ public WallSegment(double x1, double y1, double x2, double y2) {
+ this.x1 = x1;
+ this.y1 = y1;
+ this.x2 = x2;
+ this.y2 = y2;
+ this.minX = Math.min(x1, x2);
+ this.maxX = Math.max(x1, x2);
+ this.minY = Math.min(y1, y2);
+ this.maxY = Math.max(y1, y2);
+ }
+ }
+
+ // ---- state --------------------------------------------------------------
+
+ /** Floor index -> list of wall segments in ENU. */
+ private final Map> wallsByFloor = new HashMap<>();
+
+ /** Building outline wall segments — apply to ALL floors (floor-independent). */
+ private final List outlineWalls = new ArrayList<>();
+
+ private boolean initialized = false;
+
+ // ---- initialisation -----------------------------------------------------
+
+ /**
+ * Extracts wall segments from FloorShapes, converts LatLng to ENU,
+ * filters to keep only axis-aligned segments, and stores them by floor.
+ *
+ * @param floorShapesList per-floor shape data from FloorplanApiClient
+ * @param coordTransform the ENU coordinate transform (must be initialised)
+ */
+ public void initialize(List floorShapesList,
+ CoordinateTransform coordTransform) {
+ wallsByFloor.clear();
+ initialized = false;
+
+ if (floorShapesList == null || !coordTransform.isInitialized()) {
+ if (DEBUG) Log.w(TAG, "Cannot initialise: floorShapes="
+ + (floorShapesList != null) + " coordInit="
+ + coordTransform.isInitialized());
+ return;
+ }
+
+ // Step 1: collect ALL raw wall segments across all floors.
+ // Map by REAL floor number (parsed from display name) to match
+ // the particle filter's floor numbering (WiFi API: 0=GF, 1=F1, 2=F2 …).
+ List allRaw = new ArrayList<>();
+ Map> rawByFloor = new HashMap<>();
+ int[] floorNumbers = new int[floorShapesList.size()];
+
+ for (int floorIdx = 0; floorIdx < floorShapesList.size(); floorIdx++) {
+ FloorplanApiClient.FloorShapes floor = floorShapesList.get(floorIdx);
+ int floorNum = parseFloorNumber(floor.getDisplayName());
+ floorNumbers[floorIdx] = floorNum;
+ List rawSegments = new ArrayList<>();
+
+ // Diagnostic: log all indoor types on this floor
+ Map typeCounts = new HashMap<>();
+ for (FloorplanApiClient.MapShapeFeature f : floor.getFeatures()) {
+ String t = f.getIndoorType();
+ typeCounts.put(t, typeCounts.getOrDefault(t, 0) + 1);
+ }
+ if (DEBUG) Log.i(TAG, String.format("Floor %s (key=%d): feature types = %s",
+ floor.getDisplayName(), floorNum, typeCounts));
+
+ for (FloorplanApiClient.MapShapeFeature feature : floor.getFeatures()) {
+ if (!"wall".equals(feature.getIndoorType())) continue;
+
+ String geoType = feature.getGeometryType();
+ for (List part : feature.getParts()) {
+ if (part.size() < 2) continue;
+
+ for (int i = 0; i < part.size() - 1; i++) {
+ LatLng a = part.get(i);
+ LatLng b = part.get(i + 1);
+ double[] enuA = coordTransform.toEastNorth(a.latitude, a.longitude);
+ double[] enuB = coordTransform.toEastNorth(b.latitude, b.longitude);
+ rawSegments.add(new WallSegment(enuA[0], enuA[1], enuB[0], enuB[1]));
+ }
+
+ // For polygon geometry, close the ring
+ if (("Polygon".equals(geoType) || "MultiPolygon".equals(geoType))
+ && part.size() >= 3) {
+ LatLng last = part.get(part.size() - 1);
+ LatLng first = part.get(0);
+ if (Math.abs(last.latitude - first.latitude) > 1e-9
+ || Math.abs(last.longitude - first.longitude) > 1e-9) {
+ double[] enuLast = coordTransform.toEastNorth(
+ last.latitude, last.longitude);
+ double[] enuFirst = coordTransform.toEastNorth(
+ first.latitude, first.longitude);
+ rawSegments.add(new WallSegment(
+ enuLast[0], enuLast[1], enuFirst[0], enuFirst[1]));
+ }
+ }
+ }
+ }
+
+ rawByFloor.put(floorIdx, rawSegments);
+ allRaw.addAll(rawSegments);
+ }
+
+ // Step 2: detect the building's primary axis from all segments
+ double primaryAngle = detectPrimaryAngle(allRaw);
+ if (DEBUG) Log.i(TAG, String.format("Detected building primary axis: %.1f°",
+ Math.toDegrees(primaryAngle)));
+
+ // Step 3: filter each floor — keep only axis-aligned, long-enough segments.
+ // Store by REAL floor number (matching particle filter numbering).
+ for (int floorIdx = 0; floorIdx < floorShapesList.size(); floorIdx++) {
+ FloorplanApiClient.FloorShapes floor = floorShapesList.get(floorIdx);
+ int floorNum = floorNumbers[floorIdx];
+ List raw = rawByFloor.get(floorIdx);
+ if (raw == null) raw = new ArrayList<>();
+
+ List filtered = new ArrayList<>();
+ int shortCount = 0, diagonalCount = 0;
+
+ for (WallSegment seg : raw) {
+ double dx = seg.x2 - seg.x1;
+ double dy = seg.y2 - seg.y1;
+ double len = Math.sqrt(dx * dx + dy * dy);
+
+ if (len < MIN_WALL_LENGTH) {
+ shortCount++;
+ continue;
+ }
+
+ if (!isAlignedToAxis(dx, dy, primaryAngle)) {
+ diagonalCount++;
+ continue;
+ }
+
+ filtered.add(seg);
+ }
+
+ wallsByFloor.put(floorNum, filtered);
+ if (DEBUG) Log.i(TAG, String.format(
+ "Floor key=%d (%s): %d raw → %d kept (filtered %d short, %d diagonal)",
+ floorNum, floor.getDisplayName(), raw.size(), filtered.size(),
+ shortCount, diagonalCount));
+ }
+
+ initialized = !wallsByFloor.isEmpty();
+ int totalSegments = 0;
+ for (List segs : wallsByFloor.values()) totalSegments += segs.size();
+ if (DEBUG) Log.i(TAG, String.format("Initialised: %d floors, %d total wall segments (axis-aligned)",
+ wallsByFloor.size(), totalSegments));
+ }
+
+ /**
+ * Loads the building outline polygon as floor-independent wall constraints.
+ * These prevent particles from leaving the building boundary on any floor.
+ *
+ * @param outlinePolygon building boundary polygon from BuildingInfo
+ * @param coordTransform the ENU coordinate transform (must be initialised)
+ */
+ public void setOutlineConstraint(List outlinePolygon,
+ CoordinateTransform coordTransform) {
+ outlineWalls.clear();
+
+ if (outlinePolygon == null || outlinePolygon.size() < 3
+ || !coordTransform.isInitialized()) {
+ if (DEBUG) Log.w(TAG, "Cannot load outline: polygon="
+ + (outlinePolygon != null ? outlinePolygon.size() : "null")
+ + " coordInit=" + coordTransform.isInitialized());
+ return;
+ }
+
+ for (int i = 0; i < outlinePolygon.size(); i++) {
+ LatLng a = outlinePolygon.get(i);
+ LatLng b = outlinePolygon.get((i + 1) % outlinePolygon.size());
+ double[] enuA = coordTransform.toEastNorth(a.latitude, a.longitude);
+ double[] enuB = coordTransform.toEastNorth(b.latitude, b.longitude);
+ double dx = enuB[0] - enuA[0];
+ double dy = enuB[1] - enuA[1];
+ double len = Math.sqrt(dx * dx + dy * dy);
+ // Keep all outline segments >= 0.5m (outline is coarser, no doorway issue)
+ if (len >= 0.5) {
+ outlineWalls.add(new WallSegment(enuA[0], enuA[1], enuB[0], enuB[1]));
+ }
+ }
+
+ // Outline alone can mark as initialized (even without interior walls)
+ if (!outlineWalls.isEmpty()) {
+ initialized = true;
+ }
+
+ if (DEBUG) Log.i(TAG, String.format("Loaded %d building outline segments (from %d polygon points)",
+ outlineWalls.size(), outlinePolygon.size()));
+ }
+
+ /** Returns {@code true} if wall geometry has been loaded. */
+ public boolean isInitialized() {
+ return initialized;
+ }
+
+ // ---- axis detection & filtering -----------------------------------------
+
+ /**
+ * Detects the building's primary axis direction from all wall segments.
+ * Uses a length-weighted angle histogram over [0°, 180°) with smoothing
+ * to find the dominant wall direction.
+ *
+ * @return primary axis angle in radians [0, π)
+ */
+ private double detectPrimaryAngle(List segments) {
+ // 1° bins over [0, 180)
+ double[] histogram = new double[180];
+
+ for (WallSegment seg : segments) {
+ double dx = seg.x2 - seg.x1;
+ double dy = seg.y2 - seg.y1;
+ double len = Math.sqrt(dx * dx + dy * dy);
+ if (len < 0.01) continue;
+
+ // Angle in degrees, normalized to [0, 180)
+ double angleDeg = Math.toDegrees(Math.atan2(dy, dx));
+ angleDeg = ((angleDeg % 180) + 180) % 180;
+ int bin = (int) angleDeg;
+ if (bin >= 180) bin = 179;
+
+ // Weight by segment length — longer walls contribute more
+ histogram[bin] += len;
+ }
+
+ // Smooth with ±5° circular window
+ double[] smoothed = new double[180];
+ for (int i = 0; i < 180; i++) {
+ for (int d = -5; d <= 5; d++) {
+ smoothed[i] += histogram[((i + d) % 180 + 180) % 180];
+ }
+ }
+
+ // Find peak
+ int peakBin = 0;
+ for (int i = 1; i < 180; i++) {
+ if (smoothed[i] > smoothed[peakBin]) peakBin = i;
+ }
+
+ return Math.toRadians(peakBin);
+ }
+
+ /**
+ * Parses a floor display name into the numeric floor index used by
+ * the WiFi API and particle filter (0 = GF, 1 = F1, 2 = F2, …).
+ * Handles common naming conventions: "LG"→-1, "GF"/"Ground"→0,
+ * "F1"/"1st"→1, "F2"/"2nd"→2, etc.
+ */
+ static int parseFloorNumber(String displayName) {
+ if (displayName == null || displayName.isEmpty()) return 0;
+ String upper = displayName.toUpperCase().trim();
+
+ if (upper.startsWith("LG") || upper.startsWith("LOWER")
+ || upper.startsWith("B") || upper.startsWith("BASEMENT")) {
+ return -1;
+ }
+ if (upper.equals("GF") || upper.startsWith("GROUND")
+ || upper.equals("G") || upper.equals("0")) {
+ return 0;
+ }
+ // "F1", "F2", "F3" etc.
+ if (upper.startsWith("F") && upper.length() >= 2) {
+ try {
+ return Integer.parseInt(upper.substring(1));
+ } catch (NumberFormatException ignored) {}
+ }
+ // "1", "2", "3" or "1st", "2nd", "3rd"
+ try {
+ return Integer.parseInt(upper.replaceAll("[^0-9-]", ""));
+ } catch (NumberFormatException ignored) {}
+
+ return 0;
+ }
+
+ /**
+ * Checks whether a segment direction is aligned to either of the building's
+ * two orthogonal axes (primary or primary+90°) within the configured tolerance.
+ */
+ private boolean isAlignedToAxis(double dx, double dy, double primaryAngle) {
+ double angle = Math.atan2(dy, dx);
+ // Normalize to [0, π)
+ angle = ((angle % Math.PI) + Math.PI) % Math.PI;
+
+ double secondaryAngle = (primaryAngle + Math.PI / 2) % Math.PI;
+
+ return angleDiffMod(angle, primaryAngle) < AXIS_TOLERANCE_RAD
+ || angleDiffMod(angle, secondaryAngle) < AXIS_TOLERANCE_RAD;
+ }
+
+ /**
+ * Computes the minimum angular difference between two angles in [0, π),
+ * accounting for the wrap-around at 0°/180°.
+ */
+ private static double angleDiffMod(double a, double b) {
+ double diff = Math.abs(a - b);
+ if (diff > Math.PI / 2) diff = Math.PI - diff;
+ return diff;
+ }
+
+ // ---- constraint application ---------------------------------------------
+
+ /**
+ * Applies wall constraints to all particles after a predict step.
+ * Particles that cross a wall are reverted to their previous position
+ * and penalised. Uses AABB pre-filtering for performance.
+ *
+ * @return the number of particles that collided with a wall
+ */
+ public int applyConstraints(Particle[] particles,
+ double[] oldX, double[] oldY) {
+ if (!initialized) return 0;
+
+ int collisionCount = 0;
+
+ for (int i = 0; i < particles.length; i++) {
+ Particle p = particles[i];
+
+ // AABB of particle movement for fast pre-filtering
+ double moveMinX = Math.min(oldX[i], p.x);
+ double moveMaxX = Math.max(oldX[i], p.x);
+ double moveMinY = Math.min(oldY[i], p.y);
+ double moveMaxY = Math.max(oldY[i], p.y);
+
+ boolean hitWall = false;
+
+ // Check floor-specific interior walls
+ List walls = wallsByFloor.get(p.floor);
+ if (walls != null) {
+ for (int w = 0, wSize = walls.size(); w < wSize; w++) {
+ WallSegment wall = walls.get(w);
+ if (wall.maxX < moveMinX || wall.minX > moveMaxX
+ || wall.maxY < moveMinY || wall.minY > moveMaxY) {
+ continue;
+ }
+ if (segmentsIntersect(
+ oldX[i], oldY[i], p.x, p.y,
+ wall.x1, wall.y1, wall.x2, wall.y2)) {
+ hitWall = true;
+ break;
+ }
+ }
+ }
+
+ // Check building outline walls (floor-independent, all floors)
+ if (!hitWall && !outlineWalls.isEmpty()) {
+ for (int w = 0, wSize = outlineWalls.size(); w < wSize; w++) {
+ WallSegment wall = outlineWalls.get(w);
+ if (wall.maxX < moveMinX || wall.minX > moveMaxX
+ || wall.maxY < moveMinY || wall.minY > moveMaxY) {
+ continue;
+ }
+ if (segmentsIntersect(
+ oldX[i], oldY[i], p.x, p.y,
+ wall.x1, wall.y1, wall.x2, wall.y2)) {
+ hitWall = true;
+ break;
+ }
+ }
+ }
+
+ if (hitWall) {
+ // Revert to previous position + weight penalty
+ p.x = oldX[i];
+ p.y = oldY[i];
+ p.weight *= WALL_PENALTY;
+ collisionCount++;
+ }
+ }
+
+ return collisionCount;
+ }
+
+ // ---- geometry primitives ------------------------------------------------
+
+ /**
+ * Tests whether two line segments intersect using the parametric approach.
+ */
+ static boolean segmentsIntersect(
+ double px1, double py1, double px2, double py2,
+ double qx1, double qy1, double qx2, double qy2) {
+
+ double dx_p = px2 - px1;
+ double dy_p = py2 - py1;
+ double dx_q = qx2 - qx1;
+ double dy_q = qy2 - qy1;
+
+ double denom = dx_p * dy_q - dy_p * dx_q;
+
+ if (Math.abs(denom) < 1e-12) {
+ return false; // parallel or coincident
+ }
+
+ double dx_pq = qx1 - px1;
+ double dy_pq = qy1 - py1;
+
+ double t = (dx_pq * dy_q - dy_pq * dx_q) / denom;
+ if (t < 0.0 || t > 1.0) return false;
+
+ double u = (dx_pq * dy_p - dy_pq * dx_p) / denom;
+ return u >= 0.0 && u <= 1.0;
+ }
+
+ /** Returns the number of wall segments for a given floor (for logging). */
+ public int getWallCount(int floor) {
+ List walls = wallsByFloor.get(floor);
+ return walls != null ? walls.size() : 0;
+ }
+}
diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/fusion/Particle.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/fusion/Particle.java
new file mode 100644
index 00000000..eb6686b1
--- /dev/null
+++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/fusion/Particle.java
@@ -0,0 +1,41 @@
+package com.openpositioning.PositionMe.sensors.fusion;
+
+/**
+ * Represents a single particle in the particle filter.
+ * Each particle holds a hypothesis of the user's position (easting/northing)
+ * along with floor level and importance weight.
+ */
+public class Particle {
+
+ /** Easting coordinate in meters relative to the ENU origin. */
+ public double x;
+
+ /** Northing coordinate in meters relative to the ENU origin. */
+ public double y;
+
+ /** Floor index (0-based). */
+ public int floor;
+
+ /** Importance weight, normalized across all particles. */
+ public double weight;
+
+ /**
+ * Constructs a particle with the given position, floor, and weight.
+ *
+ * @param x easting in metres (ENU)
+ * @param y northing in metres (ENU)
+ * @param floor floor index (0-based)
+ * @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. */
+ public Particle copy() {
+ return new Particle(x, y, floor, weight);
+ }
+}
diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/fusion/ParticleFilter.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/fusion/ParticleFilter.java
new file mode 100644
index 00000000..869bfff8
--- /dev/null
+++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/fusion/ParticleFilter.java
@@ -0,0 +1,352 @@
+package com.openpositioning.PositionMe.sensors.fusion;
+
+import static com.openpositioning.PositionMe.BuildConstants.DEBUG;
+
+import android.util.Log;
+
+import java.util.Random;
+
+/**
+ * Sequential Importance Resampling (SIR) particle filter for indoor positioning.
+ *
+ * Algorithm cycle (per step):
+ *
+ * - Predict — propagate particles with noisy PDR displacement.
+ * - Update — re-weight particles using WiFi / GNSS observations.
+ * - Estimate — compute weighted mean position & uncertainty.
+ * - Resample — residual resampling when N_eff drops below threshold.
+ *
+ *
+ * The filter works in a local East-North-Up (ENU) frame so that PDR
+ * displacements can be handled in metres.
+ */
+public class ParticleFilter {
+
+ private static final String TAG = "ParticleFilter";
+
+ // ---- tuneable constants ---------------------------------------------------
+
+ /** Standard deviation added to each PDR step length (metres). */
+ private static final double STEP_LENGTH_STD = 0.15;
+
+ /** Standard deviation added to each PDR heading (radians ≈ 8°). */
+ private static final double HEADING_STD = Math.toRadians(8);
+
+ /** Assumed uncertainty of a WiFi position fix (metres). */
+ private static final double WIFI_POSITION_STD = 8.0;
+
+ /** Assumed uncertainty of a GNSS position fix (metres). */
+ private static final double GNSS_POSITION_STD = 15.0;
+
+ /**
+ * Fraction of total particles below which resampling is triggered.
+ * N_eff = 1 / Σ(w²); resample when N_eff < threshold × N.
+ */
+ private static final double NEFF_THRESHOLD = 0.3;
+
+ // ---- state ---------------------------------------------------------------
+
+ private Particle[] particles;
+ private final int numParticles;
+ private final Random random;
+
+ private double estimatedX;
+ private double estimatedY;
+ private int estimatedFloor;
+ private double uncertainty;
+ private boolean initialized = false;
+
+ // ---- constructor ---------------------------------------------------------
+
+ /**
+ * Creates a particle filter with the specified number of particles.
+ *
+ * @param numParticles number of particles to use
+ */
+ public ParticleFilter(int numParticles) {
+ this.numParticles = numParticles;
+ this.particles = new Particle[numParticles];
+ this.random = new Random();
+ }
+
+ // ---- lifecycle -----------------------------------------------------------
+
+ /**
+ * Initialises particles in a Gaussian cloud around an initial position.
+ *
+ * @param x easting (metres, ENU)
+ * @param y northing (metres, ENU)
+ * @param floor initial floor index
+ * @param spread standard deviation of the initial cloud (metres)
+ */
+ public void initialize(double x, double y, int floor, double spread) {
+ double uniformWeight = 1.0 / numParticles;
+ for (int i = 0; i < numParticles; i++) {
+ double px = x + random.nextGaussian() * spread;
+ double py = y + random.nextGaussian() * spread;
+ particles[i] = new Particle(px, py, floor, uniformWeight);
+ }
+ estimatedX = x;
+ estimatedY = y;
+ estimatedFloor = floor;
+ uncertainty = spread;
+ initialized = true;
+ if (DEBUG) Log.i(TAG, String.format("Initialised %d particles at (%.2f, %.2f) floor %d",
+ numParticles, x, y, floor));
+ }
+
+ // ---- prediction ----------------------------------------------------------
+
+ /**
+ * Moves every particle according to PDR displacement with added noise.
+ *
+ * @param stepLength stride length in metres (from Weiberg / settings)
+ * @param heading azimuth in radians, 0 = North, clockwise positive
+ */
+ public void predict(double stepLength, double heading) {
+ predict(stepLength, heading, HEADING_STD);
+ }
+
+ /**
+ * Moves every particle with a caller-specified heading noise.
+ * Used by the wall-collision search mode to temporarily widen the
+ * particle heading spread when most particles are stuck against walls.
+ *
+ * @param headingStd heading noise standard deviation in radians
+ */
+ public void predict(double stepLength, double heading, double headingStd) {
+ if (!initialized) return;
+
+ for (Particle p : particles) {
+ double noisyStep = stepLength + random.nextGaussian() * STEP_LENGTH_STD;
+ double noisyHeading = heading + random.nextGaussian() * headingStd;
+ noisyStep = Math.max(0.05, noisyStep);
+
+ p.x += noisyStep * Math.sin(noisyHeading);
+ p.y += noisyStep * Math.cos(noisyHeading);
+ }
+
+ normalizeWeights();
+ estimatePosition();
+ }
+
+ // ---- observation updates -------------------------------------------------
+
+ /**
+ * Updates particle weights using a position observation (WiFi or GNSS).
+ * The likelihood is an isotropic 2-D Gaussian centred on the observation.
+ */
+ private void updateWithPosition(double obsX, double obsY, double stdDev) {
+ if (!initialized) return;
+
+ double variance2 = 2.0 * stdDev * stdDev;
+ for (Particle p : particles) {
+ double dx = p.x - obsX;
+ double dy = p.y - obsY;
+ double distSq = dx * dx + dy * dy;
+ p.weight *= Math.exp(-distSq / variance2);
+ }
+
+ normalizeWeights();
+ estimatePosition();
+ resampleIfNeeded();
+ }
+
+ /** Feed a WiFi-derived position observation (in ENU metres). */
+ public void updateWifi(double obsX, double obsY) {
+ updateWithPosition(obsX, obsY, WIFI_POSITION_STD);
+ }
+
+ /** Feed a GNSS-derived position observation (in ENU metres). */
+ public void updateGnss(double obsX, double obsY) {
+ updateWithPosition(obsX, obsY, GNSS_POSITION_STD);
+ }
+
+ /** Feed an observation with caller-specified uncertainty (dynamic sigma). */
+ public void updateWithDynamicSigma(double obsX, double obsY, double sigma) {
+ updateWithPosition(obsX, obsY, sigma);
+ }
+
+ /** Feed a user manual correction with custom tight uncertainty. */
+ public void updateWithManualCorrection(double obsX, double obsY, double stdDev) {
+ updateWithPosition(obsX, obsY, stdDev);
+ }
+
+ // ---- floor handling ------------------------------------------------------
+
+ /**
+ * Adjusts particle floors when a floor change is detected (barometer).
+ */
+ public void updateFloor(int newFloor) {
+ if (!initialized) return;
+ for (Particle p : particles) {
+ p.floor = newFloor;
+ }
+
+ normalizeWeights();
+ estimatePosition();
+ }
+
+ // ---- estimation ----------------------------------------------------------
+
+ /** Weighted mean of particle positions → fused estimate + uncertainty. */
+ private void estimatePosition() {
+ double sumX = 0, sumY = 0, totalW = 0;
+ double[] floorWeights = new double[20];
+
+ for (Particle p : particles) {
+ sumX += p.x * p.weight;
+ sumY += p.y * p.weight;
+ totalW += p.weight;
+ int fi = Math.max(0, Math.min(19, p.floor));
+ floorWeights[fi] += p.weight;
+ }
+
+ if (totalW > 0) {
+ estimatedX = sumX / totalW;
+ estimatedY = sumY / totalW;
+ }
+
+ // Floor = highest total weight, but only switch if confident
+ double maxW = 0, secondW = 0;
+ int candidateFloor = estimatedFloor;
+ for (int i = 0; i < floorWeights.length; i++) {
+ if (floorWeights[i] > maxW) {
+ secondW = maxW;
+ maxW = floorWeights[i];
+ candidateFloor = i;
+ } else if (floorWeights[i] > secondW) {
+ secondW = floorWeights[i];
+ }
+ }
+ // Only switch floor if dominant (>0.6 probability or >0.2 margin over second)
+ if (maxW > 0.6 || (maxW - secondW) > 0.2) {
+ estimatedFloor = candidateFloor;
+ }
+
+ // Weighted standard deviation as uncertainty measure
+ double sumSqDist = 0;
+ for (Particle p : particles) {
+ double dx = p.x - estimatedX;
+ double dy = p.y - estimatedY;
+ sumSqDist += (dx * dx + dy * dy) * p.weight;
+ }
+ uncertainty = Math.sqrt(sumSqDist / Math.max(totalW, 1e-10));
+ }
+
+ // ---- resampling ----------------------------------------------------------
+
+ private void resampleIfNeeded() {
+ double neff = calculateNeff();
+ if (neff < NEFF_THRESHOLD * numParticles) {
+ residualResample();
+ }
+ }
+
+ private double calculateNeff() {
+ double sumWsq = 0;
+ for (Particle p : particles) {
+ sumWsq += p.weight * p.weight;
+ }
+ return 1.0 / Math.max(sumWsq, 1e-30);
+ }
+
+ /**
+ * Residual resampling: deterministic copies for high-weight particles,
+ * then systematic resampling over residual weights for the rest.
+ * Preserves more diversity than pure multinomial resampling.
+ */
+ private void residualResample() {
+ Particle[] resampled = new Particle[numParticles];
+ int idx = 0;
+
+ int[] copies = new int[numParticles];
+ double[] residuals = new double[numParticles];
+ int deterministicTotal = 0;
+
+ for (int i = 0; i < numParticles; i++) {
+ copies[i] = (int) Math.floor(numParticles * particles[i].weight);
+ residuals[i] = numParticles * particles[i].weight - copies[i];
+ deterministicTotal += copies[i];
+ }
+
+ // Deterministic part
+ for (int i = 0; i < numParticles; i++) {
+ for (int j = 0; j < copies[i] && idx < numParticles; j++) {
+ resampled[idx++] = particles[i].copy();
+ }
+ }
+
+ // Stochastic part over residuals
+ int remaining = numParticles - idx;
+ if (remaining > 0) {
+ double sumRes = 0;
+ for (double r : residuals) sumRes += r;
+
+ if (sumRes > 0) {
+ double[] cdf = new double[numParticles];
+ cdf[0] = residuals[0] / sumRes;
+ for (int i = 1; i < numParticles; i++) {
+ cdf[i] = cdf[i - 1] + residuals[i] / sumRes;
+ }
+
+ double step = 1.0 / remaining;
+ double u = random.nextDouble() * step;
+ int ci = 0;
+ for (int i = 0; i < remaining && idx < numParticles; i++) {
+ while (ci < numParticles - 1 && u > cdf[ci]) ci++;
+ resampled[idx++] = particles[ci].copy();
+ u += step;
+ }
+ }
+ }
+
+ // Edge-case fill
+ while (idx < numParticles) {
+ resampled[idx++] = particles[random.nextInt(numParticles)].copy();
+ }
+
+ // Reset all weights to uniform
+ double w = 1.0 / numParticles;
+ for (Particle p : resampled) {
+ p.weight = w;
+ }
+ particles = resampled;
+ }
+
+ // ---- helpers -------------------------------------------------------------
+
+ private void normalizeWeights() {
+ double total = 0;
+ for (Particle p : particles) total += p.weight;
+ if (total > 1e-30) {
+ for (Particle p : particles) p.weight /= total;
+ } else {
+ double w = 1.0 / numParticles;
+ for (Particle p : particles) p.weight = w;
+ }
+ }
+
+ // ---- accessors -----------------------------------------------------------
+
+ /** Returns the weighted-mean easting estimate in metres (ENU). */
+ public double getEstimatedX() { return estimatedX; }
+
+ /** Returns the weighted-mean northing estimate in metres (ENU). */
+ public double getEstimatedY() { return estimatedY; }
+
+ /** Returns the estimated floor index. */
+ public int getEstimatedFloor() { return estimatedFloor; }
+
+ /** Returns the current position uncertainty (weighted std dev) in metres. */
+ public double getUncertainty() { return uncertainty; }
+
+ /** Returns {@code true} if the filter has been initialised with particles. */
+ public boolean isInitialized() { return initialized; }
+
+ /** Returns the internal particle array. */
+ public Particle[] getParticles() { return particles; }
+
+ /** Returns the total number of particles in the filter. */
+ public int getNumParticles() { return numParticles; }
+}
diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/BarometerLogger.java b/app/src/main/java/com/openpositioning/PositionMe/utils/BarometerLogger.java
new file mode 100644
index 00000000..40e2bfeb
--- /dev/null
+++ b/app/src/main/java/com/openpositioning/PositionMe/utils/BarometerLogger.java
@@ -0,0 +1,181 @@
+package com.openpositioning.PositionMe.utils;
+
+import static com.openpositioning.PositionMe.BuildConstants.DEBUG;
+
+import android.content.Context;
+import android.hardware.Sensor;
+import android.hardware.SensorEvent;
+import android.hardware.SensorEventListener;
+import android.hardware.SensorManager;
+import android.os.Handler;
+import android.os.Looper;
+import android.util.Log;
+
+import java.io.File;
+import java.io.FileWriter;
+import java.io.IOException;
+import java.text.SimpleDateFormat;
+import java.util.Date;
+import java.util.Locale;
+
+/**
+ * Standalone barometer data logger for floor calibration.
+ *
+ * Records raw pressure (hPa), calculated altitude (m), and a smoothed
+ * altitude at 1 Hz to a CSV file. The user walks between floors while
+ * this logger runs, then the resulting CSV is analysed to determine
+ * per-floor pressure/altitude ranges.
+ *
+ * Usage:
+ * BarometerLogger logger = new BarometerLogger(context);
+ * logger.start(); // begin recording
+ * // ... user walks between floors ...
+ * logger.stop(); // stop and flush file
+ * String path = logger.getFilePath(); // CSV location
+ */
+public class BarometerLogger implements SensorEventListener {
+
+ private static final String TAG = "BarometerLogger";
+ private static final float ALPHA = 0.15f; // low-pass filter coefficient
+
+ private final SensorManager sensorManager;
+ private final Sensor pressureSensor;
+ private FileWriter writer;
+ private File outputFile;
+
+ private float smoothedPressure = 0;
+ private float startAltitude = Float.NaN;
+ private long startTimeMs;
+ private int sampleCount = 0;
+
+ // Periodic status log
+ private final Handler handler = new Handler(Looper.getMainLooper());
+ private final Runnable statusLog = new Runnable() {
+ @Override
+ public void run() {
+ if (writer != null) {
+ float alt = pressureToAltitude(smoothedPressure);
+ float rel = Float.isNaN(startAltitude) ? 0 : alt - startAltitude;
+ if (DEBUG) Log.i(TAG, String.format(
+ "[LIVE] pressure=%.2f hPa altitude=%.2f m relAlt=%.2f m samples=%d",
+ smoothedPressure, alt, rel, sampleCount));
+ handler.postDelayed(this, 5000);
+ }
+ }
+ };
+
+ public BarometerLogger(Context context) {
+ sensorManager = (SensorManager) context.getSystemService(Context.SENSOR_SERVICE);
+ pressureSensor = sensorManager.getDefaultSensor(Sensor.TYPE_PRESSURE);
+
+ // Output file in app's external storage
+ String timestamp = new SimpleDateFormat("yyyyMMdd_HHmmss", Locale.US).format(new Date());
+ File dir = context.getExternalFilesDir(null);
+ outputFile = new File(dir, "baro_calibration_" + timestamp + ".csv");
+ }
+
+ /** Start recording barometer data to CSV. */
+ public void start() {
+ if (pressureSensor == null) {
+ Log.e(TAG, "No pressure sensor available on this device!");
+ return;
+ }
+
+ try {
+ writer = new FileWriter(outputFile, false);
+ writer.write("elapsed_s,timestamp,raw_pressure_hPa,smoothed_pressure_hPa,altitude_m,relative_altitude_m\n");
+ writer.flush();
+ } catch (IOException e) {
+ Log.e(TAG, "Failed to create output file", e);
+ return;
+ }
+
+ startTimeMs = System.currentTimeMillis();
+ smoothedPressure = 0;
+ startAltitude = Float.NaN;
+ sampleCount = 0;
+
+ // Register at ~1Hz (1,000,000 μs)
+ sensorManager.registerListener(this, pressureSensor, 1_000_000);
+
+ handler.postDelayed(statusLog, 5000);
+
+ if (DEBUG) Log.i(TAG, "=== Barometer logging STARTED → " + outputFile.getAbsolutePath());
+ }
+
+ /** Stop recording and flush the CSV file. */
+ public void stop() {
+ sensorManager.unregisterListener(this);
+ handler.removeCallbacks(statusLog);
+
+ if (writer != null) {
+ try {
+ writer.flush();
+ writer.close();
+ } catch (IOException e) {
+ Log.e(TAG, "Error closing file", e);
+ }
+ writer = null;
+ }
+
+ if (DEBUG) Log.i(TAG, String.format("=== Barometer logging STOPPED. %d samples → %s",
+ sampleCount, outputFile.getAbsolutePath()));
+ }
+
+ /** Returns the absolute path of the output CSV file. */
+ public String getFilePath() {
+ return outputFile.getAbsolutePath();
+ }
+
+ @Override
+ public void onSensorChanged(SensorEvent event) {
+ if (event.sensor.getType() != Sensor.TYPE_PRESSURE) return;
+
+ float rawPressure = event.values[0];
+
+ // Low-pass filter
+ if (smoothedPressure == 0) {
+ smoothedPressure = rawPressure;
+ } else {
+ smoothedPressure = (1 - ALPHA) * smoothedPressure + ALPHA * rawPressure;
+ }
+
+ float altitude = pressureToAltitude(smoothedPressure);
+
+ // Set start altitude from first reading
+ if (Float.isNaN(startAltitude)) {
+ startAltitude = altitude;
+ }
+
+ float relAltitude = altitude - startAltitude;
+ float elapsedS = (System.currentTimeMillis() - startTimeMs) / 1000f;
+ sampleCount++;
+
+ // Write to CSV
+ if (writer != null) {
+ try {
+ writer.write(String.format(Locale.US, "%.1f,%d,%.3f,%.3f,%.3f,%.3f\n",
+ elapsedS,
+ System.currentTimeMillis(),
+ rawPressure,
+ smoothedPressure,
+ altitude,
+ relAltitude));
+ // Flush every 10 samples to avoid data loss
+ if (sampleCount % 10 == 0) {
+ writer.flush();
+ }
+ } catch (IOException e) {
+ Log.e(TAG, "Write error", e);
+ }
+ }
+ }
+
+ @Override
+ public void onAccuracyChanged(Sensor sensor, int accuracy) {}
+
+ /** Convert pressure (hPa) to altitude (m) using standard atmosphere. */
+ private static float pressureToAltitude(float pressure) {
+ return SensorManager.getAltitude(SensorManager.PRESSURE_STANDARD_ATMOSPHERE, pressure);
+ }
+}
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..d95707ac 100644
--- a/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java
+++ b/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java
@@ -1,5 +1,7 @@
package com.openpositioning.PositionMe.utils;
+import static com.openpositioning.PositionMe.BuildConstants.DEBUG;
+
import android.graphics.Color;
import android.util.Log;
@@ -49,11 +51,18 @@ public class IndoorMapManager {
// Per-floor vector shape data for the current building
private List currentFloorShapes;
- // Average floor heights per building (meters), used for barometric auto-floor
+ // Physical floor heights per building (meters), used for display
public static final float NUCLEUS_FLOOR_HEIGHT = 4.2F;
public static final float LIBRARY_FLOOR_HEIGHT = 3.6F;
public static final float MURCHISON_FLOOR_HEIGHT = 4.0F;
+ // Barometric floor heights per building (meters) — the effective altitude
+ // change the barometer sees per floor. Close to the physical height but
+ // slightly lower to add a small sensitivity margin.
+ private static final float NUCLEUS_BARO_FLOOR_HEIGHT = 5.0F;
+ private static final float LIBRARY_BARO_FLOOR_HEIGHT = 3.5F;
+ private static final float MURCHISON_BARO_FLOOR_HEIGHT = 4.0F;
+
// Colours for different indoor feature types
private static final int WALL_STROKE = Color.argb(200, 80, 80, 80);
private static final int ROOM_STROKE = Color.argb(180, 33, 150, 243);
@@ -98,6 +107,65 @@ public boolean getIsIndoorMapSet() {
return isIndoorMapSet;
}
+ /**
+ * Checks if the given position is inside a stairs or lift polygon on the current floor.
+ *
+ * @param position the LatLng to test
+ * @return true if inside a stairs/lift area
+ */
+ public boolean isNearStairs(LatLng position) {
+ if (!isIndoorMapSet || currentFloorShapes == null || position == null) return false;
+ if (currentFloor < 0 || currentFloor >= currentFloorShapes.size()) return false;
+
+ FloorplanApiClient.FloorShapes floor = currentFloorShapes.get(currentFloor);
+ for (FloorplanApiClient.MapShapeFeature feature : floor.getFeatures()) {
+ String type = feature.getIndoorType();
+ if ("stairs".equals(type)) {
+ for (List ring : feature.getParts()) {
+ if (ring.size() >= 3 && BuildingPolygon.pointInPolygon(position, ring)) {
+ return true;
+ }
+ }
+ }
+ }
+ return false;
+ }
+
+ /**
+ * Checks if the given position is within a specified distance of any
+ * stairs or lift polygon on the current floor.
+ *
+ * @param position the LatLng to test
+ * @param radiusM proximity radius in metres
+ * @return true if within radius of a stairs or lift area
+ */
+ public boolean isNearStairsOrLift(LatLng position, double radiusM) {
+ if (!isIndoorMapSet || currentFloorShapes == null || position == null) return false;
+ if (currentFloor < 0 || currentFloor >= currentFloorShapes.size()) return false;
+
+ FloorplanApiClient.FloorShapes floor = currentFloorShapes.get(currentFloor);
+ for (FloorplanApiClient.MapShapeFeature feature : floor.getFeatures()) {
+ String type = feature.getIndoorType();
+ if ("stairs".equals(type) || "lift".equals(type)) {
+ for (List ring : feature.getParts()) {
+ if (ring.size() < 3) continue;
+ // Check if inside polygon
+ if (BuildingPolygon.pointInPolygon(position, ring)) return true;
+ // Check proximity: distance to centroid of polygon
+ double cLat = 0, cLng = 0;
+ for (LatLng p : ring) { cLat += p.latitude; cLng += p.longitude; }
+ cLat /= ring.size();
+ cLng /= ring.size();
+ double dLat = (position.latitude - cLat) * 111132.92;
+ double dLng = (position.longitude - cLng) * 111132.92
+ * Math.cos(Math.toRadians(cLat));
+ if (Math.hypot(dLat, dLng) <= radiusM) return true;
+ }
+ }
+ }
+ return false;
+ }
+
/**
* Returns the identifier of the building the user is currently in.
*
@@ -108,6 +176,20 @@ public int getCurrentBuilding() {
return currentBuilding;
}
+ /**
+ * Returns a human-readable name for the current building.
+ *
+ * @return building name string, or "Outdoor" if not inside a known building
+ */
+ public String getCurrentBuildingName() {
+ switch (currentBuilding) {
+ case BUILDING_NUCLEUS: return "Nucleus";
+ case BUILDING_LIBRARY: return "Library";
+ case BUILDING_MURCHISON: return "Murchison House";
+ default: return "Outdoor";
+ }
+ }
+
/**
* Returns the current floor index being displayed.
*
@@ -189,13 +271,73 @@ public void decreaseFloor() {
}
/**
- * Sets the map overlay for the building if the user's current location is
- * inside a building and the overlay is not already set. Removes the overlay
- * if the user leaves all buildings.
+ * Forces indoor mode for a known building, bypassing pointInPolygon detection.
+ * Used by Indoor Positioning mode where we already know the user is in a building.
*
- * Detection priority: floorplan API real polygon outlines first,
- * then legacy hard-coded rectangular boundaries as fallback.
+ * @param apiName building API name (e.g. "nucleus_building")
+ * @return true if the building was set up successfully
*/
+ public boolean forceSetBuilding(String apiName) {
+ if (isIndoorMapSet) return true;
+
+ int buildingType = resolveBuildingType(apiName);
+ if (buildingType == BUILDING_NONE) {
+ if (DEBUG) Log.w(TAG, "[forceSetBuilding] Unknown building: " + apiName);
+ return false;
+ }
+
+ currentBuilding = buildingType;
+ int floorBias;
+ float baroFloorH;
+ switch (buildingType) {
+ case BUILDING_NUCLEUS:
+ floorBias = 1;
+ floorHeight = NUCLEUS_FLOOR_HEIGHT;
+ baroFloorH = NUCLEUS_BARO_FLOOR_HEIGHT;
+ break;
+ case BUILDING_LIBRARY:
+ floorBias = 0;
+ floorHeight = LIBRARY_FLOOR_HEIGHT;
+ baroFloorH = LIBRARY_BARO_FLOOR_HEIGHT;
+ break;
+ case BUILDING_MURCHISON:
+ floorBias = 1;
+ floorHeight = MURCHISON_FLOOR_HEIGHT;
+ baroFloorH = MURCHISON_BARO_FLOOR_HEIGHT;
+ break;
+ default:
+ return false;
+ }
+
+ SensorFusion.getInstance().setBaroFloorHeight(baroFloorH);
+ if (DEBUG) Log.i(TAG, "[forceSetBuilding] Building=" + apiName
+ + " baroFloorHeight=" + baroFloorH + "m");
+
+ int wifiFloor = SensorFusion.getInstance().getWifiFloor();
+ if (wifiFloor >= 0) {
+ SensorFusion.getInstance().setInitialFloor(wifiFloor);
+ currentFloor = wifiFloor + floorBias;
+ } else {
+ currentFloor = floorBias;
+ }
+
+ FloorplanApiClient.BuildingInfo building =
+ SensorFusion.getInstance().getFloorplanBuilding(apiName);
+ if (building != null) {
+ currentFloorShapes = building.getFloorShapesList();
+ }
+
+ if (currentFloorShapes != null && !currentFloorShapes.isEmpty()) {
+ drawFloorShapes(currentFloor);
+ isIndoorMapSet = true;
+ if (DEBUG) Log.i(TAG, "[forceSetBuilding] Success, floor=" + currentFloor);
+ return true;
+ }
+
+ if (DEBUG) Log.w(TAG, "[forceSetBuilding] No floor shapes for " + apiName);
+ return false;
+ }
+
private void setBuildingOverlay() {
try {
int detected = detectCurrentBuilding();
@@ -205,26 +347,54 @@ private void setBuildingOverlay() {
currentBuilding = detected;
String apiName;
+ // Floor bias: index offset so that floor 0 in the list maps to the correct physical floor
+ int floorBias;
+ float baroFloorH;
switch (detected) {
case BUILDING_NUCLEUS:
apiName = "nucleus_building";
- currentFloor = 1;
+ floorBias = 1; // LG=0, G=1, ...
floorHeight = NUCLEUS_FLOOR_HEIGHT;
+ baroFloorH = NUCLEUS_BARO_FLOOR_HEIGHT;
break;
case BUILDING_LIBRARY:
apiName = "library";
- currentFloor = 0;
+ floorBias = 0;
floorHeight = LIBRARY_FLOOR_HEIGHT;
+ baroFloorH = LIBRARY_BARO_FLOOR_HEIGHT;
break;
case BUILDING_MURCHISON:
apiName = "murchison_house";
- currentFloor = 1;
+ floorBias = 1;
floorHeight = MURCHISON_FLOOR_HEIGHT;
+ baroFloorH = MURCHISON_BARO_FLOOR_HEIGHT;
break;
default:
return;
}
+ // Tell PdrProcessing to use building-specific barometric floor height
+ SensorFusion.getInstance().setBaroFloorHeight(baroFloorH);
+ if (DEBUG) Log.i(TAG, "[BaroFloorHeight] Building=" + apiName
+ + " baroFloorHeight=" + baroFloorH + "m");
+
+ // Seed initial floor from WiFi positioning API.
+ // WiFi floor is a logical floor (0=GF, 1=F1, 2=F2, …).
+ // floorBias converts to the building's array index.
+ int wifiFloor = SensorFusion.getInstance().getWifiFloor();
+ if (wifiFloor >= 0) {
+ SensorFusion.getInstance().setInitialFloor(wifiFloor);
+ currentFloor = wifiFloor + floorBias;
+ if (DEBUG) Log.i(TAG, String.format(
+ "[WifiFloorInit] wifiFloor=%d → initialFloor=%d (idx=%d, bias=%d)",
+ wifiFloor, wifiFloor, currentFloor, floorBias));
+ } else {
+ currentFloor = floorBias; // GF default
+ if (DEBUG) Log.w(TAG, String.format(
+ "[FloorInit] WiFi floor unavailable (%d) → fallback GF idx=%d",
+ wifiFloor, currentFloor));
+ }
+
// Load floor shapes from cached API data
FloorplanApiClient.BuildingInfo building =
SensorFusion.getInstance().getFloorplanBuilding(apiName);
@@ -233,6 +403,16 @@ private void setBuildingOverlay() {
}
if (currentFloorShapes != null && !currentFloorShapes.isEmpty()) {
+ if (DEBUG) {
+ StringBuilder floorMap = new StringBuilder("[FloorMap] ");
+ for (int i = 0; i < currentFloorShapes.size(); i++) {
+ floorMap.append(String.format("idx%d=%s ", i,
+ currentFloorShapes.get(i).getDisplayName()));
+ }
+ floorMap.append("| current=").append(currentFloor);
+ Log.i(TAG, floorMap.toString());
+ }
+
drawFloorShapes(currentFloor);
isIndoorMapSet = true;
}
@@ -262,6 +442,15 @@ private void drawFloorShapes(int floorIndex) {
|| floorIndex >= currentFloorShapes.size()) return;
FloorplanApiClient.FloorShapes floor = currentFloorShapes.get(floorIndex);
+ if (DEBUG) {
+ java.util.Set seenTypes = new java.util.LinkedHashSet<>();
+ for (FloorplanApiClient.MapShapeFeature f : floor.getFeatures()) {
+ seenTypes.add(f.getIndoorType() + "(" + f.getGeometryType() + ")");
+ }
+ Log.i(TAG, "[FloorShapeTypes] floor=" + floor.getDisplayName()
+ + " types=" + seenTypes);
+ }
+
for (FloorplanApiClient.MapShapeFeature feature : floor.getFeatures()) {
String geoType = feature.getGeometryType();
String indoorType = feature.getIndoorType();
@@ -334,19 +523,30 @@ private int detectCurrentBuilding() {
// Phase 1: API real polygon outlines
List apiBuildings =
SensorFusion.getInstance().getFloorplanBuildings();
+ if (DEBUG) Log.d(TAG, "[detectBuilding] loc=" + currentLocation
+ + " apiBuildings=" + apiBuildings.size());
for (FloorplanApiClient.BuildingInfo building : apiBuildings) {
List outline = building.getOutlinePolygon();
- if (outline != null && outline.size() >= 3
- && BuildingPolygon.pointInPolygon(currentLocation, outline)) {
+ boolean inPoly = outline != null && outline.size() >= 3
+ && BuildingPolygon.pointInPolygon(currentLocation, outline);
+ if (DEBUG) Log.d(TAG, "[detectBuilding] API name=" + building.getName()
+ + " outlineSize=" + (outline != null ? outline.size() : 0)
+ + " inPolygon=" + inPoly);
+ if (inPoly) {
int type = resolveBuildingType(building.getName());
if (type != BUILDING_NONE) return type;
}
}
// Phase 2: legacy hard-coded fallback
- if (BuildingPolygon.inNucleus(currentLocation)) return BUILDING_NUCLEUS;
- if (BuildingPolygon.inLibrary(currentLocation)) return BUILDING_LIBRARY;
- if (BuildingPolygon.inMurchison(currentLocation)) return BUILDING_MURCHISON;
+ boolean inNuc = BuildingPolygon.inNucleus(currentLocation);
+ boolean inLib = BuildingPolygon.inLibrary(currentLocation);
+ boolean inMur = BuildingPolygon.inMurchison(currentLocation);
+ if (DEBUG) Log.d(TAG, "[detectBuilding] legacy: nucleus=" + inNuc
+ + " library=" + inLib + " murchison=" + inMur);
+ if (inNuc) return BUILDING_NUCLEUS;
+ if (inLib) return BUILDING_LIBRARY;
+ if (inMur) return BUILDING_MURCHISON;
return BUILDING_NONE;
}
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..b92b5c19 100644
--- a/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java
+++ b/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java
@@ -1,8 +1,11 @@
package com.openpositioning.PositionMe.utils;
+import static com.openpositioning.PositionMe.BuildConstants.DEBUG;
+
import android.content.Context;
import android.content.SharedPreferences;
import android.hardware.SensorManager;
+import android.util.Log;
import androidx.preference.PreferenceManager;
@@ -29,8 +32,10 @@ public class PdrProcessing {
//region Static variables
// Weiberg algorithm coefficient for stride calculations
private static final float K = 0.364f;
+ /** Fixed step length override (metres). Set to 0 to use Weiberg estimation. */
+ private static final float FIXED_STEP_LENGTH = 0.8f;
// Number of samples (seconds) to keep as memory for elevation calculation
- private static final int elevationSeconds = 4;
+ private static final int elevationSeconds = 6;
// Number of samples (0.01 seconds)
private static final int accelSamples = 100;
// Threshold used to detect significant movement
@@ -38,6 +43,13 @@ 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;
+ /** Hysteresis for floor detection (fraction of floorHeight).
+ * Walking/stairs: higher threshold resists baro drift from movement.
+ * Elevator (stationary): lower threshold, baro signal is clean. */
+ private static final float WALK_HYSTERESIS = 0.7f;
+ private static final float ELEVATOR_HYSTERESIS = 0.5f;
+ /** If no step for this many ms, assume elevator (stationary). */
+ private static final long ELEVATOR_IDLE_MS = 3000;
//endregion
//region Instance variables
@@ -58,8 +70,10 @@ public class PdrProcessing {
private float startElevation;
private int setupIndex = 0;
private float elevation;
- private int floorHeight;
+ private float floorHeight;
private int currentFloor;
+ private float lastSmoothedAlt; // latest smoothed altitude for baseline resets
+ private long lastStepTimeMs; // timestamp of last detected step (for elevator detection)
// Buffer of most recent elevations calculated
private CircularFloatBuffer elevationList;
@@ -68,6 +82,9 @@ public class PdrProcessing {
private CircularFloatBuffer verticalAccel;
private CircularFloatBuffer horizontalAccel;
+ // Initial floor offset (set externally when starting floor is known)
+ private int initialFloorOffset = 0;
+
// Step sum and length aggregation variables
private float sumStepLength = 0;
private int stepCount = 0;
@@ -155,16 +172,16 @@ public float[] updatePdr(long currentStepEnd, List accelMagnitudeOvertim
}
// Calculate step length
- if(!useManualStep) {
- //ArrayList accelMagnitudeFiltered = filter(accelMagnitudeOvertime);
- // Estimate stride
+ if (FIXED_STEP_LENGTH > 0) {
+ this.stepLength = FIXED_STEP_LENGTH;
+ } else if(!useManualStep) {
this.stepLength = weibergMinMax(accelMagnitudeOvertime);
- // System.err.println("Step Length" + stepLength);
}
// Increment aggregate variables
sumStepLength += stepLength;
stepCount++;
+ lastStepTimeMs = System.currentTimeMillis();
// Translate to cartesian coordinate system
float x = (float) (stepLength * Math.cos(adaptedHeading));
@@ -205,18 +222,44 @@ public float updateElevation(float absoluteElevation) {
// Add to buffer
this.elevationList.putNewest(absoluteElevation);
- // Check if there was floor movement
- // Check if there is enough data to evaluate
+ // Floor detection with hysteresis:
+ // Once on a floor, require 0.7×floorHeight deviation to trigger
+ // a change (instead of 0.5× from Math.round). This widens the
+ // drift tolerance from 1.8m to 2.6m per floor.
if(this.elevationList.isFull()) {
- // Check average of elevation array
List elevationMemory = this.elevationList.getListCopy();
OptionalDouble currentAvg = elevationMemory.stream().mapToDouble(f -> f).average();
- float finishAvg = currentAvg.isPresent() ? (float) currentAvg.getAsDouble() : 0;
-
- // Check if we moved floor by comparing with start position
- if(Math.abs(finishAvg - startElevation) > this.floorHeight) {
- // Change floors - 'floor' division
- this.currentFloor += (finishAvg - startElevation)/this.floorHeight;
+ float smoothedAlt = currentAvg.isPresent() ? (float) currentAvg.getAsDouble() : startElevation;
+ this.lastSmoothedAlt = smoothedAlt;
+ float relHeight = smoothedAlt - startElevation;
+ float fracFloors = relHeight / this.floorHeight;
+ int currentDelta = this.currentFloor - this.initialFloorOffset;
+ // Elevator vs walk detection: no step for 3s = elevator (clean baro)
+ boolean isElevator = (System.currentTimeMillis() - lastStepTimeMs) > ELEVATOR_IDLE_MS;
+ float hyst = isElevator ? ELEVATOR_HYSTERESIS : WALK_HYSTERESIS;
+ // Hysteresis: need ±hyst floor beyond current floor center
+ int deltaFloors;
+ if (fracFloors > currentDelta + hyst) {
+ deltaFloors = currentDelta + 1;
+ } else if (fracFloors < currentDelta - hyst) {
+ deltaFloors = currentDelta - 1;
+ } else {
+ deltaFloors = currentDelta; // stay on current floor
+ }
+ int targetFloor = initialFloorOffset + deltaFloors;
+ // Clamp to ±1 floor per update as additional safety net
+ int prevFloor = this.currentFloor;
+ if (targetFloor > prevFloor + 1) {
+ targetFloor = prevFloor + 1;
+ } else if (targetFloor < prevFloor - 1) {
+ targetFloor = prevFloor - 1;
+ }
+ this.currentFloor = targetFloor;
+ if (this.currentFloor != prevFloor) {
+ if (DEBUG) Log.i("PDR", String.format(
+ "[BaroCalc] relHeight=%.2fm frac=%.2f hyst=%.1f(%s) offset=%d => floor %d→%d",
+ relHeight, fracFloors, hyst, isElevator ? "ELEV" : "WALK",
+ initialFloorOffset, prevFloor, this.currentFloor));
}
}
// Return current elevation
@@ -398,19 +441,88 @@ public void resetPDR() {
}
/**
- * Getter for the average step length calculated from the aggregated distance and step count.
+ * Returns the most recently computed step length (metres).
+ * Used by the fusion module to feed the particle filter prediction step.
+ */
+ public float getLastStepLength() {
+ return this.stepLength;
+ }
+
+ /**
+ * Sets the initial floor offset. Call before recording starts when the
+ * starting floor is known (e.g. from WiFi, calibration DB, or user selection).
+ */
+ public void setInitialFloor(int floor) {
+ this.initialFloorOffset = floor;
+ this.currentFloor = floor;
+ }
+
+ /**
+ * Reseeds the floor to a known value (e.g. from WiFi API) during recording.
+ * Unlike {@link #setInitialFloor} + {@link #resetBaroBaseline}, this method
+ * uses the CURRENT smoothed baro altitude as the new baseline, avoiding the
+ * ordering bug where resetBaroBaseline sees floorsChanged=0.
*
- * @return average step length in meters.
+ * @param floor the logical floor number to reseed to (0=GF, 1=F1, …)
+ */
+ public void reseedFloor(int floor) {
+ float oldStart = this.startElevation;
+ int oldOffset = this.initialFloorOffset;
+ // Use the latest smoothed baro altitude as new baseline so relHeight ≈ 0
+ float baseAlt = this.lastSmoothedAlt > 0 ? this.lastSmoothedAlt : this.startElevation;
+ this.startElevation = baseAlt;
+ this.initialFloorOffset = floor;
+ this.currentFloor = floor;
+ if (DEBUG) Log.i("PDR", String.format(
+ "[FloorReseed] floor=%d startElev %.2f→%.2f offset %d→%d",
+ floor, oldStart, this.startElevation, oldOffset, floor));
+ }
+
+ /**
+ * Overrides the barometric floor height used for floor detection.
+ * Call when the building is identified, since HVAC systems make the
+ * actual barometric altitude difference per floor much smaller than
+ * the physical floor-to-floor height.
+ *
+ * @param height barometric floor height in metres (e.g. 2.0)
+ */
+ public void setBaroFloorHeight(float height) {
+ if (height > 0) {
+ this.floorHeight = height;
+ }
+ }
+
+ /**
+ * Resets the barometric baseline after a confirmed floor change.
+ * Uses THEORETICAL altitude (floors × floorHeight) instead of the
+ * measured smoothed altitude, because the 6-second average lags behind
+ * during transitions and would cause incorrect relHeight after reset.
+ *
+ * @param confirmedFloor the confirmed floor number after transition
+ */
+ public void resetBaroBaseline(int confirmedFloor) {
+ float oldStart = this.startElevation;
+ int oldOffset = this.initialFloorOffset;
+ // Shift baseline by the number of confirmed floors × floorHeight
+ int floorsChanged = confirmedFloor - oldOffset;
+ this.startElevation += floorsChanged * this.floorHeight;
+ this.initialFloorOffset = confirmedFloor;
+ this.currentFloor = confirmedFloor;
+ if (DEBUG) Log.i("PDR", String.format(
+ "[BaroReset] baseline %.2f→%.2f (%.1fm × %d floors) offset %d→%d",
+ oldStart, this.startElevation, this.floorHeight, floorsChanged,
+ oldOffset, confirmedFloor));
+ }
+
+ /**
+ * Returns the average step length and resets the accumulator.
+ *
+ * @return average step length in metres
*/
public float getAverageStepLength(){
- //Calculate average step length
float averageStepLength = sumStepLength/(float) stepCount;
-
- //Reset sum and number of steps
stepCount = 0;
sumStepLength = 0;
-
- //Return average step length
return averageStepLength;
}
diff --git a/app/src/main/res/drawable/ic_chevron_left.xml b/app/src/main/res/drawable/ic_chevron_left.xml
new file mode 100644
index 00000000..384133b2
--- /dev/null
+++ b/app/src/main/res/drawable/ic_chevron_left.xml
@@ -0,0 +1,9 @@
+
+
+
diff --git a/app/src/main/res/drawable/ic_chevron_right.xml b/app/src/main/res/drawable/ic_chevron_right.xml
new file mode 100644
index 00000000..4e77217f
--- /dev/null
+++ b/app/src/main/res/drawable/ic_chevron_right.xml
@@ -0,0 +1,9 @@
+
+
+
diff --git a/app/src/main/res/drawable/toggle_tab_bg.xml b/app/src/main/res/drawable/toggle_tab_bg.xml
new file mode 100644
index 00000000..d67eace3
--- /dev/null
+++ b/app/src/main/res/drawable/toggle_tab_bg.xml
@@ -0,0 +1,10 @@
+
+
+
+
+
diff --git a/app/src/main/res/layout-small/fragment_home.xml b/app/src/main/res/layout-small/fragment_home.xml
index bd713b67..9f6a2307 100644
--- a/app/src/main/res/layout-small/fragment_home.xml
+++ b/app/src/main/res/layout-small/fragment_home.xml
@@ -155,16 +155,16 @@
android:textColor="@color/md_theme_onSecondary" />
-
+
+
+
+
-
+
+ android:layout_height="match_parent"
+ android:paddingTop="20dp">
-
+
+ android:layout_marginEnd="4dp" />
@@ -53,10 +55,10 @@
android:layout_height="wrap_content"
android:text="@string/elevation"
android:textColor="@color/md_theme_secondary"
- android:textAppearance="@style/TextAppearance.Material3.BodyLarge"
+ android:textAppearance="@style/TextAppearance.Material3.LabelSmall"
android:textStyle="bold"
- android:layout_marginStart="16dp"
- android:layout_marginEnd="16dp" />
+ android:layout_marginStart="8dp"
+ android:layout_marginEnd="8dp" />
@@ -66,12 +68,34 @@
android:layout_height="wrap_content"
android:text="@string/gnss_error"
android:textColor="@color/md_theme_error"
- android:textAppearance="@style/TextAppearance.Material3.BodyMedium"
- android:layout_marginStart="16dp" />
+ android:textAppearance="@style/TextAppearance.Material3.LabelSmall"
+ android:layout_marginStart="4dp" />
+
+
+
+
+
+
+
+
+
-
+
-
+
+
+
+
+
+
+
+ app:layout_constraintStart_toStartOf="parent" />
+
+
+
+
+
+
-
-
+
-
-
+ android:textSize="13sp"
+ android:textAlignment="center"
+ android:textColor="@android:color/white"
+ app:backgroundTint="#FFFF6347"
+ app:cornerRadius="12dp" />
+
+
+
+
+
+ android:textSize="13sp"
+ android:textAlignment="center"
+ android:textColor="@android:color/white"
+ app:backgroundTint="#FF3EB489"
+ app:cornerRadius="12dp" />
diff --git a/app/src/main/res/layout/fragment_trajectory_map.xml b/app/src/main/res/layout/fragment_trajectory_map.xml
index a72425bf..c324907d 100644
--- a/app/src/main/res/layout/fragment_trajectory_map.xml
+++ b/app/src/main/res/layout/fragment_trajectory_map.xml
@@ -12,59 +12,195 @@
android:layout_width="match_parent"
android:layout_height="match_parent" />
-
-
+
+
+
-
+ android:alpha="0.85"
+ android:padding="18dp"
+ app:cardBackgroundColor="@color/md_theme_surfaceContainer"
+ app:cardElevation="4dp"
+ app:shapeAppearanceOverlay="@style/ShapeAppearance.Material3.MediumComponent">
-
+ android:orientation="vertical"
+ android:padding="8dp"
+ android:minWidth="200dp">
-
+
-
+
-
+
+
+
+
+
+
+
+
+
+
+
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ app:layout_constraintStart_toStartOf="@id/floorDownButton"
+ app:layout_constraintEnd_toEndOf="@id/floorDownButton" />
+ app:layout_constraintStart_toStartOf="@id/floorDownButton"
+ app:layout_constraintEnd_toEndOf="@id/floorDownButton" />
Default building assumptions
Floor height in meters
- Color
+ Print Location
+ Location copied to clipboard
+ Calibrating: %1$ds remaining — Please do not move\nPlease choose the map style above
+ Calibrating: %1$ds remaining\nPlease do not move
🛰 Show GNSS
+ 📡 Show WiFi
Floor Down button
Floor Up button
Choose Map
+ 🧭 Show Compass
+ 🔴 PDR Path
+ 🟣 Fused Path
+ 🟢 Smooth Path
+ 🔮 Show Estimated
+ 🌤 Show Weather
❇️ Auto Floor
+ 🧿 Navigating
GNSS error:
+ WiFi error:
Satellite
Normal
Hybrid
@@ -150,4 +162,4 @@
Upload Anyway
Cancel
-
\ No newline at end of file
+
diff --git a/app/src/main/res/values/styles.xml b/app/src/main/res/values/styles.xml
index 528421a1..0bbf2a17 100644
--- a/app/src/main/res/values/styles.xml
+++ b/app/src/main/res/values/styles.xml
@@ -8,7 +8,7 @@
- @color/md_theme_light_secondary
- @color/md_theme_light_onSecondary
- @color/md_theme_light_background
- - @color/md_theme_light_primary
+ - @android:color/transparent
- @color/md_theme_light_surface
- @color/md_theme_light_surface
- true
diff --git a/app/src/test/java/com/openpositioning/PositionMe/BuildConstantsTest.java b/app/src/test/java/com/openpositioning/PositionMe/BuildConstantsTest.java
new file mode 100644
index 00000000..9d39d8bd
--- /dev/null
+++ b/app/src/test/java/com/openpositioning/PositionMe/BuildConstantsTest.java
@@ -0,0 +1,15 @@
+package com.openpositioning.PositionMe;
+
+import static org.junit.Assert.*;
+import org.junit.Test;
+
+/**
+ * Verifies {@link BuildConstants} configuration for release.
+ */
+public class BuildConstantsTest {
+
+ @Test
+ public void debug_isDisabledForRelease() {
+ assertFalse("DEBUG must be false for release builds", BuildConstants.DEBUG);
+ }
+}
diff --git a/app/src/test/java/com/openpositioning/PositionMe/sensors/fusion/CalibrationManagerTest.java b/app/src/test/java/com/openpositioning/PositionMe/sensors/fusion/CalibrationManagerTest.java
new file mode 100644
index 00000000..f49c47c0
--- /dev/null
+++ b/app/src/test/java/com/openpositioning/PositionMe/sensors/fusion/CalibrationManagerTest.java
@@ -0,0 +1,75 @@
+package com.openpositioning.PositionMe.sensors.fusion;
+
+import static org.junit.Assert.*;
+import org.junit.Test;
+
+import com.openpositioning.PositionMe.sensors.Wifi;
+
+import java.util.ArrayList;
+import java.util.List;
+
+/**
+ * Unit tests for {@link CalibrationManager}.
+ * Verifies WKNN matching boundary conditions and null safety.
+ *
+ * Note: Tests that require {@code LatLng} (Google Maps) are excluded
+ * from local JVM tests to avoid Android stub exceptions.
+ */
+public class CalibrationManagerTest {
+
+ @Test
+ public void getRecordCount_emptyByDefault() {
+ CalibrationManager cm = new CalibrationManager();
+ assertEquals(0, cm.getRecordCount());
+ }
+
+ @Test
+ public void findBestMatch_emptyRecords_returnsNull() {
+ CalibrationManager cm = new CalibrationManager();
+ List scan = createMockScan(5);
+ assertNull(cm.findBestMatch(scan));
+ }
+
+ @Test
+ public void findBestMatch_nullScan_returnsNull() {
+ CalibrationManager cm = new CalibrationManager();
+ assertNull(cm.findBestMatch(null));
+ }
+
+ @Test
+ public void findBestMatch_emptyScan_returnsNull() {
+ CalibrationManager cm = new CalibrationManager();
+ assertNull(cm.findBestMatch(new ArrayList<>()));
+ }
+
+ @Test
+ public void findCalibrationPosition_emptyRecords_returnsNull() {
+ CalibrationManager cm = new CalibrationManager();
+ List scan = createMockScan(5);
+ assertNull(cm.findCalibrationPosition(scan, 0));
+ }
+
+ @Test
+ public void findCalibrationPosition_nullScan_returnsNull() {
+ CalibrationManager cm = new CalibrationManager();
+ assertNull(cm.findCalibrationPosition(null, 0));
+ }
+
+ /**
+ * Creates a mock WiFi scan with the specified number of APs.
+ */
+ private List createMockScan(int apCount) {
+ List scan = new ArrayList<>();
+ for (int i = 0; i < apCount; i++) {
+ Wifi w = new Wifi();
+ w.setBssid(i + 1);
+ w.setBssidString(String.format("00:11:22:33:44:%02x", i));
+ w.setSsid("TestAP_" + i);
+ w.setLevel(-50 - i);
+ w.setFrequency(2400 + i * 5);
+ w.setRttEnabled(false);
+ scan.add(w);
+ }
+ return scan;
+ }
+}
diff --git a/app/src/test/java/com/openpositioning/PositionMe/sensors/fusion/CoordinateTransformTest.java b/app/src/test/java/com/openpositioning/PositionMe/sensors/fusion/CoordinateTransformTest.java
new file mode 100644
index 00000000..d14ad30c
--- /dev/null
+++ b/app/src/test/java/com/openpositioning/PositionMe/sensors/fusion/CoordinateTransformTest.java
@@ -0,0 +1,80 @@
+package com.openpositioning.PositionMe.sensors.fusion;
+
+import static org.junit.Assert.*;
+import org.junit.Before;
+import org.junit.Test;
+
+/**
+ * Unit tests for {@link CoordinateTransform}.
+ * Verifies ENU ↔ LatLng conversions are accurate and invertible.
+ */
+public class CoordinateTransformTest {
+
+ private CoordinateTransform ct;
+ private static final double ORIGIN_LAT = 55.9230;
+ private static final double ORIGIN_LNG = -3.1742;
+
+ @Before
+ public void setUp() {
+ ct = new CoordinateTransform();
+ ct.setOrigin(ORIGIN_LAT, ORIGIN_LNG);
+ }
+
+ @Test
+ public void isInitialized_afterSetOrigin_returnsTrue() {
+ assertTrue(ct.isInitialized());
+ }
+
+ @Test
+ public void isInitialized_beforeSetOrigin_returnsFalse() {
+ CoordinateTransform fresh = new CoordinateTransform();
+ assertFalse(fresh.isInitialized());
+ }
+
+ @Test
+ public void originMapsToZeroZero() {
+ double[] en = ct.toEastNorth(ORIGIN_LAT, ORIGIN_LNG);
+ assertEquals(0.0, en[0], 0.01);
+ assertEquals(0.0, en[1], 0.01);
+ }
+
+ @Test
+ public void roundTrip_originReturnsOrigin() {
+ double[] ll = ct.toLatLng(0, 0);
+ assertEquals(ORIGIN_LAT, ll[0], 1e-6);
+ assertEquals(ORIGIN_LNG, ll[1], 1e-6);
+ }
+
+ @Test
+ public void roundTrip_arbitraryPoint() {
+ double testLat = 55.9235;
+ double testLng = -3.1750;
+ double[] en = ct.toEastNorth(testLat, testLng);
+ double[] ll = ct.toLatLng(en[0], en[1]);
+ assertEquals(testLat, ll[0], 1e-5);
+ assertEquals(testLng, ll[1], 1e-5);
+ }
+
+ @Test
+ public void fiveMetresNorth_givesPositiveNorthing() {
+ double offsetLat = ORIGIN_LAT + 5.0 / 111132.92;
+ double[] en = ct.toEastNorth(offsetLat, ORIGIN_LNG);
+ assertEquals(0.0, en[0], 0.5); // easting ~0
+ assertEquals(5.0, en[1], 0.5); // northing ~5m
+ }
+
+ @Test
+ public void fiveMetresEast_givesPositiveEasting() {
+ double metersPerDegreeLng = 111132.92 * Math.cos(Math.toRadians(ORIGIN_LAT));
+ double offsetLng = ORIGIN_LNG + 5.0 / metersPerDegreeLng;
+ double[] en = ct.toEastNorth(ORIGIN_LAT, offsetLng);
+ assertEquals(5.0, en[0], 0.5); // easting ~5m
+ assertEquals(0.0, en[1], 0.5); // northing ~0
+ }
+
+ @Test
+ public void getOrigin_returnsSetValues() {
+ assertEquals(ORIGIN_LAT, ct.getOriginLat(), 1e-10);
+ assertEquals(ORIGIN_LNG, ct.getOriginLng(), 1e-10);
+ }
+}
diff --git a/app/src/test/java/com/openpositioning/PositionMe/sensors/fusion/MapConstraintTest.java b/app/src/test/java/com/openpositioning/PositionMe/sensors/fusion/MapConstraintTest.java
new file mode 100644
index 00000000..836c84cf
--- /dev/null
+++ b/app/src/test/java/com/openpositioning/PositionMe/sensors/fusion/MapConstraintTest.java
@@ -0,0 +1,47 @@
+package com.openpositioning.PositionMe.sensors.fusion;
+
+import static org.junit.Assert.*;
+import org.junit.Test;
+
+/**
+ * Unit tests for {@link MapConstraint}.
+ * Verifies wall segment AABB computation and initialization state.
+ */
+public class MapConstraintTest {
+
+ @Test
+ public void isInitialized_beforeInit_returnsFalse() {
+ MapConstraint mc = new MapConstraint();
+ assertFalse(mc.isInitialized());
+ }
+
+ @Test
+ public void wallSegment_storesEndpoints() {
+ MapConstraint.WallSegment wall = new MapConstraint.WallSegment(1, 2, 3, 4);
+ assertEquals(1, wall.x1, 1e-10);
+ assertEquals(2, wall.y1, 1e-10);
+ assertEquals(3, wall.x2, 1e-10);
+ assertEquals(4, wall.y2, 1e-10);
+ }
+
+ @Test
+ public void wallSegment_computesMinMax() {
+ MapConstraint.WallSegment wall = new MapConstraint.WallSegment(8, 3, 2, 9);
+ assertEquals(2, wall.minX, 1e-10);
+ assertEquals(8, wall.maxX, 1e-10);
+ assertEquals(3, wall.minY, 1e-10);
+ assertEquals(9, wall.maxY, 1e-10);
+ }
+
+ @Test
+ public void wallSegment_horizontalHasEqualY() {
+ MapConstraint.WallSegment wall = new MapConstraint.WallSegment(0, 5, 10, 5);
+ assertEquals(wall.minY, wall.maxY, 1e-10);
+ }
+
+ @Test
+ public void wallSegment_verticalHasEqualX() {
+ MapConstraint.WallSegment wall = new MapConstraint.WallSegment(5, 0, 5, 10);
+ assertEquals(wall.minX, wall.maxX, 1e-10);
+ }
+}
diff --git a/app/src/test/java/com/openpositioning/PositionMe/sensors/fusion/ParticleFilterTest.java b/app/src/test/java/com/openpositioning/PositionMe/sensors/fusion/ParticleFilterTest.java
new file mode 100644
index 00000000..ed9d4393
--- /dev/null
+++ b/app/src/test/java/com/openpositioning/PositionMe/sensors/fusion/ParticleFilterTest.java
@@ -0,0 +1,107 @@
+package com.openpositioning.PositionMe.sensors.fusion;
+
+import static org.junit.Assert.*;
+import org.junit.Before;
+import org.junit.Test;
+
+/**
+ * Unit tests for {@link ParticleFilter}.
+ * Verifies initialization, prediction, observation update, and state queries.
+ */
+public class ParticleFilterTest {
+
+ private ParticleFilter pf;
+ private static final int NUM_PARTICLES = 500;
+
+ @Before
+ public void setUp() {
+ pf = new ParticleFilter(NUM_PARTICLES);
+ }
+
+ @Test
+ public void initialize_setsPositionNearOrigin() {
+ pf.initialize(10.0, 20.0, 0, 5.0);
+ assertEquals(10.0, pf.getEstimatedX(), 3.0);
+ assertEquals(20.0, pf.getEstimatedY(), 3.0);
+ }
+
+ @Test
+ public void initialize_setsFloor() {
+ pf.initialize(0, 0, 2, 5.0);
+ Particle[] particles = pf.getParticles();
+ for (Particle p : particles) {
+ assertEquals(2, p.floor);
+ }
+ }
+
+ @Test
+ public void isInitialized_afterInit_returnsTrue() {
+ pf.initialize(0, 0, 0, 5.0);
+ assertTrue(pf.isInitialized());
+ }
+
+ @Test
+ public void isInitialized_beforeInit_returnsFalse() {
+ assertFalse(pf.isInitialized());
+ }
+
+ @Test
+ public void predict_movesParticlesForward() {
+ pf.initialize(0, 0, 0, 1.0);
+ double xBefore = pf.getEstimatedX();
+ double yBefore = pf.getEstimatedY();
+
+ // Walk north (heading=0) for 10 steps of 1m
+ for (int i = 0; i < 10; i++) {
+ pf.predict(1.0, 0.0, Math.toRadians(5));
+ }
+
+ double displacement = Math.hypot(
+ pf.getEstimatedX() - xBefore,
+ pf.getEstimatedY() - yBefore);
+ assertTrue("Displacement should be roughly 10m, was " + displacement,
+ displacement > 5.0 && displacement < 15.0);
+ }
+
+ @Test
+ public void updateWithDynamicSigma_pullsTowardObservation() {
+ pf.initialize(0, 0, 0, 2.0);
+
+ for (int i = 0; i < 5; i++) {
+ pf.updateWithDynamicSigma(10.0, 10.0, 2.0);
+ }
+
+ double distToObs = Math.hypot(
+ pf.getEstimatedX() - 10.0,
+ pf.getEstimatedY() - 10.0);
+ assertTrue("Should be pulled toward (10,10), dist=" + distToObs,
+ distToObs < 12.0);
+ }
+
+ @Test
+ public void particleCount_matchesConstructor() {
+ assertEquals(NUM_PARTICLES, pf.getParticles().length);
+ assertEquals(NUM_PARTICLES, pf.getNumParticles());
+ }
+
+ @Test
+ public void updateFloor_changesAllParticleFloors() {
+ pf.initialize(0, 0, 0, 5.0);
+ pf.updateFloor(3);
+ for (Particle p : pf.getParticles()) {
+ assertEquals(3, p.floor);
+ }
+ }
+
+ @Test
+ public void getUncertainty_afterInit_isPositive() {
+ pf.initialize(0, 0, 0, 5.0);
+ assertTrue(pf.getUncertainty() > 0);
+ }
+
+ @Test
+ public void getEstimatedFloor_afterInit_matchesSeed() {
+ pf.initialize(0, 0, 2, 5.0);
+ assertEquals(2, pf.getEstimatedFloor());
+ }
+}
diff --git a/app/src/test/java/com/openpositioning/PositionMe/sensors/fusion/ParticleTest.java b/app/src/test/java/com/openpositioning/PositionMe/sensors/fusion/ParticleTest.java
new file mode 100644
index 00000000..2e0c5950
--- /dev/null
+++ b/app/src/test/java/com/openpositioning/PositionMe/sensors/fusion/ParticleTest.java
@@ -0,0 +1,32 @@
+package com.openpositioning.PositionMe.sensors.fusion;
+
+import static org.junit.Assert.*;
+import org.junit.Test;
+
+/**
+ * Unit tests for {@link Particle}.
+ */
+public class ParticleTest {
+
+ @Test
+ public void constructor_setsFields() {
+ Particle p = new Particle(3.0, 7.0, 2, 0.5);
+ assertEquals(3.0, p.x, 1e-10);
+ assertEquals(7.0, p.y, 1e-10);
+ assertEquals(2, p.floor);
+ assertEquals(0.5, p.weight, 1e-10);
+ }
+
+ @Test
+ public void fields_areMutable() {
+ Particle p = new Particle(0, 0, 0, 1.0);
+ p.x = 5.0;
+ p.y = -3.0;
+ p.floor = 1;
+ p.weight = 0.25;
+ assertEquals(5.0, p.x, 1e-10);
+ assertEquals(-3.0, p.y, 1e-10);
+ assertEquals(1, p.floor);
+ assertEquals(0.25, p.weight, 1e-10);
+ }
+}
diff --git a/secrets.properties b/secrets.properties
index f0dc54fd..7907b650 100644
--- a/secrets.properties
+++ b/secrets.properties
@@ -1,6 +1,6 @@
#
# Modify the variables to set your keys
#
-MAPS_API_KEY=
-OPENPOSITIONING_API_KEY=
-OPENPOSITIONING_MASTER_KEY=
+MAPS_API_KEY=
+OPENPOSITIONING_API_KEY=
+OPENPOSITIONING_MASTER_KEY=