diff --git a/lib/BleFingerprint/BleFingerprint.cpp b/lib/BleFingerprint/BleFingerprint.cpp index 1c2018b..6e6b8bf 100644 --- a/lib/BleFingerprint/BleFingerprint.cpp +++ b/lib/BleFingerprint/BleFingerprint.cpp @@ -22,6 +22,7 @@ BleFingerprint::BleFingerprint(BLEAdvertisedDevice *advertisedDevice) { addressType = advertisedDevice->getAddressType(); for (auto& channel : channels) channel.observe(firstSeenMillis, get1mRssi(), advertisedDevice->getRSSI()); + filter.update(channels[0].raw, 0.0); seenCount = 1; queryReport = nullptr; fingerprintAddress(); @@ -29,6 +30,7 @@ BleFingerprint::BleFingerprint(BLEAdvertisedDevice *advertisedDevice) { void BleFingerprint::setInitial(const BleFingerprint &other) { channels = other.channels; + filter = other.filter; } bool BleFingerprint::shouldHide(const String &s) { @@ -421,7 +423,12 @@ bool BleFingerprint::seen(BLEAdvertisedDevice *advertisedDevice, uint8_t channel if (ignore || hidden) return false; lastChannel = channel; - channels[ble_channel_to_index(channel)].observe(millis(), get1mRssi(), advertisedDevice->getRSSI()); + auto& observedChannel = channels[ble_channel_to_index(channel)]; + + auto now = millis(); + double deltaTime = (now - observedChannel.lastSeenMillis) / 1000.0f; + + observedChannel.observe(now, get1mRssi(), advertisedDevice->getRSSI()); int maxRssiRecent = NO_RSSI, maxRssiOverall = NO_RSSI; for (const auto& channel : channels) { @@ -433,7 +440,7 @@ bool BleFingerprint::seen(BLEAdvertisedDevice *advertisedDevice, uint8_t channel int rssi = (maxRssiRecent != NO_RSSI) ? maxRssiRecent : maxRssiOverall; auto raw = pow(10, float(get1mRssi() - rssi) / (10.0f * BleFingerprintCollection::absorption)); - filter.addMeasurement(raw); + filter.update(raw, deltaTime); if (!added) { added = true; @@ -469,10 +476,10 @@ bool BleFingerprint::fill(JsonObject *doc) { } (*doc)[F("channel")] = lastChannel; - (*doc)[F("distance")] = serialized(String(filter.getDistance(), 2)); - (*doc)[F("mean")] = serialized(String(filter.getMeanDistance(), 2)); - (*doc)[F("var")] = serialized(String(filter.getVariance(), 2)); - (*doc)[F("ci")] = serialized(String(1.959 * std::sqrt(filter.getVariance()) / std::sqrt(12), 2)); + (*doc)[F("distance")] = serialized(String(filter.distance(), 2)); + (*doc)[F("mean")] = serialized(String(filter.mean(), 2)); + (*doc)[F("var")] = serialized(String(filter.variance(), 2)); + (*doc)[F("ci")] = serialized(String(1.959 * std::sqrt(filter.variance()) / std::sqrt(12), 2)); if (close) (*doc)[F("close")] = true; diff --git a/lib/BleFingerprint/BleFingerprint.h b/lib/BleFingerprint/BleFingerprint.h index 81c3c78..bb1cdb9 100644 --- a/lib/BleFingerprint/BleFingerprint.h +++ b/lib/BleFingerprint/BleFingerprint.h @@ -12,7 +12,7 @@ #include "QueryReport.h" #include "rssi.h" #include "string_utils.h" -#include "FilteredDistance.h" +#include "PhysicsBasedFilter.h" #define NO_RSSI int8_t(-128) @@ -112,7 +112,7 @@ class BleFingerprint { const short getIdType() const { return idType; } - const float getDistance() const { return filter.getDistance(); } + const float getDistance() const { return filter.distance(); } const int getMaxObservedRssi() const { return std::max_element(channels.begin(), channels.end(), [](const BleChannelObservation& a, const BleChannelObservation& b) { return a.rssi < b.rssi; })->rssi; @@ -170,7 +170,7 @@ class BleFingerprint { uint16_t mv = 0; uint8_t battery = 0xFF, addressType = 0xFF; std::unique_ptr queryReport = nullptr; - FilteredDistance filter { ONE_EURO_FCMIN, ONE_EURO_BETA, ONE_EURO_DCUTOFF }; + PhysicsBasedFilter filter; static bool shouldHide(const String &s); void fingerprint(NimBLEAdvertisedDevice *advertisedDevice); diff --git a/lib/BleFingerprint/FilteredDistance.cpp b/lib/BleFingerprint/FilteredDistance.cpp deleted file mode 100644 index 5bbd3c7..0000000 --- a/lib/BleFingerprint/FilteredDistance.cpp +++ /dev/null @@ -1,81 +0,0 @@ -#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), totalSquared(0), readIndex(0) { -} - -void FilteredDistance::initSpike(float dist) { - for (size_t i = 0; i < NUM_READINGS; i++) { - readings[i] = dist; - } - total = dist * NUM_READINGS; - totalSquared = dist * dist * NUM_READINGS; // Initialize sum of squared distances -} - -float FilteredDistance::removeSpike(float dist) { - total -= readings[readIndex]; // Subtract the last reading - totalSquared -= readings[readIndex] * readings[readIndex]; // Subtract the square of the last reading - - readings[readIndex] = dist; // Read the sensor - total += readings[readIndex]; // Add the reading to the total - totalSquared += readings[readIndex] * readings[readIndex]; // Add the square of the reading - - 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 -} - -void FilteredDistance::addMeasurement(float dist) { - const bool initialized = lastTime != 0; - const unsigned long now = micros(); - const unsigned long elapsed = now - lastTime; - lastTime = now; - - 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); - - dist = removeSpike(dist); - x += alpha * (dist - x); - dx = dAlpha * ((dist - lastDist) / dT); - lastDist = x + beta * dx; - } -} - -const float FilteredDistance::getMeanDistance() const { - return total / static_cast(NUM_READINGS); -} - -const float FilteredDistance::getDistance() const { - return lastDist; -} - -float FilteredDistance::getAlpha(float cutoff, float dT) { - float tau = 1.0f / (2 * M_PI * cutoff); - return 1.0f / (1.0f + tau / dT); -} - -const float FilteredDistance::getVariance() const { - auto mean = total / static_cast(NUM_READINGS); - auto meanOfSquares = totalSquared / static_cast(NUM_READINGS); - auto variance = meanOfSquares - (mean * mean); // Variance formula: E(X^2) - (E(X))^2 - if (variance < 0.0f) return 0.0f; - return variance; -} diff --git a/lib/BleFingerprint/FilteredDistance.h b/lib/BleFingerprint/FilteredDistance.h deleted file mode 100644 index 0c92879..0000000 --- a/lib/BleFingerprint/FilteredDistance.h +++ /dev/null @@ -1,38 +0,0 @@ -#ifndef FILTEREDDISTANCE_H -#define FILTEREDDISTANCE_H - -#include - -#define SPIKE_THRESHOLD 1.0f // Threshold for spike detection -#define NUM_READINGS 12 // Number of readings to keep track of - -class FilteredDistance { - public: - FilteredDistance(float minCutoff = 1.0f, float beta = 0.0f, float dcutoff = 1.0f); - void addMeasurement(float dist); - const float getMeanDistance() const; - const float getDistance() const; - const float getVariance() const; - - bool hasValue() const { return lastTime != 0; } - - private: - float minCutoff; - float beta; - float dcutoff; - float x, dx; - float lastDist; - unsigned long lastTime; - - 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 - float totalSquared; // Total of the squared readings - - void initSpike(float dist); - float removeSpike(float dist); -}; - -#endif // FILTEREDDISTANCE_H diff --git a/lib/BleFingerprint/PhysicsBasedFilter.h b/lib/BleFingerprint/PhysicsBasedFilter.h new file mode 100644 index 0000000..5887fcf --- /dev/null +++ b/lib/BleFingerprint/PhysicsBasedFilter.h @@ -0,0 +1,88 @@ +#ifndef PBF_H +#define PBF_H + +#include +#include +#include +#include +#include + +#define PBF_STATIONARY_WINDOW 15 +#define PBF_MOVEMENT_DECISION_WINDOW 3 +#define PBF_MOVEMENT_LOOKBACK_WINDOW 6 + +static_assert(PBF_STATIONARY_WINDOW > PBF_MOVEMENT_DECISION_WINDOW); +static_assert(PBF_MOVEMENT_LOOKBACK_WINDOW >= PBF_MOVEMENT_DECISION_WINDOW); + +struct PhysicsBasedFilter { + void update(float measuredDistance, float deltaTime) { + //std::cout << " measured: " << distanceMeasured << "\n"; + + if (!initialized_) { + // use the first measurement to initialize the entire array because we + // have no better values at this point + std::fill(measurements_.begin(), measurements_.end(), measuredDistance); + mean_ = measuredDistance; + distance_ = measuredDistance; + initialized_ = true; + return; + } + + // shift all existing measurements to the left and insert the new measurement at the end + std::copy(measurements_.begin() + 1, measurements_.end(), measurements_.begin()); + measurements_.back() = measuredDistance; + + //std::cout << " mean: " << mean_ << " variance: " << variance_ << " stdev: " << stdev << "\n"; + + // update mean and variance - this ignores the latest PBS_MOVEMENT_LOOKBACK_WINDOW values + const auto laggedLatestMeasurement = *(measurements_.end() - PBF_MOVEMENT_LOOKBACK_WINDOW - 1); + auto newMean = mean_ + (laggedLatestMeasurement - measurements_.front()) / static_cast(measurements_.size() - PBF_MOVEMENT_LOOKBACK_WINDOW); + varianceSum_ += (laggedLatestMeasurement - mean_) * (laggedLatestMeasurement - newMean) - (measurements_.front() - mean_) * (measurements_.front() - newMean); + mean_ = newMean; + + movementTime_ = std::max(0.0f, movementTime_ - deltaTime); + + //if (movementTime_ > 0) + // std::cout << " still moving\n"; + + // remember whether we're currently moving away from the node + const bool movement = measuredDistance > mean_; + movements_ <<= 1; + movements_[0] = movement; + + //std::cout << " movement: " << movement << " movements: " << movements_ << "\n"; + + if (movements_.all() && !movementTime_) { + //std::cout << " detected movement\n"; + movementTime_ = 10; + } + + if (!movementTime_) + distance_ = *std::min_element(measurements_.begin(), measurements_.end()); + else + distance_ = *std::min_element(measurements_.end() - PBF_MOVEMENT_DECISION_WINDOW - 1, measurements_.end()); + } + + float distance() const { + return distance_; + } + + float mean() const { + return mean_; + } + + float variance() const { + return varianceSum_ / static_cast(measurements_.size()); + } + +private: + bool initialized_ { false }; + std::array measurements_; + double distance_; + double mean_; + double varianceSum_ { 0 }; + float movementTime_ { 0 }; + std::bitset movements_; +}; + +#endif /* PBF_H */