diff --git a/lib/BleFingerprint/BleFingerprint.cpp b/lib/BleFingerprint/BleFingerprint.cpp index e55751c..6396c21 100644 --- a/lib/BleFingerprint/BleFingerprint.cpp +++ b/lib/BleFingerprint/BleFingerprint.cpp @@ -411,6 +411,8 @@ void BleChannelObservation::observe(unsigned long timestamp, int rssi1m, int rss filter.addMeasurement(raw); dist = filter.getDistance(); vari = filter.getVariance(); + ci = 1.959 * std::sqrt(vari) / std::sqrt(12); + mean = filter.getMeanDistance(); lastSeenMillis = timestamp; } @@ -448,47 +450,62 @@ bool BleFingerprint::fill(JsonObject *doc) { (*doc)[F("raw37")] = serialized(String(channels[0].raw, 2)); (*doc)[F("distance37")] = serialized(String(channels[0].dist, 2)); (*doc)[F("var37")] = serialized(String(channels[0].vari, 2)); + (*doc)[F("mean37")] = serialized(String(channels[0].mean, 2)); + (*doc)[F("ci37")] = serialized(String(channels[0].ci, 2)); break; case 38: (*doc)[F("rssi38")] = channels[1].rssi; (*doc)[F("raw38")] = serialized(String(channels[1].raw, 2)); (*doc)[F("distance38")] = serialized(String(channels[1].dist, 2)); (*doc)[F("var38")] = serialized(String(channels[1].vari, 2)); + (*doc)[F("mean38")] = serialized(String(channels[1].mean, 2)); + (*doc)[F("ci39")] = serialized(String(channels[1].ci, 2)); break; case 39: (*doc)[F("rssi39")] = channels[2].rssi; (*doc)[F("raw39")] = serialized(String(channels[2].raw, 2)); (*doc)[F("distance39")] = serialized(String(channels[2].dist, 2)); (*doc)[F("var39")] = serialized(String(channels[2].vari, 2)); + (*doc)[F("mean39")] = serialized(String(channels[2].mean, 2)); + (*doc)[F("ci39")] = serialized(String(channels[2].ci, 2)); break; } (*doc)[F("channel")] = lastChannel; - float weightedDistances = 0; + float weightedDistances = 0, weightedMeans = 0, weightedVariances = 0, weightedCIs = 0; float sumWeights = 0; - float sumVariances = 0; int channelCount = 0; for (const auto& channel : channels) { if (channel.lastSeenMillis < millis() - 5000) continue; float weight = 1 / std::max(channel.vari, 0.05f); weightedDistances += channel.dist * weight; + weightedMeans += channel.mean * weight; + weightedVariances += channel.vari * weight; + weightedCIs += channel.ci * weight; sumWeights += weight; - sumVariances += channel.vari; channelCount++; } // FIXME: weight channels by timestamp of last packet, so we can ignore stale values? if (!channelCount) { - weightedDistances = getMinObservedDistance(); + weightedDistances = channels[0].dist; + weightedMeans = channels[0].mean; + weightedVariances = channels[0].vari; sumWeights = 1; - sumVariances = 0; channelCount = 1; } float fusedDistance = weightedDistances / sumWeights; - float fusedVariance = sumVariances / channelCount; (*doc)[F("distance")] = serialized(String(fusedDistance, 2)); + + float fusedMean = weightedMeans / sumWeights; + (*doc)[F("mean")] = serialized(String(fusedMean, 2)); + + float fusedVariance = weightedVariances / sumWeights; (*doc)[F("var")] = serialized(String(fusedVariance, 2)); + float fusedCI = weightedCIs / sumWeights; + (*doc)[F("ci")] = serialized(String(fusedCI, 2)); + auto minRaw = std::min_element(channels.begin(), channels.end(), [](const BleChannelObservation& a, const BleChannelObservation& b) { return a.raw < b.raw; })->raw; (*doc)[F("raw")] = serialized(String(minRaw, 2)); if (close) (*doc)[F("close")] = true; diff --git a/lib/BleFingerprint/BleFingerprint.h b/lib/BleFingerprint/BleFingerprint.h index daf9e33..3e40478 100644 --- a/lib/BleFingerprint/BleFingerprint.h +++ b/lib/BleFingerprint/BleFingerprint.h @@ -82,12 +82,14 @@ struct BleChannelObservation { int rssi { NO_RSSI }; float raw { 0 }; float dist { 0 }; + float mean { 0 }; float vari { 0 }; + float ci { 0 }; unsigned long lastSeenMillis { 0 }; void observe(unsigned long timestamp, int rssi1m, int rssi); - private: +private: FilteredDistance filter { ONE_EURO_FCMIN, ONE_EURO_BETA, ONE_EURO_DCUTOFF }; }; diff --git a/lib/BleFingerprint/FilteredDistance.cpp b/lib/BleFingerprint/FilteredDistance.cpp index 6c42b55..5bbd3c7 100644 --- a/lib/BleFingerprint/FilteredDistance.cpp +++ b/lib/BleFingerprint/FilteredDistance.cpp @@ -59,6 +59,10 @@ void FilteredDistance::addMeasurement(float dist) { } } +const float FilteredDistance::getMeanDistance() const { + return total / static_cast(NUM_READINGS); +} + const float FilteredDistance::getDistance() const { return lastDist; } diff --git a/lib/BleFingerprint/FilteredDistance.h b/lib/BleFingerprint/FilteredDistance.h index 7f936b9..0c92879 100644 --- a/lib/BleFingerprint/FilteredDistance.h +++ b/lib/BleFingerprint/FilteredDistance.h @@ -10,7 +10,7 @@ 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 getMeanDistance() const; const float getDistance() const; const float getVariance() const;