Vastly different filtering

lib/BleFingerprint/BleFingerprint.cpp

@@ -16,11 +16,11 @@ 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) : rssiSmoother{RSSISmoother(1, fcmin, beta, dcutoff)} {
     firstSeenMillis = millis();
     address = NimBLEAddress(advertisedDevice->getAddress());
     addressType = advertisedDevice->getAddressType();
-    rssi = advertisedDevice->getRSSI();
+    smooth = rssi = advertisedDevice->getRSSI();
     raw = dist = pow(10, float(get1mRssi() - rssi) / (10.0f * BleFingerprintCollection::absorption));
     seenCount = 1;
     queryReport = nullptr;
@@ -96,6 +96,14 @@ const int BleFingerprint::get1mRssi() const {
     return BleFingerprintCollection::rxRefRssi + DEFAULT_TX + BleFingerprintCollection::rxAdjRssi;
 }
 
+const int BleFingerprint::getRssi() const {
+    return smooth;
+}
+
+const float BleFingerprint::getDistance() const {
+    return dist;
+}
+
 void BleFingerprint::fingerprint(NimBLEAdvertisedDevice *advertisedDevice) {
     if (advertisedDevice->haveName()) {
         const std::string name = advertisedDevice->getName();
@@ -419,9 +427,8 @@ bool BleFingerprint::seen(BLEAdvertisedDevice *advertisedDevice) {
     if (ignore || hidden) return false;
 
     rssi = advertisedDevice->getRSSI();
+    rssiSmoother.addRSSIValue(rssi);
     raw = pow(10, float(get1mRssi() - rssi) / (10.0f * BleFingerprintCollection::absorption));
-    filteredDistance.addMeasurement(raw);
-    dist = filteredDistance.getDistance();
 
     if (!added) {
         added = true;
@@ -453,6 +460,18 @@ bool BleFingerprint::fill(JsonObject *doc) {
     return true;
 }
 
+bool BleFingerprint::filter() {
+    smooth = rssiSmoother.getSmoothedRSSI();
+    dist = pow(10, float(get1mRssi() - smooth) / (10.0f * BleFingerprintCollection::absorption));
+    return true;
+}
+
+bool BleFingerprint::filter() {
+    smooth = rssiSmoother.getSmoothedRSSI();
+    dist = pow(10, float(get1mRssi() - smooth) / (10.0f * BleFingerprintCollection::absorption));
+    return true;
+}
+
 bool BleFingerprint::report(JsonObject *doc) {
     if (ignore || idType <= ID_TYPE_RAND_MAC || hidden) return false;
     if (reported) return false;

lib/BleFingerprint/BleFingerprint.h

@@ -12,7 +12,7 @@
 #include "QueryReport.h"
 #include "rssi.h"
 #include "string_utils.h"
-#include "FilteredDistance.h"
+#include "RSSISmoother.h"
 
 #define NO_RSSI int8_t(-128)
 
@@ -72,6 +72,8 @@ class BleFingerprint {
 
     bool query();
 
+    bool filter();
+
     const String getId() const { return id; }
 
     const String getName() const { return name; }
@@ -86,9 +88,9 @@ class BleFingerprint {
 
     const short getIdType() const { return idType; }
 
-    const float getDistance() const { return dist; }
+    const float getDistance() const;
 
-    const int getRssi() const { return rssi; }
+    const int getRssi() const;
     const int getRawRssi() const { return rssi; }
 
     const int get1mRssi() const;
@@ -132,12 +134,12 @@ class BleFingerprint {
     int rssi = NO_RSSI;
     int8_t calRssi = NO_RSSI, bcnRssi = NO_RSSI, mdRssi = NO_RSSI, asRssi = NO_RSSI;
     unsigned int qryAttempts = 0, qryDelayMillis = 0;
-    float raw = 0, dist = 0, lastReported = 0, temp = 0, humidity = 0;
+    float raw = 0, dist = 0, smooth = NO_RSSI, lastReported = 0, temp = 0, humidity = 0;
     unsigned long firstSeenMillis, lastSeenMillis = 0, lastReportedMillis = 0, lastQryMillis = 0;
     unsigned long seenCount = 1, lastSeenCount = 0;
     uint16_t mv = 0;
     uint8_t battery = 0xFF, addressType = 0xFF;
-    FilteredDistance filteredDistance;
+    RSSISmoother rssiSmoother;
     std::unique_ptr<QueryReport> queryReport = nullptr;
 
     static bool shouldHide(const String &s);

lib/BleFingerprint/BleFingerprintCollection.h

@@ -3,9 +3,9 @@
 
 #include "BleFingerprint.h"
 
-#define ONE_EURO_FCMIN 1e-1f
-#define ONE_EURO_BETA 1e-3f
-#define ONE_EURO_DCUTOFF 5e-3f
+#define ONE_EURO_FCMIN 0.3f   // Respond to changes moderately fast
+#define ONE_EURO_BETA 0.002f  // Balance between smoothing and responsiveness
+#define ONE_EURO_DCUTOFF 1.0f // Control noise in the derivative
 
 #ifndef ALLOW_BLE_CONTROLLER_RESTART_AFTER_SECS
 #define ALLOW_BLE_CONTROLLER_RESTART_AFTER_SECS 1800

lib/BleFingerprint/RSSISmoother.cpp

@@ -0,0 +1,75 @@
+#include "RSSISmoother.h"
+
+#include <Arduino.h>
+
+#include <cmath>
+#include <vector>
+
+RSSISmoother::RSSISmoother(float timeConstant, float minCutoff, float beta, float dcutoff)
+    : timeConstant(timeConstant), minCutoff(minCutoff), beta(beta), dcutoff(dcutoff), x(0), dx(0), lastRSSI(0), lastTime(micros()) {
+    for (size_t i = 0; i < bufferSize; ++i) {
+        rssiBuffer[i] = std::make_pair(0, 0.0f);  // Initialize the buffer
+    }
+}
+
+void RSSISmoother::addRSSIValue(float rssi) {
+    unsigned long now = micros();
+    float dT = (now - lastTime) * 0.000001f;  // Convert microseconds to seconds
+    lastTime = now;
+
+    dT = std::max(dT, 0.001f);  // Enforce a minimum dT
+
+    float alpha = getAlpha(minCutoff, dT);
+    float dAlpha = getAlpha(dcutoff, dT);
+
+    x += alpha * (rssi - x);
+
+    float dxTemp = (rssi - lastRSSI) / dT;
+    dx = dAlpha * (dx + (1 - dAlpha) * dxTemp);
+
+    lastRSSI = x + beta * dx;
+
+    bufferIndex %= bufferSize;
+    rssiBuffer[bufferIndex] = std::make_pair(now, lastRSSI);
+    bufferIndex++;
+}
+
+float RSSISmoother::getSmoothedRSSI() {
+    unsigned long now = micros();
+
+    float total = 0.0f;
+    float weightSum = 0.0f;
+    std::vector<float> decayedWeights;
+
+    // First, calculate all the decayed weights and sum them up
+    for (size_t i = 0; i < bufferSize; ++i) {
+        size_t index = (bufferIndex + bufferSize - i - 1) % bufferSize;
+        unsigned long elementTimestamp = rssiBuffer[index].first;
+
+        // Skip if the slot is uninitialized (timestamp is 0)
+        if (elementTimestamp == 0) continue;
+
+        float age = (now - elementTimestamp) * 0.000001f;  // Convert microseconds to seconds
+        float decayedWeight = exp(-age / timeConstant);
+        decayedWeights.push_back(decayedWeight);
+        weightSum += decayedWeight;
+    }
+
+    // Check if weightSum is zero to avoid division by zero
+    if (weightSum == 0) return lastRSSI;
+
+    // Then, use the sum to normalize the weights and calculate the weighted sum
+    for (size_t i = 0; i < decayedWeights.size(); ++i) {
+        size_t index = (bufferIndex + bufferSize - i - 1) % bufferSize;
+        float normalizedWeight = decayedWeights[i] / weightSum;
+        total += rssiBuffer[index].second * normalizedWeight;
+    }
+
+    return total;
+}
+
+float RSSISmoother::getAlpha(float cutoff, float dT) {
+    float tau = 1.0f / (2 * M_PI * cutoff);
+    float te = 1.0f / (1.0f + tau / dT);
+    return te;
+}

lib/BleFingerprint/RSSISmoother.h

@@ -0,0 +1,28 @@
+#ifndef RSSISMOOTHER_H
+#define RSSISMOOTHER_H
+
+#include <Arduino.h>
+
+class RSSISmoother {
+   public:
+    RSSISmoother(float timeConstant, float minCutoff = 1.0f, float beta = 0.0f, float dcutoff = 1.0f);
+    void addRSSIValue(float rssi);
+    float getSmoothedRSSI();
+
+   private:
+    static const size_t bufferSize = 10;
+    std::pair<unsigned long, float> rssiBuffer[bufferSize];  // Fixed-size buffer
+    size_t bufferIndex = 0;                                  // Current index in the buffer
+
+    float timeConstant;
+    float minCutoff;
+    float beta;
+    float dcutoff;
+    float x, dx;
+    float lastRSSI;
+    unsigned long lastTime;
+
+    float getAlpha(float cutoff, float dT);
+};
+
+#endif  // RSSISMOOTHER_H

src/main.cpp

@@ -508,6 +508,9 @@ void scanTask(void *parameter) {
             if (f->query())
                 totalFpQueried++;
 
+        for (auto &f : BleFingerprintCollection::fingerprints)
+            f->filter();
+
         Enrollment::Loop();
 
         if (!pBLEScan->isScanning()) {