diff --git a/lib/BleFingerprint/BleFingerprint.cpp b/lib/BleFingerprint/BleFingerprint.cpp index f63abdd..82c2a42 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(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(); diff --git a/lib/BleFingerprint/FilteredDistance.cpp b/lib/BleFingerprint/FilteredDistance.cpp index b887c23..92331b3 100644 --- a/lib/BleFingerprint/FilteredDistance.cpp +++ b/lib/BleFingerprint/FilteredDistance.cpp @@ -1,64 +1,73 @@ #include "FilteredDistance.h" - #include -#include -#include -#include - -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(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; } diff --git a/lib/BleFingerprint/FilteredDistance.h b/lib/BleFingerprint/FilteredDistance.h index 5ef0bc4..2384909 100644 --- a/lib/BleFingerprint/FilteredDistance.h +++ b/lib/BleFingerprint/FilteredDistance.h @@ -1,35 +1,23 @@ #ifndef FILTEREDDISTANCE_H #define FILTEREDDISTANCE_H -#include - -#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