From 420e5cee5a627c68bec3c4d7e6c18e5f1cc704c4 Mon Sep 17 00:00:00 2001
From: DTTerastar <DT@Terastar.biz>
Date: Fri, 17 Nov 2023 08:55:34 -0500
Subject: [PATCH] Fix swapped params

---
 lib/BleFingerprint/BleFingerprint.cpp   |  2 +-
 lib/BleFingerprint/FilteredDistance.cpp | 50 ++++++++++++-------------
 2 files changed, 26 insertions(+), 26 deletions(-)

diff --git a/lib/BleFingerprint/BleFingerprint.cpp b/lib/BleFingerprint/BleFingerprint.cpp
index 82c2a42..5c527aa 100644
--- a/lib/BleFingerprint/BleFingerprint.cpp
+++ b/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(25, 0.1)} {
+BleFingerprint::BleFingerprint(BLEAdvertisedDevice *advertisedDevice, float fcmin, float beta, float dcutoff) : filteredDistance{FilteredDistance(0.1, 25)} {
     firstSeenMillis = millis();
     address = NimBLEAddress(advertisedDevice->getAddress());
     addressType = advertisedDevice->getAddressType();
diff --git a/lib/BleFingerprint/FilteredDistance.cpp b/lib/BleFingerprint/FilteredDistance.cpp
index 92331b3..354f1c7 100644
--- a/lib/BleFingerprint/FilteredDistance.cpp
+++ b/lib/BleFingerprint/FilteredDistance.cpp
@@ -1,64 +1,64 @@
 #include "FilteredDistance.h"
+
 #include <Arduino.h>
 
-FilteredDistance::FilteredDistance(float processNoise, float measurementNoise): processNoise(processNoise), measurementNoise(measurementNoise), isFirstMeasurement(true) {}
+FilteredDistance::FilteredDistance(float processNoise, float measurementNoise) : processNoise(processNoise), measurementNoise(measurementNoise), isFirstMeasurement(true) {}
 
 void FilteredDistance::addMeasurement(float dist) {
     if (isFirstMeasurement) {
         // Initialize state
-        state[0] = dist; // Distance
-        state[1] = 0;    // Rate of change in distance
+        state[0] = dist;  // Distance
+        state[1] = 0;     // Rate of change in distance
 
         // Initialize covariance matrix
-        covariance[0][0] = 1; // Initial guess
+        covariance[0][0] = 1;  // Initial guess
         covariance[0][1] = 0;
         covariance[1][0] = 0;
-        covariance[1][1] = 1; // Initial guess
+        covariance[1][1] = 1;  // Initial guess
 
-        lastUpdateTime = micros(); // Set the update time
+        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;
+    const unsigned long now = micros();
+    const unsigned long elapsed = now - now;
+    float dT = std::max(elapsed * 0.000001f, 0.05f);  // Convert microseconds to seconds, enforce a minimum dT
+    lastUpdateTime = now;
 
     // Perform prediction and update
-    prediction(deltaTime);
+    prediction(dT);
     update(dist);
 }
 
 const float FilteredDistance::getDistance() const {
-    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;
+    const unsigned long now = micros();
+    const unsigned long elapsed = now - now;
+    if (elapsed < 5000) return state[0];  // If the last update was less than 5ms ago, return the current state
+    float dT = elapsed * 0.000001f;
+    return state[0] + state[1] * dT;
 }
 
-void FilteredDistance::prediction(float deltaTime) {
+void FilteredDistance::prediction(float dT) {
     // Update state estimate
-    state[0] += state[1] * deltaTime;
+    state[0] += state[1] * dT;
 
     // 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];
+    covariance[0][0] += dT * (dT * covariance[1][1] + covariance[1][0]) + processNoise;
+    covariance[0][1] += dT * covariance[1][1] + processNoise;
+    covariance[1][0] += dT * covariance[1][1] + processNoise;
+    covariance[1][1] += processNoise;
 }
 
 void FilteredDistance::update(float distanceMeasurement) {
     // Kalman gain calculation
     float S = covariance[0][0] + measurementNoise;
-    float K[2]; // Kalman gain
+    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
+    float y = distanceMeasurement - state[0];  // measurement residual
     state[0] += K[0] * y;
     state[1] += K[1] * y;