MirrorSave
/
ESPresense
mirror of https://github.com/ESPresense/ESPresense.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Implement a new experimental filter 

This filter is based on one key observation:

Assuming stationary receivers and transmitters: Given several RSSI
measurements, e.g. -70, -50 and -90 we can deduce that the measurement
with the smallest error must be -50, i.e. the highest observed measurement.
This is because the measured signal strength can never be above the true
signal strength and can only ever get worse due to varying levels of
attenuation or multipath issues.
This commit is contained in:
Gunnar Beutner 2023-11-28 22:26:45 +01:00
parent 00ff50e71f
commit 05812bd11a
5 changed files with 104 additions and 128 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
lib/BleFingerprint/BleFingerprint.cpp
View File

@ -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;

6
lib/BleFingerprint/BleFingerprint.h
View File

@ -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> 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);

81
lib/BleFingerprint/FilteredDistance.cpp
View File

@ -1,81 +0,0 @@
#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), 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<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
}
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<float>(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<float>(NUM_READINGS);
auto meanOfSquares = totalSquared / static_cast<float>(NUM_READINGS);
auto variance = meanOfSquares - (mean * mean); // Variance formula: E(X^2) - (E(X))^2
if (variance < 0.0f) return 0.0f;
return variance;
}

38
lib/BleFingerprint/FilteredDistance.h
View File

@ -1,38 +0,0 @@
#ifndef FILTEREDDISTANCE_H
#define FILTEREDDISTANCE_H
#include <Arduino.h>
#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

88
lib/BleFingerprint/PhysicsBasedFilter.h Normal file
View File

@ -0,0 +1,88 @@
#ifndef PBF_H
#define PBF_H
#include <cstddef>
#include <algorithm>
#include <utility>
#include <bitset>
#include <numeric>
#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<double>(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<float>(measurements_.size());
}
private:
bool initialized_ { false };
std::array<float, PBF_STATIONARY_WINDOW> measurements_;
double distance_;
double mean_;
double varianceSum_ { 0 };
float movementTime_ { 0 };
std::bitset<PBF_MOVEMENT_DECISION_WINDOW> movements_;
};
#endif /* PBF_H */