wifiPoints = new ArrayList<>(wifiPolyline.getPoints());
+ wifiPoints.add(wifiLocation);
+ wifiPolyline.setPoints(wifiPoints);
+ }
+ lastwifiLocation = wifiLocation;
+ }
+ }
/**
* Remove GNSS marker if user toggles it off
@@ -406,6 +455,10 @@ public boolean isGnssEnabled() {
return isGnssOn;
}
+ public boolean isWiFiLocationOn(){
+ return isWiFiLocationOn;
+ }
+
private void setFloorControlsVisibility(int visibility) {
floorUpButton.setVisibility(visibility);
floorDownButton.setVisibility(visibility);
@@ -421,6 +474,10 @@ public void clearMapAndReset() {
gnssPolyline.remove();
gnssPolyline = null;
}
+ if (wifiPolyline != null) {
+ wifiPolyline.remove();
+ wifiPolyline = null;
+ }
if (orientationMarker != null) {
orientationMarker.remove();
orientationMarker = null;
@@ -430,6 +487,7 @@ public void clearMapAndReset() {
gnssMarker = null;
}
lastGnssLocation = null;
+ lastwifiLocation = null;
currentLocation = null;
// Re-create empty polylines with your chosen colors
@@ -442,6 +500,10 @@ public void clearMapAndReset() {
.color(Color.BLUE)
.width(5f)
.add());
+ wifiPolyline = gMap.addPolyline(new PolylineOptions()
+ .color(Color.YELLOW)
+ .width(5f)
+ .add());
}
}
diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java
index 6eca847c..dd1efb7e 100644
--- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java
+++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java
@@ -52,10 +52,37 @@
*
* The class provides a number of setters and getters so that other classes can have access to the
* sensor data and influence the behaviour of data collection.
- *
+ *
+ *
+ * functions in this class:
+ *
+ * - {@link #getInstance()} - Returns the singleton instance of SensorFusion.
+ * - {@link #setContext(Context)} - Initializes the SensorFusion instance with the application context.
+ * - {@link #resumeListening()} - Registers all device listeners and starts data collection.
+ * - {@link #stopListening()} - Unregisters all device listeners and pauses data collection.
+ * - {@link #startRecording()} - Enables saving sensor values to the trajectory object.
+ * - {@link #stopRecording()} - Disables saving sensor values to the trajectory object.
+ * - {@link #sendTrajectoryToCloud()} - Sends the trajectory object to the server.
+ * - {@link #getGNSSLatitude(boolean)} - Returns the GNSS latitude and longitude data.
+ * - {@link #setStartGNSSLatitude(float[])} - Sets the initial GNSS latitude and longitude data.
+ * - {@link #redrawPath(float)} - Redraws the path in the corrections fragment.
+ * - {@link #passAverageStepLength()} - Returns the average step length calculated by PDR.
+ * - {@link #passOrientation()} - Returns the current device orientation.
+ * - {@link #getSensorValueMap()} - Returns a map of the most recent sensor readings.
+ * - {@link #getWifiList()} - Returns the most recent list of WiFi names and levels.
+ * - {@link #getSensorInfos()} - Returns information about all the sensors registered in SensorFusion.
+ * - {@link #registerForServerUpdate(Observer)} - Registers an observer to receive updates from the server.
+ * - {@link #getElevation()} - Returns the estimated elevation value in meters.
+ * - {@link #getElevator()} - Returns whether the user is currently in an elevator.
+ * - {@link #getHoldMode()} - Estimates the phone's position based on proximity and light sensors.
+ * - {@link #getLatLngWifiPositioning()} - Returns the user's position obtained using WiFi positioning.
+ * - {@link #getWifiFloor()} - Returns the current floor the user is on using WiFi positioning.
+ *
+ *
* @author Michal Dvorak
* @author Mate Stodulka
* @author Virginia Cangelosi
+ * @author JackShenYt
*/
public class SensorFusion implements SensorEventListener, Observer {
@@ -159,6 +186,15 @@ public class SensorFusion implements SensorEventListener, Observer {
// WiFi positioning object
private WiFiPositioning wiFiPositioning;
+ //km filter
+ private final Timer fuseTimer = new Timer();
+
+ // Add gyroOrientation and accMagOrientation arrays
+ private final float[] gyroOrientation = new float[3];
+ private final float[] accMagOrientation = new float[3];
+ private float[] gyroMatrix = new float[9];
+ private final float[] fusedOrientation = new float[3];
+
//region Initialisation
/**
* Private constructor for implementing singleton design pattern for SensorFusion.
@@ -191,8 +227,14 @@ private SensorFusion() {
this.R = new float[9];
// GNSS initial Long-Lat array
this.startLocation = new float[2];
- }
+ // Initialize gyroMatrix in the constructor
+ gyroOrientation[0] = 0.0f;
+ gyroOrientation[1] = 0.0f;
+ gyroOrientation[2] = 0.0f;
+
+ gyroMatrix = new float[]{1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f};
+ }
/**
* Static function to access singleton instance of SensorFusion.
@@ -259,6 +301,10 @@ public void setContext(Context context) {
// Keep app awake during the recording (using stored appContext)
PowerManager powerManager = (PowerManager) this.appContext.getSystemService(Context.POWER_SERVICE);
wakeLock = powerManager.newWakeLock(PowerManager.PARTIAL_WAKE_LOCK, "MyApp::MyWakelockTag");
+
+ //kalmanFilter service
+ fuseTimer.schedule(new calculateFusedOrientationTask(), 5000, 30);
+ Log.d("SensorFusionService", "Service started");
}
//endregion
@@ -284,11 +330,11 @@ 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");
-// }
+ // // 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
@@ -302,6 +348,7 @@ public void onSensorChanged(SensorEvent sensorEvent) {
acceleration[0] = sensorEvent.values[0];
acceleration[1] = sensorEvent.values[1];
acceleration[2] = sensorEvent.values[2];
+ calculateAccMagOrientation();
break;
case Sensor.TYPE_PRESSURE:
@@ -317,6 +364,8 @@ public void onSensorChanged(SensorEvent sensorEvent) {
angularVelocity[0] = sensorEvent.values[0];
angularVelocity[1] = sensorEvent.values[1];
angularVelocity[2] = sensorEvent.values[2];
+ gyroFunction(sensorEvent);
+ break;
case Sensor.TYPE_LINEAR_ACCELERATION:
filteredAcc[0] = sensorEvent.values[0];
@@ -330,12 +379,8 @@ public void onSensorChanged(SensorEvent sensorEvent) {
Math.pow(filteredAcc[2], 2)
);
this.accelMagnitude.add(accelMagFiltered);
-
-// // Debug logging
-// Log.v("SensorFusion",
-// "Added new linear accel magnitude: " + accelMagFiltered
-// + "; accelMagnitude size = " + accelMagnitude.size());
-
+ // Debug logging
+ //Log.v("SensorFusion", "Added new linear accel magnitude: " + accelMagFiltered + "; accelMagnitude size = " + accelMagnitude.size());
elevator = pdrProcessing.estimateElevator(gravity, filteredAcc);
break;
@@ -396,7 +441,7 @@ public void onSensorChanged(SensorEvent sensorEvent) {
float[] newCords = this.pdrProcessing.updatePdr(
stepTime,
this.accelMagnitude,
- this.orientation[0]
+ this.fusedOrientation[0] //km filter
);
// Clear the accelMagnitude after using it
@@ -506,6 +551,7 @@ private void createWifiPositioningRequest(){
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
@@ -620,6 +666,32 @@ private float[] matrixMultiplication(float[] A, float[] B) {
return result;
}
+ /**
+ * KalmanFilter time irs
+ */
+ class calculateFusedOrientationTask extends TimerTask {
+ private final kalmanfilter[] kalmanFilters = new kalmanfilter[3];
+
+ public calculateFusedOrientationTask() {
+ // Initialize one Kalman filter for each axis (yaw, pitch, roll)
+ for (int i = 0; i < 3; i++) {
+ kalmanFilters[i] = new kalmanfilter();
+ }
+ }
+
+ public void run() {
+ float dt = 0.03f; // Assume 33Hz update rate
+
+ for (int i = 0; i < 3; i++) {
+ fusedOrientation[i] = kalmanFilters[i].update(accMagOrientation[i], gyroOrientation[i], dt);
+ }
+
+ // Update gyro matrix with the fused orientation
+ gyroMatrix = getRotationMatrixFromOrientation(fusedOrientation);
+ System.arraycopy(fusedOrientation, 0, gyroOrientation, 0, 3);
+ }
+ }
+
/**
* {@inheritDoc}
*/
@@ -968,10 +1040,10 @@ public void run() {
.setMagY(magneticField[1])
.setMagZ(magneticField[2])
.setRelativeTimestamp(SystemClock.uptimeMillis()-bootTime))
-// .addGnssData(Traj.GNSS_Sample.newBuilder()
-// .setLatitude(latitude)
-// .setLongitude(longitude)
-// .setRelativeTimestamp(SystemClock.uptimeMillis()-bootTime))
+ // .addGnssData(Traj.GNSS_Sample.newBuilder()
+ // .setLatitude(latitude)
+ // .setLongitude(longitude)
+ // .setRelativeTimestamp(SystemClock.uptimeMillis()-bootTime))
;
// Divide timer with a counter for storing data every 1 second
@@ -997,6 +1069,8 @@ public void run() {
.setMac(currentWifi.getBssid())
.setSsid(currentWifi.getSsid())
.setFrequency(currentWifi.getFrequency()));
+
+ //trajectory.addAllWifiData
}
else {
secondCounter++;
@@ -1011,4 +1085,50 @@ public void run() {
//endregion
+ // Add calculateAccMagOrientation method
+ private void calculateAccMagOrientation() {
+ if (SensorManager.getRotationMatrix(R, null, acceleration, magneticField)) {
+ SensorManager.getOrientation(R, accMagOrientation);
+ }
+ }
+
+ // Add gyroFunction method
+ private static final float EPSILON = 0.000000001f;
+ private void gyroFunction(SensorEvent event) {
+ if (gyroMatrix == null) return;
+
+ float[] deltaVector = new float[4];
+ if (lastEventTimestamps.containsKey(Sensor.TYPE_GYROSCOPE)) {
+ float dT = (event.timestamp - lastEventTimestamps.get(Sensor.TYPE_GYROSCOPE)) * (1.0f / 1_000_000_000.0f);
+ getRotationVectorFromGyro(event.values, deltaVector, dT / 2.0f);
+ }
+
+ float[] deltaMatrix = new float[9];
+ SensorManager.getRotationMatrixFromVector(deltaMatrix, deltaVector);
+ gyroMatrix = matrixMultiplication(gyroMatrix, deltaMatrix);
+ SensorManager.getOrientation(gyroMatrix, gyroOrientation);
+ }
+
+ // Add getRotationVectorFromGyro method
+ private void getRotationVectorFromGyro(float[] gyroValues, float[] deltaRotationVector, float timeFactor) {
+ float omegaMagnitude = (float) Math.sqrt(gyroValues[0] * gyroValues[0] +
+ gyroValues[1] * gyroValues[1] +
+ gyroValues[2] * gyroValues[2]);
+
+ if (omegaMagnitude > EPSILON) {
+ gyroValues[0] /= omegaMagnitude;
+ gyroValues[1] /= omegaMagnitude;
+ gyroValues[2] /= omegaMagnitude;
+ }
+
+ float thetaOverTwo = omegaMagnitude * timeFactor;
+ float sinThetaOverTwo = (float) Math.sin(thetaOverTwo);
+ float cosThetaOverTwo = (float) Math.cos(thetaOverTwo);
+
+ deltaRotationVector[0] = sinThetaOverTwo * gyroValues[0];
+ deltaRotationVector[1] = sinThetaOverTwo * gyroValues[1];
+ deltaRotationVector[2] = sinThetaOverTwo * gyroValues[2];
+ deltaRotationVector[3] = cosThetaOverTwo;
+ }
+
}
diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/kalmanfilter.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/kalmanfilter.java
new file mode 100644
index 00000000..87199ce1
--- /dev/null
+++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/kalmanfilter.java
@@ -0,0 +1,57 @@
+package com.openpositioning.PositionMe.sensors;
+
+public class kalmanfilter {
+ private float theta; // Estimated angle (θ)
+ private float thetaDotBias; // Bias in angular rate (θ̇b)
+
+ private float p00, p01, p10, p11; // Error covariance matrix
+
+ // Constructor
+ public kalmanfilter() {
+ this.theta = 0.0f; // Initial estimated angle
+ this.thetaDotBias = 0.0f; // Initial bias
+
+ this.p00 = 1.0f; // Initial error covariance
+ this.p01 = 0.0f;
+ this.p10 = 0.0f;
+ this.p11 = 1.0f;
+ }
+
+ public float update(float accMagAngle, float gyroAngle, float dt) {
+ // Prediction step
+ theta += gyroAngle * dt;
+ theta -= thetaDotBias * dt;
+
+ // Process noise variance for the angle
+ float qAngle = 0.01f;
+ p00 += dt * (dt * p11 - p01 - p10 + qAngle);
+ p01 -= dt * p11;
+ p10 -= dt * p11;
+
+ // Process noise variance for the gyro bias
+ float qBias = 0.003f;
+ p11 += qBias * dt;
+
+ // Update step
+ float z = accMagAngle - theta;
+
+ // Measurement noise variance
+ float rMeasure = 0.01f;
+ float k0 = p00 / (p00 + rMeasure);
+ float k1 = p10 / (p00 + rMeasure);
+
+ theta += k0 * z;
+ thetaDotBias += k1 * z;
+
+ // Update covariance matrix
+ float tempP00 = p00;
+ float tempP01 = p01;
+
+ p00 -= k0 * tempP00;
+ p01 -= k0 * tempP01;
+ p10 -= k1 * tempP00;
+ p11 -= k1 * tempP01;
+
+ return theta;
+ }
+}
diff --git a/app/src/main/res/layout/fragment_trajectory_map.xml b/app/src/main/res/layout/fragment_trajectory_map.xml
index a8ab9118..0573e232 100644
--- a/app/src/main/res/layout/fragment_trajectory_map.xml
+++ b/app/src/main/res/layout/fragment_trajectory_map.xml
@@ -37,6 +37,12 @@
android:layout_height="wrap_content"
android:text="@string/gnssSwitch" />
+
+
Floor height in meters
Color
🛰 Show GNSS
+ 📶WiFi Loc
Floor Down button
Floor Up button
Choose Map