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

Improve 1-Euro distance filtering, add spike filter (#1108) 

* Spike detection
* Remove SoftFilters
This commit is contained in:
Darrell 2023-11-12 13:27:18 -05:00 committed by GitHub
parent bb81432836
commit 72d786c148
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
12 changed files with 161 additions and 93 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

94
lib/BleFingerprint/BleFingerprint.cpp
View File

@ -16,16 +16,24 @@ class ClientCallbacks : public BLEClientCallbacks {
static ClientCallbacks clientCB;
BleFingerprint::BleFingerprint(BLEAdvertisedDevice *advertisedDevice, float fcmin, float beta, float dcutoff) : oneEuro{OneEuroFilter<float, unsigned long>(1, fcmin, beta, dcutoff)} {
BleFingerprint::BleFingerprint(BLEAdvertisedDevice *advertisedDevice, float fcmin, float beta, float dcutoff) : filteredDistance{FilteredDistance(fcmin, beta, dcutoff)} {
firstSeenMillis = millis();
address = NimBLEAddress(advertisedDevice->getAddress());
addressType = advertisedDevice->getAddressType();
newest = recent = oldest = rssi = advertisedDevice->getRSSI();
rssi = advertisedDevice->getRSSI();
raw = dist = pow(10, float(get1mRssi() - rssi) / (10.0f * BleFingerprintCollection::absorption));
seenCount = 1;
queryReport = nullptr;
fingerprintAddress();
}
void BleFingerprint::setInitial(const BleFingerprint &other) {
rssi = other.rssi;
dist = other.dist;
raw = other.raw;
filteredDistance = other.filteredDistance;
}
bool BleFingerprint::shouldHide(const String &s) {
if (BleFingerprintCollection::include.length() > 0 && !prefixExists(BleFingerprintCollection::include, s)) return true;
return (BleFingerprintCollection::exclude.length() > 0 && prefixExists(BleFingerprintCollection::exclude, s));
@ -196,7 +204,7 @@ void BleFingerprint::fingerprintAddress() {
}
void BleFingerprint::fingerprintServiceAdvertisements(NimBLEAdvertisedDevice *advertisedDevice, size_t serviceAdvCount, bool haveTxPower, int8_t txPower) {
for (size_t i = 0; i < serviceAdvCount; i++) {
for (auto i = 0; i < serviceAdvCount; i++) {
auto uuid = advertisedDevice->getServiceUUID(i);
#ifdef VERBOSE
Serial.printf("Verbose | %s | %-58s%ddBm AD: %s\r\n", getMac().c_str(), getId().c_str(), rssi, advertisedDevice->getServiceUUID(i).toString().c_str());
@ -345,7 +353,6 @@ void BleFingerprint::fingerprintManufactureData(NimBLEAdvertisedDevice *advertis
String pid = Sprintf("apple:%02x%02x:%u", strManufacturerData[2], strManufacturerData[3], strManufacturerData.length());
if (haveTxPower) pid += -txPower;
setId(pid, ID_TYPE_APPLE_NEARBY);
disc = hexStr(strManufacturerData.substr(4)).c_str();
mdRssi = BleFingerprintCollection::rxRefRssi + APPLE_TX;
} else if (strManufacturerData.length() >= 4 && strManufacturerData[2] == 0x12 && strManufacturerData.length() == 29) {
String pid = "apple:findmy";
@ -401,14 +408,6 @@ void BleFingerprint::fingerprintManufactureData(NimBLEAdvertisedDevice *advertis
}
}
bool BleFingerprint::filter() {
Reading<float, unsigned long> inter1, inter2;
inter1.timestamp = millis();
inter1.value = raw;
return oneEuro.push(&inter1, &inter2) && diffFilter.push(&inter2, &output);
}
bool BleFingerprint::seen(BLEAdvertisedDevice *advertisedDevice) {
lastSeenMillis = millis();
reported = false;
@ -417,29 +416,12 @@ bool BleFingerprint::seen(BLEAdvertisedDevice *advertisedDevice) {
fingerprint(advertisedDevice);
if (ignore) return false;
if (ignore || hidden) return false;
oldest = recent;
recent = newest;
newest = advertisedDevice->getRSSI();
auto the_min = min(min(oldest, recent), newest);
auto the_max = max(max(oldest, recent), newest);
auto the_median = the_max ^ the_min ^ oldest ^ recent ^ newest;
rssi = the_median;
const auto ratio = float(get1mRssi() - rssi) / (10.0f * BleFingerprintCollection::absorption);
raw = pow(10, ratio);
if (filter()) hasValue = true;
if (!close && the_min > CLOSE_RSSI + BleFingerprintCollection::rxAdjRssi) {
BleFingerprintCollection::Close(this, true);
close = true;
} else if (close && the_max < LEFT_RSSI + BleFingerprintCollection::rxAdjRssi) {
BleFingerprintCollection::Close(this, false);
close = false;
}
rssi = advertisedDevice->getRSSI();
raw = pow(10, float(get1mRssi() - rssi) / (10.0f * BleFingerprintCollection::absorption));
filteredDistance.addMeasurement(raw);
dist = filteredDistance.getDistance();
if (!added) {
added = true;
@ -449,29 +431,15 @@ bool BleFingerprint::seen(BLEAdvertisedDevice *advertisedDevice) {
return false;
}
void BleFingerprint::setInitial(const BleFingerprint &other) {
newest = other.newest;
recent = other.recent;
oldest = other.oldest;
rssi = other.rssi;
raw = other.rssi;
output = other.output;
oneEuro = other.oneEuro;
diffFilter = other.diffFilter;
hasValue = other.hasValue;
}
void BleFingerprint::fill(JsonObject *doc) {
bool BleFingerprint::fill(JsonObject *doc) {
(*doc)[F("mac")] = getMac();
(*doc)[F("id")] = id;
if (!name.isEmpty()) (*doc)[F("name")] = name;
if (!disc.isEmpty()) (*doc)[F("disc")] = disc;
if (idType) (*doc)[F("idType")] = idType;
(*doc)[F("rssi@1m")] = get1mRssi();
(*doc)[F("rssi")] = rssi;
auto dist = hasValue ? output.value.position : raw;
if (isnormal(raw)) (*doc)[F("raw")] = serialized(String(raw, 2));
if (isnormal(dist)) (*doc)[F("distance")] = serialized(String(dist, 2));
if (close) (*doc)[F("close")] = true;
@ -482,14 +450,13 @@ void BleFingerprint::fill(JsonObject *doc) {
if (battery != 0xFF) (*doc)[F("batt")] = battery;
if (temp) (*doc)[F("temp")] = serialized(String(temp, 1));
if (humidity) (*doc)[F("rh")] = serialized(String(humidity, 1));
return true;
}
bool BleFingerprint::report(JsonObject *doc) {
if (ignore || idType <= ID_TYPE_RAND_MAC || hidden) return false;
if (reported) return false;
auto dist = hasValue ? output.value.position : raw;
auto maxDistance = BleFingerprintCollection::maxDistance;
if (maxDistance > 0 && dist > maxDistance)
return false;
@ -498,11 +465,14 @@ bool BleFingerprint::report(JsonObject *doc) {
if ((abs(dist - lastReported) < BleFingerprintCollection::skipDistance) && (lastReportedMillis > 0) && (now - lastReportedMillis < BleFingerprintCollection::skipMs))
return false;
lastReportedMillis = now;
lastReported = dist;
reported = true;
fill(doc);
return true;
if (fill(doc)) {
lastReportedMillis = now;
lastReported = dist;
reported = true;
return true;
}
return false;
}
bool BleFingerprint::query() {
@ -551,14 +521,22 @@ bool BleFingerprint::query() {
}
bool BleFingerprint::shouldCount() {
if (!close && rssi > CLOSE_RSSI + BleFingerprintCollection::rxAdjRssi) {
BleFingerprintCollection::Close(this, true);
close = true;
} else if (close && rssi < LEFT_RSSI + BleFingerprintCollection::rxAdjRssi) {
BleFingerprintCollection::Close(this, false);
close = false;
}
bool prevCounting = counting;
if (ignore || !countable)
counting = false;
else if (getMsSinceLastSeen() > BleFingerprintCollection::countMs)
counting = false;
else if (counting && output.value.position > BleFingerprintCollection::countExit)
else if (counting && dist > BleFingerprintCollection::countExit)
counting = false;
else if (!counting && output.value.position <= BleFingerprintCollection::countEnter)
else if (!counting && dist <= BleFingerprintCollection::countEnter)
counting = true;
if (prevCounting != counting) {

33
lib/BleFingerprint/BleFingerprint.h
View File

@ -6,13 +6,13 @@
#include <NimBLEDevice.h>
#include <NimBLEEddystoneTLM.h>
#include <NimBLEEddystoneURL.h>
#include <SoftFilters.h>
#include <memory>
#include "QueryReport.h"
#include "rssi.h"
#include "string_utils.h"
#include "FilteredDistance.h"
#define NO_RSSI int8_t(-128)
@ -66,7 +66,7 @@ class BleFingerprint {
bool seen(BLEAdvertisedDevice *advertisedDevice);
void fill(JsonObject *doc);
bool fill(JsonObject *doc);
bool report(JsonObject *doc);
@ -86,17 +86,13 @@ class BleFingerprint {
const short getIdType() const { return idType; }
const String getDiscriminator() const { return disc; }
const float getDistance() const { return output.value.position; }
const float getDistance() const { return dist; }
const int getRssi() const { return rssi; }
const int getNewestRssi() const { return newest; }
const int getRawRssi() const { return rssi; }
const int get1mRssi() const;
void set1mRssi(int8_t rssi) { this->calRssi = rssi; }
void set1mRssi(int8_t rssi) { calRssi = rssi; }
const NimBLEAddress getAddress() const { return address; }
@ -125,33 +121,26 @@ class BleFingerprint {
bool shouldCount();
void fingerprintAddress();
void expire();
private:
static bool shouldHide(const String &s);
bool hasValue = false, added = false, close = false, reported = false, ignore = false, allowQuery = false, isQuerying = false, hidden = false, connectable = false, countable = false, counting = false;
bool added = false, close = false, reported = false, ignore = false, allowQuery = false, isQuerying = false, hidden = false, connectable = false, countable = false, counting = false;
NimBLEAddress address;
String id, name, disc;
String id, name;
short int idType = NO_ID_TYPE;
int rssi = NO_RSSI, newest = NO_RSSI, recent = NO_RSSI, oldest = NO_RSSI;
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, lastReported = 0, temp = 0, humidity = 0;
float raw = 0, dist = 0, 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;
Reading<Differential<float>> output;
OneEuroFilter<float, unsigned long> oneEuro;
DifferentialFilter<float, unsigned long> diffFilter;
FilteredDistance filteredDistance;
std::unique_ptr<QueryReport> queryReport = nullptr;
bool filter();
static bool shouldHide(const String &s);
void fingerprint(NimBLEAdvertisedDevice *advertisedDevice);
void fingerprintServiceAdvertisements(NimBLEAdvertisedDevice *advertisedDevice, size_t serviceAdvCount, bool haveTxPower, int8_t txPower);
void fingerprintServiceData(NimBLEAdvertisedDevice *advertisedDevice, size_t serviceDataCount, bool haveTxPower, int8_t txPower);

6
lib/BleFingerprint/BleFingerprintCollection.h
View File

@ -3,9 +3,9 @@
#include "BleFingerprint.h"
#define ONE_EURO_FCMIN 1e-5f
#define ONE_EURO_BETA 1e-7f
#define ONE_EURO_DCUTOFF 1e-5f
#define ONE_EURO_FCMIN 1e-1f
#define ONE_EURO_BETA 1e-3f
#define ONE_EURO_DCUTOFF 5e-3f
#ifndef ALLOW_BLE_CONTROLLER_RESTART_AFTER_SECS
#define ALLOW_BLE_CONTROLLER_RESTART_AFTER_SECS 1800

64
lib/BleFingerprint/FilteredDistance.cpp Normal file
View File

@ -0,0 +1,64 @@
#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), 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<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::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);
}

35
lib/BleFingerprint/FilteredDistance.h Normal file
View File

@ -0,0 +1,35 @@
#ifndef FILTEREDDISTANCE_H
#define FILTEREDDISTANCE_H
#include <Arduino.h>
#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);
void addMeasurement(float dist);
const float getMedianDistance() const;
const float getDistance() 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
void initSpike(float dist);
float removeSpike(float dist);
};
#endif // FILTEREDDISTANCE_H

1
lib/handlers/MiFloraHandler.h
View File

@ -5,7 +5,6 @@
#include <BleFingerprint.h>
#include <NimBLEClient.h>
#include <NimBLEDevice.h>
#include <SoftFilters.h>
namespace MiFloraHandler
{

1
lib/handlers/NameModelHandler.h
View File

@ -3,7 +3,6 @@
#include <NimBLEDevice.h>
#include <ArduinoJson.h>
#include <sstream>
#include <SoftFilters.h>
#include <AsyncMqttClient.h>
#include <BleFingerprint.h>
#include <AsyncWiFiSettings.h>

1
platformio.ini
View File

@ -53,7 +53,6 @@ lib_deps =
AsyncTCP = https://github.com/pbolduc/AsyncTCP.git#v1.2.0
https://github.com/esphome/ESPAsyncWebServer.git#v3.1.0
https://github.com/ESPresense/AsyncWiFiSettings.git#1.0.9
https://github.com/ESPresense/SoftFilters.git
https://github.com/ESPresense/NimBLE-Arduino.git
marvinroger/AsyncMqttClient@^0.9.0
bblanchon/ArduinoJson@^6.21.3

8
src/GUI.cpp
View File

@ -53,21 +53,21 @@ void Loop() {
void Added(BleFingerprint *f) {
if (f->getIgnore()) return;
Serial.printf("%u New %s | %s | %-58s%ddBm %s\r\n", xPortGetCoreID(), f->getAllowQuery() ? "Q" : " ", f->getMac().c_str(), f->getId().c_str(), f->getRssi(), f->getDiscriminator().c_str());
Serial.printf("%u New %s | %s | %-58s%ddBm\r\n", xPortGetCoreID(), f->getAllowQuery() ? "Q" : " ", f->getMac().c_str(), f->getId().c_str(), f->getRssi());
}
void Removed(BleFingerprint *f) {
if (f->getIgnore() || !f->getAdded()) return;
Serial.printf("\u001b[38;5;236m%u Del | %s | %-58s%ddBm %s\u001b[0m\r\n", xPortGetCoreID(), f->getMac().c_str(), f->getId().c_str(), f->getRssi(), f->getDiscriminator().c_str());
Serial.printf("\u001b[38;5;236m%u Del | %s | %-58s%ddBm\u001b[0m\r\n", xPortGetCoreID(), f->getMac().c_str(), f->getId().c_str(), f->getRssi());
}
void Close(BleFingerprint *f) {
Serial.printf("\u001b[32m%u Close | %s | %-58s%ddBm\u001b[0m\r\n", xPortGetCoreID(), f->getMac().c_str(), f->getId().c_str(), f->getNewestRssi());
Serial.printf("\u001b[32m%u Close | %s | %-58s%ddBm\u001b[0m\r\n", xPortGetCoreID(), f->getMac().c_str(), f->getId().c_str(), f->getRawRssi());
Display::Status("C:%s\r\n", f->getId().c_str());
}
void Left(BleFingerprint *f) {
Serial.printf("\u001b[33m%u Left | %s | %-58s%ddBm\u001b[0m\r\n", xPortGetCoreID(), f->getMac().c_str(), f->getId().c_str(), f->getNewestRssi());
Serial.printf("\u001b[33m%u Left | %s | %-58s%ddBm\u001b[0m\r\n", xPortGetCoreID(), f->getMac().c_str(), f->getId().c_str(), f->getRawRssi());
Display::Status("L:%s\r\n", f->getId().c_str());
}
void Motion(bool pir, bool radar) {

6
src/HttpWebServer.cpp
View File

@ -48,8 +48,10 @@ void serializeDevices(JsonObject &root, bool showAll) {
bool visible = (*it)->getVisible();
if (showAll || visible) {
JsonObject node = devices.createNestedObject();
(*it)->fill(&node);
if (showAll && visible) node[F("vis")] = true;
if ((*it)->fill(&node)) {
if (showAll && visible) node[F("vis")] = true;
} else
devices.remove(devices.size() - 1);
}
}
}

4
src/main.cpp
View File

@ -104,6 +104,8 @@ bool sendTelemetry(unsigned int totalSeen, unsigned int totalFpSeen, unsigned in
doc["queried"] = totalFpQueried;
if (totalFpReported > 0)
doc["reported"] = totalFpReported;
if (reportFailed > 0)
doc["failed"] = reportFailed;
if (teleFails > 0)
doc["teleFails"] = teleFails;
if (reconnectTries > 0)
@ -427,7 +429,7 @@ bool reportDevice(BleFingerprint *f) {
delay(20);
}
teleFails++;
reportFailed++;
return false;
}

1
src/main.h
View File

@ -54,6 +54,7 @@ unsigned long updateStartedMillis = 0;
unsigned long lastTeleMillis = 0;
int reconnectTries = 0;
int teleFails = 0;
int reportFailed = 0;
bool online = false; // Have we successfully sent status=online
bool sentDiscovery = false; // Have we successfully sent discovery
UBaseType_t bleStack = 0;