diff --git a/.gitignore b/.gitignore index d4c3a57e..adec9b09 100644 --- a/.gitignore +++ b/.gitignore @@ -12,5 +12,6 @@ /captures .externalNativeBuild .cxx +secrets.properties local.properties /.idea/ diff --git a/app/build.gradle b/app/build.gradle index 3e29b13f..ecdf3498 100644 --- a/app/build.gradle +++ b/app/build.gradle @@ -2,6 +2,7 @@ plugins { id 'com.android.application' id 'com.google.gms.google-services' id 'androidx.navigation.safeargs' + id 'com.google.android.libraries.mapsplatform.secrets-gradle-plugin' } @@ -23,18 +24,21 @@ android { versionCode 1 versionName "1.0" + testInstrumentationRunner "androidx.test.runner.AndroidJUnitRunner" // Example of referencing secrets (if you use secrets.properties): - buildConfigField "String", "MAPS_API_KEY", - "\"${localProperties['MAPS_API_KEY'] ?: ''}\"" - buildConfigField "String", "OPENPOSITIONING_API_KEY", - "\"${localProperties['OPENPOSITIONING_API_KEY'] ?: ''}\"" - buildConfigField "String", "OPENPOSITIONING_MASTER_KEY", - "\"${localProperties['OPENPOSITIONING_MASTER_KEY'] ?: ''}\"" + buildConfigField "String", "MAPS_API_KEY", "\"${localProperties['MAPS_API_KEY'] ?: ''}\"" + buildConfigField "String", "OPENPOSITIONING_API_KEY", "\"${localProperties['OPENPOSITIONING_API_KEY'] ?: ''}\"" + buildConfigField "String", "OPENPOSITIONING_MASTER_KEY", "\"${localProperties['OPENPOSITIONING_MASTER_KEY'] ?: ''}\"" + + resValue("string", "google_maps_key", localProperties.getProperty("MAPS_API_KEY")) + } buildFeatures { + + // For example: // compose true // if you want Jetpack Compose // viewBinding true @@ -92,4 +96,15 @@ dependencies { testImplementation 'junit:junit:4.13.2' androidTestImplementation 'androidx.test.ext:junit:1.2.1' androidTestImplementation 'androidx.test.espresso:espresso-core:3.5.1' + + // Added EJML for matrix operations used in sensor fusion algorithms + implementation 'org.ejml:ejml-all:0.43.1' + + // Added these for enhanced math operations + implementation 'org.apache.commons:commons-math3:3.6.1' + + implementation 'com.google.maps.android:android-maps-utils:3.4.0' + + implementation 'com.google.maps.android:android-maps-utils:2.3.0' + } diff --git a/app/src/main/AndroidManifest.xml b/app/src/main/AndroidManifest.xml index 678711fd..6967c95f 100644 --- a/app/src/main/AndroidManifest.xml +++ b/app/src/main/AndroidManifest.xml @@ -84,7 +84,7 @@ + android:value="@string/google_maps_key" /> State=" + stateVector.get(STATE_IDX_EAST, 0) + ", " + + stateVector.get(STATE_IDX_NORTH, 0) + ", bearing=" + + Math.toDegrees(stateVector.get(STATE_IDX_BEARING, 0)) + "°"); + } + + /** + * Process all pending updates (GNSS, WiFi, and opportunistic) when a PDR step is detected. + * + * @param currentTime Current system time in milliseconds + * @param pdrEast Current PDR east position for comparison + * @param pdrNorth Current PDR north position for comparison + */ + private void processAllPendingUpdates(long currentTime, double pdrEast, double pdrNorth) { + // Process pending GNSS update if available + if (hasPendingGnssUpdate) { + processPendingGnssUpdate(currentTime); + } + + // Process pending WiFi update if available + if (hasPendingWifiUpdate) { + processPendingWifiUpdate(currentTime); + } + + // Process pending opportunistic update if available + processOpportunisticUpdateIfAvailable(currentTime, pdrEast, pdrNorth); + } + + @Override + public void processGnssUpdate(LatLng position, double altitude) { + long currentTime = System.currentTimeMillis(); + + // Store first GNSS position for reference initialization if needed + if (!hasInitialGnssPosition) { + firstGnssPosition = position; + hasInitialGnssPosition = true; + + // Try to initialize reference position if needed + if (!referenceInitialized) { + initializeReferencePosition(); + } + } + + // Instead of applying the update immediately, store it for the next PDR update + pendingGnssPosition = position; + pendingGnssAltitude = altitude; + pendingGnssTime = currentTime; + hasPendingGnssUpdate = true; + + Log.d(TAG, "GNSS update stored for next PDR step: " + + position.latitude + ", " + position.longitude + ", altitude=" + altitude); + } + + /** + * Process a pending GNSS update that was stored. + * + * @param currentTime Current system time in milliseconds + */ + private void processPendingGnssUpdate(long currentTime) { + if (!hasPendingGnssUpdate || pendingGnssPosition == null) { + return; + } + + // If reference still not initialized, we can't process + if (!referenceInitialized) { + Log.w(TAG, "Skipping pending GNSS update: reference position not initialized"); + hasPendingGnssUpdate = false; + return; + } + + // Check if the update is too old (more than 10 seconds) + if (currentTime - pendingGnssTime > MAX_TIME_WITHOUT_UPDATE) { + Log.d(TAG, "Skipping pending GNSS update: too old"); + hasPendingGnssUpdate = false; + return; + } + + // Convert GNSS position to ENU (using the reference position) + double[] enu = CoordinateConverter.convertGeodeticToEnu( + pendingGnssPosition.latitude, pendingGnssPosition.longitude, pendingGnssAltitude, + referencePosition[0], referencePosition[1], referencePosition[2] + ); + + // Debug the conversion + Log.d(TAG, "Pending GNSS geodetic->ENU: " + + pendingGnssPosition.latitude + "," + pendingGnssPosition.longitude + " -> " + + "E=" + enu[0] + ", N=" + enu[1]); + + // If this is the first position, initialize the state + if (stateVector.get(STATE_IDX_EAST, 0) == 0 && stateVector.get(STATE_IDX_NORTH, 0) == 0) { + stateVector.set(STATE_IDX_EAST, 0, enu[0]); + stateVector.set(STATE_IDX_NORTH, 0, enu[1]); + lastPredictTime = currentTime; + lastUpdateTime = currentTime; + + Log.d(TAG, "First GNSS update: initializing state with E=" + enu[0] + ", N=" + enu[1]); + hasPendingGnssUpdate = false; + return; + } + + // Update measurement model + measurementModel.updateGnssUncertainty(null, currentTime); + + // Apply Kalman update with GNSS measurement + SimpleMatrix measurementVector = new SimpleMatrix(MEAS_SIZE, 1); + measurementVector.set(MEAS_IDX_EAST, 0, enu[0]); + measurementVector.set(MEAS_IDX_NORTH, 0, enu[1]); + + // Apply additional uncertainty scaling factor for GNSS + double gnssUncertaintyScaleFactor = 1; // Makes GNSS 1.5x less trusted + double gnssVar = Math.pow(measurementModel.getGnssStd() * gnssUncertaintyScaleFactor, 2); + measurementNoiseMatrixGnss.set(MEAS_IDX_EAST, MEAS_IDX_EAST, gnssVar); + measurementNoiseMatrixGnss.set(MEAS_IDX_NORTH, MEAS_IDX_NORTH, gnssVar); + + // Check for outliers by comparing GNSS with current state + double currentEast = stateVector.get(STATE_IDX_EAST, 0); + double currentNorth = stateVector.get(STATE_IDX_NORTH, 0); + double distance = Math.sqrt(Math.pow(enu[0] - currentEast, 2) + + Math.pow(enu[1] - currentNorth, 2)); + + if (fusionOutlierDetector.evaluateDistance(distance)) { + Log.w(TAG, "Skipping pending GNSS update: detected as outlier (distance=" + distance + "m)"); + hasPendingGnssUpdate = false; + return; + } + + // Perform Kalman update + performUpdate(measurementVector, measurementNoiseMatrixGnss, currentTime); + + // Reset step counter since we've applied a GNSS update + stepsSinceLastUpdate = 0; + + // Clear pending update flag + hasPendingGnssUpdate = false; + + Log.d(TAG, "Applied pending GNSS update: E=" + enu[0] + ", N=" + enu[1] + + " -> State=" + stateVector.get(STATE_IDX_EAST, 0) + ", " + + stateVector.get(STATE_IDX_NORTH, 0)); + } + + @Override + public void processWifiUpdate(LatLng position, int floor) { + long currentTime = System.currentTimeMillis(); + + // Instead of applying the update immediately, store it for the next PDR update + pendingWifiPosition = position; + pendingWifiFloor = floor; + pendingWifiTime = currentTime; + hasPendingWifiUpdate = true; + + Log.d(TAG, "WiFi update stored for next PDR step: " + + position.latitude + ", " + position.longitude + ", floor=" + floor); + } + + /** + * Process a pending WiFi update that was stored. + * + * @param currentTime Current system time in milliseconds + */ + private void processPendingWifiUpdate(long currentTime) { + if (!hasPendingWifiUpdate || pendingWifiPosition == null) { + return; + } + + // If reference still not initialized, we can't process + if (!referenceInitialized) { + Log.w(TAG, "Skipping pending WiFi update: reference position not initialized"); + hasPendingWifiUpdate = false; + return; + } + + // Check if the update is too old (more than 10 seconds) + if (currentTime - pendingWifiTime > MAX_TIME_WITHOUT_UPDATE) { + Log.d(TAG, "Skipping pending WiFi update: too old"); + hasPendingWifiUpdate = false; + return; + } + + // Convert WiFi position to ENU (using the reference position) + // TODO: Fix altitude estimate for wifi + double[] enu = CoordinateConverter.convertGeodeticToEnu( + pendingWifiPosition.latitude, pendingWifiPosition.longitude, referencePosition[2], + referencePosition[0], referencePosition[1], referencePosition[2] + ); + + // Debug the conversion + Log.d(TAG, "Pending WiFi geodetic->ENU: " + + pendingWifiPosition.latitude + "," + pendingWifiPosition.longitude + " -> " + + "E=" + enu[0] + ", N=" + enu[1]); + + // If this is the first position, initialize the state + if (stateVector.get(STATE_IDX_EAST, 0) == 0 && stateVector.get(STATE_IDX_NORTH, 0) == 0) { + stateVector.set(STATE_IDX_EAST, 0, enu[0]); + stateVector.set(STATE_IDX_NORTH, 0, enu[1]); + lastPredictTime = currentTime; + lastUpdateTime = currentTime; + + Log.d(TAG, "First WiFi update: initializing state with E=" + enu[0] + ", N=" + enu[1]); + hasPendingWifiUpdate = false; + return; + } + + // Update measurement model + measurementModel.updateWifiUncertainty(null, currentTime); + + // Apply Kalman update with WiFi measurement + SimpleMatrix measurementVector = new SimpleMatrix(MEAS_SIZE, 1); + measurementVector.set(MEAS_IDX_EAST, 0, enu[0]); + measurementVector.set(MEAS_IDX_NORTH, 0, enu[1]); + + // Prepare measurement noise matrix + double wifiConfidenceScaleFactor = 2.5; // Makes WiFi less trusted + double wifiVar = Math.pow(measurementModel.getWifiStd() * wifiConfidenceScaleFactor, 2); + measurementNoiseMatrixWifi.set(MEAS_IDX_EAST, MEAS_IDX_EAST, wifiVar); + measurementNoiseMatrixWifi.set(MEAS_IDX_NORTH, MEAS_IDX_NORTH, wifiVar); + + // Perform Kalman update + performUpdate(measurementVector, measurementNoiseMatrixWifi, currentTime); + + // Reset step counter since we've applied a WiFi update + stepsSinceLastUpdate = 0; + + // Clear pending update flag + hasPendingWifiUpdate = false; + + Log.d(TAG, "Applied pending WiFi update: E=" + enu[0] + ", N=" + enu[1] + + " -> State=" + stateVector.get(STATE_IDX_EAST, 0) + ", " + + stateVector.get(STATE_IDX_NORTH, 0)); + + } + + @Override + public LatLng getFusedPosition() { + if (!referenceInitialized) { + if (hasInitialGnssPosition) { + Log.w(TAG, "Using initial GNSS position as fusion result (reference not initialized)"); + return firstGnssPosition; + } else { + Log.e(TAG, "Cannot get fused position: no reference position and no GNSS position"); + return null; + } + } + + // If we haven't received any updates yet, return the reference position + if (stateVector.get(STATE_IDX_EAST, 0) == 0 && stateVector.get(STATE_IDX_NORTH, 0) == 0) { + Log.d(TAG, "Returning reference position as fusion result (no updates yet)"); + return new LatLng(referencePosition[0], referencePosition[1]); + } + + double east = stateVector.get(STATE_IDX_EAST, 0); + double north = stateVector.get(STATE_IDX_NORTH, 0); + + // Apply smoothing if needed + double[] smoothed = smoothingFilter.update(new double[]{east, north}); + + // Debug the state before conversion + Log.d(TAG, "Fused position (before conversion): E=" + smoothed[0] + ", N=" + smoothed[1]); + + // Convert ENU back to latitude/longitude + LatLng result = CoordinateConverter.convertEnuToGeodetic( + smoothed[0], smoothed[1], 0, // Assume altitude=0 for the return value + referencePosition[0], referencePosition[1], referencePosition[2] + ); + + Log.d(TAG, "Fused position (after conversion): " + + result.latitude + "," + result.longitude); + + return result; + } + + @Override + public void reset() { + // Reset state vector + stateVector = new SimpleMatrix(STATE_SIZE, 1); + + // Reset covariance matrix with high uncertainty + covarianceMatrix = SimpleMatrix.identity(STATE_SIZE); + covarianceMatrix = covarianceMatrix.scale(100); + + // Reset timing variables + lastUpdateTime = System.currentTimeMillis(); + lastPredictTime = lastUpdateTime; + stepsSinceLastUpdate = 0; + + // Keep reference position but reset first GNSS position + hasInitialGnssPosition = false; + + // Reset pending measurement variables + pendingMeasurement = null; + pendingMeasurementTime = 0; + hasPendingMeasurement = false; + usePendingMeasurement = false; + + // Reset pending GNSS update variables + hasPendingGnssUpdate = false; + pendingGnssPosition = null; + pendingGnssAltitude = 0; + pendingGnssTime = 0; + + // Reset pending WiFi update variables + hasPendingWifiUpdate = false; + pendingWifiPosition = null; + pendingWifiFloor = 0; + pendingWifiTime = 0; + + // Reset helper models + movementModel.resetState(); + measurementModel.reset(); + smoothingFilter.reset(); + fusionOutlierDetector.clearHistory(); + + Log.d(TAG, "Kalman filter reset"); + } + + @Override + public void staticUpdate() { + + } + + @Override + public void retrieveContext(Context context) { + + } + + @Override + public void setElevationStatus(PdrProcessing.ElevationDirection elevationDirection) { + + } + + /** + * Handles opportunistic updates to the state estimation process. + * This allows the filter to utilize additional data points when available. + * + * @param position An array containing the East and North positions in meters + * @param timeMillis The timestamp of the measurement in milliseconds + */ + public void onOpportunisticUpdate(double[] position, long timeMillis) { + // Store this measurement for later use + pendingMeasurement = position; + pendingMeasurementTime = timeMillis; + hasPendingMeasurement = true; + + // Check if the measurement is significantly different from the last one + usePendingMeasurement = true; + + Log.d(TAG, "Received opportunistic update: E=" + position[0] + ", N=" + position[1] + + " at time " + timeMillis); + } + + /** + * Process a pending opportunistic update if available and valid. + * + * @param currentTime Current system time in milliseconds + * @param pdrEast Current PDR east position for comparison + * @param pdrNorth Current PDR north position for comparison + */ + private void processOpportunisticUpdateIfAvailable(long currentTime, double pdrEast, double pdrNorth) { + // Check if we have a pending measurement and should use it + if (!hasPendingMeasurement || !usePendingMeasurement) { + return; + } + + // Check if the measurement is too old (more than 10 seconds) + if (currentTime - pendingMeasurementTime > MAX_TIME_WITHOUT_UPDATE) { + Log.d(TAG, "Skipping opportunistic update: too old"); + hasPendingMeasurement = false; + return; + } + + // Calculate distance between PDR and opportunistic measurement + double distance = CoordinateConverter.calculateEnuDistance( + pdrEast, pdrNorth, + pendingMeasurement[0], pendingMeasurement[1] + ); + + // Check if this is an outlier + if (fusionOutlierDetector.evaluateDistance(distance)) { + Log.d(TAG, "Skipping opportunistic update: detected as outlier (distance=" + distance + "m)"); + hasPendingMeasurement = false; + return; + } + + // Create measurement vector + SimpleMatrix measurementVector = new SimpleMatrix(MEAS_SIZE, 1); + measurementVector.set(MEAS_IDX_EAST, 0, pendingMeasurement[0]); + measurementVector.set(MEAS_IDX_NORTH, 0, pendingMeasurement[1]); + + // Create measurement noise matrix (higher uncertainty for opportunistic updates) + double uncertaintyFactor = 2.0; // Increase uncertainty for opportunistic updates + double variance = Math.pow(measurementModel.getGnssStd() * uncertaintyFactor, 2); + measurementNoiseMatrixGnss.set(MEAS_IDX_EAST, MEAS_IDX_EAST, variance); + measurementNoiseMatrixGnss.set(MEAS_IDX_NORTH, MEAS_IDX_NORTH, variance); + + // Perform Kalman update + performUpdate(measurementVector, measurementNoiseMatrixGnss, currentTime); + + // Reset state + hasPendingMeasurement = false; + stepsSinceLastUpdate = 0; + + Log.d(TAG, "Processed opportunistic update: E=" + pendingMeasurement[0] + + ", N=" + pendingMeasurement[1]); + } + + /** + * Initializes the reference position if it wasn't provided or was zeros. + * Uses the first GNSS position as the reference. + */ + private void initializeReferencePosition() { + if (referenceInitialized || !hasInitialGnssPosition) { + return; + } + + // Use the first GNSS position as reference + referencePosition[0] = firstGnssPosition.latitude; + referencePosition[1] = firstGnssPosition.longitude; + referencePosition[2] = 0; // Assume zero altitude if not provided + + referenceInitialized = true; + + Log.d(TAG, "Reference position initialized from GNSS: " + + "lat=" + referencePosition[0] + ", lng=" + referencePosition[1] + + ", alt=" + referencePosition[2]); + } + + /** + * Predicts the next state based on the movement model. + * + * @param newBearing New bearing (orientation) in radians + * @param stepLength Length of the current step in meters + * @param orientationStdDev Standard deviation of orientation in radians + * @param timePenalty Time-based penalty factor + * @param currentTime Current system time in milliseconds + * @param movementType Type of movement detected + */ + private void predictState(double newBearing, double stepLength, double orientationStdDev, + double timePenalty, long currentTime, MovementModel.MovementType movementType) { + // Calculate time delta + double deltaTime = (currentTime - lastPredictTime) / 1000.0; // Convert to seconds + if (deltaTime <= 0) { + deltaTime = 0.1; // Minimum delta time + } + + // Update process matrix for constant velocity model with bearing changes + updateProcessMatrix(deltaTime, newBearing); + + // Update process noise matrix + updateProcessNoiseMatrix(deltaTime, stepLength, orientationStdDev, timePenalty, movementType); + + // Predict state: x_k|k-1 = F_k * x_k-1|k-1 + SimpleMatrix predictedState = processMatrix.mult(stateVector); + + // Predict covariance: P_k|k-1 = F_k * P_k-1|k-1 * F_k^T + Q_k + SimpleMatrix predictedCovariance = processMatrix.mult(covarianceMatrix) + .mult(processMatrix.transpose()) + .plus(processNoiseMatrix); + + // Update state and covariance + stateVector = predictedState; + covarianceMatrix = predictedCovariance; + + // Normalize bearing to [-π, π] + double normalizedBearing = CoordinateConverter.normalizeAngleToPi(stateVector.get(STATE_IDX_BEARING, 0)); + stateVector.set(STATE_IDX_BEARING, 0, normalizedBearing); + } + + /** + * Updates the process matrix based on the time delta. + * + * @param deltaTime Time delta in seconds + * @param bearing Current bearing in radians + */ + private void updateProcessMatrix(double deltaTime, double bearing) { + // Start with identity matrix + processMatrix = SimpleMatrix.identity(STATE_SIZE); + + // Add time evolution: position += velocity * dt + processMatrix.set(STATE_IDX_EAST, STATE_IDX_VE, deltaTime); // x += vx * dt + processMatrix.set(STATE_IDX_NORTH, STATE_IDX_VN, deltaTime); // y += vy * dt + + // Update bearing directly (no integration) + processMatrix.set(STATE_IDX_BEARING, STATE_IDX_BEARING, 0); // Zero out existing bearing + stateVector.set(STATE_IDX_BEARING, 0, bearing); // Set new bearing directly + } + + /** + * Updates the process noise covariance matrix based on movement parameters. + * + * @param deltaTime Time delta in seconds + * @param stepLength Length of the current step in meters + * @param orientationStdDev Standard deviation of orientation in radians + * @param timePenalty Time-based penalty factor + * @param movementType Type of movement detected + */ + private void updateProcessNoiseMatrix(double deltaTime, double stepLength, double orientationStdDev, + double timePenalty, MovementModel.MovementType movementType) { + // Calculate step error based on movement model + double stepError = movementModel.estimateStepError(stepLength, timePenalty); + + // Calculate position process noise (from velocity uncertainty) + double dt2 = deltaTime * deltaTime; + double posNoise = stepError * stepError * dt2; + + // Calculate velocity process noise + double velNoise = (stepError / deltaTime) * (stepError / deltaTime); + + // Calculate bearing process noise + double bearingNoise = orientationStdDev * orientationStdDev; + if (movementType == MovementModel.MovementType.TURN) { + bearingNoise *= 2.0; // Increase noise for turns + } + + // Apply time penalty to all noise values + posNoise *= timePenalty; + velNoise *= timePenalty; + bearingNoise *= timePenalty; + + // Build the process noise matrix + processNoiseMatrix = SimpleMatrix.identity(STATE_SIZE); + processNoiseMatrix.set(STATE_IDX_BEARING, STATE_IDX_BEARING, bearingNoise); + processNoiseMatrix.set(STATE_IDX_EAST, STATE_IDX_EAST, posNoise); + processNoiseMatrix.set(STATE_IDX_NORTH, STATE_IDX_NORTH, posNoise); + processNoiseMatrix.set(STATE_IDX_VE, STATE_IDX_VE, velNoise); + processNoiseMatrix.set(STATE_IDX_VN, STATE_IDX_VN, velNoise); + } + + /** + * Performs the Kalman filter update step with a measurement. + * + * @param measurementVector The measurement vector (East, North) + * @param measurementNoiseMatrix The measurement noise matrix + * @param currentTime Current system time in milliseconds + */ + private void performUpdate(SimpleMatrix measurementVector, SimpleMatrix measurementNoiseMatrix, long currentTime) { + // Calculate innovation: y_k = z_k - H_k * x_k|k-1 + SimpleMatrix innovation = measurementVector.minus(measurementMatrix.mult(stateVector)); + + // Calculate innovation covariance: S_k = H_k * P_k|k-1 * H_k^T + R_k + SimpleMatrix innovationCovariance = measurementMatrix.mult(covarianceMatrix) + .mult(measurementMatrix.transpose()) + .plus(measurementNoiseMatrix); + + // Calculate Kalman gain: K_k = P_k|k-1 * H_k^T * S_k^-1 + SimpleMatrix kalmanGain = covarianceMatrix.mult(measurementMatrix.transpose()) + .mult(innovationCovariance.invert()); + + // Update state: x_k|k = x_k|k-1 + K_k * y_k + stateVector = stateVector.plus(kalmanGain.mult(innovation)); + + // Update covariance: P_k|k = (I - K_k * H_k) * P_k|k-1 + covarianceMatrix = identityMatrix.minus(kalmanGain.mult(measurementMatrix)) + .mult(covarianceMatrix); + + // Normalize bearing to [-π, π] + double normalizedBearing = CoordinateConverter.normalizeAngleToPi(stateVector.get(STATE_IDX_BEARING, 0)); + stateVector.set(STATE_IDX_BEARING, 0, normalizedBearing); + + // Update timing + lastUpdateTime = currentTime; + } + + /** + * Detects the type of movement based on bearing change. + * + * @param newBearing New bearing in radians + * @param oldBearing Old bearing in radians + * @return The type of movement detected + */ + private MovementModel.MovementType detectMovementType(double newBearing, double oldBearing) { + // Calculate absolute bearing change + double bearingChange = Math.abs(CoordinateConverter.normalizeAngleToPi(newBearing - oldBearing)); + + // Classify movement based on bearing change thresholds + if (bearingChange > Math.toRadians(30)) { + return MovementModel.MovementType.TURN; + } else if (bearingChange > Math.toRadians(10)) { + return MovementModel.MovementType.PSEUDO_TURN; + } else { + return MovementModel.MovementType.STRAIGHT; + } + } + + /** + * Implements an exponential smoothing filter designed to mitigate noise + * in sequential data streams, such as position estimates. By averaging current + * measurements with previous filtered values, it produces smoother trajectories, + * reducing the visual jitter often present in raw sensor outputs. + */ + public static class FusionFilter { + private static final String TAG = FusionFilter.class.getSimpleName(); // Use class name for TAG + + /** + * The smoothing factor (often denoted as 'alpha') controls the balance + * between responsiveness to new data and the degree of smoothing. + * A value of 1.0 means only the current input is used (no smoothing). + * A value of 0.0 means the output never changes (infinite smoothing). + * Values typically range from 0.1 (heavy smoothing) to 0.6 (moderate smoothing). + */ + private final double smoothingFactor; + + /** + * The number of independent dimensions in the data being filtered + * (e.g., 2 for 2D coordinates, 3 for 3D coordinates). + */ + private final int numDimensions; + + /** + * Stores the most recently calculated smoothed values for each dimension. + * This state is used in the subsequent filtering step. + */ + private double[] previousFilteredState; + + /** + * Tracks whether the filter has processed its first data point. + * The first point is used directly to initialize the filter's state. + */ + private boolean isInitialized; + + /** + * Constructs a new exponential smoothing filter. + * + * @param alpha The desired smoothing factor, clamped between 0.0 and 1.0. + * @param dimension The dimensionality of the data vectors to be filtered. + * Must be a positive integer. + */ + public FusionFilter(double alpha, int dimension) { + if (dimension <= 0) { + throw new IllegalArgumentException("Dimension must be positive."); + } + // Ensure the smoothing factor is within the valid range [0.0, 1.0] + this.smoothingFactor = Math.max(0.0, Math.min(1.0, alpha)); + this.numDimensions = dimension; + this.previousFilteredState = new double[dimension]; + this.isInitialized = false; + + Log.i(TAG, "FusionFilter initialized with factor=" + this.smoothingFactor + + ", dimensions=" + this.numDimensions); + } + + /** + * Processes a new data point (vector) using the configured smoothing factor. + * + * @param currentInput A double array representing the latest measurement vector. + * Its length must match the filter's dimensionality. + * @return A double array containing the newly computed smoothed values. + * Returns the input array unmodified if dimensions mismatch or on error. + */ + public double[] update(double[] currentInput) { + // Validate input dimensions + if (currentInput == null || currentInput.length != this.numDimensions) { + Log.e(TAG, "Input array dimension mismatch or null. Expected: " + + this.numDimensions + ", Got: " + + (currentInput == null ? "null" : currentInput.length)); + // Return input defensively, though throwing might be alternative + return (currentInput != null) ? currentInput.clone() : new double[this.numDimensions]; + } + + // Handle the first data point: Initialize state and return input directly + if (!this.isInitialized) { + System.arraycopy(currentInput, 0, this.previousFilteredState, 0, this.numDimensions); + this.isInitialized = true; + Log.d(TAG, "Filter initialized with first values."); + // Return a clone to prevent external modification of the input array + // affecting future initializations if the caller reuses the array. + return currentInput.clone(); + } + + // Apply the exponential smoothing formula for each dimension + double[] filteredOutput = new double[this.numDimensions]; + for (int i = 0; i < this.numDimensions; i++) { + // smoothed = alpha * new + (1 - alpha) * previous_smoothed + filteredOutput[i] = this.smoothingFactor * currentInput[i] + + (1.0 - this.smoothingFactor) * this.previousFilteredState[i]; + } + + // Update the internal state for the next iteration + // It's safe to directly assign filteredOutput here if we ensure it's not modified externally + // However, using arraycopy is safer if there were complex return paths. + // Let's use System.arraycopy for consistency and explicit state update. + System.arraycopy(filteredOutput, 0, this.previousFilteredState, 0, this.numDimensions); + + return filteredOutput; + } + + /** + * Processes a new data point using a temporarily specified smoothing factor, + * overriding the instance's default factor for this single operation. + * Useful for adaptive smoothing scenarios. + * + * @param currentInput A double array representing the latest measurement vector. + * Its length must match the filter's dimensionality. + * @param temporalFactor The smoothing factor to use for this specific update, + * clamped between 0.0 and 1.0. + * @return A double array containing the newly computed smoothed values using the temporal factor. + * Returns the input array unmodified if dimensions mismatch or on error. + */ + public double[] updateWithTemporalFactor(double[] currentInput, double temporalFactor) { + // Validate input dimensions + if (currentInput == null || currentInput.length != this.numDimensions) { + Log.e(TAG, "Input array dimension mismatch or null. Expected: " + + this.numDimensions + ", Got: " + + (currentInput == null ? "null" : currentInput.length)); + return (currentInput != null) ? currentInput.clone() : new double[this.numDimensions]; + } + + // Ensure the temporal factor is valid + double effectiveFactor = Math.max(0.0, Math.min(1.0, temporalFactor)); + + // Handle the first data point (same as regular update) + if (!this.isInitialized) { + System.arraycopy(currentInput, 0, this.previousFilteredState, 0, this.numDimensions); + this.isInitialized = true; + Log.d(TAG, "Filter initialized with first values (using temporal factor update)."); + return currentInput.clone(); + } + + // Apply exponential smoothing using the *temporal* factor + double[] filteredOutput = new double[this.numDimensions]; + for (int i = 0; i < this.numDimensions; i++) { + filteredOutput[i] = effectiveFactor * currentInput[i] + + (1.0 - effectiveFactor) * this.previousFilteredState[i]; + } + + // Update the internal state using the result calculated with the temporal factor + System.arraycopy(filteredOutput, 0, this.previousFilteredState, 0, this.numDimensions); + + return filteredOutput; + } + + /** + * Retrieves the configured default smoothing factor (alpha) of the filter. + * + * @return The smoothing factor used by the {@link #update(double[])} method. + */ + public double getSmoothingFactor() { + return this.smoothingFactor; + } + + /** + * Retrieves the last calculated filtered state vector. + * + * @return A *copy* of the double array containing the most recent smoothed values. + * Returns a zeroed array if the filter hasn't been initialized yet. + */ + public double[] getCurrentEstimate() { + // Return a clone to prevent external modification of the internal state + if (this.isInitialized) { + return this.previousFilteredState.clone(); + } else { + // Return a new zero array of the correct dimension if not initialized + Log.w(TAG, "getCurrentEstimate called before filter initialization. Returning zero vector."); + return new double[this.numDimensions]; + } + } + + /** + * Manually sets the internal state of the filter. This effectively initializes + * or re-initializes the filter with a specific known state. + * + * @param state A double array representing the desired state vector. Its length + * must match the filter's dimensionality. + */ + public void setState(double[] state) { + if (state == null || state.length != this.numDimensions) { + Log.e(TAG, "Cannot set filter state: dimension mismatch or null input. Expected: " + + this.numDimensions + ", Got: " + (state == null ? "null" : state.length)); + return; // Do not change state if input is invalid + } + + // Copy the provided state into the internal state array + System.arraycopy(state, 0, this.previousFilteredState, 0, this.numDimensions); + this.isInitialized = true; // Mark as initialized since we now have a valid state + Log.d(TAG, "Filter state manually set."); + } + + /** + * Resets the filter to its initial, uninitialized state. + * The internal state vector is cleared (typically to zeros), and the + * filter will require a new data point to become initialized again. + */ + public void reset() { + // Consider filling with NaN or another indicator, but zeros are common. + Arrays.fill(this.previousFilteredState, 0.0); + this.isInitialized = false; + Log.i(TAG, "FusionFilter has been reset."); + } + } + + /** + * Detects statistical outliers within a stream of distance measurements. + * + * This implementation employs a robust modified Z-score methodology based on the + * Median Absolute Deviation (MAD) calculated over a sliding window of recent measurements. + * It includes a hard limit check for immediate rejection of implausible values. + */ + public static class FusionOutlierDetector { + private static final String TAG = FusionOutlierDetector.class.getSimpleName(); + + // --- Configuration Constants --- + + /** The modified Z-score threshold above which a measurement is considered an outlier. */ + private static final double MODIFIED_Z_SCORE_THRESHOLD = 3.5; + /** The normalization constant used in the modified Z-score calculation (1 / Φ⁻¹(3/4) ≈ 0.6745). */ + private static final double MAD_NORMALIZATION_FACTOR = 0.6745; + /** An absolute maximum distance allowed; measurements exceeding this are immediately flagged as outliers. */ + private static final double ABSOLUTE_DISTANCE_THRESHOLD = 7.0; // meters + /** A small value to prevent division by zero or near-zero MAD values. */ + private static final double MINIMUM_MAD_VALUE = 1e-4; + /** The minimum number of samples required in the buffer before statistical analysis can be performed. */ + private static final int MINIMUM_SAMPLES_FOR_STATS = 4; + /** The default size of the sliding window used for statistical calculations. */ + private static final int DEFAULT_BUFFER_SIZE = 10; + + // --- State Variables --- + + /** The maximum number of distance measurements to retain in the sliding window. */ + private final int bufferCapacity; + /** Stores the recent distance measurements within the sliding window. */ + private final List distanceWindow; + /** A reusable list for performing median calculations without repeated allocations. */ + private final List calculationWorkList; + + /** + * Constructs a FusionOutlierDetector with the default window size. + * + * @see #DEFAULT_BUFFER_SIZE + */ + public FusionOutlierDetector() { + this(DEFAULT_BUFFER_SIZE); + } + + /** + * Constructs a FusionOutlierDetector with a specified window size. + * + * @param windowSize The number of recent distance measurements to consider for outlier detection. + * Must be at least 5. + * @throws IllegalArgumentException if windowSize is less than 5. + */ + public FusionOutlierDetector(int windowSize) { + if (windowSize < 5) { + // Ensure a reasonable minimum size for statistical stability and median calculation. + Log.w(TAG, "Requested window size " + windowSize + " is too small. Using minimum of 5."); + this.bufferCapacity = 5; + } else { + this.bufferCapacity = windowSize; + } + this.distanceWindow = new ArrayList<>(this.bufferCapacity); + this.calculationWorkList = new ArrayList<>(this.bufferCapacity); + + Log.i(TAG, "Initialized with buffer capacity: " + this.bufferCapacity); + } + + /** + * Evaluates a new distance measurement to determine if it is an outlier relative + * to the recent history of measurements. + * + * The method first checks against an absolute hard limit. If the value is plausible, + * it's added to a sliding window. Once enough samples are collected, it calculates + * the modified Z-score based on the median and MAD of the window data. + * + * @param newDistance The distance measurement (in meters) to evaluate. + * @return {@code true} if the measurement is identified as an outlier, {@code false} otherwise. + */ + public boolean evaluateDistance(double newDistance) { + // 1. Absolute Hard Limit Check + if (newDistance > ABSOLUTE_DISTANCE_THRESHOLD) { + Log.d(TAG, String.format("Outlier (Hard Limit): Distance %.2fm exceeds threshold %.2fm", + newDistance, ABSOLUTE_DISTANCE_THRESHOLD)); + // Note: We don't add this obviously invalid measurement to the buffer. + return true; + } + + // 2. Add to Sliding Window & Maintain Size + addToWindow(newDistance); + + // 3. Check Minimum Sample Requirement + if (distanceWindow.size() < MINIMUM_SAMPLES_FOR_STATS) { + // Not enough data for reliable statistics yet. + return false; + } + + // 4. Calculate Statistics + double median = computeMedian(distanceWindow); + double mad = computeMedianAbsoluteDeviation(median); + + // Prevent division by very small MAD values + double effectiveMad = Math.max(mad, MINIMUM_MAD_VALUE); + + // 5. Calculate Modified Z-Score + // Formula: M_i = (0.6745 * |x_i - median|) / MAD + double modifiedZScore = MAD_NORMALIZATION_FACTOR * Math.abs(newDistance - median) / effectiveMad; + + // 6. Compare Score to Threshold + boolean isOutlier = modifiedZScore > MODIFIED_Z_SCORE_THRESHOLD; + + // 7. Logging + if (isOutlier) { + Log.d(TAG, String.format("Outlier (Statistical): Distance %.2fm, Median %.2fm, MAD %.2f, Z-Score %.2f > %.2f", + newDistance, median, mad, modifiedZScore, MODIFIED_Z_SCORE_THRESHOLD)); + } else { + // Optional: Log non-outliers for debugging if needed + // Log.v(TAG, String.format("Inlier: Distance %.2fm, Median %.2fm, MAD %.2f, Z-Score %.2f <= %.2f", + // newDistance, median, mad, modifiedZScore, MODIFIED_Z_SCORE_THRESHOLD)); + } + + return isOutlier; + } + + /** + * Adds a new distance measurement to the sliding window, removing the oldest + * measurement if the buffer capacity is exceeded. + * + * @param distance The distance measurement to add. + */ + private void addToWindow(double distance) { + distanceWindow.add(distance); + if (distanceWindow.size() > bufferCapacity) { + distanceWindow.remove(0); // Remove the oldest element + } + } + + /** + * Computes the median value from a list of numerical values. + * Uses a reusable internal list to avoid allocations and sorts it. + * + * @param values The list of values for which to compute the median. Cannot be empty. + * @return The median value. + */ + private double computeMedian(List values) { + Objects.requireNonNull(values, "Input list cannot be null"); + if (values.isEmpty()) { + throw new IllegalArgumentException("Input list cannot be empty for median calculation"); + } + + // Use the work list to avoid modifying the original and reduce allocations + calculationWorkList.clear(); + calculationWorkList.addAll(values); + Collections.sort(calculationWorkList); + + int size = calculationWorkList.size(); + int midIndex = size / 2; + + if (size % 2 == 0) { + // Even number of elements: average the two middle elements + return (calculationWorkList.get(midIndex - 1) + calculationWorkList.get(midIndex)) / 2.0; + } else { + // Odd number of elements: return the middle element + return calculationWorkList.get(midIndex); + } + } + + /** + * Computes the Median Absolute Deviation (MAD) for the current distance window, + * relative to a provided median value. + * + * @param median The pre-calculated median of the {@code distanceWindow}. + * @return The Median Absolute Deviation. + */ + private double computeMedianAbsoluteDeviation(double median) { + if (distanceWindow.isEmpty()) { + return 0.0; // Or handle as an error case if appropriate + } + + // Reuse the work list for calculating absolute deviations + calculationWorkList.clear(); + for (double value : distanceWindow) { + calculationWorkList.add(Math.abs(value - median)); + } + + // The MAD is the median of these absolute deviations + Collections.sort(calculationWorkList); // Need to sort the deviations before finding their median + + int size = calculationWorkList.size(); + int midIndex = size / 2; + + if (size % 2 == 0) { + // Even number of elements: average the two middle elements + return (calculationWorkList.get(midIndex - 1) + calculationWorkList.get(midIndex)) / 2.0; + } else { + // Odd number of elements: return the middle element + return calculationWorkList.get(midIndex); + } + } + + /** + * Clears all recorded distance measurements from the internal buffer. + * Resets the detector to its initial state. + */ + public void clearHistory() { + distanceWindow.clear(); + calculationWorkList.clear(); // Also clear the work list + Log.i(TAG, "History cleared."); + } + + /** + * Retrieves a copy of the current list of distance measurements stored in the window. + * + * @return A new {@link List} containing the distances currently in the buffer. + * Modifying this list will not affect the internal state of the detector. + */ + public List getDistanceHistory() { + return new ArrayList<>(distanceWindow); + } + + /** + * Gets the configured capacity of the internal sliding window buffer. + * + * @return The maximum number of measurements the buffer can hold. + */ + public int getBufferCapacity() { + return bufferCapacity; + } + } +} \ No newline at end of file diff --git a/app/src/main/java/com/openpositioning/PositionMe/fusion/MovementModel.java b/app/src/main/java/com/openpositioning/PositionMe/fusion/MovementModel.java new file mode 100644 index 00000000..27433b92 --- /dev/null +++ b/app/src/main/java/com/openpositioning/PositionMe/fusion/MovementModel.java @@ -0,0 +1,278 @@ +// Refactored MovementModel.java +package com.openpositioning.PositionMe.fusion; + +import android.util.Log; +import androidx.annotation.NonNull; // Optional, for clarity + +import java.lang.Math; // Explicit import + +/** + * Models pedestrian movement characteristics for use in position fusion algorithms. + * + * This class provides estimates for movement parameters like bearing uncertainty + * based on turn detection, temporal penalties for increasing uncertainty over time, + * and step error estimation. It also incorporates outlier detection for position deviations. + * The core logic of calculating penalties and uncertainties remains consistent with the + * original implementation, but method names and structure have been revised for clarity. + * + * @author Michal Wiercigroch + */ +public class MovementModel { + + private static final String TAG = MovementModel.class.getSimpleName(); + + /** Enumerates the types of pedestrian movement based on bearing changes. */ + public enum MovementType { + /** Movement generally in a straight line (small bearing change). */ + STRAIGHT, + /** A noticeable but not sharp change in direction. */ + PSEUDO_TURN, + /** A significant change in direction (sharp turn). */ + TURN + } + + // --- Pedestrian Movement Parameter Constants --- + private static final double DEFAULT_AVERAGE_STEP_LENGTH_METERS = 0.7; + private static final double STEP_LENGTH_RELATIVE_ERROR = 0.10; // 10% uncertainty + private static final double STEP_DIRECTION_ABSOLUTE_ERROR_METERS = 0.20; // Base uncertainty in direction projection + + // --- Bearing Uncertainty Constants (Standard Deviations in Radians) --- + private static final double BEARING_STD_DEV_TURN_RAD = Math.toRadians(15.0); + private static final double BEARING_STD_DEV_PSEUDO_TURN_RAD = Math.toRadians(8.0); + private static final double BEARING_STD_DEV_STRAIGHT_RAD = Math.toRadians(2.0); + + // --- Time-Based Uncertainty Constants --- + /** Time duration (ms) over which the temporal penalty factor scales from 1.0 to 1.5. */ + private static final long TEMPORAL_PENALTY_MAX_DURATION_MS = 6000L; // 6 seconds + /** Time duration (ms) over which the bearing penalty scales towards its maximum. */ + private static final long BEARING_PENALTY_MAX_DURATION_MS = 15 * 60 * 1000L; // 15 minutes + /** Maximum additional bearing uncertainty (radians) applied due to elapsed time. */ + private static final double MAX_ADDITIONAL_BEARING_PENALTY_RAD = Math.toRadians(22.5); + + // --- Outlier Detection Constant --- + /** A hard threshold (meters) for position deviations; larger deviations are immediately flagged. */ + private static final double POSITION_DEVIATION_HARD_THRESHOLD_METERS = 10.0; + + // --- State Variables --- + /** Detector for statistical outliers in position deviations. */ + private KalmanFilterFusion.FusionOutlierDetector distanceDeviationChecker; + /** The length (meters) of the previously processed step or a default value. */ + private double lastRecordedStepLengthMeters; + /** Timestamp (milliseconds since epoch) of the last model update. */ + private long lastUpdateTimestampMillis; + + /** + * Constructs a new MovementModel with default settings and an outlier detector. + */ + public MovementModel() { + this.distanceDeviationChecker = new KalmanFilterFusion.FusionOutlierDetector(); // Default window size + this.lastRecordedStepLengthMeters = DEFAULT_AVERAGE_STEP_LENGTH_METERS; + this.lastUpdateTimestampMillis = 0L; + Log.i(TAG, "Initialized MovementModel with default step length: " + + DEFAULT_AVERAGE_STEP_LENGTH_METERS + "m"); + } + + /** + * Records the current time as the last update timestamp. + * Used for calculating time-dependent penalties. + * + * @param timestampMillis The current system time in milliseconds. + */ + public void recordTimestamp(long timestampMillis) { + this.lastUpdateTimestampMillis = timestampMillis; + } + + /** + * Updates the last recorded step length. + * Input validation ensures the step length is positive. + * + * @param stepLengthMeters The calculated step length in meters for the latest step. + */ + public void updateLastStepLength(double stepLengthMeters) { + if (stepLengthMeters > 0) { + this.lastRecordedStepLengthMeters = stepLengthMeters; + } else { + Log.w(TAG, "Attempted to set non-positive step length: " + stepLengthMeters); + // Keep the previous value or default if it was the first attempt + } + } + + /** + * Retrieves the estimated standard deviation of the bearing (orientation) + * based on the detected type of movement. + * + * @param movementType The classified movement type (STRAIGHT, PSEUDO_TURN, TURN). + * @return The standard deviation of bearing uncertainty in radians. + */ + public double getBearingUncertainty(@NonNull MovementType movementType) { + switch (movementType) { + case TURN: + return BEARING_STD_DEV_TURN_RAD; + case PSEUDO_TURN: + return BEARING_STD_DEV_PSEUDO_TURN_RAD; + case STRAIGHT: + return BEARING_STD_DEV_STRAIGHT_RAD; + default: + // Should not happen with enum, but default to highest uncertainty + Log.w(TAG, "Unknown movement type provided: " + movementType + ". Using TURN uncertainty."); + return BEARING_STD_DEV_TURN_RAD; + } + } + + /** + * Calculates a time-dependent penalty factor (>= 1.0) that increases + * uncertainty based on the time elapsed since the last update. + * + * The penalty scales linearly from 1.0 to 1.5 over `TEMPORAL_PENALTY_MAX_DURATION_MS`. + * + * @param currentTimestampMillis The current system time in milliseconds. + * @return A penalty factor (>= 1.0) to scale uncertainty values. + */ + public double computeTemporalPenaltyFactor(long currentTimestampMillis) { + if (lastUpdateTimestampMillis <= 0) { + return 1.0; // No penalty if this is the first update or time hasn't been recorded + } + + long elapsedTimeMillis = currentTimestampMillis - lastUpdateTimestampMillis; + if (elapsedTimeMillis <= 0) { + return 1.0; // No penalty if time hasn't advanced + } + + // Calculate the fraction of the max duration elapsed, capped at 1.0 + double timeFraction = Math.min((double) elapsedTimeMillis / TEMPORAL_PENALTY_MAX_DURATION_MS, 1.0); + + // Linearly interpolate penalty factor from 1.0 (at 0ms) to 1.5 (at max duration) + double penaltyFactor = 1.0 + 0.5 * timeFraction; + + return penaltyFactor; + } + + /** + * Calculates an adjusted bearing uncertainty (standard deviation in radians) + * by adding a time-dependent penalty to a base uncertainty. + * + * The penalty increases linearly with elapsed time, up to a maximum defined by + * `MAX_ADDITIONAL_BEARING_PENALTY_RAD`, over the duration `BEARING_PENALTY_MAX_DURATION_MS`. + * + * @param baseBearingStdDevRad The base bearing standard deviation (e.g., from {@link #getBearingUncertainty}). + * @param currentTimestampMillis The current system time in milliseconds. + * @return The adjusted bearing standard deviation in radians, including the time penalty. + */ + public double computeAdjustedBearingUncertainty(double baseBearingStdDevRad, long currentTimestampMillis) { + if (lastUpdateTimestampMillis <= 0) { + return baseBearingStdDevRad; // No penalty if time hasn't been recorded + } + + long elapsedTimeMillis = currentTimestampMillis - lastUpdateTimestampMillis; + if (elapsedTimeMillis <= 0) { + return baseBearingStdDevRad; // No penalty if time hasn't advanced + } + + // Calculate the fraction of the max duration elapsed for bearing penalty, capped at 1.0 + double timeFraction = Math.min((double) elapsedTimeMillis / BEARING_PENALTY_MAX_DURATION_MS, 1.0); + + // Linearly interpolate the additional penalty from 0 to MAX_ADDITIONAL_BEARING_PENALTY_RAD + double additionalPenalty = MAX_ADDITIONAL_BEARING_PENALTY_RAD * timeFraction; + + // Combine base uncertainty with the time-based additional penalty + // Note: Original code seemed to interpolate *between* base and max, which is less intuitive. + // This interpretation adds penalty *to* the base uncertainty. Let's stick to the *original logic*: + // Linear interpolation between baseStd and maxPenalty = baseStd + (maxPenalty - baseStd) * fraction + // Let's define maxTotalBearingUncertainty = MAX_ADDITIONAL_BEARING_PENALTY_RAD (assuming this was the intended *total max*, not *additional*) + // If MAX_ADDITIONAL_BEARING_PENALTY_RAD was truly meant as *additional*, the calculation would be: + // double adjustedStdDev = baseBearingStdDevRad + additionalPenalty; + + // Replicating original logic: Interpolate between baseStd and a target max value. + // Let's assume MAX_ADDITIONAL_BEARING_PENALTY_RAD defines the upper limit target. + double targetMaxStdDev = MAX_ADDITIONAL_BEARING_PENALTY_RAD; // As per original formula structure + double adjustedStdDev = baseBearingStdDevRad + (targetMaxStdDev - baseBearingStdDevRad) * timeFraction; + + // Ensure the adjusted value doesn't go below the base (can happen if targetMaxStdDev < baseBearingStdDevRad) + return Math.max(adjustedStdDev, baseBearingStdDevRad); + } + + + /** + * Estimates the uncertainty (error) in meters associated with a single step. + * This combines relative step length error and absolute directional error, + * scaled by a temporal penalty factor. + * + * @param averageStepLengthMeters The average or estimated length of the step in meters. + * @param temporalPenaltyFactor The time-dependent penalty factor (from {@link #computeTemporalPenaltyFactor}). + * @return The estimated step error in meters. + */ + public double estimateStepError(double averageStepLengthMeters, double temporalPenaltyFactor) { + // Combine relative error (proportional to step length) and absolute error + double baseError = (STEP_LENGTH_RELATIVE_ERROR * averageStepLengthMeters) + STEP_DIRECTION_ABSOLUTE_ERROR_METERS; + + // Scale the base error by the temporal penalty + return baseError * temporalPenaltyFactor; + } + + /** + * Checks if a given position deviation (distance between expected and measured position) + * is considered an outlier. + * + * It first checks against a hard threshold and then uses the statistical outlier detector. + * + * @param deviationMeters The distance in meters representing the position deviation. + * @return {@code true} if the deviation is considered an outlier, {@code false} otherwise. + */ + public boolean isDeviationAnOutlier(double deviationMeters) { + // 1. Hard Threshold Check + if (deviationMeters > POSITION_DEVIATION_HARD_THRESHOLD_METERS) { + Log.d(TAG, String.format("Deviation %.2fm exceeds hard threshold %.2fm", + deviationMeters, POSITION_DEVIATION_HARD_THRESHOLD_METERS)); + return true; + } + + // 2. Statistical Check using FusionOutlierDetector + // The internal detector maintains its own history of deviations evaluated by this method. + return distanceDeviationChecker.evaluateDistance(deviationMeters); + } + + /** + * Gets the default average step length used by the model. + * + * @return The default step length in meters. + */ + public double getDefaultStepLength() { + return DEFAULT_AVERAGE_STEP_LENGTH_METERS; + } + + /** + * Gets the last recorded step length. + * + * @return The last step length in meters updated via {@link #updateLastStepLength}. + */ + public double getLastRecordedStepLength() { + return lastRecordedStepLengthMeters; + } + + /** + * Gets the timestamp of the last model update. + * + * @return The last update timestamp in milliseconds since epoch. + */ + public long getLastUpdateTimestamp() { + return lastUpdateTimestampMillis; + } + + /** + * Resets the movement model state to its initial default values. + * This includes resetting the last step length, the update timestamp, + * and clearing the history of the internal outlier detector. + */ + public void resetState() { + this.lastRecordedStepLengthMeters = DEFAULT_AVERAGE_STEP_LENGTH_METERS; + this.lastUpdateTimestampMillis = 0L; + // Reset the outlier detector by creating a new instance or clearing history + // Re-creating ensures complete reset, including buffer size if constructor changes + this.distanceDeviationChecker = new KalmanFilterFusion.FusionOutlierDetector(); + // Alternatively, if detector state needs preservation across resets (unlikely): + // if (distanceDeviationChecker != null) { + // distanceDeviationChecker.clearHistory(); + // } + Log.i(TAG, "MovementModel state reset."); + } +} \ No newline at end of file diff --git a/app/src/main/java/com/openpositioning/PositionMe/fusion/TrajectoryFilter.java b/app/src/main/java/com/openpositioning/PositionMe/fusion/TrajectoryFilter.java new file mode 100644 index 00000000..f5975a12 --- /dev/null +++ b/app/src/main/java/com/openpositioning/PositionMe/fusion/TrajectoryFilter.java @@ -0,0 +1,374 @@ +package com.openpositioning.PositionMe.fusion; + +import java.util.ArrayList; +import java.util.List; +import com.google.android.gms.maps.model.*; +import com.openpositioning.PositionMe.utils.CoordinateConverter; + +import java.util.Queue; +import java.util.LinkedList; +import java.util.stream.Collectors; +import java.util.stream.Stream; + +import org.apache.commons.math3.analysis.interpolation.LinearInterpolator; +import org.ejml.simple.SimpleMatrix; +import org.apache.commons.math3.analysis.interpolation.SplineInterpolator; +import org.apache.commons.math3.analysis.polynomials.PolynomialSplineFunction; + +/** + * Provides methods to smooth coordinate data for pedestrian tracking. + * + *

TrajectoryFilter smooths trajectory data using the Visvalingam-Whyatt algorithm + * and Catmull-Rom spline interpolation. It handles the simultaneous smoothing of x and y coordinates + * for pedestrian movement tracking applications. + * + * @author Nick Manturov + */ +public class TrajectoryFilter { + + // Represents a point in 2D space + static class Point { + double x, y; + + Point(double x, double y) { + this.x = x; + this.y = y; + } + } + + private static final double FRACTION_RETAIN = 0.5; + public static final int RAW_WINDOW_SIZE = 10; + public static final int INTERPOLATION_FACTOR = 10; + public static final double TENSION = 0.6; + + private final List fusionBufferEnu; + private final List fusionBuffer; + private final List reducedBufferEnu; + private final List interpolatedBuffer; + private int count; + + /** + * Creates a Moving Average filter for smoothing pedestrian positioning data. + */ + public TrajectoryFilter() { + + this.fusionBufferEnu = new ArrayList<>(); + this.fusionBuffer = new ArrayList<>(); + this.reducedBufferEnu = new ArrayList<>(); + this.interpolatedBuffer = new ArrayList<>(); + this.count = 0; + } + + /** + * Process new positioning data (step length and heading angle). + * + * @param rawPoint Unfiltered new LatLng point + * @param referencePosition Reference position for LatLng to ENU conversion + * @return Filtered [east, north] or null if window isn't filled yet + */ + public List processData(LatLng rawPoint, double[] referencePosition) { + + fusionBuffer.add(rawPoint); + + double[] enu = CoordinateConverter.convertGeodeticToEnu(rawPoint.latitude, rawPoint.longitude, referencePosition[2], + referencePosition[0], referencePosition[1], referencePosition[2]); + fusionBufferEnu.add(new Point(enu[0], enu[1])); + + int pointsRetained = (int) Math.round(fusionBufferEnu.size()*FRACTION_RETAIN); + + if (pointsRetained < 5) { + return fusionBuffer; + } + + if (fusionBufferEnu.size() >= RAW_WINDOW_SIZE) { + List dataArray = fusionBufferEnu.subList(count, count+RAW_WINDOW_SIZE); + List reducedPolyline = simplifyPolyline(dataArray, pointsRetained); + + int numPrevious = Math.min(INTERPOLATION_FACTOR, reducedBufferEnu.size()); + List EnuBufferSublist = reducedBufferEnu.subList(reducedBufferEnu.size()-numPrevious, reducedBufferEnu.size()); + List extendedPolyline = Stream.concat(EnuBufferSublist.stream(), reducedPolyline.stream()) + .collect(Collectors.toList()); + reducedBufferEnu.add(reducedPolyline.get(0)); + + List interpolatedPolyline = interpolateNaturalMovement(extendedPolyline); + + List geoPolyline = new ArrayList<>(); + for (Point point : interpolatedPolyline) { + geoPolyline.add(CoordinateConverter.convertEnuToGeodetic(point.x, point.y, 0, + referencePosition[0], referencePosition[1], referencePosition[2])); + } + + List result = Stream.concat(interpolatedBuffer.stream(), geoPolyline.stream()) + .collect(Collectors.toList()); + interpolatedBuffer.addAll(geoPolyline.subList(0, INTERPOLATION_FACTOR)); + count++; + return result; + } + else { + List reducedPolyline = simplifyPolyline(fusionBufferEnu, pointsRetained); + + List geoPolyline = new ArrayList<>(); + for (Point point : reducedPolyline) { + geoPolyline.add(CoordinateConverter.convertEnuToGeodetic(point.x, point.y, 0, + referencePosition[0], referencePosition[1], referencePosition[2])); + } + return geoPolyline; + } + } + + /** + * Calculates the area of a triangle formed by three points. + * + * @param p1 First point of the triangle + * @param p2 Second point of the triangle + * @param p3 Third point of the triangle + * @return The area of the triangle + */ + public static double calculateTriangleArea(Point p1, Point p2, Point p3) { + return 0.5 * Math.abs(p1.x * (p2.y - p3.y) + p2.x * (p3.y - p1.y) + p3.x * (p1.y - p2.y)); + } + + /** + * Simplifies a polyline using the Visvalingam-Whyatt algorithm. + * + * @param points List of points to simplify + * @param targetPointCount Target number of points to retain + * @return A simplified list of points + */ + public static List simplifyPolyline(List points, int targetPointCount) { + if (points.size() <= targetPointCount) { + return new ArrayList<>(points); // No simplification needed if already under target + } + + // List to hold the simplified points and the areas + List simplifiedPoints = new ArrayList<>(points); + List areas = new ArrayList<>(); + + // Calculate the area for each triangle formed by three consecutive points + // We can only calculate areas for internal points (not endpoints) + for (int i = 1; i < simplifiedPoints.size() - 1; i++) { + double area = calculateTriangleArea( + simplifiedPoints.get(i - 1), + simplifiedPoints.get(i), + simplifiedPoints.get(i + 1) + ); + areas.add(area); + } + + // Remove points iteratively based on the smallest area + while (simplifiedPoints.size() > targetPointCount) { + // Find the index with the smallest area + int minAreaIndex = findMinAreaIndex(areas); + + // The actual point index to remove is minAreaIndex + 1 because: + // - areas[0] corresponds to point[1] + // - areas[n] corresponds to point[n+1] + int pointIndexToRemove = minAreaIndex + 1; + + // Remove the point + simplifiedPoints.remove(pointIndexToRemove); + + // Remove the corresponding area + areas.remove(minAreaIndex); + + // Update the affected areas + // If we removed a point other than the second point + if (minAreaIndex > 0) { + // Update the area before the removed point + double newArea = calculateTriangleArea( + simplifiedPoints.get(minAreaIndex - 1), + simplifiedPoints.get(minAreaIndex), + simplifiedPoints.get(minAreaIndex + 1) + ); + areas.set(minAreaIndex - 1, newArea); + } + + // If we have a point after the removed one that isn't the last point + if (minAreaIndex < areas.size()) { + // Update the area after the removed point + double newArea = calculateTriangleArea( + simplifiedPoints.get(minAreaIndex), + simplifiedPoints.get(minAreaIndex + 1), + minAreaIndex + 2 < simplifiedPoints.size() ? simplifiedPoints.get(minAreaIndex + 2) : null + ); + + // Check if we calculated a valid area (we might be at the end) + if (minAreaIndex + 2 < simplifiedPoints.size()) { + areas.set(minAreaIndex, newArea); + } + } + } + + return simplifiedPoints; + } + + /** + * Helper method to find the index of the minimum area in the list. + * + * @param areas List of triangle areas + * @return Index of the minimum area value + */ + private static int findMinAreaIndex(List areas) { + if (areas.isEmpty()) { + return -1; // Handle empty list case + } + + int minIndex = 0; + double minValue = areas.get(0); + + for (int i = 1; i < areas.size(); i++) { + if (areas.get(i) < minValue) { + minValue = areas.get(i); + minIndex = i; + } + } + return minIndex; + } + + + /** + * Interpolates points using Catmull-Rom splines to create natural, smooth curves + * that round out sharp turns while preserving the original path's character. + * + * @param reducedPoints The original points to interpolate + * @return A list of smoothly interpolated points + */ + public static List interpolateNaturalMovement(List reducedPoints) { + int n = reducedPoints.size(); + + // Need at least 2 points for any interpolation + if (n < 2) { + return new ArrayList<>(reducedPoints); + } + + // For just 2 points, use linear interpolation + if (n == 2) { + return interpolateLinearSegment(reducedPoints.get(0), reducedPoints.get(1), INTERPOLATION_FACTOR); + } + + ArrayList interpolatedPoints = new ArrayList<>(); + + // Create extended points list with duplicated endpoints to handle boundary conditions + List extendedPoints = new ArrayList<>(); + + // Duplicate first point to serve as "pre-first" control point + extendedPoints.add(createExtrapolatedPoint(reducedPoints.get(0), reducedPoints.get(1), true)); + // Add all original points + extendedPoints.addAll(reducedPoints); + // Duplicate last point to serve as "post-last" control point + extendedPoints.add(createExtrapolatedPoint(reducedPoints.get(n-2), reducedPoints.get(n-1), false)); + + // Define tension parameter (0.5 is standard for Catmull-Rom) + // Lower values create tighter curves, higher values create looser curves + double tension = TENSION; + + // Iterate through all segments + for (int i = 0; i < n - 1; i++) { + Point p0 = extendedPoints.get(i); // First control point + Point p1 = extendedPoints.get(i + 1); // Start point of segment + Point p2 = extendedPoints.get(i + 2); // End point of segment + Point p3 = extendedPoints.get(i + 3); // Last control point + + // Number of steps to interpolate for this segment + int steps = INTERPOLATION_FACTOR; + + // Add the start point + if (i == 0) { + interpolatedPoints.add(new Point(p1.x, p1.y)); + } + + // Generate points along the spline for this segment + for (int step = 1; step <= steps; step++) { + double t = (double) step / steps; + + // Catmull-Rom spline formula + double x = catmullRomInterpolate(p0.x, p1.x, p2.x, p3.x, t, tension); + double y = catmullRomInterpolate(p0.y, p1.y, p2.y, p3.y, t, tension); + + interpolatedPoints.add(new Point(x, y)); + } + } + + return interpolatedPoints; + } + + /** + * Calculates a point value using Catmull-Rom interpolation formula. + * + * @param p0 First control point + * @param p1 Second control point (curve start) + * @param p2 Third control point (curve end) + * @param p3 Fourth control point + * @param t Parameter between 0 and 1 + * @param tension Tension parameter controlling curve tightness + * @return The interpolated value + */ + private static double catmullRomInterpolate(double p0, double p1, double p2, double p3, double t, double tension) { + double t2 = t * t; + double t3 = t2 * t; + + // Catmull-Rom matrix coefficients + double a = -tension * p0 + (2 - tension) * p1 + (tension - 2) * p2 + tension * p3; + double b = 2 * tension * p0 + (tension - 3) * p1 + (3 - 2 * tension) * p2 - tension * p3; + double c = -tension * p0 + tension * p2; + double d = p1; + + return a * t3 + b * t2 + c * t + d; + } + + /** + * Creates an extrapolated control point beyond the endpoints. + * + * @param p1 First existing point + * @param p2 Second existing point + * @param isFirst True if creating a point before the sequence, false for after + * @return A new extrapolated control point + */ + private static Point createExtrapolatedPoint(Point p1, Point p2, boolean isFirst) { + // For first point, extrapolate backwards; for last point, extrapolate forwards + double dx = p2.x - p1.x; + double dy = p2.y - p1.y; + + if (isFirst) { + // Place extrapolated point before the first point + return new Point(p1.x - dx, p1.y - dy); + } else { + // Place extrapolated point after the last point + return new Point(p2.x + dx, p2.y + dy); + } + } + + /** + * Simple linear interpolation between two points. + * + * @param start Starting point + * @param end Ending point + * @param steps Number of steps to interpolate + * @return List of interpolated points + */ + private static List interpolateLinearSegment(Point start, Point end, int steps) { + List result = new ArrayList<>(); + result.add(start); + + for (int i = 1; i < steps; i++) { + double t = (double) i / steps; + double x = start.x + t * (end.x - start.x); + double y = start.y + t * (end.y - start.y); + result.add(new Point(x, y)); + } + + result.add(end); + return result; + } + + /** + * Resets the filter's data window. + */ + public void reset() { + fusionBufferEnu.clear(); + fusionBuffer.clear(); + reducedBufferEnu.clear(); + interpolatedBuffer.clear(); + count = 0; + } +} \ No newline at end of file diff --git a/app/src/main/java/com/openpositioning/PositionMe/fusion/particle/Particle.java b/app/src/main/java/com/openpositioning/PositionMe/fusion/particle/Particle.java new file mode 100644 index 00000000..36ac8d63 --- /dev/null +++ b/app/src/main/java/com/openpositioning/PositionMe/fusion/particle/Particle.java @@ -0,0 +1,73 @@ +package com.openpositioning.PositionMe.fusion.particle; + +import com.openpositioning.PositionMe.utils.WallDetectionManager; + +import org.apache.commons.math3.distribution.TDistribution; + +import java.util.Random; + +/** + * Represents a particle for use in the particle filter algorithm. + * + *

Each particle maintains a position (x,y), orientation (theta) and weight. + * Particles are updated based on sensor measurements and weighted according to + * their likelihood of representing the true position. + * + * @author Nick Manturov + */ +public class Particle { + Double x, y, theta; // Position and orientation of the particle + Double weight; // Weight assigned to each particle + Double logWeight; // Log-weight for numerical stability + + /** + * Creates a new particle with specified position and orientation. + * + * @param x x-coordinate position + * @param y y-coordinate position + * @param theta orientation angle in radians + */ + public Particle(double x, double y, double theta) { + this.x = x; + this.y = y; + this.theta = theta; + + this.weight = 1.0; + this.logWeight = 0.0; + } + + /** + * Updates particle position when step is detected. + * + *

Applies noise to the heading and step length, then moves the particle. + * Checks for wall collisions if a wall detector is provided. + * + * @param stepLength detected step length in meters + * @param currentHeading current heading angle in radians + * @param dynamicPDRStds standard deviations for PDR noise model + * @param wallChecker wall detection manager for collision checking + * @param buildingType building identifier + * @param floor floor number + */ + public void updateDynamic(double stepLength, double currentHeading, double[] dynamicPDRStds, WallDetectionManager wallChecker, int buildingType, int floor) { + Random rand = new Random(); + + // Add noise to heading change + this.theta = currentHeading + rand.nextGaussian() * dynamicPDRStds[1]; + stepLength += rand.nextGaussian() * dynamicPDRStds[0]; + + // Update position using angle + double dx = stepLength * Math.cos(this.theta); + double dy = stepLength * Math.sin(this.theta); + + if (wallChecker != null) { + boolean collision = wallChecker.doesTrajectoryIntersectWall(new double[] {this.x, this.y}, new double[] {this.x+dx, this.y+dy}, buildingType, floor); + this.weight = collision ? 0.0 : this.weight; + this.logWeight = collision ? Math.log(0.0) : this.logWeight; + } + + this.x += dx; + this.y += dy; + } + +} \ No newline at end of file diff --git a/app/src/main/java/com/openpositioning/PositionMe/fusion/particle/ParticleFilterFusion.java b/app/src/main/java/com/openpositioning/PositionMe/fusion/particle/ParticleFilterFusion.java new file mode 100644 index 00000000..1d2d5045 --- /dev/null +++ b/app/src/main/java/com/openpositioning/PositionMe/fusion/particle/ParticleFilterFusion.java @@ -0,0 +1,1073 @@ +package com.openpositioning.PositionMe.fusion.particle; + +import android.content.Context; +import android.util.Log; + +import com.google.android.gms.maps.model.LatLng; +import com.openpositioning.PositionMe.fusion.IPositionFusionAlgorithm; +import com.openpositioning.PositionMe.utils.CoordinateConverter; +import com.openpositioning.PositionMe.utils.PdrProcessing; +import com.openpositioning.PositionMe.utils.WallDetectionManager; + +import org.apache.commons.math3.distribution.TDistribution; +import org.ejml.simple.SimpleMatrix; + +import java.util.ArrayList; +import java.util.LinkedList; +import java.util.List; +import java.util.Queue; +import java.util.Random; +import java.util.Timer; +import java.util.TimerTask; + +/** + * Particle filter implementation for sensor fusion in indoor positioning. + * + *

This class implements a particle filter to fuse GPS, WiFi, and PDR (Pedestrian Dead Reckoning) + * data for indoor positioning. It maintains a set of particles representing position hypotheses, + * updates them based on sensor measurements, and provides the estimated position as a weighted + * average of particle positions. + * + * @author Nick Manturov + */ + +public class ParticleFilterFusion extends IPositionFusionAlgorithm { + private static final String TAG = "ParticleFilterFusion"; + + private static final int STATE_SIZE = 3; // Total state vector size + private static final int MEAS_SIZE = 2; // Total measurement vector size + + // Process noise scale (PDR) update + private static final double[] dynamicPdrStds = {0.2, Math.PI/12}; // std of step length, std of heading angle + private static final double[] staticPdrStds = {1.0, 1.0, 0}; // std of x, y, heading angle (theta) + private SimpleMatrix measGnssMat; // GNSS Measurement covariance + private SimpleMatrix measWifiMat; // WiFi Measurement covariance + private SimpleMatrix forwardModel; // Forward Model Matrix + + // Adjustable GNSS noise based on accuracy + private static final double BASE_GNSS_NOISE = 20.0; // Base measurement noise for GNSS (in meters) + private static final double BASE_WIFI_NOISE = 5.0; // Measurement noise for WiFi (in meters) + private static final double UNCERT_INC_PS = 2.0; // Incremental uncertainty increase WiFi + + // Outlier detection parameters + private static final double OUTLIER_THRESHOLD = 3.0; // Number of standard deviations for outlier detection + private static final double OUTPUT_OUTLIER_THRESHOLD = 2.5; // Slightly stricter threshold for output + + // The reference point for ENU coordinates + private double[] referencePosition; // [lat, lng, alt] + private boolean referenceInitialized; // Flag to track if reference is properly set + private boolean headingInitialized; // Flag to track if particle heading was initialized + + // Particle parameters + private List particles; // List of particles + private int numParticles; // Number of particles + + // Position history for outlier detection + private List gnssPositionHistory; // Position History of GNSS measurements + private List wifiPositionHistory; // Position History of WiFi measurements + private List fusionPositionHistory; // Position History of fusion measurements (outlier detection) + private static final int HISTORY_SIZE = 5; // Memory Length position history + + private static final int OUTPUT_HISTORY_SIZE = 7; // Slightly larger window for better statistics + + // Movement state tracking + private long lastUpdateTime; + private static final long STATIC_TIMEOUT_MS = 1000; // 1 seconds without updates = static update + private Timer updateTimer; // Timer for regular updates + private static final long UPDATE_INTERVAL_MS = 1000; // Update every 1 second + + // First position values to handle initialization correctly + private LatLng firstGnssPosition; + private LatLng firstWifiPosition; + private LatLng prevFusionPos; + private boolean hasInitialGnssPosition; + private boolean hasInitialWifiPosition; + private boolean particlesInitialized; + private boolean wallDetectorInitialized; + private boolean timerInitialized; + private boolean dynamicUpdate; + private boolean updateReady; + + // Initialize PDR parameters + private double previousHeading; + private double currentHeading; + private double[] previousPdrPos; + private double[] currentPdrPos; + private double enuAltitude; + private double latlngAltitude; + + // Initialize WiFi and GNSS parameters + private double[] currentGNSSpositionEMU; + private SimpleMatrix gnssMeasurementVector; + private double[] currentWiFipositionEMU; + private SimpleMatrix wifiMeasurementVector; + + private PdrProcessing.ElevationDirection elevationDirection; + private WallDetectionManager wallDetectionManager; // Wall detection - currently disabled + private Context FragmentContext; // Context to load Indoor Map images - currently disabled + private int buildingType; // 1 - Nucleus, 2 - Library, 0 - None + private int currentFloor; // WiFi floor estimate + private boolean buildingTypeInitialized; + + // Added to track the last valid output position + private double[] lastValidOutputPosition; + + // Constructor + public ParticleFilterFusion(int numParticles, double[] referencePosition) { + // Initialize particle array + this.numParticles = numParticles; + this.particles = new ArrayList<>(); + + // Initialize moving average history + this.gnssPositionHistory = new ArrayList<>(); + this.wifiPositionHistory = new ArrayList<>(); + this.fusionPositionHistory = new ArrayList<>(); + + // Initialize reference position + this.referencePosition = referencePosition.clone(); // Clone to prevent modification + this.referenceInitialized = (referencePosition[0] != 0 || referencePosition[1] != 0); + this.elevationDirection = PdrProcessing.ElevationDirection.NEUTRAL; + + // Initialize first position tracking + hasInitialGnssPosition = false; + particlesInitialized = false; + dynamicUpdate = false; + buildingTypeInitialized = false; + wallDetectorInitialized = false; + updateReady = false; + lastUpdateTime = System.currentTimeMillis(); + + // Initialize heading and position estimates + previousHeading = 0.0; + currentHeading = 0.0; + + previousPdrPos = new double[]{0.0, 0.0}; + currentPdrPos = new double[]{0.0, 0.0}; + + currentGNSSpositionEMU = new double[]{0.0, 0.0}; + currentWiFipositionEMU = new double[]{0.0, 0.0}; + + // Initialize the last valid output position + lastValidOutputPosition = new double[]{0.0, 0.0, 0.0}; + + // Set up measurement models + forwardModel = new SimpleMatrix(MEAS_SIZE, STATE_SIZE); + forwardModel.set(0, 0, 1.0); // Measure east position + forwardModel.set(1, 1, 1.0); // Measure north position + + measGnssMat = SimpleMatrix.identity(MEAS_SIZE); + measGnssMat = measGnssMat.scale(Math.pow(BASE_GNSS_NOISE, 2.0)); + + measWifiMat = SimpleMatrix.identity(MEAS_SIZE); + measWifiMat = measWifiMat.scale(Math.pow(BASE_WIFI_NOISE, 2.0)); + + } + + /** + * Static Update function - called after a 1000 ms interval if PDR step is not detected, + * updates particles following the static noise model, + * reweighs and resamples particles + */ + @Override + public void staticUpdate() { + + // Check if particles are initialized + if (!particlesInitialized) { + return; + } + + long currentTime = System.currentTimeMillis(); + + // Check static update needed + Log.d(TAG, "Performing static update with timer"); + + // Apply static update to particles + moveParticlesStatic(staticPdrStds); + + // Reweight particles with available measurements + // Executed only if there was no PDR movement for 1000ms + if (currentTime - lastUpdateTime > 1000) { + // Gaussian Mixture Measurement Model + // For each GNSS Measurement Position - Calculate Covariance, add logProbability to logWeight of each particle + if (currentGNSSpositionEMU != null) { + gnssMeasurementVector = new SimpleMatrix(MEAS_SIZE, 1); + for (int i = 0; i < gnssPositionHistory.size(); i++) { + double[] dummyPos = gnssPositionHistory.get(i); + gnssMeasurementVector.set(0, 0, dummyPos[0]); + gnssMeasurementVector.set(1, 0, dummyPos[1]); + + // Increase estimate variance with time + int j = gnssPositionHistory.size() - 1 - i; + SimpleMatrix effMeasGnssMat = new SimpleMatrix(MEAS_SIZE, MEAS_SIZE); + effMeasGnssMat.set(0, 0, Math.pow(BASE_GNSS_NOISE + j * UNCERT_INC_PS, 2)); + effMeasGnssMat.set(1, 1, Math.pow(BASE_GNSS_NOISE + j * UNCERT_INC_PS, 2)); + + reweightParticles(effMeasGnssMat, forwardModel, gnssMeasurementVector); + } + } + // For each WiFi Measurement Position - Calculate Covariance, add logProbability to logWeight of each particle + if (currentWiFipositionEMU != null) { + wifiMeasurementVector = new SimpleMatrix(MEAS_SIZE, 1); + for (int i = 0; i < wifiPositionHistory.size(); i++) { + double[] dummyPos = wifiPositionHistory.get(i); + wifiMeasurementVector.set(0, 0, dummyPos[0]); + wifiMeasurementVector.set(1, 0, dummyPos[1]); + + // Increase estimate variance with time + int j = wifiPositionHistory.size() - 1 - i; + SimpleMatrix effMeasWifiMat = new SimpleMatrix(MEAS_SIZE, MEAS_SIZE); + effMeasWifiMat.set(0, 0, Math.pow(BASE_WIFI_NOISE + j * UNCERT_INC_PS, 2)); + effMeasWifiMat.set(1, 1, Math.pow(BASE_WIFI_NOISE + j * UNCERT_INC_PS, 2)); + + reweightParticles(effMeasWifiMat, forwardModel, wifiMeasurementVector); + } + } + } + + // Resample the particles + resampleParticles(); + + // Store latest update time + lastUpdateTime = currentTime; + + } + + /** + * PDR update processor, follows dynamic update model using the heading and step length + * + * @param eastMeters The east position in meters relative to the reference point + * @param northMeters The north position in meters relative to the reference point + * @param altitude The altitude in meters + */ + @Override + public void processPdrUpdate(float eastMeters, float northMeters, float altitude) { + long currentTime = System.currentTimeMillis(); + + // Handle reference position and particles if not initialized + if (!referenceInitialized && hasInitialGnssPosition) { + initializeReferencePosition(); + } + /* // Wall Detection Initialization - Disabled due to very high computational expense + if (!wallDetectorInitialized || ! referenceInitialized) { + this.wallDetectionManager = new WallDetectionManager(referencePosition); + + if (FragmentContext != null) { + this.wallDetectionManager.initializeWallData(this.FragmentContext); + Log.d(TAG, "Wall data initialized at" + this.FragmentContext); + } + + wallDetectorInitialized = true; + } */ + + // If reference still not initialized cannot process + if (!referenceInitialized) { + Log.w(TAG, "Skipping PDR update: reference position not initialized"); + return; + } + + // Skip if initial particle position was not found yet + if (!particlesInitialized) { + return; + } + + currentPdrPos[0] = eastMeters; + currentPdrPos[1] = northMeters; + + // Calculate PDR movement and speed + double dx = currentPdrPos[0] - previousPdrPos[0]; + double dy = currentPdrPos[1] - previousPdrPos[1]; + double stepLength = 0; + if (this.elevationDirection != PdrProcessing.ElevationDirection.NEUTRAL){ + stepLength = 0.25; + dynamicPdrStds[0] = 0.05; + }else { + stepLength = Math.sqrt(dx * dx + dy * dy); + dynamicPdrStds[0] = 0.2 + 0.1*stepLength; + } + currentHeading = Math.atan2(dy, dx); + if (!headingInitialized) { + initializeHeading(); + } + + // Generate dynamic particle movement + moveParticlesDynamic(stepLength, currentHeading, dynamicPdrStds, null, buildingType, currentFloor); + + // Gaussian Mixture Measurement Model + // For each GNSS Measurement Position - Calculate Covariance, add logProbability to logWeight of each particle + if (currentGNSSpositionEMU != null) { + gnssMeasurementVector = new SimpleMatrix(MEAS_SIZE, 1); + for (int i = 0; i < gnssPositionHistory.size(); i++) { + double[] dummyPos = gnssPositionHistory.get(i); + gnssMeasurementVector.set(0, 0, dummyPos[0]); + gnssMeasurementVector.set(1, 0, dummyPos[1]); + + // Increase estimate variance with time + int j = gnssPositionHistory.size() - 1 - i; + SimpleMatrix effMeasGnssMat = new SimpleMatrix(MEAS_SIZE, MEAS_SIZE); + effMeasGnssMat.set(0, 0, Math.pow(BASE_GNSS_NOISE + j*UNCERT_INC_PS, 2)); + effMeasGnssMat.set(1, 1, Math.pow(BASE_GNSS_NOISE + j*UNCERT_INC_PS, 2)); + + reweightParticles(effMeasGnssMat, forwardModel, gnssMeasurementVector); + } + } + + // For each WiFi Measurement Position - Calculate Covariance, add logProbability to logWeight of each particle + if (currentWiFipositionEMU != null) { + wifiMeasurementVector = new SimpleMatrix(MEAS_SIZE, 1); + for (int i = 0; i < wifiPositionHistory.size(); i++) { + double[] dummyPos = wifiPositionHistory.get(i); + wifiMeasurementVector.set(0, 0, dummyPos[0]); + wifiMeasurementVector.set(1, 0, dummyPos[1]); + + // Increase estimated variance with time + int j = wifiPositionHistory.size() - 1 - i; + SimpleMatrix effMeasWifiMat = new SimpleMatrix(MEAS_SIZE, MEAS_SIZE); + effMeasWifiMat.set(0, 0, Math.pow(BASE_WIFI_NOISE + j*UNCERT_INC_PS, 2)); + effMeasWifiMat.set(1, 1, Math.pow(BASE_WIFI_NOISE + j*UNCERT_INC_PS, 2)); + + reweightParticles(effMeasWifiMat, forwardModel, wifiMeasurementVector); + } + } + + resampleParticles(); + + previousHeading = currentHeading; + previousPdrPos = currentPdrPos.clone(); + + // Update last update time + lastUpdateTime = currentTime; + } + + /** + * + * @param position The GNSS position (latitude, longitude) + * @param altitude The altitude in meters + */ + @Override + public void processGnssUpdate(LatLng position, double altitude) { + // Store first GNSS position for reference initialization if needed + if (!hasInitialGnssPosition) { + firstGnssPosition = position; + hasInitialGnssPosition = true; + + // Try to initialize reference position if needed + if (!referenceInitialized) { + initializeReferencePosition(); + } + } + + // If reference still not initialized, we can't process + if (!referenceInitialized) { + Log.w(TAG, "Skipping GNSS update: reference position not initialized"); + return; + } + + latlngAltitude = altitude; + + // Initialize particles if not yet initialized + if (!particlesInitialized) { + initializeParticles(); + } + /* + if (!wallDetectorInitialized || ! referenceInitialized) { + this.wallDetectionManager = new WallDetectionManager(referencePosition); + + if (FragmentContext != null) { + this.wallDetectionManager.initializeWallData(this.FragmentContext); + Log.d(TAG, "Wall data initialized at" + this.FragmentContext); + } + + wallDetectorInitialized = true; + }*/ + + Random rand = new Random(); + + // Convert GNSS position to ENU (using the reference position) + double[] enu = CoordinateConverter.convertGeodeticToEnu( + position.latitude, position.longitude, latlngAltitude, + referencePosition[0], referencePosition[1], referencePosition[2] + ); + + //if (wallDetectionManager != null) { + // setBuildingType(new double[] {enu[0], enu[1]}); + // buildingTypeInitialized = true; + //} + + currentGNSSpositionEMU[0] = enu[0]; + currentGNSSpositionEMU[1] = enu[1]; + enuAltitude = enu[2]; + + double[] currMovAvg = getMovingAvgGnss(); + double[] stdMovAvg = new double[] {0.0, 0.0}; + if (gnssPositionHistory.size() > 3) { + stdMovAvg = gnssEstimateStd(currMovAvg); + stdMovAvg[0] += BASE_GNSS_NOISE; + stdMovAvg[1] += BASE_GNSS_NOISE; + + // Perform outlier detection + if (outlierDetection(currentGNSSpositionEMU, stdMovAvg, currMovAvg)) { + Log.w(TAG, "GNSS outlier detected and ignored"); + return; + } + } else { + stdMovAvg[0] = BASE_GNSS_NOISE; + stdMovAvg[1] = BASE_GNSS_NOISE; + } + + // Update the moving average + updateMovingAvgGnss(currentGNSSpositionEMU); + } + + /** + * + * @param position The WiFi position in LatLng format (latitude, longitude) + * @param floor The floor index + */ + @Override + public void processWifiUpdate(LatLng position, int floor) { + if (!hasInitialGnssPosition) { + firstWifiPosition = position; + hasInitialWifiPosition = true; + // Try to initialize reference position if needed + if (!referenceInitialized) { + initializeReferencePosition(); + } + } + + // If reference still not initialized, we can't process + if (!referenceInitialized) { + Log.w(TAG, "Skipping WiFi update: reference position not initialized"); + return; + } + + currentFloor = floor; + hasInitialWifiPosition = true; + /* + if (!wallDetectorInitialized || !referenceInitialized) { + this.wallDetectionManager = new WallDetectionManager(referencePosition); + + if (FragmentContext != null) { + this.wallDetectionManager.initializeWallData(this.FragmentContext); + Log.d(TAG, "Wall data initialized at" + this.FragmentContext); + } + + wallDetectorInitialized = true; + } */ + + Random rand = new Random(); + + // Convert GNSS position to ENU (using the reference position) + double[] enu = CoordinateConverter.convertGeodeticToEnu( + position.latitude, position.longitude, 78, + referencePosition[0], referencePosition[1], referencePosition[2] + ); + + currentWiFipositionEMU[0] = enu[0]; + currentWiFipositionEMU[1] = enu[1]; + enuAltitude = enu[2]; + + double[] currMovAvg = getMovingAvgWifi(); + double[] stdMovAvg = new double[] {0.0, 0.0}; + if (wifiPositionHistory.size() > 3) { + stdMovAvg = wifiEstimateStd(currMovAvg); + stdMovAvg[0] += BASE_WIFI_NOISE; + stdMovAvg[1] += BASE_WIFI_NOISE; + + // Perform outlier detection + if (outlierDetection(currentWiFipositionEMU, stdMovAvg, currMovAvg)) { + Log.w(TAG, "WiFi outlier detected and ignored"); + return; + } + } + + // Update the moving average + updateMovingAvgWifi(currentWiFipositionEMU); + + // Update last update time + } + + /** + * Method to get the resulting fused position + * @return result - Fused Position in LatLng format + */ + public LatLng getFusedPosition() { + if (!referenceInitialized) { + if (hasInitialGnssPosition) { + Log.w(TAG, "Using initial GNSS position as fusion result (reference not initialized)"); + return firstGnssPosition; + } else { + Log.e(TAG, "Cannot get fused position: no reference position and no GNSS position"); + return null; + } + } + + double[] currentPosition = getEstimatedPosition(); + + // If we haven't received any updates yet, return the reference position + if (currentPosition[0] == 0 && currentPosition[1] == 0) { + Log.d(TAG, "Returning reference position as fusion result (no updates yet)"); + return new LatLng(referencePosition[0], referencePosition[1]); + } + + // Check if there is enough history for outlier detection + double[] positionToUse = currentPosition; + if (fusionPositionHistory.size() > 3) { + double[] movingAvg = getMovingAvgFusion(); + double[] stdev = fusionEstimateStd(movingAvg); + + // Perform outlier detection on the fusion position + if (outlierDetectionOutput(currentPosition, stdev, movingAvg)) { + Log.w(TAG, "Fusion position outlier detected - using last valid position"); + positionToUse = lastValidOutputPosition; + } else { + // Update last valid position if this one is good + lastValidOutputPosition = currentPosition.clone(); + } + } else { + // Not enough history yet, trust this position + lastValidOutputPosition = currentPosition.clone(); + } + + // Update the position history for future outlier detection + updateMovingAvgFusion(currentPosition); + + // Check if building was changed during propagation + //if (avgPosition != null && this.wallDetectionManager != null) { + // setBuildingType(new double[] {avgPosition[0], avgPosition[1]}); + //} + + // Convert ENU back to latitude/longitude + LatLng result = CoordinateConverter.convertEnuToGeodetic( + positionToUse[0], positionToUse[1], enuAltitude, + referencePosition[0], referencePosition[1], referencePosition[2] + ); + + Log.d(TAG, "Fused position (after conversion): " + + result.latitude + "," + result.longitude); + + return result; + } + + /** + * Update Fusion buffer with latest value + * @param fusionPosition + */ + private void updateMovingAvgFusion(double[] fusionPosition) { + // Create a new array to avoid reference issues + double[] positionCopy = new double[fusionPosition.length]; + System.arraycopy(fusionPosition, 0, positionCopy, 0, fusionPosition.length); + + fusionPositionHistory.add(positionCopy); + + // Maintain history size + if (fusionPositionHistory.size() > OUTPUT_HISTORY_SIZE) { + fusionPositionHistory.remove(0); + } + } + + /** + * Get moving average of fusion positions within the buffer + */ + private double[] getMovingAvgFusion() { + double size = (double) fusionPositionHistory.size(); + if (size == 0) return new double[]{0.0, 0.0, 0.0}; + + double[] result = new double[]{0.0, 0.0, 0.0}; + for (double[] pos : fusionPositionHistory) { + result[0] += pos[0]; + result[1] += pos[1]; + result[2] += pos[2]; + } + + return new double[]{ + result[0] / size, + result[1] / size, + result[2] / size + }; + } + + /** + * Estimate standard deviation of fusion positions + * @param movingAvg - mean position of N measurements + * @return + */ + private double[] fusionEstimateStd(double[] movingAvg) { + if (fusionPositionHistory.size() < 2) { + return new double[]{1.0, 1.0, 0.5}; // Default values if not enough history + } + + // Calculate standard deviation + double varX = 0, varY = 0, varTheta = 0; + double cosSum = 0, sinSum = 0; + + for (double[] pos : fusionPositionHistory) { + varX += Math.pow(pos[0] - movingAvg[0], 2); + varY += Math.pow(pos[1] - movingAvg[1], 2); + + // Handle circular angle correctly + cosSum += Math.cos(pos[2]); + sinSum += Math.sin(pos[2]); + } + + double avgTheta = Math.atan2(sinSum, sinSum); + for (double[] pos : fusionPositionHistory) { + double angleDiff = CoordinateConverter.normalizeAngleToPi(pos[2] - avgTheta); + varTheta += angleDiff * angleDiff; + } + + double stdX = Math.sqrt(varX / fusionPositionHistory.size()); + double stdY = Math.sqrt(varY / fusionPositionHistory.size()); + double stdTheta = Math.sqrt(varTheta / fusionPositionHistory.size()); + + return new double[]{stdX, stdY, stdTheta}; + } + + /** + * Outlier detection for a position + * @param position + * @param stdVal + * @param movingAvg + * @return + */ + private boolean outlierDetectionOutput(double[] position, double[] stdVal, double[] movingAvg) { + // Skip outlier detection if standard deviations are too small (prevents false positives) + if (stdVal[0] < 0.5 || stdVal[1] < 0.5) { + return false; + } + + // Check if position is an outlier based on X and Y coordinates + boolean isOutlierX = Math.abs(position[0] - movingAvg[0]) > OUTPUT_OUTLIER_THRESHOLD * stdVal[0]; + boolean isOutlierY = Math.abs(position[1] - movingAvg[1]) > OUTPUT_OUTLIER_THRESHOLD * stdVal[1]; + + // Handle heading outlier check - need to handle circular values + double headingDiff = CoordinateConverter.normalizeAngleToPi(position[2] - movingAvg[2]); + boolean isOutlierHeading = Math.abs(headingDiff) > OUTPUT_OUTLIER_THRESHOLD * stdVal[2]; + + // Using position outliers only for rejection, heading can be noisy + return isOutlierX || isOutlierY; + } + + /** + * Reset + */ + @Override + public void reset() { + // Stop the update timer + if (updateTimer != null) { + updateTimer.cancel(); + updateTimer = null; + } + + referenceInitialized = false; + hasInitialGnssPosition = false; + particlesInitialized = false; + dynamicUpdate = false; + particles.clear(); + gnssPositionHistory.clear(); + wifiPositionHistory.clear(); + Log.d(TAG, "Particle filter reset"); + } + + /** + * Simulate motion update of particles + * @param stepLength + * @param headingChange + * @param pdrStds + * @param wallDetectionManager + * @param buildingType + * @param currentFloor + */ + public void moveParticlesDynamic(double stepLength, double headingChange, double[] pdrStds, WallDetectionManager wallDetectionManager, int buildingType, int currentFloor) { + for (Particle particle : particles) { + particle.updateDynamic(stepLength, headingChange, pdrStds, wallDetectionManager, buildingType, currentFloor); + } + } + + + /** + * Static particle movement + * @param staticStds + */ + public void moveParticlesStatic(double[] staticStds) { + Random rand = new Random(); + + for (Particle particle : particles) { + // Add random noise to represent random motion in static state + particle.x += rand.nextGaussian() * staticStds[0]; + particle.y += rand.nextGaussian() * staticStds[1]; + particle.theta += rand.nextGaussian() * staticStds[2]; + + // Normalize angle + particle.theta = CoordinateConverter.normalizeAngleToPi(particle.theta); + } + + Log.d(TAG, "Applied static motion model to particles"); + } + + public void reweightParticles(SimpleMatrix measurementCovariance, + SimpleMatrix forwardModel, + SimpleMatrix measurementVector) { + + // First pass - calculate log weights + for (Particle particle : particles) { + SimpleMatrix positionVector = new SimpleMatrix(3, 1); + positionVector.set(0, 0, particle.x); + positionVector.set(1, 0, particle.y); + positionVector.set(2, 0, particle.theta); + + SimpleMatrix predicted = forwardModel.mult(positionVector); + SimpleMatrix error = measurementVector.minus(predicted); + + // Calculate LogWeight + double logWeight = -0.5 * error.transpose().mult(measurementCovariance.invert()).mult(error).get(0, 0) + -0.5 * Math.log(measurementCovariance.determinant()); + + // Store log-weight temporarily + particle.logWeight += logWeight; + } + + // Second pass + double sumWeights = 0.0; + for (Particle particle : particles) { + + particle.weight = Math.exp(particle.logWeight); + sumWeights += particle.weight; + } + + // Normalize weights + for (Particle particle : particles) { + particle.weight /= sumWeights; + } + } + + /** + * Resample particles based on their weights using systematic resampling + */ + public void resampleParticles() { + List newParticles = new ArrayList<>(); + + // Calculate cumulative sum of the weights + double[] cumulativeSum = new double[numParticles]; + cumulativeSum[0] = particles.get(0).weight; + for (int i = 1; i < numParticles; i++) { + cumulativeSum[i] = cumulativeSum[i - 1] + particles.get(i).weight; + } + + // Generate a random offset (u) between 0 and 1/numParticles + Random rand = new Random(); + double u = rand.nextDouble() / numParticles; + + // Systematic resampling + int index = 0; + for (int i = 0; i < numParticles; i++) { + double target = u + i * (1.0 / numParticles); + while (index < (numParticles-1) && target > cumulativeSum[index]) { + index++; + } + + // Create a copy of the selected particle + Particle original = particles.get(index); + Particle newParticle = new Particle(original.x, original.y, original.theta); + newParticle.weight = 1.0 / numParticles; // Reset weights to uniform + newParticle.logWeight = 0.0; + newParticles.add(newParticle); + } + + particles = newParticles; + + // Add jitter to prevent sample impoverishment + addJitterToParticles(); + + // Remove outliers + removeOutlierParticles(); + + Log.d(TAG, "Resampled particles with jitter: " + numParticles); + } + + /** + * Adds random jitter to particles to prevent sample impoverishment. + * The amount of jitter is proportional to the particle dispersion. + */ + private void addJitterToParticles() { + // Calculate current particle dispersion to scale jitter appropriately + double dispersion = calculateParticleDispersion(); + + // Jitter scale - smaller values for more precise distributions + // Typically between 0.01 (very small jitter) to 0.2 (significant jitter) + double jitterScale = 0.01; + + // Calculate jitter standard deviations + double positionJitterStd = jitterScale * dispersion; + double headingJitterStd = jitterScale * Math.PI / 12; // Was 6, ~30 degrees scaled by jitter + + Random rand = new Random(); + + // Apply jitter to each particle + for (Particle p : particles) { + // Add position jitter + p.x += rand.nextGaussian() * positionJitterStd; + p.y += rand.nextGaussian() * positionJitterStd; + + // Add heading jitter and normalize to [-π, π] + p.theta += rand.nextGaussian() * headingJitterStd; + p.theta = CoordinateConverter.normalizeAngleToPi(p.theta); + } + + Log.d(TAG, "Added jitter with scale: " + jitterScale + ", dispersion: " + dispersion); + } + + /** + * Get the estimated position (weighted average of all particles) + * @return + */ + public double[] getEstimatedPosition() { + double avgX=0, avgY=0, cosSum = 0, sinSum = 0; + + for (Particle particle : particles) { + avgX += particle.weight * particle.x; + avgY += particle.weight * particle.y; + + // Average angle using trigonometry to handle circular values correctly + cosSum += particle.weight * Math.cos(particle.theta); + sinSum += particle.weight * Math.sin(particle.theta); + } + + double avgTheta = Math.atan2(sinSum, cosSum); + return new double[]{avgX, avgY, avgTheta}; + } + + /** + * Update position history for outlier detection + * @param gnssEmuPosition + */ + private void updateMovingAvgGnss(double[] gnssEmuPosition) { + gnssPositionHistory.add(gnssEmuPosition); + + // Add current position to history + if (gnssPositionHistory.size() > HISTORY_SIZE) { + gnssPositionHistory.remove(0); + } + } + + private double[] getMovingAvgGnss() { + double size = (double) gnssPositionHistory.size(); + + double[] result = new double[]{0.0, 0.0}; + for (int i = 0; i < gnssPositionHistory.size(); i++) { + result[0] += gnssPositionHistory.get(i)[0]; + result[1] += gnssPositionHistory.get(i)[1]; + } + + return new double[] {result[0]/size, result[1]/size}; + + } + + // Update position history for outlier detection + private void updateMovingAvgWifi(double[] wifiEmuPosition) { + wifiPositionHistory.add(wifiEmuPosition); + + // Add current position to history + if (wifiPositionHistory.size() > HISTORY_SIZE) { + wifiPositionHistory.remove(0); + } + } + + private double[] getMovingAvgWifi() { + double size = (double) wifiPositionHistory.size(); + + double[] result = new double[]{0.0, 0.0}; + for (int i = 0; i < wifiPositionHistory.size(); i++) { + result[0] += wifiPositionHistory.get(i)[0]; + result[1] += wifiPositionHistory.get(i)[1]; + } + + return new double[] {result[0]/size, result[1]/size}; + + } + + private double[] gnssEstimateStd(double[] movingAvg) { + + // Calculate standard deviation + double varX = 0, varY = 0; + for (double[] pos : gnssPositionHistory) { + varX += Math.pow(pos[0] - movingAvg[0], 2); + varY += Math.pow(pos[1] - movingAvg[1], 2); + } + double stdX = Math.sqrt(varX / gnssPositionHistory.size()); + double stdY = Math.sqrt(varY / gnssPositionHistory.size()); + + return new double[] {stdX, stdY}; + } + + private double[] wifiEstimateStd(double[] movingAvg) { + + // Calculate standard deviation + double varX = 0, varY = 0; + for (double[] pos : wifiPositionHistory) { + varX += Math.pow(pos[0] - movingAvg[0], 2); + varY += Math.pow(pos[1] - movingAvg[1], 2); + } + double stdX = Math.sqrt(varX / wifiPositionHistory.size()); + double stdY = Math.sqrt(varY / wifiPositionHistory.size()); + + return new double[] {stdX, stdY}; + } + + private boolean outlierDetection(double[] measEnu, double[] stdVal, double[] movingAvg){ + + //Check if measurement measurement is within threshold of standard deviations + boolean isOutlierX = Math.abs(measEnu[0] - movingAvg[0]) > OUTLIER_THRESHOLD * stdVal[0]; + boolean isOutlierY = Math.abs(measEnu[1] - movingAvg[1]) > OUTLIER_THRESHOLD * stdVal[1]; + + // If standard deviation is too small, don't mark as outlier (avoids false positives) + if (stdVal[0] < 1.0 || stdVal[1] < 1.0) { + return false; + } + + return isOutlierX || isOutlierY; + } + + /** + * Calculate particle dispersion as a measure of uncertainty + */ + private double calculateParticleDispersion() { + double[] mean = getEstimatedPosition(); + double sumSquaredDistance = 0.0; + + for (Particle p : particles) { + double dx = p.x - mean[0]; + double dy = p.y - mean[1]; + sumSquaredDistance += dx*dx + dy*dy; + } + + return Math.sqrt(sumSquaredDistance / numParticles); + } + + private void removeOutlierParticles() { + double[] mean = getEstimatedPosition(); + double Varx = 0, Vary = 0; + for (Particle p : particles) { + Varx += Math.pow(p.x - mean[0], 2) / numParticles; + Vary += Math.pow(p.y - mean[0], 2) / numParticles; + } + double Stdx = Math.sqrt(Varx); + double Stdy = Math.sqrt(Vary); + for (int i = 0; i < particles.size(); i++) { + if (particles.get(i).x - mean[0] > OUTLIER_THRESHOLD * Stdx || + particles.get(i).y - mean[1] > OUTLIER_THRESHOLD * Stdy) { + Particle dummyPart = new Particle(mean[0], mean[1], currentHeading); + dummyPart.weight = 0.0; + dummyPart.logWeight = Double.NEGATIVE_INFINITY; + particles.set(i, dummyPart); + } + } + } + + public void setElevationStatus(PdrProcessing.ElevationDirection elevationDirection) { + this.elevationDirection = elevationDirection; + } + + /** + * Initializes the particles, centered at the reference position. + * Particles have uniform weights of 1/numParticles. + */ + private void initializeParticles() { + + double[] initGnssCoordEnu = CoordinateConverter.convertGeodeticToEnu( + firstGnssPosition.latitude, firstGnssPosition.longitude, latlngAltitude, + referencePosition[0], referencePosition[1], referencePosition[2]); + + currentPdrPos[0] = initGnssCoordEnu[0]; + currentPdrPos[1] = initGnssCoordEnu[1]; + + previousPdrPos[0] = initGnssCoordEnu[0]; + previousPdrPos[1] = initGnssCoordEnu[1]; + + // Initial dispersion - spread particles in a reasonable area + double initialDispersion = 2.5; // 2.5 meters initial uncertainty + + for (int i = 0; i < numParticles; i++) { + Random rand = new Random(); + + // Add some random noise to initial positions + double x = previousPdrPos[0] + rand.nextGaussian() * initialDispersion; + double y = previousPdrPos[1] + rand.nextGaussian() * initialDispersion; + double theta = rand.nextGaussian() * 2 * Math.PI; // Random orientation + + Particle particle = new Particle(x, y, theta); + particle.weight = 1.0 / numParticles; + particles.add(particle); + } + + particlesInitialized = true; + } + /** + * Initializes the heading estimate for particles + */ + private void initializeHeading() { + for (Particle particle : particles) { + Random rand = new Random(); + particle.theta = currentHeading; + } + + headingInitialized = true; + } + @Override + public void retrieveContext(Context context) { + this.FragmentContext = context; + } + + public boolean checkUpdate() { + if (updateReady) { + updateReady = false; + return true; + } + else { + return false; + } + } + + private void setBuildingType(double[] position) { + if (this.wallDetectionManager.inNucleusENU(position)) { + buildingType = 1; + } else if (this.wallDetectionManager.inLibraryENU(position)) { + buildingType = 2; + } else { + buildingType = 0; + } + Log.w(TAG, "Building type set to" + buildingType); + } + + /** + * Initializes the reference position if it wasn't provided or was zeros. + * Uses the first GNSS position as the reference. + */ + private void initializeReferencePosition() { + if (referenceInitialized || !hasInitialGnssPosition) { + return; + } + + // Use the first GNSS position as reference + referencePosition[0] = firstGnssPosition.latitude; + referencePosition[1] = firstGnssPosition.longitude; + referencePosition[2] = 78; + + referenceInitialized = true; + + Log.d(TAG, "Reference position initialized from GNSS: " + + "lat=" + referencePosition[0] + ", lng=" + referencePosition[1] + + ", alt=" + referencePosition[2]); + } + + /** + * Called when the object is being destroyed or the application is shutting down + * Ensures the timer is properly cleaned up + */ + public void shutdown() { + if (updateTimer != null) { + updateTimer.cancel(); + updateTimer = null; + Log.d(TAG, "Update timer cancelled during shutdown"); + } + } +} \ No newline at end of file diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/RecordingFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/RecordingFragment.java index 6362a971..db9c16ec 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/RecordingFragment.java +++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/RecordingFragment.java @@ -7,6 +7,8 @@ import android.os.Bundle; import android.os.CountDownTimer; import android.os.Handler; +import android.os.Looper; +import android.util.Log; import android.view.LayoutInflater; import android.view.View; import android.view.ViewGroup; @@ -18,6 +20,8 @@ import android.widget.ImageView; import android.widget.ProgressBar; import android.widget.TextView; +import android.widget.Toast; + import com.google.android.material.button.MaterialButton; import androidx.annotation.NonNull; @@ -29,6 +33,7 @@ import com.openpositioning.PositionMe.presentation.activity.RecordingActivity; import com.openpositioning.PositionMe.sensors.SensorFusion; import com.openpositioning.PositionMe.sensors.SensorTypes; +import com.openpositioning.PositionMe.utils.PositionListener; import com.openpositioning.PositionMe.utils.UtilFunctions; import com.google.android.gms.maps.model.LatLng; @@ -47,6 +52,7 @@ * - Provides UI controls to cancel or complete recording. * - Uses {@link TrajectoryMapFragment} to visualize recorded paths. * - Manages GNSS tracking and error display. + * - Visualizes fusion results from combined PDR and GNSS data. * * @see TrajectoryMapFragment The map fragment displaying the recorded trajectory. * @see RecordingActivity The activity managing the recording workflow. @@ -54,15 +60,19 @@ * @see SensorTypes Enumeration of available sensor types. * * @author Shu Gu + * @author Michal Wiercigroch + * @author Kalliopi Vakali */ -public class RecordingFragment extends Fragment { +public class RecordingFragment extends Fragment implements PositionListener { + + private static final String TAG = "RecordingFragment"; // UI elements private MaterialButton completeButton, cancelButton; private ImageView recIcon; private ProgressBar timeRemaining; - private TextView elevation, distanceTravelled, gnssError; + private TextView elevation, distanceTravelled, gnssError, fusionInfoText; // App settings private SharedPreferences settings; @@ -77,6 +87,9 @@ public class RecordingFragment extends Fragment { private float previousPosX = 0f; private float previousPosY = 0f; + // Fusion tracking + private LatLng lastFusionPosition = null; + // References to the child map fragment private TrajectoryMapFragment trajectoryMapFragment; @@ -135,6 +148,8 @@ public void onViewCreated(@NonNull View view, distanceTravelled = view.findViewById(R.id.currentDistanceTraveled); gnssError = view.findViewById(R.id.gnssError); + + //UI buttons and icons view completeButton = view.findViewById(R.id.stopButton); cancelButton = view.findViewById(R.id.cancelButton); recIcon = view.findViewById(R.id.redDot); @@ -181,6 +196,11 @@ public void onViewCreated(@NonNull View view, dialog.show(); // Finally, show the dialog }); + if (!sensorFusion.isStepDetectionWorking()) { + Log.w(TAG, "Step detection may not be working - this will affect PDR tracking"); + // Could show a warning to the user here + } + // The blinking effect for recIcon blinkingRecordingIcon(); @@ -216,7 +236,14 @@ public void onFinish() { */ private void updateUIandPosition() { float[] pdrValues = sensorFusion.getSensorValueMap().get(SensorTypes.PDR); - if (pdrValues == null) return; + if (pdrValues == null) { + Log.e(TAG, "PDR values are null in updateUIandPosition"); + return; + } + + if(!trajectoryMapFragment.isPdrEnabled()){ + trajectoryMapFragment.clearPdrTrajectory(); + } // Distance distance += Math.sqrt(Math.pow(pdrValues[0] - previousPosX, 2) @@ -228,9 +255,6 @@ private void updateUIandPosition() { elevation.setText(getString(R.string.elevation, String.format("%.1f", elevationVal))); // Current location - // Convert PDR coordinates to actual LatLng if you have a known starting lat/lon - // Or simply pass relative data for the TrajectoryMapFragment to handle - // For example: float[] latLngArray = sensorFusion.getGNSSLatitude(true); if (latLngArray != null) { LatLng oldLocation = trajectoryMapFragment.getCurrentLocation(); // or store locally @@ -239,11 +263,23 @@ private void updateUIandPosition() { new float[]{ pdrValues[0] - previousPosX, pdrValues[1] - previousPosY } ); + Log.d(TAG, "PDR update in updateUIandPosition: deltaX=" + (pdrValues[0] - previousPosX) + + ", deltaY=" + (pdrValues[1] - previousPosY) + + " -> newLocation=" + newLocation.latitude + ", " + newLocation.longitude); + // Pass the location + orientation to the map if (trajectoryMapFragment != null) { - trajectoryMapFragment.updateUserLocation(newLocation, - (float) Math.toDegrees(sensorFusion.passOrientation())); + trajectoryMapFragment.updatePdr(newLocation); + + // Force polyline update if there are no points yet + if (trajectoryMapFragment.isPolylineEmpty()) { + Log.d(TAG, "Forcing initial polyline update with location: " + + newLocation.latitude + ", " + newLocation.longitude); + trajectoryMapFragment.forcePolylineUpdate(newLocation); + } } + } else { + Log.e(TAG, "latLngArray is null in updateUIandPosition"); } // GNSS logic if you want to show GNSS error, etc. @@ -265,11 +301,32 @@ private void updateUIandPosition() { } } + // If there's a fusion position, we can calculate fusion-PDR error + if (lastFusionPosition != null && fusionInfoText != null) { + LatLng currentLoc = trajectoryMapFragment.getCurrentLocation(); + if (currentLoc != null) { + double fusionPdrError = UtilFunctions.distanceBetweenPoints(currentLoc, lastFusionPosition); + fusionInfoText.setVisibility(View.VISIBLE); + fusionInfoText.setText(String.format("Fusion-PDR error: %.2fm", fusionPdrError)); + } + } + + LatLng wifiLocation = sensorFusion.getLatLngWifiPositioning(); + // Update WiFi marker if the switch is enabled + if (trajectoryMapFragment.isWifiEnabled() && wifiLocation != null) { + trajectoryMapFragment.updateWiFi(wifiLocation); + } else { + trajectoryMapFragment.clearWiFi(); + } + + // Update previous previousPosX = pdrValues[0]; previousPosY = pdrValues[1]; } + + /** * Start the blinking effect for the recording icon. */ @@ -282,6 +339,20 @@ private void blinkingRecordingIcon() { recIcon.startAnimation(blinking); } + @Override + public void onStart() { + super.onStart(); + // Register for position updates + sensorFusion.registerPositionListener(this); + } + + @Override + public void onStop() { + super.onStop(); + // Unregister when fragment stops + sensorFusion.unregisterPositionListener(this); + } + @Override public void onPause() { super.onPause(); @@ -295,4 +366,74 @@ public void onResume() { refreshDataHandler.postDelayed(refreshDataTask, 500); } } -} + + // Implementation of PositionListener interface + // In RecordingFragment.java, enhance the onPositionUpdate method: + + @Override + public void onPositionUpdate(PositionListener.UpdateType updateType, LatLng position) { + + /** + * + *Triggers a toast message when WIFI_ERROR is triggered from SensorFusion + * + */ + + + if (updateType == PositionListener.UpdateType.WIFI_ERROR) { + Log.e("RecordingFragment", "WiFi positioning failed while RecordingFragment is active."); + Context context = getContext(); + if (context != null) { + Toast.makeText(context, "WiFi positioning failed. Check your connection.", Toast.LENGTH_SHORT).show(); + } else { + Log.e("RecordingFragment", "Context is null - cannot show Toast"); + } + + } + + + + + if (position == null) { + Log.w("RecordingFragment", "Received null position update for type: " + updateType); + return; + } + + // Use a handler to ensure UI updates happen on the main thread + new Handler(Looper.getMainLooper()).post(() -> { + // Process different types of position updates + switch (updateType) { + case PDR_POSITION: + // PDR updates are already handled in updateUIandPosition + Log.d("RecordingFragment", "PDR position update: " + position.latitude + ", " + position.longitude); + break; + + case GNSS_POSITION: + // GNSS updates are already handled in updateUIandPosition + Log.d("RecordingFragment", "GNSS position update: " + position.latitude + ", " + position.longitude); + break; + + case FUSED_POSITION: + // Update fusion position on map + Log.d("RecordingFragment", "Fusion position update received: " + position.latitude + ", " + position.longitude); + + if (trajectoryMapFragment != null) { + trajectoryMapFragment.updateFusionPosition(position, (float) Math.toDegrees(sensorFusion.passOrientation())); + } else { + Log.e("RecordingFragment", "Cannot update fusion position: trajectoryMapFragment is null"); + } + break; + + case ORIENTATION_UPDATE: + // Orientation updates are handled elsewhere + break; + + + + + } + }); + } + + +} \ No newline at end of file diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/ReplayFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/ReplayFragment.java index d15a4a83..7ac6cb64 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/ReplayFragment.java +++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/ReplayFragment.java @@ -333,7 +333,7 @@ private void updateMapForIndex(int newIndex) { trajectoryMapFragment.clearMapAndReset(); for (int i = 0; i <= newIndex; i++) { TrajParser.ReplayPoint p = replayData.get(i); - trajectoryMapFragment.updateUserLocation(p.pdrLocation, p.orientation); + trajectoryMapFragment.updatePdr(p.pdrLocation); if (p.gnssLocation != null) { trajectoryMapFragment.updateGNSS(p.gnssLocation); } @@ -341,7 +341,7 @@ private void updateMapForIndex(int newIndex) { } else { // Normal sequential forward step: add just the new point TrajParser.ReplayPoint p = replayData.get(newIndex); - trajectoryMapFragment.updateUserLocation(p.pdrLocation, p.orientation); + trajectoryMapFragment.updatePdr(p.pdrLocation); if (p.gnssLocation != null) { trajectoryMapFragment.updateGNSS(p.gnssLocation); } diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/StartLocationFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/StartLocationFragment.java index ee14f69f..95dede4d 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/StartLocationFragment.java +++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/StartLocationFragment.java @@ -1,6 +1,7 @@ package com.openpositioning.PositionMe.presentation.fragment; import android.os.Bundle; +import android.util.Log; import android.view.LayoutInflater; import android.view.View; import android.view.ViewGroup; @@ -155,37 +156,97 @@ public void onMarkerDrag(Marker marker) {} public void onViewCreated(@NonNull View view, @Nullable Bundle savedInstanceState) { super.onViewCreated(view, savedInstanceState); - this.button = view.findViewById(R.id.startLocationDone); - this.button.setOnClickListener(new View.OnClickListener() { + Button kalmanButton = view.findViewById(R.id.useKalmanFilter); + Button particleButton = view.findViewById(R.id.useParticleFilter); + + // Set click listener for Kalman filter button + kalmanButton.setOnClickListener(new View.OnClickListener() { /** * {@inheritDoc} - * When button clicked the PDR recording can start and the start position is stored for - * the {@link CorrectionFragment} to display. The {@link RecordingFragment} is loaded. + * When Kalman button clicked, store start position, set filter type to Kalman, and start recording */ @Override public void onClick(View view) { + // Get the chosen location from the map float chosenLat = startPosition[0]; float chosenLon = startPosition[1]; - // If the Activity is RecordingActivity - if (requireActivity() instanceof RecordingActivity) { - // Start sensor recording + set the start location - sensorFusion.startRecording(); - sensorFusion.setStartGNSSLatitude(startPosition); + // Log the start location and filter choice + Log.d("StartLocation", "Setting start location: " + chosenLat + ", " + chosenLon + " with Kalman filter"); - // Now switch to the recording screen - ((RecordingActivity) requireActivity()).showRecordingScreen(); + // Set both reference positions consistently + // 1. Set as float array (used by pdrProcessing) + sensorFusion.setStartGNSSLatitude(startPosition); + + // 2. Set as double array (used by fusion algorithm) + double[] referencePosition = new double[]{chosenLat, chosenLon, 0.0}; + sensorFusion.setStartGNSSLatLngAlt(referencePosition); + + // Set filter type to Kalman + sensorFusion.setFilterType("kalman"); - // If the Activity is ReplayActivity - } else if (requireActivity() instanceof ReplayActivity) { - // *Do not* cast to RecordingActivity here - // Just call the Replay method + // Initialize the fusion algorithm with the correct reference position + sensorFusion.initializeFusionAlgorithm(); + + // Start recording sensors + sensorFusion.startRecording(); + + if (requireActivity() instanceof RecordingActivity) { + ((RecordingActivity) requireActivity()).showRecordingScreen(); + } + // For other activity types + else if (requireActivity() instanceof ReplayActivity) { ((ReplayActivity) requireActivity()).onStartLocationChosen(chosenLat, chosenLon); + } + else { + // Optional: log or handle error + Log.e("StartLocationFragment", "Unknown host Activity: " + requireActivity()); + } + } + }); + + // Set click listener for Particle filter button + particleButton.setOnClickListener(new View.OnClickListener() { + /** + * {@inheritDoc} + * When Particle button clicked, store start position, set filter type to Particle, and start recording + */ + @Override + public void onClick(View view) { + // Get the chosen location from the map + float chosenLat = startPosition[0]; + float chosenLon = startPosition[1]; + + // Log the start location and filter choice + Log.d("StartLocation", "Setting start location: " + chosenLat + ", " + chosenLon + " with Particle filter"); + + // Set both reference positions consistently + // 1. Set as float array (used by pdrProcessing) + sensorFusion.setStartGNSSLatitude(startPosition); + + // 2. Set as double array (used by fusion algorithm) + double[] referencePosition = new double[]{chosenLat, chosenLon, 0.0}; + sensorFusion.setStartGNSSLatLngAlt(referencePosition); - // Otherwise (unexpected host) - } else { + // Set filter type to Particle + sensorFusion.setFilterType("particle"); + + // Initialize the fusion algorithm with the correct reference position + sensorFusion.initializeFusionAlgorithm(); + + // Start recording sensors + sensorFusion.startRecording(); + + if (requireActivity() instanceof RecordingActivity) { + ((RecordingActivity) requireActivity()).showRecordingScreen(); + } + // For other activity types + else if (requireActivity() instanceof ReplayActivity) { + ((ReplayActivity) requireActivity()).onStartLocationChosen(chosenLat, chosenLon); + } + else { // Optional: log or handle error - // Log.e("StartLocationFragment", "Unknown host Activity: " + requireActivity()); + Log.e("StartLocationFragment", "Unknown host Activity: " + requireActivity()); } } }); diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java index eb0bad65..8e1e1b15 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java +++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java @@ -1,5 +1,6 @@ package com.openpositioning.PositionMe.presentation.fragment; +import android.content.BroadcastReceiver; import android.graphics.Color; import android.os.Bundle; import android.util.Log; @@ -10,6 +11,8 @@ import android.widget.ArrayAdapter; import android.widget.Button; import android.widget.Spinner; +import android.widget.Toast; + import com.google.android.material.switchmaterial.SwitchMaterial; import androidx.annotation.NonNull; @@ -18,6 +21,7 @@ import com.google.android.gms.maps.OnMapReadyCallback; import com.openpositioning.PositionMe.R; +import com.openpositioning.PositionMe.fusion.TrajectoryFilter; import com.openpositioning.PositionMe.sensors.SensorFusion; import com.openpositioning.PositionMe.utils.IndoorMapManager; import com.openpositioning.PositionMe.utils.UtilFunctions; @@ -26,6 +30,9 @@ import com.google.android.gms.maps.SupportMapFragment; import com.google.android.gms.maps.model.*; +import com.google.maps.android.SphericalUtil; + + import java.util.ArrayList; import java.util.List; @@ -49,6 +56,8 @@ * @see com.openpositioning.PositionMe.utils.UtilFunctions Utility functions for UI and graphics handling. * * @author Mate Stodulka + * @author Laura Maryakhina + * @author Kalliopi Vakali */ public class TrajectoryMapFragment extends Fragment { @@ -58,17 +67,39 @@ public class TrajectoryMapFragment extends Fragment { private Marker orientationMarker; // Marker representing user's heading private Marker gnssMarker; // GNSS position marker private Polyline polyline; // Polyline representing user's movement path - private boolean isRed = true; // Tracks whether the polyline color is red - private boolean isGnssOn = false; // Tracks if GNSS tracking is enabled + private boolean isGreen = true; // Tracks whether the polyline color is red + private boolean isGnssOn = true; // Tracks if GNSS tracking is enabled private Polyline gnssPolyline; // Polyline for GNSS path private LatLng lastGnssLocation = null; // Stores the last GNSS location + private Polyline fusionPolyline; // Polyline for the fusion path + + private List fusionPoints; + + private LatLng lastFusionLocation = null; // Last fusion position + private Marker fusionMarker; // Marker for current fusion position + private Marker pdrMarker; + private Polyline pdrPolyline; // Polyline representing PDR trajectory + private boolean isPdrOn = false; // Tracks if PDR trajectory is enabled + + private SwitchMaterial pdrSwitch; // PDR switch control + + + private Marker wifiMarker; // WiFi Position Marker + private Polyline wifiPolyline; // WiFi Position Path + private LatLng lastWifiLocation = null; // Last WiFi position + private boolean isWifiOn = false; // Toggle for WiFi tracking + + private int lastAutoFloorLevel = Integer.MIN_VALUE; + + private LatLng pendingCameraPosition = null; // Stores pending camera movement private boolean hasPendingCameraMove = false; // Tracks if camera needs to move private IndoorMapManager indoorMapManager; // Manages indoor mapping private SensorFusion sensorFusion; + private TrajectoryFilter trajectoryFilter; // UI @@ -76,11 +107,19 @@ public class TrajectoryMapFragment extends Fragment { private SwitchMaterial gnssSwitch; private SwitchMaterial autoFloorSwitch; + private SwitchMaterial wifiSwitch; + private static final double MAX_DISTANCE_THRESHOLD = 100; //wifi max distance between readings to detect/remove outliers + private com.google.android.material.floatingactionbutton.FloatingActionButton floorUpButton, floorDownButton; private Button switchColorButton; private Polygon buildingPolygon; + private BroadcastReceiver wifiErrorReceiver; + + + + public TrajectoryMapFragment() { // Required empty public constructor @@ -103,11 +142,17 @@ public void onViewCreated(@NonNull View view, // Grab references to UI controls switchMapSpinner = view.findViewById(R.id.mapSwitchSpinner); gnssSwitch = view.findViewById(R.id.gnssSwitch); + wifiSwitch = view.findViewById(R.id.wifiSwitch); + pdrSwitch = view.findViewById(R.id.pdrSwitch); + + autoFloorSwitch = view.findViewById(R.id.autoFloor); floorUpButton = view.findViewById(R.id.floorUpButton); floorDownButton = view.findViewById(R.id.floorDownButton); switchColorButton = view.findViewById(R.id.lineColorButton); + gnssSwitch.setChecked(true); + // Setup floor up/down UI hidden initially until we know there's an indoor map setFloorControlsVisibility(View.GONE); @@ -149,30 +194,74 @@ public void onMapReady(@NonNull GoogleMap googleMap) { gnssMarker.remove(); gnssMarker = null; } + if (gnssPolyline != null) { + gnssPolyline.setPoints(new ArrayList<>()); // Clear the polyline + } + }); + + + //wifi switch + wifiSwitch.setOnCheckedChangeListener((buttonView, isChecked) -> { + isWifiOn = isChecked; + if (!isChecked && wifiMarker != null) { + wifiMarker.remove(); + wifiMarker = null; + } + if (wifiPolyline != null) { + wifiPolyline.setPoints(new ArrayList<>()); // Clear the polyline + } }); + pdrSwitch.setOnCheckedChangeListener((buttonView, isChecked) -> { + isPdrOn = isChecked; + + // Clear PDR trajectory if toggled off + if (!isChecked && pdrPolyline != null) { + pdrPolyline.setPoints(new ArrayList<>()); // Clear the polyline + } + // Remove the PDR marker if it exists + if (pdrMarker != null) { + pdrMarker.remove(); + pdrMarker = null; + } + }); + + + + // Color switch + // New logic for switching fusionPolyline color switchColorButton.setOnClickListener(v -> { - if (polyline != null) { - if (isRed) { + if (fusionPolyline != null) { + if (isGreen) { + fusionPolyline.setColor(Color.BLACK); switchColorButton.setBackgroundColor(Color.BLACK); - polyline.setColor(Color.BLACK); - isRed = false; } else { - switchColorButton.setBackgroundColor(Color.RED); - polyline.setColor(Color.RED); - isRed = true; + fusionPolyline.setColor(Color.GREEN); + switchColorButton.setBackgroundColor(Color.GREEN); } + isGreen = !isGreen; } }); + + sensorFusion = SensorFusion.getInstance(); + trajectoryFilter = new TrajectoryFilter(); + fusionPoints = new ArrayList<>(); + //sensorFusion.passContext(requireContext()); + // Floor up/down logic autoFloorSwitch.setOnCheckedChangeListener((compoundButton, isChecked) -> { - - //TODO - fix the sensor fusion method to get the elevation (cannot get it from the current method) -// float elevationVal = sensorFusion.getElevation(); -// indoorMapManager.setCurrentFloor((int)(elevationVal/indoorMapManager.getFloorHeight()) -// ,true); + if (indoorMapManager != null) { + if (isChecked) { + // Get WiFi floor from SensorFusion and set it + int wifiFloor = sensorFusion.getWifiFloor(); + Log.d("AutoFloor", "Auto floor enabled. WiFi floor = " + wifiFloor); + indoorMapManager.setCurrentFloor(wifiFloor, true); + } else { + Log.d("AutoFloor", "Auto floor disabled"); + } + } }); floorUpButton.setOnClickListener(v -> { @@ -189,8 +278,19 @@ public void onMapReady(@NonNull GoogleMap googleMap) { indoorMapManager.decreaseFloor(); } }); + + + + + } + + + + + + /** * Initialize the map settings with the provided GoogleMap instance. *

@@ -202,6 +302,117 @@ public void onMapReady(@NonNull GoogleMap googleMap) { * @param map */ + // Add this method to update the fusion position on the map + /** + * Updates the fusion position on the map with the latest estimate. + * + * @param fusionLocation The latest fusion position estimate + */ + // In the updateFusionPosition method, add style enhancements: + public void updateFusionPosition(@NonNull LatLng fusionLocation, float orientation) { + if (gMap == null || fusionLocation == null) { + Log.e("TrajectoryMapFragment", "Cannot update fusion: gMap or fusionLocation is null"); + return; + } + + Log.d("TrajectoryMapFragment", "updateFusionPosition called with: " + + fusionLocation.latitude + ", " + fusionLocation.longitude); + + + // If fusion marker doesn't exist, create it + if (fusionMarker == null) { + fusionMarker = gMap.addMarker(new MarkerOptions() + .position(fusionLocation) + .title("Fusion Position") + .icon(BitmapDescriptorFactory.fromBitmap( + UtilFunctions.getBitmapFromVector(requireContext(), + R.drawable.ic_baseline_navigation_24)))); + gMap.moveCamera(CameraUpdateFactory.newLatLngZoom(fusionLocation, 19f)); + } else { + // Update marker position + orientation + fusionMarker.setPosition(fusionLocation); + fusionMarker.setRotation(orientation); + // Move camera a bit + gMap.moveCamera(CameraUpdateFactory.newLatLng(fusionLocation)); + } + + // Check if fusionPolyline is null and create it if needed + if (fusionPolyline == null) { + fusionPolyline = gMap.addPolyline(new PolylineOptions() + .color(Color.GREEN) + .width(8f) + .zIndex(100) + .add(fusionLocation)); + Log.d("TrajectoryMapFragment", "Created new fusion polyline"); + } else { + // Add new point to fusion path + //List fusionPoints = new ArrayList<>(fusionPolyline.getPoints()); + fusionPoints.add(fusionLocation); + + // Utilize Trajectory Filter to smooth the trajectory line + List fusionPoints = trajectoryFilter.processData(fusionLocation, sensorFusion.getReferencePosition()); + + fusionPolyline.setPoints(fusionPoints); + Log.d("TrajectoryMapFragment", "Added point to fusion polyline, total points: " + fusionPoints.size()); + } + if (indoorMapManager != null) { + indoorMapManager.setCurrentLocation(fusionLocation); + setFloorControlsVisibility(indoorMapManager.getIsIndoorMapSet() ? View.VISIBLE : View.GONE); + } + + + UpdateAutoFloor(); + + } + + private void UpdateAutoFloor() { + if (autoFloorSwitch != null && autoFloorSwitch.isChecked() + && sensorFusion != null && indoorMapManager != null) { + + int wifiFloor = sensorFusion.getWifiFloor(); + + if (wifiFloor != lastAutoFloorLevel) { + lastAutoFloorLevel = wifiFloor; + Log.d("AutoFloor", "Auto floor change detected. Setting floor to: " + wifiFloor); + indoorMapManager.setCurrentFloor(wifiFloor, true); + } + } + } + + + + /** + * Checks if the main polyline is empty (has no points) + * @return true if polyline has no points, false otherwise + */ + public boolean isPolylineEmpty() { + return polyline == null || polyline.getPoints().isEmpty(); + } + + /** + * Force updates the polyline with a given location even if no movement was detected + * @param location The location to add to the polyline + */ + public void forcePolylineUpdate(LatLng location) { + if (gMap == null || location == null) return; + + Log.d("TrajectoryMapFragment", "Forcing polyline update with: " + + location.latitude + ", " + location.longitude); + + if (polyline == null) { + // Initialize polyline if it doesn't exist + polyline = gMap.addPolyline(new PolylineOptions() + .color(Color.RED) + .width(6f) + .zIndex(1) + .add(location)); // add the location immediately + } else { + List points = new ArrayList<>(polyline.getPoints()); + points.add(location); + polyline.setPoints(points); + } + } + private void initMapSettings(GoogleMap map) { // Basic map settings map.getUiSettings().setCompassEnabled(true); @@ -213,19 +424,51 @@ private void initMapSettings(GoogleMap map) { // Initialize indoor manager indoorMapManager = new IndoorMapManager(map); - // Initialize an empty polyline + // Initialize wall detection + + // Initialize the main PDR polyline (red) polyline = map.addPolyline(new PolylineOptions() .color(Color.RED) - .width(5f) + .width(6f) .add() // start empty + .zIndex(1000) ); - // GNSS path in blue + // Initialize the GNSS polyline (blue) gnssPolyline = map.addPolyline(new PolylineOptions() .color(Color.BLUE) + .width(6f) + .add() // start empty + .zIndex(1000) + ); + + + // Initialize the fusion polyline (green) - make it prominent + fusionPolyline = map.addPolyline(new PolylineOptions() + .color(Color.GREEN) + .width(8f) // Slightly thicker for visibility + .zIndex(100) + .add()); // start empty + + Log.d("TrajectoryMapFragment", "Map initialized with fusion polyline"); + + // WiFi path in green + wifiPolyline = map.addPolyline(new PolylineOptions() + .color(Color.MAGENTA) .width(5f) .add() // start empty + .zIndex(1000) ); + // PDR path in red + pdrPolyline = map.addPolyline(new PolylineOptions() + .color(Color.RED) // Use a distinct color for PDR + .width(5f) + .add() // Start empty + .zIndex(1000) + ); + + + } @@ -281,43 +524,35 @@ public void onNothingSelected(AdapterView parent) {} } /** - * Update the user's current location on the map, create or move orientation marker, - * and append to polyline if the user actually moved. + * Update the user's PDR location on the map, create or moves the PDR marker, + * and append to polyline if the user actually moved. Also updates indoor map + * if available. * * @param newLocation The new location to plot. - * @param orientation The user’s heading (e.g. from sensor fusion). */ - public void updateUserLocation(@NonNull LatLng newLocation, float orientation) { + public void updatePdr(@NonNull LatLng newLocation) { if (gMap == null) return; + if (!isPdrOn) 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() + if (pdrMarker == null) { + pdrMarker = gMap.addMarker(new MarkerOptions() .position(newLocation) .flat(true) - .title("Current Position") - .icon(BitmapDescriptorFactory.fromBitmap( - UtilFunctions.getBitmapFromVector(requireContext(), - R.drawable.ic_baseline_navigation_24))) + .title("PDR Position") + .icon(BitmapDescriptorFactory.defaultMarker(BitmapDescriptorFactory.HUE_RED)) ); - gMap.moveCamera(CameraUpdateFactory.newLatLngZoom(newLocation, 19f)); } else { - // Update marker position + orientation - orientationMarker.setPosition(newLocation); - orientationMarker.setRotation(orientation); - // Move camera a bit - gMap.moveCamera(CameraUpdateFactory.newLatLng(newLocation)); + pdrMarker.setPosition(newLocation); } - // Extend polyline if movement occurred - if (oldLocation != null && !oldLocation.equals(newLocation) && polyline != null) { - List points = new ArrayList<>(polyline.getPoints()); + if (oldLocation != null && !oldLocation.equals(newLocation) && pdrPolyline != null) { + List points = new ArrayList<>(pdrPolyline.getPoints()); points.add(newLocation); - polyline.setPoints(points); + pdrPolyline.setPoints(points); } // Update indoor map overlay @@ -327,7 +562,14 @@ public void updateUserLocation(@NonNull LatLng newLocation, float orientation) { } } - + /** + * Remove PDR trajectory if the user toggles it off. + */ + public void clearPdrTrajectory() { + if (pdrPolyline != null) { + pdrPolyline.setPoints(new ArrayList<>()); // Clear the polyline + } + } /** * Set the initial camera position for the map. @@ -367,25 +609,34 @@ public void updateGNSS(@NonNull LatLng gnssLocation) { if (!isGnssOn) return; if (gnssMarker == null) { - // Create the GNSS marker for the first time + // Create the GNSS marker for the first time with enhanced visibility gnssMarker = gMap.addMarker(new MarkerOptions() .position(gnssLocation) .title("GNSS Position") - .icon(BitmapDescriptorFactory - .defaultMarker(BitmapDescriptorFactory.HUE_AZURE))); + .icon(BitmapDescriptorFactory.defaultMarker(BitmapDescriptorFactory.HUE_AZURE)) + .zIndex(8f) // High z-index, but below fusion marker + ); lastGnssLocation = gnssLocation; + gMap.moveCamera(CameraUpdateFactory.newLatLngZoom(gnssLocation, 19f)); } else { // Move existing GNSS marker gnssMarker.setPosition(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); - } - lastGnssLocation = gnssLocation; + // Add a segment to the blue GNSS line, with force update if empty + if (lastGnssLocation != null && + (!lastGnssLocation.equals(gnssLocation) || gnssPolyline.getPoints().isEmpty())) { + + List gnssPoints = new ArrayList<>(gnssPolyline.getPoints()); + gnssPoints.add(gnssLocation); + gnssPolyline.setPoints(gnssPoints); + gnssPolyline.setColor(Color.rgb(0, 0, 255)); // Bright blue for visibility + gnssPolyline.setWidth(8f); // Ensure width is set + gnssPolyline.setZIndex(2); // Ensure z-index is set + + Log.d("TrajectoryMapFragment", "GNSS polyline updated, total points: " + gnssPoints.size()); } + lastGnssLocation = gnssLocation; } @@ -398,6 +649,86 @@ public void clearGNSS() { gnssMarker = null; } } + /** + * Updates the WiFi marker and polyline on the map based on the provided location. + * + * This method ensures that the WiFi marker is either created or updated at the given location, + * only if the user is inside a location with Wifi coverage. It also adds a segment to the polyline + * representing the WiFi path if the new location is different from the previous one. Updates are + * ignored if the Wifi location is an outlier or if WiFi tracking is disabled. + * + * + * @param wifiLocation + */ + public void updateWiFi(@NonNull LatLng wifiLocation) { + if (gMap == null) return; + if (!isWifiOn) return; // Ignore updates if WiFi is turned off + + // Check if the new wifi location is an outlier + if (isOutlier(lastWifiLocation, wifiLocation)) { + Log.w("OutlierDetection", "Rejected outlier WiFi location: " + wifiLocation.toString()); + return; // Skip updating the marker + } + + boolean isInsideWifiBounds = indoorMapManager.getIsIndoorMapSet(); // Check bounds + if (!isInsideWifiBounds) { + // WiFi readings are not available; show a message + showNoWiFiCoverageMessage(); + clearWiFi(); // Clear the WiFi marker and polyline if any + wifiSwitch.setChecked(false); + return; + } + + if (wifiMarker == null) { + // Create the WiFi marker for the first time + wifiMarker = gMap.addMarker(new MarkerOptions() + .position(wifiLocation) + .title("WiFi Position") + .icon(BitmapDescriptorFactory.defaultMarker(BitmapDescriptorFactory.HUE_MAGENTA))); + lastWifiLocation = wifiLocation; + } else { + // Move existing WiFi marker + wifiMarker.setPosition(wifiLocation); + + // Add a segment to the WiFi polyline if this is a new location + if (lastWifiLocation != null && !lastWifiLocation.equals(wifiLocation)) { + List wifiPoints = new ArrayList<>(wifiPolyline.getPoints()); + wifiPoints.add(wifiLocation); + wifiPolyline.setPoints(wifiPoints); + } + lastWifiLocation = wifiLocation; + } + } + + /** + * Clears Wifi marker so only one is present on the map at a time + */ + public void clearWiFi() { + if (wifiMarker != null) { + wifiMarker.remove(); + wifiMarker = null; + } + } + + /** + * Displays a dialog to notify the user that there is no WiFi coverage. + */ + private void showNoWiFiCoverageMessage() { + Toast.makeText(requireContext(), "No WiFi coverage available in this area.", Toast.LENGTH_SHORT).show(); + } + + /** + * Method to check if a Wifi location is an outlier + * @param lastLocation - last wifi location + * @param newLocation - current wifi location + * @return boolean - True if new wifi location is too far from last location + */ + private boolean isOutlier(LatLng lastLocation, LatLng newLocation) { + if (lastLocation == null || newLocation == null) return false; + double distance = SphericalUtil.computeDistanceBetween(lastLocation, newLocation); + return distance > MAX_DISTANCE_THRESHOLD; // True if the new location is too far + } + /** * Whether user is currently showing GNSS or not @@ -405,6 +736,18 @@ public void clearGNSS() { public boolean isGnssEnabled() { return isGnssOn; } + /** + * Whether user is currently showing Wifi or not + */ + public boolean isWifiEnabled() { + return isWifiOn; + } + + public boolean isPdrEnabled() { + return isPdrOn; + } + + private void setFloorControlsVisibility(int visibility) { floorUpButton.setVisibility(visibility); @@ -412,37 +755,67 @@ private void setFloorControlsVisibility(int visibility) { autoFloorSwitch.setVisibility(visibility); } + // In TrajectoryMapFragment.java, update the clearMapAndReset method: + public void clearMapAndReset() { - if (polyline != null) { - polyline.remove(); - polyline = null; - } - if (gnssPolyline != null) { - gnssPolyline.remove(); - gnssPolyline = null; - } - if (orientationMarker != null) { - orientationMarker.remove(); - orientationMarker = null; - } - if (gnssMarker != null) { - gnssMarker.remove(); - gnssMarker = null; + if (gMap == null) { + Log.e("TrajectoryMapFragment", "Cannot reset map, gMap is null"); + return; } + + // Clear all polylines + //if (polyline != null) polyline.remove(); + if (gnssPolyline != null) gnssPolyline.remove(); + if (fusionPolyline != null) fusionPolyline.remove(); + if (wifiPolyline != null) wifiPolyline.remove(); + if (pdrMarker != null) pdrPolyline.remove(); + + // Clear all markers + //if (orientationMarker != null) orientationMarker.remove(); + if (gnssMarker != null) gnssMarker.remove(); + if (fusionMarker != null) fusionMarker.remove(); + if (wifiMarker != null) wifiMarker.remove(); + if (pdrMarker != null) pdrMarker.remove(); + + + // Reset state variables + + lastWifiLocation = null; lastGnssLocation = null; - currentLocation = null; + currentLocation = null; + fusionMarker = null; + pdrMarker = null; + + + // Create new polylines +// polyline = gMap.addPolyline(new PolylineOptions() +// .color(Color.RED) +// .width(6f) +// .add()); + + gnssPolyline = gMap.addPolyline(new PolylineOptions() + .color(Color.BLUE) + .width(6f) + .add()); + + fusionPolyline = gMap.addPolyline(new PolylineOptions() + .color(Color.GREEN) + .width(8f) + .zIndex(100) + .add()); + + wifiPolyline = gMap.addPolyline(new PolylineOptions() + .color(Color.MAGENTA) + .width(5f) + .add()); + + pdrPolyline = gMap.addPolyline(new PolylineOptions() + .color(Color.RED) + .width(5f) + .add()); + + Log.d("TrajectoryMapFragment", "Map cleared and reset"); - // 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) - .add()); - } } /** @@ -538,4 +911,5 @@ private void drawBuildingPolygon() { } + } 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 6eca847c..f24c6491 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -1,6 +1,7 @@ package com.openpositioning.PositionMe.sensors; import android.content.Context; +import android.content.Intent; import android.content.SharedPreferences; import android.hardware.Sensor; import android.hardware.SensorEvent; @@ -9,14 +10,17 @@ import android.location.Location; import android.location.LocationListener; import android.os.Build; +import android.os.Looper; import android.os.PowerManager; import android.os.SystemClock; import android.util.Log; +import android.widget.Toast; import androidx.annotation.NonNull; import androidx.preference.PreferenceManager; import com.google.android.gms.maps.model.LatLng; +import com.openpositioning.PositionMe.fusion.particle.ParticleFilterFusion; import com.openpositioning.PositionMe.presentation.activity.MainActivity; import com.openpositioning.PositionMe.utils.PathView; import com.openpositioning.PositionMe.utils.PdrProcessing; @@ -24,6 +28,14 @@ import com.openpositioning.PositionMe.Traj; import com.openpositioning.PositionMe.presentation.fragment.SettingsFragment; +// New imports for positioning fusion +import com.openpositioning.PositionMe.fusion.IPositionFusionAlgorithm; +import com.openpositioning.PositionMe.fusion.KalmanFilterFusion; +import com.openpositioning.PositionMe.fusion.particle.ParticleFilterFusion; +import com.openpositioning.PositionMe.utils.PositionListener; +import com.openpositioning.PositionMe.utils.CoordinateConverter; +import com.openpositioning.PositionMe.utils.SimpleFusionConverter; + import org.json.JSONException; import org.json.JSONObject; @@ -56,6 +68,8 @@ * @author Michal Dvorak * @author Mate Stodulka * @author Virginia Cangelosi + * @author Michal Wiercigroch + * @author Kalliopi Vakali */ public class SensorFusion implements SensorEventListener, Observer { @@ -81,6 +95,12 @@ public class SensorFusion implements SensorEventListener, Observer { private static final String WIFI_FINGERPRINT= "wf"; //endregion + private static final int NUM_PARTICLES=10000; + + // Stairs detection + private static final int MIN_CONSECUTIVE_CHANGES = 4; + private static final float MIN_ABSOLUTE_CHANGE = 1.5f; + //region Instance variables // Keep device awake while recording private PowerManager.WakeLock wakeLock; @@ -147,7 +167,6 @@ public class SensorFusion implements SensorEventListener, Observer { // Wifi values private List wifiList; - // Over time accelerometer magnitude values since last step private List accelMagnitude; @@ -159,6 +178,30 @@ public class SensorFusion implements SensorEventListener, Observer { // WiFi positioning object private WiFiPositioning wiFiPositioning; + // New variables for position fusion + private List positionListeners = new ArrayList<>(); + private IPositionFusionAlgorithm fusionAlgorithm; + private LatLng fusedPosition; + private LatLng wifiPosition; + private double altitude; + private double[] referencePosition = new double[3]; // lat, lng, alt + private boolean useFusion = true; + private String filterType; // for choosing filter type (kalman/particle) + + + // WiFi failure tracking + private static final long WIFI_FAILURE_TIME_THRESHOLD_MS = 10000; // 10 seconds + private long wifiFailureStartTime = 0; + private boolean isWifiFailureOngoing = false; + + + + + + private Timer updateTimer; // Timer for regular updates + private static final long UPDATE_INTERVAL_MS = 1000; // Update every 1 second + private Context FragmentContext; + //region Initialisation /** * Private constructor for implementing singleton design pattern for SensorFusion. @@ -191,6 +234,9 @@ private SensorFusion() { this.R = new float[9]; // GNSS initial Long-Lat array this.startLocation = new float[2]; + // Initialize list for position listeners + this.positionListeners = new ArrayList<>(); + this.referencePosition = new double[3]; } @@ -283,12 +329,6 @@ public void onSensorChanged(SensorEvent sensorEvent) { if (lastTimestamp != null) { long timeGap = currentTime - lastTimestamp; - -// // Log a warning if the time gap is larger than the threshold -// if (timeGap > LARGE_GAP_THRESHOLD_MS) { -// Log.e("SensorFusion", "Large time gap detected for sensor " + sensorType + -// " | Time gap: " + timeGap + " ms"); -// } } // Update timestamp and frequency counter for this sensor @@ -310,6 +350,8 @@ public void onSensorChanged(SensorEvent sensorEvent) { this.elevation = pdrProcessing.updateElevation( SensorManager.getAltitude(SensorManager.PRESSURE_STANDARD_ATMOSPHERE, pressure) ); + PdrProcessing.ElevationDirection elevationDirection = pdrProcessing.detectContinuousElevationChange(MIN_CONSECUTIVE_CHANGES, MIN_ABSOLUTE_CHANGE); + updateFusionWithStairs(elevationDirection); } break; @@ -331,11 +373,6 @@ public void onSensorChanged(SensorEvent sensorEvent) { ); this.accelMagnitude.add(accelMagFiltered); -// // Debug logging -// Log.v("SensorFusion", -// "Added new linear accel magnitude: " + accelMagFiltered -// + "; accelMagnitude size = " + accelMagnitude.size()); - elevator = pdrProcessing.estimateElevator(gravity, filteredAcc); break; @@ -369,39 +406,31 @@ public void onSensorChanged(SensorEvent sensorEvent) { float[] rotationVectorDCM = new float[9]; SensorManager.getRotationMatrixFromVector(rotationVectorDCM, this.rotation); SensorManager.getOrientation(rotationVectorDCM, this.orientation); + + // Notify orientation update if needed + notifyPositionListeners(PositionListener.UpdateType.ORIENTATION_UPDATE, null); break; case Sensor.TYPE_STEP_DETECTOR: long stepTime = SystemClock.uptimeMillis() - bootTime; + currentTime = System.currentTimeMillis(); + Log.d("SensorFusion", "Step detected at time: " + currentTime); - if (currentTime - lastStepTime < 20) { - Log.e("SensorFusion", "Ignoring step event, too soon after last step event:" + (currentTime - lastStepTime) + " ms"); - // Ignore rapid successive step events + if (currentTime - lastStepTime < 200) { + Log.e("SensorFusion", "Ignoring step event, too soon after last step"); break; } - else { lastStepTime = currentTime; - // Log if accelMagnitude is empty - 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()); - } float[] newCords = this.pdrProcessing.updatePdr( stepTime, this.accelMagnitude, - this.orientation[0] - ); - - // Clear the accelMagnitude after using it - this.accelMagnitude.clear(); + this.orientation[0] // Initially 0 degrees is North + ); // At output 0 degrees is East + Log.d("SensorFusion", "PDR update calculated: X=" + newCords[0] + ", Y=" + newCords[1]); if (saveRecording) { this.pathView.drawTrajectory(newCords); @@ -410,10 +439,23 @@ public void onSensorChanged(SensorEvent sensorEvent) { .setRelativeTimestamp(SystemClock.uptimeMillis() - bootTime) .setX(newCords[0]) .setY(newCords[1])); + + // Call fusion update with PDR + updateFusionWithPdr(); } + + // Notify listeners of PDR update + float[] pdrPosition = pdrProcessing.getPDRMovement(); + LatLng pdrLatLng = CoordinateConverter.convertEnuToGeodetic( + pdrPosition[0], pdrPosition[1], getElevation(), + referencePosition[0], referencePosition[1], referencePosition[2] + ); + notifyPositionListeners(PositionListener.UpdateType.PDR_POSITION, pdrLatLng); + + // Clear the accelMagnitude after using it + this.accelMagnitude.clear(); break; } - } } @@ -438,23 +480,29 @@ public void logSensorFrequencies() { class myLocationListener implements LocationListener{ @Override public void onLocationChanged(@NonNull Location location) { - //Toast.makeText(context, "Location Changed", Toast.LENGTH_SHORT).show(); latitude = (float) location.getLatitude(); longitude = (float) location.getLongitude(); - float altitude = (float) location.getAltitude(); + altitude = (float) location.getAltitude(); float accuracy = (float) location.getAccuracy(); float speed = (float) location.getSpeed(); String provider = location.getProvider(); + if(saveRecording) { trajectory.addGnssData(Traj.GNSS_Sample.newBuilder() .setAccuracy(accuracy) - .setAltitude(altitude) + .setAltitude((float) altitude) .setLatitude(latitude) .setLongitude(longitude) .setSpeed(speed) .setProvider(provider) .setRelativeTimestamp(System.currentTimeMillis()-absoluteStartTime)); + + // Call fusion update with GNSS + updateFusionWithGnss(); } + + // Notify GNSS position update + notifyPositionListeners(PositionListener.UpdateType.GNSS_POSITION, new LatLng(latitude, longitude)); } } @@ -480,6 +528,9 @@ public void update(Object[] wifiList) { } // Adding WiFi data to Trajectory this.trajectory.addWifiData(wifiData); + + // Call fusion update with WiFi + updateFusionWithWifi(); } createWifiPositioningRequest(); } @@ -488,34 +539,100 @@ public void update(Object[] wifiList) { * Function to create a request to obtain a wifi location for the obtained wifi fingerprint * */ - private void createWifiPositioningRequest(){ + private void createWifiPositioningRequest() { // Try catch block to catch any errors and prevent app crashing try { // Creating a JSON object to store the WiFi access points - JSONObject wifiAccessPoints=new JSONObject(); - for (Wifi data : this.wifiList){ + JSONObject wifiAccessPoints = new JSONObject(); + for (Wifi data : this.wifiList) { wifiAccessPoints.put(String.valueOf(data.getBssid()), data.getLevel()); } // Creating POST Request JSONObject wifiFingerPrint = new JSONObject(); wifiFingerPrint.put(WIFI_FINGERPRINT, wifiAccessPoints); - this.wiFiPositioning.request(wifiFingerPrint); + + // Using standard WiFi positioning without fusion integration + // this.wiFiPositioning.request(wifiFingerPrint); + + + this.wiFiPositioning.request(wifiFingerPrint, new WiFiPositioning.VolleyCallback() { + @Override + public void onSuccess(LatLng wifiLocation, int floor) { + // WiFi succeeded — reset failure streak + if (isWifiFailureOngoing) { + isWifiFailureOngoing = false; + wifiFailureStartTime = 0; + Log.d("SensorFusion", "WiFi recovered. Resetting failure timer."); + } + + // Existing logic... + Log.d("WiFiPositioning", "Success! Location: " + wifiLocation); + } + + + /** + * Sends a wifi error trigger to the listener when there are consecutive errors withing the failure time threshold. + * + * + * + */ + + + @Override + public void onError(String message) { + Log.e("WiFiPositioning", "Failed to get WiFi position: " + message); + + long now = System.currentTimeMillis(); + + if (!isWifiFailureOngoing) { + wifiFailureStartTime = now; + isWifiFailureOngoing = true; + Log.d("SensorFusion", "WiFi failure streak started."); + } else { + long failureDuration = now - wifiFailureStartTime; + if (failureDuration >= WIFI_FAILURE_TIME_THRESHOLD_MS) { + Log.e("SensorFusion", "WiFi has failed for over 10 seconds. Notifying listeners."); + + if (hasPositionListeners()) { + for (PositionListener listener : positionListeners) { + listener.onPositionUpdate(PositionListener.UpdateType.WIFI_ERROR, null); + } + } + + // Optional: Reset so you only notify once every 10s streak + wifiFailureStartTime = now; + } + } + } + + + + + + }); + + + + + + + } catch (JSONException e) { // Catching error while making JSON object, to prevent crashes - // Error log to keep record of errors (for secure programming and maintainability) - Log.e("jsonErrors","Error creating json object"+e.toString()); + Log.e("jsonErrors", "Error creating json object" + e.toString()); } } + // Callback Example Function /** * Function to create a request to obtain a wifi location for the obtained wifi fingerprint * using Volley Callback */ - private void createWifiPositionRequestCallback(){ + private void createWifiPositionRequestCallback() { try { // Creating a JSON object to store the WiFi access points - JSONObject wifiAccessPoints=new JSONObject(); - for (Wifi data : this.wifiList){ + JSONObject wifiAccessPoints = new JSONObject(); + for (Wifi data : this.wifiList) { wifiAccessPoints.put(String.valueOf(data.getBssid()), data.getLevel()); } // Creating POST Request @@ -524,20 +641,24 @@ private void createWifiPositionRequestCallback(){ this.wiFiPositioning.request(wifiFingerPrint, new WiFiPositioning.VolleyCallback() { @Override public void onSuccess(LatLng wifiLocation, int floor) { - // Handle the success response + // Store WiFi position and notify listeners, but don't update fusion + wifiPosition = wifiLocation; + + // Call fusion update with WiFi + updateFusionWithWifi(); + } @Override public void onError(String message) { // Handle the error response + Log.e("WiFiFusion", "Failed to get WiFi position: " + message); } }); } catch (JSONException e) { // Catching error while making JSON object, to prevent crashes - // Error log to keep record of errors (for secure programming and maintainability) - Log.e("jsonErrors","Error creating json object"+e.toString()); + Log.e("jsonErrors", "Error creating json object" + e.toString()); } - } /** @@ -556,6 +677,26 @@ public int getWifiFloor(){ return this.wiFiPositioning.getFloor(); } + /** + * Check if step detection is available and working + * @return true if step detection is available, false otherwise + */ + public boolean isStepDetectionWorking() { + boolean hasSensor = stepDetectionSensor != null && + stepDetectionSensor.sensor != null; + + long timeSinceLastStep = System.currentTimeMillis() - lastStepTime; + boolean recentlyDetectedStep = lastStepTime > 0 && timeSinceLastStep < 30000; // 30 seconds + + String status = hasSensor ? + (recentlyDetectedStep ? "working" : "available but no recent steps") : + "unavailable"; + + Log.d("SensorFusion", "Step detection status: " + status); + + return hasSensor; + } + /** * Method used for converting an array of orientation angles into a rotation matrix. * @@ -646,6 +787,24 @@ public float[] getGNSSLatitude(boolean start) { return latLong; } + /** + * Getter function for core location data including altitude. + * + * @param start set true to get the initial location + * @return longitude and latitude data in a double[3]. + */ + public double[] getGNSSLatLngAlt(boolean start) { + double[] latLongAlt = new double[3]; + if (!start) { + latLongAlt[0] = latitude; + latLongAlt[1] = longitude; + latLongAlt[2] = altitude; + } else { + latLongAlt = referencePosition; + } + return latLongAlt; + } + /** * Setter function for core location data. * @@ -655,6 +814,14 @@ public void setStartGNSSLatitude(float[] startPosition){ startLocation = startPosition; } + /** + * Setter function for core location data. + * + * @param startPosition contains the initial location set by the user + */ + public void setStartGNSSLatLngAlt(double[] startPosition) { + this.referencePosition = startPosition; + } /** * Function to redraw path in corrections fragment. @@ -765,6 +932,8 @@ public boolean getElevator() { return this.elevator; } + public double[] getReferencePosition() {return this.referencePosition; } + /** * Estimates position of the phone based on proximity and light sensors. * @@ -780,6 +949,53 @@ public int getHoldMode(){ } } + // Position fusion getters and setters + + /** + * Registers a listener to receive position updates. + * + * @param listener The listener to register + */ + public void registerPositionListener(PositionListener listener) { + if (!positionListeners.contains(listener)) { + positionListeners.add(listener); + } + } + + /** + * Unregisters a position listener. + * + * @param listener The listener to unregister + */ + public void unregisterPositionListener(PositionListener listener) { + positionListeners.remove(listener); + } + + /** + * Notifies all registered listeners of a position update. + * + * @param updateType The type of position update + * @param position The updated position (may be null for some update types) + */ + private void notifyPositionListeners(PositionListener.UpdateType updateType, LatLng position) { + if (positionListeners.isEmpty()) { + Log.w("SensorFusion", "No position listeners registered to notify about " + updateType); + return; + } + + if (position == null) { + Log.w("SensorFusion", "Cannot notify listeners: position is null for update type " + updateType); + return; + } + + Log.d("SensorFusion", "Notifying " + positionListeners.size() + + " listeners of " + updateType + " update: " + position.latitude + ", " + position.longitude); + + for (PositionListener listener : positionListeners) { + listener.onPositionUpdate(updateType, position); + } + } + //endregion //region Start/Stop @@ -852,8 +1068,8 @@ public void stopListening() { * @see Traj object for storing data. */ public void startRecording() { - // If wakeLock is null (e.g. not initialized or was cleared), reinitialize it. - if (wakeLock == null) { + // Check if wakeLock is null or was released, and reinitialize it if needed + if (wakeLock == null || !wakeLock.isHeld()) { PowerManager powerManager = (PowerManager) this.appContext.getSystemService(Context.POWER_SERVICE); wakeLock = powerManager.newWakeLock(PowerManager.PARTIAL_WAKE_LOCK, "MyApp::MyWakelockTag"); } @@ -863,7 +1079,8 @@ public void startRecording() { this.stepCounter = 0; this.absoluteStartTime = System.currentTimeMillis(); this.bootTime = SystemClock.uptimeMillis(); - // Protobuf trajectory class for sending sensor data to restful API + + // Initialize trajectory builder this.trajectory = Traj.Trajectory.newBuilder() .setAndroidVersion(Build.VERSION.RELEASE) .setStartTimestamp(absoluteStartTime) @@ -873,18 +1090,30 @@ public void startRecording() { .setBarometerInfo(createInfoBuilder(barometerSensor)) .setLightSensorInfo(createInfoBuilder(lightSensor)); - - this.storeTrajectoryTimer = new Timer(); this.storeTrajectoryTimer.schedule(new storeDataInTrajectory(), 0, TIME_CONST); this.pdrProcessing.resetPDR(); + + // Load filter coefficient from settings if(settings.getBoolean("overwrite_constants", false)) { this.filter_coefficient = Float.parseFloat(settings.getString("accel_filter", "0.96")); } else { this.filter_coefficient = FILTER_COEFFICIENT; } + + // Initialize reference position if not already set + if (referencePosition[0] == 0 && referencePosition[1] == 0) { + referencePosition[0] = latitude; + referencePosition[1] = longitude; + referencePosition[2] = altitude; + } + + // Initialize fusion algorithm + initializeFusionAlgorithm(); } + + /** * Disables saving sensor values to the trajectory object. * @@ -900,9 +1129,16 @@ public void stopRecording() { this.saveRecording = false; storeTrajectoryTimer.cancel(); } - if(wakeLock.isHeld()) { + + // Release wakeLock if it's held + if(wakeLock != null && wakeLock.isHeld()) { this.wakeLock.release(); } + + // Clean up fusion algorithm + if (fusionAlgorithm != null) { + fusionAlgorithm.reset(); + } } //endregion @@ -950,19 +1186,19 @@ private class storeDataInTrajectory extends TimerTask { public void run() { // Store IMU and magnetometer data in Trajectory class trajectory.addImuData(Traj.Motion_Sample.newBuilder() - .setRelativeTimestamp(SystemClock.uptimeMillis()-bootTime) - .setAccX(acceleration[0]) - .setAccY(acceleration[1]) - .setAccZ(acceleration[2]) - .setGyrX(angularVelocity[0]) - .setGyrY(angularVelocity[1]) - .setGyrZ(angularVelocity[2]) - .setGyrZ(angularVelocity[2]) - .setRotationVectorX(rotation[0]) - .setRotationVectorY(rotation[1]) - .setRotationVectorZ(rotation[2]) - .setRotationVectorW(rotation[3]) - .setStepCount(stepCounter)) + .setRelativeTimestamp(SystemClock.uptimeMillis()-bootTime) + .setAccX(acceleration[0]) + .setAccY(acceleration[1]) + .setAccZ(acceleration[2]) + .setGyrX(angularVelocity[0]) + .setGyrY(angularVelocity[1]) + .setGyrZ(angularVelocity[2]) + .setGyrZ(angularVelocity[2]) + .setRotationVectorX(rotation[0]) + .setRotationVectorY(rotation[1]) + .setRotationVectorZ(rotation[2]) + .setRotationVectorW(rotation[3]) + .setStepCount(stepCounter)) .addPositionData(Traj.Position_Sample.newBuilder() .setMagX(magneticField[0]) .setMagY(magneticField[1]) @@ -1005,10 +1241,324 @@ public void run() { else { counter++; } - } } //endregion -} + //region Position Fusion + + /** + * Starts a timer that triggers updates at regular intervals + */ + private void startUpdateTimer() { + // Cancel any existing timer + if (updateTimer != null) { + updateTimer.cancel(); + updateTimer = null; + } + + // Create a new timer + updateTimer = new Timer("ParticleFilterUpdateTimer"); + updateTimer.schedule(new TimerTask() { + @Override + public void run() { + performIntervalUpdate(); + } + }, (int) 0, UPDATE_INTERVAL_MS); + } + + private void performIntervalUpdate() { + if (fusionAlgorithm != null) { + if (System.currentTimeMillis() - lastStepTime > 1000) { + fusionAlgorithm.staticUpdate(); + fusedPosition = fusionAlgorithm.getFusedPosition(); + // Enhanced logging + if (fusedPosition != null) { + Log.d("SensorFusion", "Fusion after PDR: " + fusedPosition.latitude + ", " + fusedPosition.longitude); + + // Notify listeners + notifyPositionListeners(PositionListener.UpdateType.FUSED_POSITION, fusedPosition); + } else { + Log.e("SensorFusion", "Fusion algorithm returned null position after PDR update"); + } + lastStepTime = System.currentTimeMillis(); + } + } + } + + /** + * Updates the fusion algorithm with new PDR data. + * Called when a new step is detected. + */ + private void updateFusionWithPdr() { + if (fusionAlgorithm == null) { + Log.e("SensorFusion", "Cannot update fusion: fusionAlgorithm is null"); + + // Initialize fusion algorithm if not already done + initializeFusionAlgorithm(); + + // If still null, return + if (fusionAlgorithm == null) return; + } + + // Get current PDR position + float[] pdrPosition = pdrProcessing.getPDRMovement(); + float pdrElevation = getElevation(); + + // Log PDR position for debugging + Log.d("SensorFusion", "PDR update: E=" + pdrPosition[0] + ", N=" + pdrPosition[1]); + + // Update the fusion algorithm with new PDR data + fusionAlgorithm.processPdrUpdate(pdrPosition[0], pdrPosition[1], pdrElevation); + + // Get the fused position + fusedPosition = fusionAlgorithm.getFusedPosition(); + + // Enhanced logging + if (fusedPosition != null) { + Log.d("SensorFusion", "Fusion after PDR: " + fusedPosition.latitude + ", " + fusedPosition.longitude); + + // Notify listeners + notifyPositionListeners(PositionListener.UpdateType.FUSED_POSITION, fusedPosition); + } else { + Log.e("SensorFusion", "Fusion algorithm returned null position after PDR update"); + } + + lastStepTime = System.currentTimeMillis(); + + startUpdateTimer(); + } + + /** + * Updates the fusion algorithm with new GNSS data. + * Called from the location listener when new GNSS data is available. + */ + private void updateFusionWithGnss() { + if (fusionAlgorithm == null) { + Log.e("SensorFusion", "Cannot update fusion: fusionAlgorithm is null"); + + // Initialize fusion algorithm if not already done + initializeFusionAlgorithm(); + + // If still null, return + if (fusionAlgorithm == null) return; + } + + LatLng gnssPosition = new LatLng(latitude, longitude); + + // Log GNSS position for debugging + Log.d("SensorFusion", "GNSS update: " + latitude + ", " + longitude); + + // Update the fusion algorithm with new GNSS data + fusionAlgorithm.processGnssUpdate(gnssPosition, altitude); + + // Get the fused position + fusedPosition = fusionAlgorithm.getFusedPosition(); + + // Enhanced logging + if (fusedPosition != null) { + Log.d("SensorFusion", "Fusion after GNSS: " + fusedPosition.latitude + ", " + fusedPosition.longitude); + + // Notify listeners + notifyPositionListeners(PositionListener.UpdateType.FUSED_POSITION, fusedPosition); + } else { + Log.e("SensorFusion", "Fusion algorithm returned null position after GNSS update"); + } + } + + /** + * Updates the fusion algorithm with new WiFi positioning data. + * Called when new WiFi positioning data is available. + */ + public void updateFusionWithWifi() { + if (fusionAlgorithm == null) { + Log.e("SensorFusion", "Cannot update fusion: fusionAlgorithm is null"); + + // Initialize fusion algorithm if not already done + initializeFusionAlgorithm(); + + // If still null, return + if (fusionAlgorithm == null) return; + } + + // Retrieve WiFi LatLng position and floor + LatLng wifiPosition = getLatLngWifiPositioning(); + int floor = getWifiFloor(); + + // If no WiFi position available. return + if (wifiPosition == null) return; + + // Log WiFi position for debugging + Log.d("SensorFusion", "Wifi LatLng update: " + wifiPosition.latitude + ", " + wifiPosition.longitude); + + fusionAlgorithm.retrieveContext(appContext); + + // Update the fusion algorithm with new GNSS data + fusionAlgorithm.processWifiUpdate(wifiPosition, floor); + + // Get the fused position + fusedPosition = fusionAlgorithm.getFusedPosition(); + + // Enhanced logging + if (fusedPosition != null) { + Log.d("SensorFusion", "Fusion after WiFi: " + fusedPosition.latitude + ", " + fusedPosition.longitude); + + // Notify listeners + notifyPositionListeners(PositionListener.UpdateType.FUSED_POSITION, fusedPosition); + } else { + Log.e("SensorFusion", "Fusion algorithm returned null position after WiFi update"); + } + } + + public void updateFusionWithStairs(PdrProcessing.ElevationDirection elevationDirection){ + if (fusionAlgorithm == null) { + Log.e("SensorFusion", "Cannot update fusion: fusionAlgorithm is null"); + + // Initialize fusion algorithm if not already done + initializeFusionAlgorithm(); + + // If still null, return + if (fusionAlgorithm == null) return; + } + if (this.filterType == "particle") { + fusionAlgorithm.setElevationStatus(elevationDirection); + } + } + + public boolean hasPositionListeners() { + boolean hasListeners = !positionListeners.isEmpty(); + Log.d("SensorFusion", "Position listeners registered: " + hasListeners + + " (Count: " + positionListeners.size() + ")"); + return hasListeners; + } + + + /** + * This replaces any previous implementation of this method + */ + + public void onStepDetected(double pdrEast, double pdrNorth, double altitude, long refTime) { + // Call our simple fusion method - overrides any reference to fusionAlgorithm + updateSimpleFusion(); + } + + public void processGnssUpdate(LatLng position, double altitude) { + // Call our simple fusion method instead of using fusionAlgorithm + updateSimpleFusion(); + } + public void processPdrUpdate(float eastMeters, float northMeters, float altitude) { + updateSimpleFusion(); + } + + /** + * Set the type of fusion filter to use (kalman or particle) + * @param filterType The filter type to use + */ + public void setFilterType(String filterType) { + this.filterType = filterType.toLowerCase(); + } + + /** + * Initialize the fusion algorithm + */ + public void initializeFusionAlgorithm() { + // Ensure reference position is valid + if (referencePosition[0] == 0 && referencePosition[1] == 0) { + // If we have a valid GNSS position, use it + if (latitude != 0 || longitude != 0) { + referencePosition[0] = latitude; + referencePosition[1] = longitude; + referencePosition[2] = altitude; + Log.d("SensorFusion", "Using current GNSS position as reference: " + + latitude + ", " + longitude); + } else { + Log.w("SensorFusion", "No valid reference position available for fusion"); + return; + } + } + + // Log the reference position being used + Log.d("SensorFusion", "Initializing fusion algorithm with reference position: " + + referencePosition[0] + ", " + referencePosition[1] + ", " + referencePosition[2]); + + // Create fusion algorithm based on selected filter type + if ("particle".equals(filterType)) { + Log.d("SensorFusion", "Using Particle Filter fusion algorithm"); + fusionAlgorithm = new ParticleFilterFusion(NUM_PARTICLES, referencePosition); + } else { + // Default to Kalman filter + Log.d("SensorFusion", "Using Kalman Filter fusion algorithm"); + fusionAlgorithm = new KalmanFilterFusion(referencePosition); + } + } + + /** + * Getter for current fusion position + */ + public LatLng getFusedPosition() { + // Check if we have a valid fusedPosition from our simple fusion + if (fusedPosition != null) { + return fusedPosition; + } + + // Return current location as fallback + if (latitude != 0 || longitude != 0) { + return new LatLng(latitude, longitude); + } + + // Return reference position as last resort + return new LatLng(referencePosition[0], referencePosition[1]); + } + + /** + * Simple test fusion method that averages PDR and GNSS positions + * This is for debugging purposes only + */ + /** + * Simple fusion method that averages PDR and GNSS positions + * Used for testing coordinate transformation issues + */ + private void updateSimpleFusion() { + // Get current PDR position + float[] pdrPosition = pdrProcessing.getPDRMovement(); + + // Skip if PDR or GNSS data is invalid (0,0) + if ((pdrPosition[0] == 0 && pdrPosition[1] == 0) || + (latitude == 0 && longitude == 0)) { + Log.d("SimpleFusion", "Can't fuse: missing valid positions"); + return; + } + + // Get reference position + if (referencePosition[0] == 0 && referencePosition[1] == 0) { + // No reference position set, try to use current position + Log.e("SimpleFusion", "No reference position, using current GNSS as reference"); + referencePosition[0] = latitude; + referencePosition[1] = longitude; + referencePosition[2] = altitude; + } + + try { + // Use our SimpleFusionConverter to do the fusion + LatLng fusedPos = SimpleFusionConverter.fusePdrAndGnss( + pdrPosition[0], pdrPosition[1], getElevation(), + latitude, longitude, + referencePosition[0], referencePosition[1], referencePosition[2] + ); + + // Set the fused position and notify listeners + fusedPosition = fusedPos; + + // Log successful fusion + Log.d("SimpleFusion", "Successfully calculated fusion position: " + + fusedPosition.latitude + ", " + fusedPosition.longitude); + + // Notify listeners - this is critical for updating the map + notifyPositionListeners(PositionListener.UpdateType.FUSED_POSITION, fusedPosition); + } catch (Exception e) { + Log.e("SimpleFusion", "Error in fusion calculation: " + e.getMessage(), e); + } + } + +} \ No newline at end of file diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiDataProcessor.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiDataProcessor.java index fa8a17dd..1ed609f8 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiDataProcessor.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiDataProcessor.java @@ -41,6 +41,7 @@ public class WifiDataProcessor implements Observable { //Time over which a new scan will be initiated + // TODO: Check whether inc/decreasing interval improves positioning private static final long scanInterval = 5000; // Application context for handling permissions and WifiManager instances @@ -211,12 +212,10 @@ private boolean checkWifiPermissions() { private void startWifiScan() { //Check settings for wifi permissions if(checkWifiPermissions()) { - //if(sharedPreferences.getBoolean("wifi", false)) { //Register broadcast receiver for wifi scans context.registerReceiver(wifiScanReceiver, new IntentFilter(WifiManager.SCAN_RESULTS_AVAILABLE_ACTION)); wifiManager.startScan(); - //} } } diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/CoordinateConverter.java b/app/src/main/java/com/openpositioning/PositionMe/utils/CoordinateConverter.java new file mode 100644 index 00000000..1856d095 --- /dev/null +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/CoordinateConverter.java @@ -0,0 +1,449 @@ +package com.openpositioning.PositionMe.utils; + +import android.util.Log; +import com.google.android.gms.maps.model.LatLng; + +/** + * Provides utility functions for converting coordinates between different + * reference systems: Geodetic (WGS84 Latitude, Longitude, Height), + * ECEF (Earth-Centered, Earth-Fixed), and ENU (East, North, Up). + * + *

This implementation is based on standard geodetic formulas and ensures + * consistency across transformations. All angles used as input parameters are expected + * in degrees, while internal calculations often use radians.

+ * + * @author Michal Wiercigroch + */ +public final class CoordinateConverter { // Added final as it's a utility class with only static members + + // Logger Tag + private static final String TAG = CoordinateConverter.class.getSimpleName(); + // Flag to enable/disable detailed logging for debugging + private static final boolean ENABLE_DEBUG_LOGGING = true; // Set to false for release builds + + // --- WGS84 Ellipsoid Constants --- + private static final double WGS84_SEMI_MAJOR_AXIS = 6378137.0; // a, semi-major axis (meters) + private static final double WGS84_SEMI_MINOR_AXIS = 6356752.31424; // b, semi-minor axis (meters) + private static final double WGS84_FLATTENING = (WGS84_SEMI_MAJOR_AXIS - WGS84_SEMI_MINOR_AXIS) / WGS84_SEMI_MAJOR_AXIS; // f, flattening + private static final double WGS84_ECCENTRICITY_SQUARED = WGS84_FLATTENING * (2.0 - WGS84_FLATTENING); // e^2, first eccentricity squared + // e'^2 = (a^2 - b^2) / b^2, second eccentricity squared (often used in ECEF to Geodetic) + private static final double WGS84_ECCENTRICITY_SQUARED_PRIME = (WGS84_SEMI_MAJOR_AXIS * WGS84_SEMI_MAJOR_AXIS - WGS84_SEMI_MINOR_AXIS * WGS84_SEMI_MINOR_AXIS) / (WGS84_SEMI_MINOR_AXIS * WGS84_SEMI_MINOR_AXIS); + + // --- Angular Conversion Factors --- + private static final double DEGREES_TO_RADIANS = Math.PI / 180.0; + private static final double RADIANS_TO_DEGREES = 180.0 / Math.PI; + + /** + * Private constructor to prevent instantiation of this utility class. + */ + private CoordinateConverter() { + throw new UnsupportedOperationException("CoordinateConverter is a utility class and cannot be instantiated."); + } + + // --- Geodetic <-> ECEF Conversions --- + + /** + * Converts Geodetic coordinates (latitude, longitude, height) to + * ECEF (Earth-Centered, Earth-Fixed) coordinates. + * + * @param latitudeDegrees Latitude in decimal degrees. + * @param longitudeDegrees Longitude in decimal degrees. + * @param heightMeters Height above the WGS84 ellipsoid in meters. + * @return A double array containing ECEF coordinates [X, Y, Z] in meters. + */ + public static double[] convertGeodeticToEcef(final double latitudeDegrees, final double longitudeDegrees, final double heightMeters) { + final double latRad = latitudeDegrees * DEGREES_TO_RADIANS; + final double lonRad = longitudeDegrees * DEGREES_TO_RADIANS; + final double cosLat = Math.cos(latRad); + final double sinLat = Math.sin(latRad); + final double cosLon = Math.cos(lonRad); + final double sinLon = Math.sin(lonRad); + + // Calculate the prime vertical radius of curvature (N) + final double N = WGS84_SEMI_MAJOR_AXIS / Math.sqrt(1.0 - WGS84_ECCENTRICITY_SQUARED * sinLat * sinLat); + + // Calculate ECEF coordinates + final double[] ecef = new double[3]; + ecef[0] = (N + heightMeters) * cosLat * cosLon; // X (meters) + ecef[1] = (N + heightMeters) * cosLat * sinLon; // Y (meters) + ecef[2] = (N * (1.0 - WGS84_ECCENTRICITY_SQUARED) + heightMeters) * sinLat; // Z (meters) + + if (ENABLE_DEBUG_LOGGING) { + Log.d(TAG, String.format("convertGeodeticToEcef: (Lat=%.6f°, Lon=%.6f°, H=%.2fm) -> (X=%.3fm, Y=%.3fm, Z=%.3fm)", + latitudeDegrees, longitudeDegrees, heightMeters, ecef[0], ecef[1], ecef[2])); + } + + return ecef; + } + + /** + * Converts ECEF (Earth-Centered, Earth-Fixed) coordinates to + * Geodetic coordinates (latitude, longitude, height). + * Uses Bowring's iterative method (approximated here) for improved accuracy, especially near poles. + * + * @param ecefX ECEF X coordinate in meters. + * @param ecefY ECEF Y coordinate in meters. + * @param ecefZ ECEF Z coordinate in meters. + * @return A double array containing Geodetic coordinates [latitude (deg), longitude (deg), height (m)]. + */ + public static double[] convertEcefToGeodetic(final double ecefX, final double ecefY, final double ecefZ) { + final double a = WGS84_SEMI_MAJOR_AXIS; + final double b = WGS84_SEMI_MINOR_AXIS; + final double eSquared = WGS84_ECCENTRICITY_SQUARED; + final double ePrimeSquared = WGS84_ECCENTRICITY_SQUARED_PRIME; // (a^2 - b^2) / b^2 + + // Calculate distance from Z-axis (projection onto equatorial plane) + final double p = Math.sqrt(ecefX * ecefX + ecefY * ecefY); + + // Calculate longitude (atan2 ensures correct quadrant) + final double longitudeRad = Math.atan2(ecefY, ecefX); + + // Bowring's method for latitude (iterative, but this is a common first approximation) + final double theta = Math.atan2(ecefZ * a, p * b); // Angle for parametric latitude approximation + final double sinTheta = Math.sin(theta); + final double cosTheta = Math.cos(theta); + + final double latitudeRad = Math.atan2( + ecefZ + ePrimeSquared * b * sinTheta * sinTheta * sinTheta, + p - eSquared * a * cosTheta * cosTheta * cosTheta + ); + + // Calculate height + final double sinLat = Math.sin(latitudeRad); + final double cosLat = Math.cos(latitudeRad); + // Prime vertical radius of curvature + final double N = a / Math.sqrt(1.0 - eSquared * sinLat * sinLat); + final double heightMeters = (p / cosLat) - N; + + // Prepare result array + final double[] geodetic = new double[3]; + geodetic[0] = latitudeRad * RADIANS_TO_DEGREES; // Latitude (degrees) + geodetic[1] = longitudeRad * RADIANS_TO_DEGREES; // Longitude (degrees) + geodetic[2] = heightMeters; // Height (meters) + + if (ENABLE_DEBUG_LOGGING) { + Log.d(TAG, String.format("convertEcefToGeodetic: (X=%.3fm, Y=%.3fm, Z=%.3fm) -> (Lat=%.6f°, Lon=%.6f°, H=%.2fm)", + ecefX, ecefY, ecefZ, geodetic[0], geodetic[1], geodetic[2])); + } + + return geodetic; + } + + // --- ECEF Delta <-> ENU Conversions --- + + /** + * Converts a delta in ECEF coordinates (dX, dY, dZ) to local ENU + * (East, North, Up) coordinates relative to a reference point's latitude and longitude. + * This transformation uses the rotation matrix based on the reference location. + * + * @param deltaX Change in ECEF X coordinate in meters. + * @param deltaY Change in ECEF Y coordinate in meters. + * @param deltaZ Change in ECEF Z coordinate in meters. + * @param refLatitudeDegrees Reference point latitude in decimal degrees. + * @param refLongitudeDegrees Reference point longitude in decimal degrees. + * @return A double array containing ENU coordinates [East (m), North (m), Up (m)]. + */ + public static double[] convertEcefDeltaToEnu(final double deltaX, final double deltaY, final double deltaZ, + final double refLatitudeDegrees, final double refLongitudeDegrees) { + final double latRad = refLatitudeDegrees * DEGREES_TO_RADIANS; + final double lonRad = refLongitudeDegrees * DEGREES_TO_RADIANS; + final double cosLat = Math.cos(latRad); + final double sinLat = Math.sin(latRad); + final double cosLon = Math.cos(lonRad); + final double sinLon = Math.sin(lonRad); + + // Rotation matrix (transposed) application: + // [-sinLon cosLon 0 ] [deltaX] + // [-sinLat*cosLon -sinLat*sinLon cosLat] [deltaY] + // [ cosLat*cosLon cosLat*sinLon sinLat] [deltaZ] + + final double[] enu = new double[3]; + final double t = cosLon * deltaX + sinLon * deltaY; // Temporary variable for common dot product part + + enu[0] = -sinLon * deltaX + cosLon * deltaY; // East (meters) + enu[1] = -sinLat * t + cosLat * deltaZ; // North (meters) + enu[2] = cosLat * t + sinLat * deltaZ; // Up (meters) + + if (ENABLE_DEBUG_LOGGING) { + Log.d(TAG, String.format("convertEcefDeltaToEnu: (dX=%.3fm, dY=%.3fm, dZ=%.3fm) @ (RefLat=%.6f°, RefLon=%.6f°) -> (E=%.3fm, N=%.3fm, U=%.3fm)", + deltaX, deltaY, deltaZ, refLatitudeDegrees, refLongitudeDegrees, enu[0], enu[1], enu[2])); + } + + return enu; + } + + /** + * Converts local ENU (East, North, Up) coordinates relative to a reference point + * back to a delta in ECEF coordinates (dX, dY, dZ). This is the inverse + * transformation of {@link #convertEcefDeltaToEnu}. + * + * @param eastMeters East coordinate in meters. + * @param northMeters North coordinate in meters. + * @param upMeters Up coordinate in meters. + * @param refLatitudeDegrees Reference point latitude in decimal degrees. + * @param refLongitudeDegrees Reference point longitude in decimal degrees. + * @return A double array containing ECEF delta coordinates [dX (m), dY (m), dZ (m)]. + */ + public static double[] convertEnuToEcefDelta(final double eastMeters, final double northMeters, final double upMeters, + final double refLatitudeDegrees, final double refLongitudeDegrees) { + final double latRad = refLatitudeDegrees * DEGREES_TO_RADIANS; + final double lonRad = refLongitudeDegrees * DEGREES_TO_RADIANS; + final double cosLat = Math.cos(latRad); + final double sinLat = Math.sin(latRad); + final double cosLon = Math.cos(lonRad); + final double sinLon = Math.sin(lonRad); + + // Inverse rotation matrix application: + // [-sinLon -sinLat*cosLon cosLat*cosLon] [East ] + // [ cosLon -sinLat*sinLon cosLat*sinLon] [North] + // [ 0 cosLat sinLat ] [Up ] + + final double[] ecefDelta = new double[3]; + ecefDelta[0] = -sinLon * eastMeters - sinLat * cosLon * northMeters + cosLat * cosLon * upMeters; // dX (meters) + ecefDelta[1] = cosLon * eastMeters - sinLat * sinLon * northMeters + cosLat * sinLon * upMeters; // dY (meters) + ecefDelta[2] = cosLat * northMeters + sinLat * upMeters; // dZ (meters) + + if (ENABLE_DEBUG_LOGGING) { + Log.d(TAG, String.format("convertEnuToEcefDelta: (E=%.3fm, N=%.3fm, U=%.3fm) @ (RefLat=%.6f°, RefLon=%.6f°) -> (dX=%.3fm, dY=%.3fm, dZ=%.3fm)", + eastMeters, northMeters, upMeters, refLatitudeDegrees, refLongitudeDegrees, ecefDelta[0], ecefDelta[1], ecefDelta[2])); + } + + return ecefDelta; + } + + // --- Composite Conversions --- + + /** + * Converts Geodetic coordinates (latitude, longitude, height) to local ENU + * (East, North, Up) coordinates relative to a reference Geodetic point. + * This involves converting both points to ECEF, finding the ECEF difference, + * and then rotating that difference vector into the ENU frame of the reference point. + * + * @param latitudeDegrees Latitude of the point in decimal degrees. + * @param longitudeDegrees Longitude of the point in decimal degrees. + * @param heightMeters Height of the point above the ellipsoid in meters. + * @param refLatitudeDegrees Reference point latitude in decimal degrees. + * @param refLongitudeDegrees Reference point longitude in decimal degrees. + * @param refHeightMeters Reference point height above the ellipsoid in meters. + * @return A double array containing ENU coordinates [East (m), North (m), Up (m)]. + */ + public static double[] convertGeodeticToEnu(final double latitudeDegrees, final double longitudeDegrees, final double heightMeters, + final double refLatitudeDegrees, final double refLongitudeDegrees, final double refHeightMeters) { + // Step 1: Convert both points from Geodetic to ECEF + final double[] pointEcef = convertGeodeticToEcef(latitudeDegrees, longitudeDegrees, heightMeters); + final double[] referenceEcef = convertGeodeticToEcef(refLatitudeDegrees, refLongitudeDegrees, refHeightMeters); + + // Step 2: Calculate the difference vector in ECEF coordinates + final double deltaX = pointEcef[0] - referenceEcef[0]; + final double deltaY = pointEcef[1] - referenceEcef[1]; + final double deltaZ = pointEcef[2] - referenceEcef[2]; + + // Step 3: Convert the ECEF difference vector to ENU coordinates using the reference location + final double[] enu = convertEcefDeltaToEnu(deltaX, deltaY, deltaZ, refLatitudeDegrees, refLongitudeDegrees); + + if (ENABLE_DEBUG_LOGGING) { + Log.d(TAG, String.format("convertGeodeticToEnu: (Lat=%.6f°, Lon=%.6f°, H=%.2fm) relative to (RefLat=%.6f°, RefLon=%.6f°, RefH=%.2fm) -> (E=%.3fm, N=%.3fm, U=%.3fm)", + latitudeDegrees, longitudeDegrees, heightMeters, refLatitudeDegrees, refLongitudeDegrees, refHeightMeters, enu[0], enu[1], enu[2])); + } + + return enu; + } + + /** + * Converts local ENU (East, North, Up) coordinates relative to a reference + * Geodetic point back to Geodetic coordinates (latitude, longitude). + * Note: The resulting height is calculated internally but discarded as {@link LatLng} + * only stores latitude and longitude. + * + * @param eastMeters East coordinate in meters. + * @param northMeters North coordinate in meters. + * @param upMeters Up coordinate in meters. + * @param refLatitudeDegrees Reference point latitude in decimal degrees. + * @param refLongitudeDegrees Reference point longitude in decimal degrees. + * @param refHeightMeters Reference point height above the ellipsoid in meters. + * @return A {@link LatLng} object containing the calculated latitude and longitude in degrees. + * Returns the reference LatLng wrapped in a new object on calculation error. + */ + public static LatLng convertEnuToGeodetic(final double eastMeters, final double northMeters, final double upMeters, + final double refLatitudeDegrees, final double refLongitudeDegrees, final double refHeightMeters) { + final LatLng fallbackResult = new LatLng(refLatitudeDegrees, refLongitudeDegrees); + try { + // Step 1: Get ECEF coordinates of the reference point + final double[] referenceEcef = convertGeodeticToEcef(refLatitudeDegrees, refLongitudeDegrees, refHeightMeters); + + // Step 2: Convert ENU coordinates to an ECEF delta vector relative to the reference + final double[] ecefDelta = convertEnuToEcefDelta(eastMeters, northMeters, upMeters, refLatitudeDegrees, refLongitudeDegrees); + + // Step 3: Add the ECEF delta to the reference ECEF coordinates to get the target point's ECEF coordinates + final double[] pointEcef = new double[3]; + pointEcef[0] = referenceEcef[0] + ecefDelta[0]; + pointEcef[1] = referenceEcef[1] + ecefDelta[1]; + pointEcef[2] = referenceEcef[2] + ecefDelta[2]; + + // Step 4: Convert the absolute ECEF coordinates back to Geodetic coordinates + final double[] geodetic = convertEcefToGeodetic(pointEcef[0], pointEcef[1], pointEcef[2]); + + // Step 5: Create LatLng object (height is available in geodetic[2] if needed elsewhere) + final LatLng result = new LatLng(geodetic[0], geodetic[1]); + + if (ENABLE_DEBUG_LOGGING) { + Log.d(TAG, String.format("convertEnuToGeodetic: (E=%.3fm, N=%.3fm, U=%.3fm) relative to (RefLat=%.6f°, RefLon=%.6f°, RefH=%.2fm) -> (Lat=%.6f°, Lon=%.6f°)", + eastMeters, northMeters, upMeters, refLatitudeDegrees, refLongitudeDegrees, refHeightMeters, result.latitude, result.longitude)); + } + + return result; + + } catch (Exception e) { + Log.e(TAG, "Error in convertEnuToGeodetic: " + e.getMessage(), e); + // Return a new instance of the reference location as a fallback + return fallbackResult; + } + } + + // --- Distance and Bearing Calculations --- + + /** + * Calculates the great-circle distance between two points on the Earth's surface + * using the Haversine formula. Assumes a spherical Earth using the WGS84 semi-major axis + * as the radius for simplicity. For higher accuracy over long distances, consider + * Vincenty's formulae on an ellipsoid. + * + * @param lat1Degrees Latitude of the first point in decimal degrees. + * @param lon1Degrees Longitude of the first point in decimal degrees. + * @param lat2Degrees Latitude of the second point in decimal degrees. + * @param lon2Degrees Longitude of the second point in decimal degrees. + * @return The approximate distance between the two points in meters. + */ + public static double calculateHaversineDistance(final double lat1Degrees, final double lon1Degrees, + final double lat2Degrees, final double lon2Degrees) { + final double lat1Rad = lat1Degrees * DEGREES_TO_RADIANS; + final double lon1Rad = lon1Degrees * DEGREES_TO_RADIANS; + final double lat2Rad = lat2Degrees * DEGREES_TO_RADIANS; + final double lon2Rad = lon2Degrees * DEGREES_TO_RADIANS; + + final double deltaLat = lat2Rad - lat1Rad; + final double deltaLon = lon2Rad - lon1Rad; + + // Haversine formula + final double a = Math.sin(deltaLat / 2.0) * Math.sin(deltaLat / 2.0) + + Math.cos(lat1Rad) * Math.cos(lat2Rad) * + Math.sin(deltaLon / 2.0) * Math.sin(deltaLon / 2.0); + final double c = 2.0 * Math.atan2(Math.sqrt(a), Math.sqrt(1.0 - a)); + + // Distance = radius * central angle (using semi-major axis as radius) + return WGS84_SEMI_MAJOR_AXIS * c; + } + + /** + * Calculates the Euclidean distance between two points in a 2D ENU plane (East, North). + * Ignores the 'Up' component. + * + * @param east1Meters East coordinate of the first point in meters. + * @param north1Meters North coordinate of the first point in meters. + * @param east2Meters East coordinate of the second point in meters. + * @param north2Meters North coordinate of the second point in meters. + * @return The 2D Euclidean distance between the points in meters. + */ + public static double calculateEnuDistance(final double east1Meters, final double north1Meters, + final double east2Meters, final double north2Meters) { + final double deltaEast = east2Meters - east1Meters; + final double deltaNorth = north2Meters - north1Meters; + return Math.sqrt(deltaEast * deltaEast + deltaNorth * deltaNorth); + } + + /** + * Calculates the bearing (azimuth) from the first ENU point to the second ENU point. + * The bearing is measured clockwise from North (0 degrees). + * + * @param east1Meters East coordinate of the starting point in meters. + * @param north1Meters North coordinate of the starting point in meters. + * @param east2Meters East coordinate of the destination point in meters. + * @param north2Meters North coordinate of the destination point in meters. + * @return The bearing in degrees, ranging from [0, 360). 0° is North, 90° is East, 180° is South, 270° is West. + */ + public static double calculateEnuBearing(final double east1Meters, final double north1Meters, + final double east2Meters, final double north2Meters) { + final double deltaEast = east2Meters - east1Meters; // Corresponds to 'x' in atan2(y, x) + final double deltaNorth = north2Meters - north1Meters; // Corresponds to 'y' in atan2(y, x) + + // Calculate angle using atan2(y, x), which is atan2(deltaNorth, deltaEast) + // Result is in radians, measured counter-clockwise from the East axis (+X). + final double angleRadFromEast = Math.atan2(deltaNorth, deltaEast); + + // Convert to degrees + final double angleDegFromEast = angleRadFromEast * RADIANS_TO_DEGREES; + + // Convert angle from "math angle" (0° East, CCW) to "bearing" (0° North, CW) + // Bearing = 90 - Math Angle + double bearingDeg = 90.0 - angleDegFromEast; + + // Normalize bearing to be within [0, 360) degrees + // Equivalent to: bearingDeg = (bearingDeg % 360.0 + 360.0) % 360.0; + if (bearingDeg < 0) { + bearingDeg += 360.0; + } + if (bearingDeg >= 360.0) { // Handle exact multiples of 360 becoming 0 + bearingDeg -= 360.0; + } + + + return bearingDeg; + } + + // --- Angle Utilities --- + + /** + * Normalizes an angle in radians to the range [-PI, PI]. + * + * @param angleRadians The angle in radians. + * @return The equivalent angle normalized to the range [-PI, PI]. + */ + public static double normalizeAngleToPi(double angleRadians) { // Keep mutable param name for loop + // More efficient for angles close to the range than modulo + while (angleRadians > Math.PI) { + angleRadians -= 2.0 * Math.PI; + } + while (angleRadians <= -Math.PI) { // Use <= to include -PI if it maps exactly + angleRadians += 2.0 * Math.PI; + } + return angleRadians; + /* Alternative using modulo (careful with negative results): + angleRadians = angleRadians % (2.0 * Math.PI); + if (angleRadians > Math.PI) { + angleRadians -= 2.0 * Math.PI; + } else if (angleRadians <= -Math.PI) { // Ensure consistency with while loop version + angleRadians += 2.0 * Math.PI; + } + return angleRadians; + */ + } + + /** + * Normalizes an angle in radians to the range [0, 2*PI). + * + * @param angleRadians The angle in radians. + * @return The equivalent angle normalized to the range [0, 2*PI). + */ + public static double normalizeAngleTo2Pi(double angleRadians) { // Keep mutable param name for loop/modulo + // Modulo is generally efficient here + angleRadians = angleRadians % (2.0 * Math.PI); + if (angleRadians < 0) { + angleRadians += 2.0 * Math.PI; + } + // Ensure result is strictly less than 2*PI if input is a multiple of 2*PI + // Although modulo usually handles this, floating point might leave exactly 2*PI + if (angleRadians >= 2.0 * Math.PI) { + angleRadians = 0.0; // or angleRadians -= 2.0 * Math.PI; + } + return angleRadians; + /* While loop alternative: + while (angleRadians >= 2.0 * Math.PI) { + angleRadians -= 2.0 * Math.PI; + } + while (angleRadians < 0) { + angleRadians += 2.0 * Math.PI; + } + return angleRadians; + */ + } +} \ No newline at end of file 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 9d7167df..b44ae9c4 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java @@ -24,7 +24,7 @@ */ public class IndoorMapManager { // To store the map instance - private GoogleMap gMap; + public GoogleMap gMap; //Stores the overlay of the indoor maps private GroundOverlay groundOverlay; // Stores the current Location of user @@ -36,10 +36,10 @@ public class IndoorMapManager { // Floor height of current building private float floorHeight; //Images of the Nucleus Building and Library indoor floor maps - private final List NUCLEUS_MAPS =Arrays.asList( + public final List NUCLEUS_MAPS =Arrays.asList( R.drawable.nucleuslg, R.drawable.nucleusg, R.drawable.nucleus1, R.drawable.nucleus2,R.drawable.nucleus3); - private final List LIBRARY_MAPS =Arrays.asList( + public final List LIBRARY_MAPS =Arrays.asList( R.drawable.libraryg, R.drawable.library1, R.drawable.library2, R.drawable.library3); // South-west and north east Bounds of Nucleus building and library to set the Overlay diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/MeasurementModel.java b/app/src/main/java/com/openpositioning/PositionMe/utils/MeasurementModel.java new file mode 100644 index 00000000..a666e497 --- /dev/null +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/MeasurementModel.java @@ -0,0 +1,255 @@ +package com.openpositioning.PositionMe.utils; + +import android.util.Log; + +/** + * Handles measurement models for different sensors, providing noise characteristics + * and uncertainty estimates for fusion algorithms. + * + *

This class manages sensor measurement uncertainties and provides methods + * for updating and accessing these uncertainty values for use in fusion algorithms. + * It handles PDR, GNSS, and WiFi measurement sources with time-based degradation.

+ * + * @author Michal Wiercigroch + */ +public class MeasurementModel { + private static final String TAG = "MeasurementModel"; + + // Standard deviations for different measurement sources (in meters) + private static final double PDR_BASE_STD = 0.5; // Base PDR noise per step + private static final double GNSS_BASE_STD = 5.0; // Typical GPS accuracy + private static final double WIFI_BASE_STD = 4.0; // Typical WiFi fingerprinting accuracy + + // Maximum uncertainty (meters) to consider a measurement + private static final double MAX_UNCERTAINTY = 50.0; + + // Time relevance thresholds (milliseconds) + private static final long PDR_RELEVANCE_THRESHOLD = 2000; // 2 seconds + private static final long GNSS_RELEVANCE_THRESHOLD = 20000; // 20 seconds + private static final long WIFI_RELEVANCE_THRESHOLD = 20000; // 20 seconds + + // Current measurement uncertainties + private double pdrEastStd; + private double pdrNorthStd; + private double gnssStd; + private double wifiStd; + + // Timestamp of last measurements + private long lastPdrTimestamp; + private long lastGnssTimestamp; + private long lastWifiTimestamp; + + /** + * Constructor to initialize the measurement model with default values. + */ + public MeasurementModel() { + this.pdrEastStd = PDR_BASE_STD; + this.pdrNorthStd = PDR_BASE_STD; + this.gnssStd = GNSS_BASE_STD; + this.wifiStd = WIFI_BASE_STD; + this.lastPdrTimestamp = 0; + this.lastGnssTimestamp = 0; + this.lastWifiTimestamp = 0; + } + + /** + * Updates PDR measurement uncertainty based on step count and time since last update. + * PDR uncertainty grows with each step due to accumulating errors. + * + * @param stepCount Number of steps since reset + * @param currentTimeMillis Current system time in milliseconds + */ + public void updatePdrUncertainty(int stepCount, long currentTimeMillis) { + // PDR uncertainty grows with the square root of number of steps + double stepFactor = Math.sqrt(Math.max(1, stepCount)); + + // Calculate time factor (uncertainty grows with time since last update) + double timeFactor = 1.0; + if (lastPdrTimestamp > 0) { + long timeDiff = currentTimeMillis - lastPdrTimestamp; + timeFactor = 1.0 + Math.min(5.0, timeDiff / 1000.0 / 10.0); // Max 6x increase after 50 seconds + } + + // Update uncertainties + this.pdrEastStd = PDR_BASE_STD * stepFactor * timeFactor; + this.pdrNorthStd = PDR_BASE_STD * stepFactor * timeFactor; + + // Update timestamp + this.lastPdrTimestamp = currentTimeMillis; + + Log.d(TAG, "Updated PDR uncertainty: E=" + pdrEastStd + ", N=" + pdrNorthStd + + " (steps=" + stepCount + ", timeFactor=" + timeFactor + ")"); + } + + /** + * Updates GNSS measurement uncertainty based on reported accuracy and time. + * + * @param accuracy Reported GNSS accuracy in meters (can be null) + * @param currentTimeMillis Current system time in milliseconds + */ + public void updateGnssUncertainty(Float accuracy, long currentTimeMillis) { + double baseUncertainty = GNSS_BASE_STD; + + // Use reported accuracy if available, otherwise use default + if (accuracy != null && accuracy > 0) { + baseUncertainty = accuracy; + } + + // Apply time factor if we have a previous measurement + double timeFactor = 1.0; + if (lastGnssTimestamp > 0) { + long timeDiff = currentTimeMillis - lastGnssTimestamp; + timeFactor = 1.0 + Math.min(3.0, timeDiff / 1000.0 / 20.0); // Max 4x increase after 60 seconds + } + + // Update uncertainty + this.gnssStd = baseUncertainty * timeFactor; + + // Update timestamp + this.lastGnssTimestamp = currentTimeMillis; + + Log.d(TAG, "Updated GNSS uncertainty: " + gnssStd + + " (baseAccuracy=" + baseUncertainty + ", timeFactor=" + timeFactor + ")"); + } + + public void updateWifiUncertainty(Float accuracy, long currentTimeMillis) { + double baseUncertainty = WIFI_BASE_STD; + + // Use reported accuracy if available, otherwise use default + if (accuracy != null && accuracy > 0) { + baseUncertainty = accuracy; + } + + // Apply time factor if we have a previous measurement + double timeFactor = 1.0; + if (lastWifiTimestamp > 0) { + long timeDiff = currentTimeMillis - lastWifiTimestamp; + timeFactor = 1.0 + Math.min(2.0, timeDiff / 1000.0 / 30.0); // Max 3x increase after 60 seconds + } + + // Update uncertainty + this.wifiStd = baseUncertainty * timeFactor; + + // Update timestamp + this.lastWifiTimestamp = currentTimeMillis; + + Log.d(TAG, "Updated WiFi uncertainty: " + wifiStd + + " (baseAccuracy=" + baseUncertainty + ", timeFactor=" + timeFactor + ")"); + } + + /** + * Checks if a PDR measurement is still relevant based on time. + * + * @param currentTimeMillis Current system time in milliseconds + * @return True if PDR data is still relevant, false otherwise + */ + public boolean isPdrRelevant(long currentTimeMillis) { + if (lastPdrTimestamp == 0) { + return false; // No measurements yet + } + + long timeDiff = currentTimeMillis - lastPdrTimestamp; + return timeDiff <= PDR_RELEVANCE_THRESHOLD; + } + + /** + * Checks if a GNSS measurement is still relevant based on time. + * + * @param currentTimeMillis Current system time in milliseconds + * @return True if GNSS data is still relevant, false otherwise + */ + public boolean isGnssRelevant(long currentTimeMillis) { + if (lastGnssTimestamp == 0) { + return false; // No measurements yet + } + + long timeDiff = currentTimeMillis - lastGnssTimestamp; + return timeDiff <= GNSS_RELEVANCE_THRESHOLD; + } + + /** + * Gets the current PDR east uncertainty. + * + * @return Standard deviation in meters + */ + public double getPdrEastStd() { + return pdrEastStd; + } + + /** + * Gets the current PDR north uncertainty. + * + * @return Standard deviation in meters + */ + public double getPdrNorthStd() { + return pdrNorthStd; + } + + /** + * Gets the current GNSS uncertainty. + * + * @return Standard deviation in meters + */ + public double getGnssStd() { + return gnssStd; + } + + /** + * Gets the current GNSS uncertainty. + * + * @return Standard deviation in meters + */ + public double getWifiStd() { + return wifiStd; + } + + /** + * Calculates a weight for PDR measurements in fusion, based on uncertainty. + * Lower uncertainty gives higher weight, capped at maximum value. + * + * @return Weight value between 0.0 and 1.0 + */ + public double getPdrWeight() { + double uncertainty = Math.max(pdrEastStd, pdrNorthStd); + return Math.max(0.0, Math.min(1.0, 1.0 - uncertainty / MAX_UNCERTAINTY)); + } + + /** + * Calculates a weight for GNSS measurements in fusion, based on uncertainty. + * Lower uncertainty gives higher weight, capped at maximum value. + * + * @return Weight value between 0.0 and 1.0 + */ + public double getGnssWeight() { + return Math.max(0.0, Math.min(1.0, 1.0 - gnssStd / MAX_UNCERTAINTY)); + } + + /** + * Gets the timestamp of the last PDR update. + * + * @return Timestamp in milliseconds + */ + public long getLastPdrTimestamp() { + return lastPdrTimestamp; + } + + /** + * Gets the timestamp of the last GNSS update. + * + * @return Timestamp in milliseconds + */ + public long getLastGnssTimestamp() { + return lastGnssTimestamp; + } + + /** + * Resets the measurement model to its initial state. + */ + public void reset() { + this.pdrEastStd = PDR_BASE_STD; + this.pdrNorthStd = PDR_BASE_STD; + this.gnssStd = GNSS_BASE_STD; + this.lastPdrTimestamp = 0; + this.lastGnssTimestamp = 0; + } +} \ No newline at end of file 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..9c740e66 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java @@ -8,6 +8,7 @@ import com.openpositioning.PositionMe.sensors.SensorFusion; +import java.util.ArrayList; import java.util.Arrays; import java.util.Collections; import java.util.List; @@ -38,6 +39,8 @@ public class PdrProcessing { // Threshold under which movement is considered non-existent private static final float epsilon = 0.18f; private static final int MIN_REQUIRED_SAMPLES = 2; + + private static final int MIN_ELEVATIONS_NEEDED = 20; //endregion //region Instance variables @@ -63,6 +66,7 @@ public class PdrProcessing { // Buffer of most recent elevations calculated private CircularFloatBuffer elevationList; + private List relativeElevation; // Buffer for most recent directional acceleration magnitudes private CircularFloatBuffer verticalAccel; @@ -71,6 +75,24 @@ public class PdrProcessing { // Step sum and length aggregation variables private float sumStepLength = 0; private int stepCount = 0; + + // List to store raw elevation values + private List rawElevation = new ArrayList<>(); + + // List to store filtered elevation values + private List filteredElevation = new ArrayList<>(); + + // Butterworth filter parameters + private static final double CUTOFF_FREQUENCY = 0.1; // Adjust as needed (0.0-1.0) + private static final int FILTER_ORDER = 2; // 2nd order filter + private static final double SAMPLING_FREQUENCY = 1.0; // Adjust based on your sampling rate + + // Filter coefficients (will be calculated once) + private double[] a; + private double[] b; + private double[] xv; + private double[] yv; + //endregion /** @@ -128,6 +150,8 @@ public PdrProcessing(Context context) { this.startElevationBuffer = new Float[3]; // Start floor - assumed to be zero this.currentFloor = 0; + + this.relativeElevation = new ArrayList<>(); } /** @@ -205,6 +229,8 @@ public float updateElevation(float absoluteElevation) { // Add to buffer this.elevationList.putNewest(absoluteElevation); + this.relativeElevation.add(this.elevation); + // Check if there was floor movement // Check if there is enough data to evaluate if(this.elevationList.isFull()) { @@ -414,4 +440,145 @@ public float getAverageStepLength(){ return averageStepLength; } + // Elevation direction enum + public enum ElevationDirection { + ASCENDING, DESCENDING, NEUTRAL + } + + public ElevationDirection detectContinuousElevationChange(int minConsecutiveChanges, float minElevationDelta) { + // Need enough data points for detection + if (relativeElevation.size() < MIN_ELEVATIONS_NEEDED) { + return ElevationDirection.NEUTRAL; + } + + // Apply Butterworth low-pass filter to the elevation data + List filteredElevation = applyButterworthFilter(relativeElevation); + + // Count consecutive increases and decreases + int consecutiveIncreases = 0; + int consecutiveDecreases = 0; + float totalDelta = 0; + + // Start from most recent values (assuming newest values are at end of list) + for (int i = filteredElevation.size() - 1; i > 0; i--) { + float currentElevation = filteredElevation.get(i); + float previousElevation = filteredElevation.get(i - 1); + float delta = currentElevation - previousElevation; + + // Check if the change is significant enough + if (Math.abs(delta) >= minElevationDelta) { + if (delta > 0) { + // Elevation increasing + consecutiveIncreases++; + consecutiveDecreases = 0; // Reset opposite counter + totalDelta += delta; + } else if (delta < 0) { + // Elevation decreasing + consecutiveDecreases++; + consecutiveIncreases = 0; // Reset opposite counter + totalDelta += delta; + } + } else { + // Change not significant - reset both counters + consecutiveIncreases = 0; + consecutiveDecreases = 0; + } + + // Check if we've detected enough consecutive changes in the same direction + if (consecutiveIncreases >= minConsecutiveChanges) { + return ElevationDirection.ASCENDING; + } else if (consecutiveDecreases >= minConsecutiveChanges) { + return ElevationDirection.DESCENDING; + } + + // Optional early termination if we've looked at enough data points + if (i < filteredElevation.size() - (minConsecutiveChanges * 2)) { + break; + } + } + + // Check total elevation change as an additional condition + float significantTotalChange = minElevationDelta * minConsecutiveChanges * 0.7f; // 70% threshold + if (totalDelta > significantTotalChange) { + return ElevationDirection.ASCENDING; + } else if (totalDelta < -significantTotalChange) { + return ElevationDirection.DESCENDING; + } + + // No sustained directional change detected + return ElevationDirection.NEUTRAL; + } + + /** + * Helper method to apply a Butterworth low-pass filter to elevation data + * @param inputData List of input elevation readings + * @return List of filtered elevation values + */ + private List applyButterworthFilter(List inputData) { + // Filter parameters + final double cutoffFrequency = 0.1; // Adjust as needed (0.0-1.0) + final int filterOrder = 2; // 2nd order filter + final double samplingFrequency = 1.0; // Adjust based on your sampling rate + + // Calculate filter coefficients + double omega = Math.tan(Math.PI * cutoffFrequency / samplingFrequency); + double scale = 1.0 / (1.0 + Math.sqrt(2.0) * omega + omega * omega); + + // Filter coefficients + double[] b = new double[3]; // Numerator coefficients + b[0] = omega * omega * scale; + b[1] = 2.0 * b[0]; + b[2] = b[0]; + + double[] a = new double[3]; // Denominator coefficients + a[0] = 1.0; + a[1] = 2.0 * (omega * omega - 1.0) * scale; + a[2] = (1.0 - Math.sqrt(2.0) * omega + omega * omega) * scale; + + // Apply the filter + List filteredData = new ArrayList<>(inputData.size()); + + // Initialize with first few values to handle filter startup + if (inputData.size() > 0) { + filteredData.add(inputData.get(0)); + } + if (inputData.size() > 1) { + filteredData.add(inputData.get(1)); + } + + // Apply filter to the rest of the data + double[] x = new double[3]; // Input samples buffer + double[] y = new double[3]; // Output samples buffer + + // Initialize buffers with available data + if (inputData.size() > 0) { + x[0] = inputData.get(0); + y[0] = filteredData.get(0); + } + if (inputData.size() > 1) { + x[1] = inputData.get(1); + y[1] = filteredData.get(1); + } + + // Process remaining samples + for (int i = 2; i < inputData.size(); i++) { + // Shift values in the buffer + x[2] = x[1]; + x[1] = x[0]; + x[0] = inputData.get(i); + + y[2] = y[1]; + y[1] = y[0]; + + // Apply filter equation + y[0] = b[0] * x[0] + b[1] * x[1] + b[2] * x[2] + - a[1] * y[1] - a[2] * y[2]; + + filteredData.add((float) y[0]); + } + + return filteredData; + } } + + diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/PositionListener.java b/app/src/main/java/com/openpositioning/PositionMe/utils/PositionListener.java new file mode 100644 index 00000000..8a766071 --- /dev/null +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/PositionListener.java @@ -0,0 +1,39 @@ +package com.openpositioning.PositionMe.utils; + +import com.google.android.gms.maps.model.LatLng; + +/** + * Interface for listening to position updates from various sources. + * Implementing classes can receive updates from PDR, GNSS, and fused positions. + * + * @author Michal Wiercigroch + */ +public interface PositionListener { + + /** + * Enumeration of different types of position updates. + */ + enum UpdateType { + /** Update from Pedestrian Dead Reckoning */ + PDR_POSITION, + + /** Update from Global Navigation Satellite System */ + GNSS_POSITION, + + /** Update from the fusion algorithm (combined position) */ + FUSED_POSITION, + + /** Update for heading/orientation */ + ORIENTATION_UPDATE, + + WIFI_ERROR + } + + /** + * Called when a position is updated. + * + * @param updateType The type of position update + * @param position The new position (may be null for some update types) + */ + void onPositionUpdate(UpdateType updateType, LatLng position); +} \ No newline at end of file diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/SimpleFusionConverter.java b/app/src/main/java/com/openpositioning/PositionMe/utils/SimpleFusionConverter.java new file mode 100644 index 00000000..3ed5d600 --- /dev/null +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/SimpleFusionConverter.java @@ -0,0 +1,171 @@ +package com.openpositioning.PositionMe.utils; + +import android.util.Log; +import com.google.android.gms.maps.model.LatLng; + +/** + * Utility class for simple fusion of PDR and GNSS positions. + * Uses exact coordinate transformation logic as in CoordinateTransform. + */ +public class SimpleFusionConverter { + + // Constants (same as in CoordinateTransform) + public static final double SEMI_MAJOR_AXIS = 6378137.0; + public static final double SEMI_MINOR_AXIS = 6356752.31424518; + public static final double FLATTENING = (SEMI_MAJOR_AXIS-SEMI_MINOR_AXIS)/SEMI_MAJOR_AXIS; + public static final double ECCENTRICITY_SQUARED = FLATTENING * (2-FLATTENING); + + /** + * Convert PDR position (ENU) to geodetic coordinates (LatLng) and average with GNSS + * @param pdrEast PDR East position in meters + * @param pdrNorth PDR North position in meters + * @param pdrUp PDR Up position in meters + * @param gnssLat GNSS latitude + * @param gnssLng GNSS longitude + * @param refLat Reference latitude + * @param refLng Reference longitude + * @param refAlt Reference altitude + * @return The averaged position as LatLng + */ + public static LatLng fusePdrAndGnss(double pdrEast, double pdrNorth, double pdrUp, + double gnssLat, double gnssLng, + double refLat, double refLng, double refAlt) { + + // First convert PDR position (ENU) to LatLng using exact conversion logic + LatLng pdrLatLng = enuToGeodetic(pdrEast, pdrNorth, pdrUp, refLat, refLng, refAlt); + + // Log all coordinates for debugging + Log.d("SimpleFusion", "Reference: " + refLat + ", " + refLng + ", Alt: " + refAlt); + Log.d("SimpleFusion", "PDR ENU: " + pdrEast + ", " + pdrNorth + ", " + pdrUp); + Log.d("SimpleFusion", "PDR LatLng: " + pdrLatLng.latitude + ", " + pdrLatLng.longitude); + Log.d("SimpleFusion", "GNSS: " + gnssLat + ", " + gnssLng); + + // Simple average of coordinates + double avgLat = (pdrLatLng.latitude + gnssLat) / 2.0; + double avgLng = (pdrLatLng.longitude + gnssLng) / 2.0; + LatLng fusedPosition = new LatLng(avgLat, avgLng); + + Log.d("SimpleFusion", "Fused Position: " + fusedPosition.latitude + ", " + fusedPosition.longitude); + return fusedPosition; + } + + /** + * Converts WSG84 coordinates to Earth-Centered, Earth-Fixed (ECEF) coordinates. + * Exact logic from CoordinateTransform. + */ + public static double[] geodeticToEcef(double latitude, double longitude, double altitude) { + double[] ecefCoords = new double[3]; + double latRad = Math.toRadians(latitude); + double lngRad = Math.toRadians(longitude); + + //Calculate Prime Vertical Radius of Curvature + double N = Math.pow(SEMI_MAJOR_AXIS,2) / + Math.hypot((SEMI_MAJOR_AXIS*Math.cos(latRad)), (SEMI_MINOR_AXIS * Math.sin(latRad))); + + ecefCoords[0] = (N + altitude) * Math.cos(latRad) * Math.cos(lngRad); + ecefCoords[1] = (N + altitude) * Math.cos(latRad) * Math.sin(lngRad); + ecefCoords[2] = (N * Math.pow((SEMI_MINOR_AXIS / SEMI_MAJOR_AXIS),2) + altitude) * Math.sin(latRad); + + return ecefCoords; + } + + /** + * Converts Earth-Centered, Earth-Fixed (ECEF) delta coordinates to East-North-Up (ENU) coordinates. + * Exact logic from CoordinateTransform. + */ + public static double[] ecefToENU(double east, double north, double up, double refLatitude, double refLongitude) { + double[] enuCoords = new double[3]; + double latRad = Math.toRadians(refLatitude); + double lngRad = Math.toRadians(refLongitude); + + double t = Math.cos(lngRad) * east + Math.sin(lngRad) * north; + enuCoords[0] = -Math.sin(lngRad) * east + Math.cos(lngRad) * north; + enuCoords[2] = Math.cos(latRad)*t + Math.sin(latRad) * up; + enuCoords[1] = -Math.sin(latRad) *t + Math.cos(latRad) * up; + + return enuCoords; + } + + /** + * Converts ENU coordinates to ECEF coordinates. + * Exact logic from CoordinateTransform. + */ + public static double[] enuToEcef(double east, double north, double up, double refLatitude, double refLongitude, double refAlt) { + double[] calCoords = new double[3]; + double[] ecefRefCoords = geodeticToEcef(refLatitude, refLongitude, refAlt); + double latRad = Math.toRadians(refLatitude); + double lngRad = Math.toRadians(refLongitude); + + calCoords[0] = (Math.cos(lngRad) * (Math.cos(latRad)*up - Math.sin(latRad)*north) - Math.sin(lngRad)*east) + ecefRefCoords[0]; + calCoords[1] = (Math.sin(lngRad)*(Math.cos(latRad)*up - Math.sin(latRad)*north) + Math.cos(lngRad)*east) + ecefRefCoords[1]; + calCoords[2] = (Math.sin(latRad)*up + Math.cos(latRad)*north) + ecefRefCoords[2]; + + return calCoords; + } + + /** + * Converts ECEF coordinates to geodetic coordinates. + * Exact logic from CoordinateTransform. + */ + public static LatLng ecefToGeodetic(double[] ecefCoords) { + double asq = Math.pow(SEMI_MAJOR_AXIS,2); + double bsq = Math.pow(SEMI_MINOR_AXIS,2); + + double ep = Math.sqrt((asq-bsq)/bsq); + + double p = Math.sqrt(Math.pow(ecefCoords[0],2) + Math.pow(ecefCoords[1],2)); + + double th = Math.atan2(SEMI_MAJOR_AXIS * ecefCoords[2], SEMI_MINOR_AXIS * p); + + double longitude = Math.atan2(ecefCoords[1], ecefCoords[0]); + + double latitude = Math.atan2((ecefCoords[2] + Math.pow(ep,2) * + SEMI_MINOR_AXIS * Math.pow(Math.sin(th),3)), + (p - ECCENTRICITY_SQUARED*SEMI_MAJOR_AXIS*Math.pow(Math.cos(th),3))); + + double N = SEMI_MAJOR_AXIS/ + (Math.sqrt(1-ECCENTRICITY_SQUARED* + Math.pow(Math.sin(latitude),2))); + + double altitude = p / Math.cos(latitude) - N; + + longitude = longitude % (2*Math.PI); + + return new LatLng(toDegrees(latitude), toDegrees(longitude)); + } + + /** + * Converts ENU coordinates to geodetic (LatLng). + * Exact logic from CoordinateTransform. + */ + public static LatLng enuToGeodetic(double east, double north, double up, double refLatitude, double refLongitude, double refAlt) { + double[] ecefCoords = enuToEcef(east, north, up, refLatitude, refLongitude, refAlt); + + // Debug log the ECEF coordinates + Log.d("SimpleFusion", "ECEF Coords: x=" + ecefCoords[0] + ", y=" + ecefCoords[1] + ", z=" + ecefCoords[2]); + + return ecefToGeodetic(ecefCoords); + } + + /** + * Converts geodetic coordinates to ENU. + * Exact logic from CoordinateTransform. + */ + public static double[] geodeticToEnu(double latitude, double longitude, double altitude, + double refLatitude, double refLongitude, double refAltitude) { + double[] newPosition = geodeticToEcef(latitude, longitude, altitude); + double[] ecefRefCoords = geodeticToEcef(refLatitude, refLongitude, refAltitude); + + return ecefToENU((newPosition[0]-ecefRefCoords[0]), + (newPosition[1]-ecefRefCoords[1]), + (newPosition[2]-ecefRefCoords[2]), + refLatitude, refLongitude); + } + + /** + * Helper method to convert radians to degrees. + */ + public static double toDegrees(double val) { + return val * (180/Math.PI); + } +} \ No newline at end of file diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/WallDetectionManager.java b/app/src/main/java/com/openpositioning/PositionMe/utils/WallDetectionManager.java new file mode 100644 index 00000000..d6783142 --- /dev/null +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/WallDetectionManager.java @@ -0,0 +1,550 @@ +package com.openpositioning.PositionMe.utils; + +import android.content.Context; +import android.graphics.Bitmap; +import android.graphics.BitmapFactory; +import android.graphics.Color; +import android.graphics.Point; +import android.util.Log; + +import com.google.android.gms.maps.model.LatLng; +import com.google.android.gms.maps.model.LatLngBounds; +import com.openpositioning.PositionMe.R; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.HashMap; +import java.util.List; +import java.util.Map; + +/** + * Handles wall detection from floor plan images and trajectory intersection checking. + * + *

This class processes floor plan images to extract wall data and provides + * methods to check if a trajectory would intersect with walls. It supports + * multiple buildings and floors. + * + * @author Nick Manturov + */ +public class WallDetectionManager extends BuildingPolygon{ + private static final String TAG = "WallDetectionManager"; + // Threshold for detecting wall pixels (black lines) - may need tuning + private static final int WALL_COLOR_THRESHOLD = 220; // Lower values are closer to black + + // Maps to store processed wall data for each floor + private final Map> xWallsMap; + private final List xWalls; + private final Map> yWallsMap; + private final List yWalls; + // Reference to the parent IndoorMapManager + + private double[] referencePosition; + // Building type constants + private static final int BUILDING_NUCLEUS = 1; + private static final int BUILDING_LIBRARY = 2; + + //Average Floor Heights of the Buildings + public static final float NUCLEUS_FLOOR_HEIGHT = 4.2F; + public static final float LIBRARY_FLOOR_HEIGHT = 3.6F; + + private int widthImg; + private int heightImg; + + public final List NUCLEUS_MAPS = Arrays.asList( + R.drawable.nucleuslg_simplified, R.drawable.nucleusg_simplified, R.drawable.nucleus1_simplified, + R.drawable.nucleus2_simplified,R.drawable.nucleus3_simplified); + private double[] ReferenceEastSouth; + public final List LIBRARY_MAPS =Arrays.asList( + R.drawable.libraryg_simplified); + private double widthENU; + private double heightENU; + private double ENUtoPixRatioWidth; + private double ENUtoPixRatioHeight; + + + /** + * Constructs a WallDetectionManager with a reference position. + * + * @param referencePosition the reference position for coordinate conversion + */ + public WallDetectionManager(double[] referencePosition) { + this.xWallsMap = new HashMap<>(); + this.yWallsMap = new HashMap<>(); + this.yWalls = new ArrayList<>(); + this.xWalls = new ArrayList<>(); + this.referencePosition = referencePosition; + this.heightImg = 0; + this.widthImg = 0; + Log.d(TAG, "Wall data initialized successfully"); + } + + /** + * Initializes wall detection data for all floor plans. + * + * @param context Android context to load resources + */ + public void initializeWallData(Context context) { + try { + // Process Nucleus building floor plans + for (int i = 0; i < NUCLEUS_MAPS.size(); i++) { + processFloorPlan(context, NUCLEUS_MAPS.get(i), BUILDING_NUCLEUS, i); + } + + // Process Library building floor plans + for (int i = 0; i < LIBRARY_MAPS.size(); i++) { + processFloorPlan(context, LIBRARY_MAPS.get(i), BUILDING_LIBRARY, i); + } + + Log.d(TAG, "Wall data initialized successfully"); + } catch (Exception e) { + Log.e(TAG, "Error initializing wall data", e); + } + } + + /** + * Processes a floor plan image to extract wall data. + * + * @param context Android context to load resources + * @param resourceId Resource ID of the floor plan image + * @param buildingType Type of building (NUCLEUS or LIBRARY) + * @param floor Floor number + */ + private void processFloorPlan(Context context, int resourceId, int buildingType, int floor) { + try { + // Create new lists for this floor plan + List floorXWalls = new ArrayList<>(); + List floorYWalls = new ArrayList<>(); + + // Load image as bitmap + Bitmap floorPlan = BitmapFactory.decodeResource(context.getResources(), resourceId); + if (floorPlan == null) { + Log.e(TAG, "Failed to load floor plan: " + resourceId); + return; + } + + // Extract wall lines from the bitmap + extractWallsFromBitmap(floorPlan, buildingType, floor, floorXWalls, floorYWalls); + + // Store the extracted walls with a unique key for building and floor + xWallsMap.put(generateMapKey(buildingType, floor), floorXWalls); + yWallsMap.put(generateMapKey(buildingType, floor), floorYWalls); + + // Recycle bitmap to free memory + floorPlan.recycle(); + + Log.d(TAG, "Processed floor plan: " + resourceId + ", extracted " + + (floorXWalls.size() + floorYWalls.size()) + " wall segments"); + } catch (Exception e) { + Log.e(TAG, "Error processing floor plan: " + resourceId, e); + } + } + + /** + * Generates a unique key for the wallsMap. + * + * @param buildingType building identifier + * @param floor floor number + * @return unique key for map storage + */ + private int generateMapKey(int buildingType, int floor) { + return buildingType * 100 + floor; + } + + /** + * Extracts wall lines from a bitmap image. + * + * @param bitmap the floor plan bitmap + * @param buildingType type of building (NUCLEUS or LIBRARY) + * @param floor floor number + * @param xWallsList list to store horizontal wall lines + * @param yWallsList list to store vertical wall lines + */ + private void extractWallsFromBitmap(Bitmap bitmap, int buildingType, int floor, + List xWallsList, List yWallsList) { + + this.widthImg = bitmap.getWidth(); + this.heightImg = bitmap.getHeight(); + + setBounds(buildingType, floor); + + // Create a 2D array to represent wall pixels + boolean[][] wallPixels = new boolean[this.widthImg][this.heightImg]; + + // Identify wall pixels (black lines) + for (int x = 0; x < this.widthImg; x++) { + for (int y = 0; y < this.heightImg; y++) { + int pixel = bitmap.getPixel(x, y); + int red = Color.red(pixel); + int green = Color.green(pixel); + int blue = Color.blue(pixel); + + // Check if pixel is dark enough to be considered a wall + if (red < WALL_COLOR_THRESHOLD && green < WALL_COLOR_THRESHOLD && blue < WALL_COLOR_THRESHOLD) { + wallPixels[x][y] = true; + } + } + } + + // Convert wall pixels to line segments (simplified approach) + // For horizontal lines + for (int y = 0; y < this.heightImg; y++) { + int startX = -1; + for (int x = 0; x < this.widthImg; x++) { + if (wallPixels[x][y] && startX == -1) { + startX = x; + } else if (!wallPixels[x][y] && startX != -1) { + // End of line segment + if (x - startX > 30) { // Only consider lines longer than 15 pixels + xWallsList.add(getXLine(startX, x-1, y)); + } + startX = -1; + } + } + // Check if line extends to the edge + if (startX != -1) { + xWallsList.add(getXLine(startX, this.widthImg-1, y)); + } + } + + // For vertical lines + for (int x = 0; x < this.widthImg; x++) { + int startY = -1; + for (int y = 0; y < this.heightImg; y++) { + if (wallPixels[x][y] && startY == -1) { + startY = y; + } else if (!wallPixels[x][y] && startY != -1) { + // End of line segment + if (y - startY > 30) { // Only consider lines longer than 5 pixels + yWallsList.add(getYLine(startY, y-1, x)); + } + startY = -1; + } + } + // Check if line extends to the edge + if (startY != -1) { + yWallsList.add(getYLine(startY, this.heightImg-1, x)); + } + } + } + + /** + * Sets the bounds for the current floor plan. + * + * @param buildingType type of building (NUCLEUS or LIBRARY) + * @param floor floor number + */ + private void setBounds(int buildingType, int floor) { + List bounds; + double altGeo = 0; + if (buildingType == BUILDING_NUCLEUS) { + altGeo = (double) floor * NUCLEUS_FLOOR_HEIGHT; + bounds = NUCLEUS_POLYGON; + } else { + altGeo = (double) floor * LIBRARY_FLOOR_HEIGHT; + bounds = LIBRARY_POLYGON; + } + List boundsENU = new ArrayList<>(); + for (LatLng bound : bounds) { + boundsENU.add(CoordinateConverter.convertGeodeticToEnu(bound.latitude, bound.longitude, altGeo, + referencePosition[0], referencePosition[1], referencePosition[2])); + } + + this.widthENU = Math.abs(boundsENU.get(0)[0] - boundsENU.get(2)[0]); + this.ENUtoPixRatioWidth = widthENU / ((double) this.widthImg); + + this.heightENU = Math.abs(boundsENU.get(0)[1] - boundsENU.get(2)[1]); + this.ENUtoPixRatioHeight = heightENU / ((double) this.heightImg); + + this.ReferenceEastSouth = boundsENU.get(1); + + } + + /** + * Checks if a point is in the Nucleus Building. + * + * @param point the point to be checked [east, north] + * @return true if point is in Nucleus building, false otherwise + */ + public boolean inNucleusENU(double[] point){ + if (point == null) { + return false; + } + return (pointInPolygonENU(point,NUCLEUS_POLYGON)); + + } + + /** + * Checks if a point is in the Library Building. + * + * @param point the point to be checked [east, north] + * @return true if point is in Library building, false otherwise + */ + public boolean inLibraryENU(double[] point){ + if (point == null) { + return false; + } + return (pointInPolygonENU(point,LIBRARY_POLYGON)); + } + + /** + * Checks if a point is inside a polygon using ray casting algorithm. + * + *

Ray casting algorithm: https://en.wikipedia.org/wiki/Point_in_polygon + * Approximates earth as flat. + * + * @param point point to be checked [east, north] + * @param polygon boundaries of the building + * @return true if point is in polygon, false otherwise + */ + private boolean pointInPolygonENU(double[] point, List polygon) { + int numCrossings = 0; + List path=polygon; + List boundsENU = new ArrayList<>(); + for (LatLng element : path) { + boundsENU.add(CoordinateConverter.convertGeodeticToEnu(element.latitude, element.longitude, 78, + referencePosition[0], referencePosition[1], referencePosition[2])); + } + // For each edge + for (int i=0; i < boundsENU.size(); i++) { + double[] a = boundsENU.get(i); + int j = i + 1; + // Last edge (includes first point of Polygon) + if (j >= path.size()) { + j = 0; + } + double[] b = boundsENU.get(j); + if (crossingSegment(point, a, b)) { + numCrossings++; + } + } + + //if odd number of numCrossings return true (point is in polygon) + return (numCrossings % 2 == 1); + } + + /** + * Ray casting algorithm for a segment joining ab in ENU coordinates. + * + * @param point the point we check, represented as {E, N, U} + * @param a the line segment's starting point, represented as {E, N, U} + * @param b the line segment's ending point, represented as {E, N, U} + * @return true if the point is (1) to the left of the segment ab and + * (2) not above nor below the segment ab, false otherwise + */ + private static boolean crossingSegment(double[] point, double[] a, double[] b) { + double pointE = point[0], pointN = point[1]; + double aE = a[0], aN = a[1]; + double bE = b[0], bN = b[1]; + + if (aN > bN) { + double tempE = aE, tempN = aN; + aE = bE; aN = bN; + bE = tempE; bN = tempN; + } + + // If the point has the same N as a or b, increase slightly pointN + if (pointN == aN || pointN == bN) pointN += 0.00000001; + + // If the point is above, below, or to the right of the segment, return false + if ((pointN > bN || pointN < aN) || (pointE > Math.max(aE, bE))) { + return false; + } + // If the point is to the left of both segment endpoints, return true + else if (pointE < Math.min(aE, bE)) { + return true; + } + // Compare slopes to determine if the point is to the left of segment ab + else { + double slope1 = (aE != bE) ? ((bN - aN) / (bE - aE)) : Double.POSITIVE_INFINITY; + double slope2 = (aE != pointE) ? ((pointN - aN) / (pointE - aE)) : Double.POSITIVE_INFINITY; + return (slope2 >= slope1); + } + } + + /** + * Creates an xLine from pixel coordinates. + * + * @param xStartPix x-start pixel coordinate + * @param xEndPix x-end pixel coordinate + * @param yPix y pixel coordinate + * @return an xLine in ENU coordinates + */ + private xLine getXLine(double xStartPix, double xEndPix, double yPix) { + + // Interpolate position within the building bounds + double xStart = this.ReferenceEastSouth[0] + this.ENUtoPixRatioWidth*xStartPix; + double xEnd = this.ReferenceEastSouth[0] + this.ENUtoPixRatioWidth*xEndPix; + double y = this.ReferenceEastSouth[1] + this.ENUtoPixRatioHeight*yPix; + + return new xLine(xStart, xEnd, y); + + } + + /** + * Creates a yLine from pixel coordinates. + * + * @param yStartPix y-start pixel coordinate + * @param yEndPix y-end pixel coordinate + * @param xPix x pixel coordinate + * @return a yLine in ENU coordinates + */ + private yLine getYLine(double yStartPix, double yEndPix, double xPix) { + + // Interpolate position within the building bounds + double yStart = this.ReferenceEastSouth[1] + this.ENUtoPixRatioHeight*yStartPix; + double yEnd = this.ReferenceEastSouth[1] + this.ENUtoPixRatioHeight*yEndPix; + double x = this.ReferenceEastSouth[0] + this.ENUtoPixRatioWidth*xPix; + + return new yLine(yStart, yEnd, x); + + } + + /** + * Checks if a trajectory intersects any walls. + * + * @param start starting point of trajectory [east, north] + * @param end ending point of trajectory [east, north] + * @param buildingType building identifier + * @param floor floor number + * @return true if the trajectory intersects any wall, false otherwise + */ + public boolean doesTrajectoryIntersectWall(double[] start, double[] end, int buildingType, int floor) { + // Get walls for current building and floor + List xWalls = xWallsMap.get(generateMapKey(buildingType, floor)); + List yWalls = yWallsMap.get(generateMapKey(buildingType, floor)); + if (xWalls == null || yWalls == null) { + Log.w(TAG, "No wall data available for building " + buildingType + " floor " + floor); + return false; + } + + // Check for intersection with each wall + for (xLine xWall : xWalls) { + if (doLinesIntersect(xWall.x1, xWall.y, xWall.x2, xWall.y, + start[0], start[1], end[0], end[1])) { + return true; + } + } + + for (yLine yWall : yWalls) { + if (doLinesIntersect(yWall.x, yWall.y1, yWall.x, yWall.y2, + start[0], start[1], end[0], end[1])) { + return true; + } + } + + return false; + } + + /** + * Determines if two line segments intersect. + * + *

Each line is defined by its two endpoints: (x1, y1) -> (x2, y2) + * + * @param line1X1 X-coordinate of first endpoint of line 1 + * @param line1Y1 Y-coordinate of first endpoint of line 1 + * @param line1X2 X-coordinate of second endpoint of line 1 + * @param line1Y2 Y-coordinate of second endpoint of line 1 + * @param line2X1 X-coordinate of first endpoint of line 2 + * @param line2Y1 Y-coordinate of first endpoint of line 2 + * @param line2X2 X-coordinate of second endpoint of line 2 + * @param line2Y2 Y-coordinate of second endpoint of line 2 + * @return true if the line segments intersect, false otherwise + */ + public boolean doLinesIntersect(double line1X1, double line1Y1, double line1X2, double line1Y2, + double line2X1, double line2Y1, double line2X2, double line2Y2) { + + // Calculate the direction vectors + double line1DirX = line1X2 - line1X1; + double line1DirY = line1Y2 - line1Y1; + double line2DirX = line2X2 - line2X1; + double line2DirY = line2Y2 - line2Y1; + + // Calculate the determinant + double det = line1DirX * line2DirY - line1DirY * line2DirX; + + // If det is zero, lines are parallel + if (Math.abs(det) < 1e-10) { + // Check if they are collinear + double dx = line2X1 - line1X1; + double dy = line2Y1 - line1Y1; + + // Cross product should be zero for collinearity + if (Math.abs(dx * line1DirY - dy * line1DirX) < 1e-10) { + // Check if there's overlap by projecting onto x-axis or y-axis + // Choose the axis with the larger variation + if (Math.abs(line1DirX) > Math.abs(line1DirY)) { + // Project onto x-axis + double t1 = (line2X1 - line1X1) / line1DirX; + double t2 = (line2X2 - line1X1) / line1DirX; + return (t1 >= 0 && t1 <= 1) || (t2 >= 0 && t2 <= 1) || + (line2X1 <= line1X1 && line1X1 <= line2X2) || + (line2X1 <= line1X2 && line1X2 <= line2X2); + } else { + // Project onto y-axis + double t1 = (line2Y1 - line1Y1) / line1DirY; + double t2 = (line2Y2 - line1Y1) / line1DirY; + return (t1 >= 0 && t1 <= 1) || (t2 >= 0 && t2 <= 1) || + (line2Y1 <= line1Y1 && line1Y1 <= line2Y2) || + (line2Y1 <= line1Y2 && line1Y2 <= line2Y2); + } + } + return false; + } + + // Calculate parameters for the point of intersection + double s = (line1DirX * (line2Y1 - line1Y1) - line1DirY * (line2X1 - line1X1)) / det; + double t = (line2DirX * (line2Y1 - line1Y1) - line2DirY * (line2X1 - line1X1)) / det; + + // Check if the point of intersection lies within both line segments + return (s >= 0 && s <= 1 && t >= 0 && t <= 1); + } + + /** + * Sets the reference position for coordinate conversion. + * + * @param referencePosition new reference position [lat, lng, alt] + */ + public void setReferencePosition(double[] referencePosition) { + this.referencePosition = referencePosition; + } + + /** + * Horizontal line class to represent wall segments. + */ + public class xLine { + double x1, x2, y; + + /** + * Creates a horizontal line. + * + * @param x1 start x-coordinate + * @param x2 end x-coordinate + * @param y y-coordinate + */ + xLine(double x1, double x2, double y) { + this.x1 = x1; + this.y = y; + this.x2 = x2; + } + } + + /** + * Vertical line class to represent wall segments. + */ + public class yLine { + double y1, y2, x; + + /** + * Creates a vertical line. + * + * @param y1 start y-coordinate + * @param y2 end y-coordinate + * @param x x-coordinate + */ + yLine(double y1, double y2, double x) { + this.y1 = y1; + this.y2 = y2; + this.x = x; + } + } +} \ No newline at end of file diff --git a/app/src/main/res/drawable/circle_blue.xml b/app/src/main/res/drawable/circle_blue.xml new file mode 100644 index 00000000..e3c8a2bb --- /dev/null +++ b/app/src/main/res/drawable/circle_blue.xml @@ -0,0 +1,7 @@ + + + + + + diff --git a/app/src/main/res/drawable/circle_magenta.xml b/app/src/main/res/drawable/circle_magenta.xml new file mode 100644 index 00000000..4f9d9a5a --- /dev/null +++ b/app/src/main/res/drawable/circle_magenta.xml @@ -0,0 +1,7 @@ + + + + + + diff --git a/app/src/main/res/drawable/circle_red.xml b/app/src/main/res/drawable/circle_red.xml new file mode 100644 index 00000000..e04cd50f --- /dev/null +++ b/app/src/main/res/drawable/circle_red.xml @@ -0,0 +1,7 @@ + + + + + + diff --git a/app/src/main/res/drawable/ic_baseline_navigation_24.xml b/app/src/main/res/drawable/ic_baseline_navigation_24.xml index 409078ae..0c534f50 100644 --- a/app/src/main/res/drawable/ic_baseline_navigation_24.xml +++ b/app/src/main/res/drawable/ic_baseline_navigation_24.xml @@ -5,6 +5,6 @@ android:viewportHeight="24" android:tint="?attr/textureBlurFactor"> diff --git a/app/src/main/res/drawable/library1_stairs.png b/app/src/main/res/drawable/library1_stairs.png new file mode 100644 index 00000000..1434f805 Binary files /dev/null and b/app/src/main/res/drawable/library1_stairs.png differ diff --git a/app/src/main/res/drawable/libraryg_simplified.png b/app/src/main/res/drawable/libraryg_simplified.png new file mode 100644 index 00000000..397b61c1 Binary files /dev/null and b/app/src/main/res/drawable/libraryg_simplified.png differ diff --git a/app/src/main/res/drawable/libraryg_stairs.png b/app/src/main/res/drawable/libraryg_stairs.png new file mode 100644 index 00000000..665959fa Binary files /dev/null and b/app/src/main/res/drawable/libraryg_stairs.png differ diff --git a/app/src/main/res/drawable/nucleus1_simplified.png b/app/src/main/res/drawable/nucleus1_simplified.png new file mode 100644 index 00000000..1f40adb0 Binary files /dev/null and b/app/src/main/res/drawable/nucleus1_simplified.png differ diff --git a/app/src/main/res/drawable/nucleus1_stairs.png b/app/src/main/res/drawable/nucleus1_stairs.png new file mode 100644 index 00000000..43b5df01 Binary files /dev/null and b/app/src/main/res/drawable/nucleus1_stairs.png differ diff --git a/app/src/main/res/drawable/nucleus2_simplified.png b/app/src/main/res/drawable/nucleus2_simplified.png new file mode 100644 index 00000000..5f7b7336 Binary files /dev/null and b/app/src/main/res/drawable/nucleus2_simplified.png differ diff --git a/app/src/main/res/drawable/nucleus2_stairs.png b/app/src/main/res/drawable/nucleus2_stairs.png new file mode 100644 index 00000000..f9798afd Binary files /dev/null and b/app/src/main/res/drawable/nucleus2_stairs.png differ diff --git a/app/src/main/res/drawable/nucleus3_simplified.png b/app/src/main/res/drawable/nucleus3_simplified.png new file mode 100644 index 00000000..88b86a9f Binary files /dev/null and b/app/src/main/res/drawable/nucleus3_simplified.png differ diff --git a/app/src/main/res/drawable/nucleus3_stairs.png b/app/src/main/res/drawable/nucleus3_stairs.png new file mode 100644 index 00000000..ecc7ed41 Binary files /dev/null and b/app/src/main/res/drawable/nucleus3_stairs.png differ diff --git a/app/src/main/res/drawable/nucleusg_simplified.png b/app/src/main/res/drawable/nucleusg_simplified.png new file mode 100644 index 00000000..01312475 Binary files /dev/null and b/app/src/main/res/drawable/nucleusg_simplified.png differ diff --git a/app/src/main/res/drawable/nucleusg_stairs.png b/app/src/main/res/drawable/nucleusg_stairs.png new file mode 100644 index 00000000..e8cb16b1 Binary files /dev/null and b/app/src/main/res/drawable/nucleusg_stairs.png differ diff --git a/app/src/main/res/drawable/nucleuslg_simplified.png b/app/src/main/res/drawable/nucleuslg_simplified.png new file mode 100644 index 00000000..21ea1597 Binary files /dev/null and b/app/src/main/res/drawable/nucleuslg_simplified.png differ diff --git a/app/src/main/res/drawable/nucleuslg_stairs.png b/app/src/main/res/drawable/nucleuslg_stairs.png new file mode 100644 index 00000000..0eaf9311 Binary files /dev/null and b/app/src/main/res/drawable/nucleuslg_stairs.png differ diff --git a/app/src/main/res/layout-small/fragment_home.xml b/app/src/main/res/layout-small/fragment_home.xml index bd713b67..2e130621 100644 --- a/app/src/main/res/layout-small/fragment_home.xml +++ b/app/src/main/res/layout-small/fragment_home.xml @@ -170,7 +170,9 @@ app:layout_constraintTop_toBottomOf="@id/buttonGrid" app:layout_constraintBottom_toBottomOf="parent" app:layout_constraintStart_toStartOf="parent" - app:layout_constraintEnd_toEndOf="parent" /> + app:layout_constraintEnd_toEndOf="parent" + android:visibility="invisible" + /> diff --git a/app/src/main/res/layout/fragment_home.xml b/app/src/main/res/layout/fragment_home.xml index 99c1ef13..35fa7238 100644 --- a/app/src/main/res/layout/fragment_home.xml +++ b/app/src/main/res/layout/fragment_home.xml @@ -167,7 +167,9 @@ app:layout_constraintTop_toBottomOf="@id/buttonGrid" app:layout_constraintBottom_toBottomOf="parent" app:layout_constraintStart_toStartOf="parent" - app:layout_constraintEnd_toEndOf="parent" /> + app:layout_constraintEnd_toEndOf="parent" + android:visibility="invisible" + /> diff --git a/app/src/main/res/layout/fragment_startlocation.xml b/app/src/main/res/layout/fragment_startlocation.xml index 61de731a..91e29b7b 100644 --- a/app/src/main/res/layout/fragment_startlocation.xml +++ b/app/src/main/res/layout/fragment_startlocation.xml @@ -36,31 +36,48 @@ app:layout_constraintEnd_toEndOf="parent"> + android:textColor="@color/md_theme_onPrimary" /> +