Kalman filter

lib/BleFingerprint/BleFingerprint.cpp

@@ -16,7 +16,7 @@ class ClientCallbacks : public BLEClientCallbacks {
 
 static ClientCallbacks clientCB;
 
-BleFingerprint::BleFingerprint(BLEAdvertisedDevice *advertisedDevice, float fcmin, float beta, float dcutoff) : filteredDistance{FilteredDistance(fcmin, beta, dcutoff)} {
+BleFingerprint::BleFingerprint(BLEAdvertisedDevice *advertisedDevice, float fcmin, float beta, float dcutoff) : filteredDistance{FilteredDistance(25, 0.1)} {
     firstSeenMillis = millis();
     address = NimBLEAddress(advertisedDevice->getAddress());
     addressType = advertisedDevice->getAddressType();

lib/BleFingerprint/FilteredDistance.cpp

@@ -1,64 +1,73 @@
 #include "FilteredDistance.h"
-
 #include <Arduino.h>
 
-#include <cmath>
-#include <numeric>
-#include <vector>
-
-FilteredDistance::FilteredDistance(float minCutoff, float beta, float dcutoff)
-    : minCutoff(minCutoff), beta(beta), dcutoff(dcutoff), x(0), dx(0), lastDist(0), lastTime(0), total(0), readIndex(0) {
-}
-
-void FilteredDistance::initSpike(float dist) {
-    for (size_t i = 0; i < NUM_READINGS; i++) {
-        readings[i] = dist;
-    }
-    total = dist * NUM_READINGS;
-}
-
-float FilteredDistance::removeSpike(float dist) {
-    total -= readings[readIndex];                // Subtract the last reading
-    readings[readIndex] = dist;              // Read the sensor
-    total += readings[readIndex];                // Add the reading to the total
-    readIndex = (readIndex + 1) % NUM_READINGS;  // Advance to the next position in the array
-
-    auto average = total / static_cast<float>(NUM_READINGS);  // Calculate the average
-
-    if (std::fabs(dist - average) > SPIKE_THRESHOLD)
-        return average;  // Spike detected, use the average as the filtered value
-
-    return dist;  // No spike, return the new value
-}
+FilteredDistance::FilteredDistance(float processNoise, float measurementNoise): processNoise(processNoise), measurementNoise(measurementNoise), isFirstMeasurement(true) {}
 
 void FilteredDistance::addMeasurement(float dist) {
-    const bool initialized = lastTime != 0;
-    const unsigned long now = micros();
-    const unsigned long elapsed = now - lastTime;
-    lastTime = now;
+    if (isFirstMeasurement) {
+        // Initialize state
+        state[0] = dist; // Distance
+        state[1] = 0;    // Rate of change in distance
 
-    if (!initialized) {
-        x = dist;  // Set initial filter state to the first reading
-        dx = 0;    // Initial derivative is unknown, so we set it to zero
-        lastDist = dist;
-        initSpike(dist);
-    } else {
-        float dT = std::max(elapsed * 0.000001f, 0.05f);  // Convert microseconds to seconds, enforce a minimum dT
-        const float alpha = getAlpha(minCutoff, dT);
-        const float dAlpha = getAlpha(dcutoff, dT);
+        // Initialize covariance matrix
+        covariance[0][0] = 1; // Initial guess
+        covariance[0][1] = 0;
+        covariance[1][0] = 0;
+        covariance[1][1] = 1; // Initial guess
 
-        dist = removeSpike(dist);
-        x += alpha * (dist - x);
-        dx = dAlpha * ((dist - lastDist) / dT);
-        lastDist = x + beta * dx;
+        lastUpdateTime = micros(); // Set the update time
+        isFirstMeasurement = false;
+        return;
     }
+
+    // Calculate time delta for subsequent measurements
+    unsigned long currentTime = micros();
+    float deltaTime = (currentTime - lastUpdateTime) / 1.0e6; // Convert micros to seconds
+    lastUpdateTime = currentTime;
+
+    // Perform prediction and update
+    prediction(deltaTime);
+    update(dist);
 }
 
 const float FilteredDistance::getDistance() const {
-    return lastDist;
+    unsigned long currentTime = micros();
+    float deltaTime = (currentTime - lastUpdateTime) / 1.0e6; // Convert micros to seconds
+
+    // Calculate predicted distance
+    float predictedDistance = state[0] + state[1] * deltaTime;
+
+    return predictedDistance;
 }
 
-float FilteredDistance::getAlpha(float cutoff, float dT) {
-    float tau = 1.0f / (2 * M_PI * cutoff);
-    return 1.0f / (1.0f + tau / dT);
+void FilteredDistance::prediction(float deltaTime) {
+    // Update state estimate
+    state[0] += state[1] * deltaTime;
+
+    // Update covariance
+    covariance[0][0] += deltaTime * (covariance[1][0] + covariance[0][1]) + processNoise;
+    covariance[0][1] += deltaTime * covariance[1][1];
+    covariance[1][0] += deltaTime * covariance[1][1];
+}
+
+void FilteredDistance::update(float distanceMeasurement) {
+    // Kalman gain calculation
+    float S = covariance[0][0] + measurementNoise;
+    float K[2]; // Kalman gain
+    K[0] = covariance[0][0] / S;
+    K[1] = covariance[1][0] / S;
+
+    // Update state
+    float y = distanceMeasurement - state[0]; // measurement residual
+    state[0] += K[0] * y;
+    state[1] += K[1] * y;
+
+    // Update covariance
+    float covariance00_temp = covariance[0][0];
+    float covariance01_temp = covariance[0][1];
+
+    covariance[0][0] -= K[0] * covariance00_temp;
+    covariance[0][1] -= K[0] * covariance01_temp;
+    covariance[1][0] -= K[1] * covariance00_temp;
+    covariance[1][1] -= K[1] * covariance01_temp;
 }

lib/BleFingerprint/FilteredDistance.h

@@ -1,35 +1,23 @@
 #ifndef FILTEREDDISTANCE_H
 #define FILTEREDDISTANCE_H
 
-#include <Arduino.h>
-
-#define SPIKE_THRESHOLD 1.0f  // Threshold for spike detection
-#define NUM_READINGS 10       // Number of readings to keep track of
-
 class FilteredDistance {
    public:
-    FilteredDistance(float minCutoff = 1.0f, float beta = 0.0f, float dcutoff = 1.0f);
+    FilteredDistance(float processNoise, float measurementNoise);
     void addMeasurement(float dist);
-    const float getMedianDistance() const;
     const float getDistance() const;
-    bool hasValue() const { return lastTime != 0; }
+    const bool hasValue() const { return !isFirstMeasurement; }
 
    private:
-    float minCutoff;
-    float beta;
-    float dcutoff;
-    float x, dx;
-    float lastDist;
-    unsigned long lastTime;
+    float state[2];                // State: [0] is distance, [1] is rate of change in distance
+    float covariance[2][2];        // State covariance
+    unsigned long lastUpdateTime;  // Time of the last update
+    float processNoise;            // Process noise (Q)
+    float measurementNoise;        // Measurement noise (R)
+    bool isFirstMeasurement;
 
-    float getAlpha(float cutoff, float dT);
-
-    float readings[NUM_READINGS];  // Array to store readings
-    int readIndex;                 // Current position in the array
-    float total;                   // Total of the readings
-
-    void initSpike(float dist);
-    float removeSpike(float dist);
+    void prediction(float deltaTime);
+    void update(float distanceMeasurement);
 };
 
 #endif  // FILTEREDDISTANCE_H