Rework NaN handling in sensor filters (#3610)

Co-authored-by: Otto Winter <otto@otto-winter.com>
This commit is contained in:
DAVe3283 2022-07-27 22:39:11 -06:00 committed by GitHub
parent 2a49811f6e
commit ed26c57d99
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
2 changed files with 90 additions and 77 deletions

View file

@ -37,31 +37,38 @@ MedianFilter::MedianFilter(size_t window_size, size_t send_every, size_t send_fi
void MedianFilter::set_send_every(size_t send_every) { this->send_every_ = send_every; } void MedianFilter::set_send_every(size_t send_every) { this->send_every_ = send_every; }
void MedianFilter::set_window_size(size_t window_size) { this->window_size_ = window_size; } void MedianFilter::set_window_size(size_t window_size) { this->window_size_ = window_size; }
optional<float> MedianFilter::new_value(float value) { optional<float> MedianFilter::new_value(float value) {
if (!std::isnan(value)) {
while (this->queue_.size() >= this->window_size_) { while (this->queue_.size() >= this->window_size_) {
this->queue_.pop_front(); this->queue_.pop_front();
} }
this->queue_.push_back(value); this->queue_.push_back(value);
ESP_LOGVV(TAG, "MedianFilter(%p)::new_value(%f)", this, value); ESP_LOGVV(TAG, "MedianFilter(%p)::new_value(%f)", this, value);
}
if (++this->send_at_ >= this->send_every_) { if (++this->send_at_ >= this->send_every_) {
this->send_at_ = 0; this->send_at_ = 0;
float median = 0.0f; float median = NAN;
if (!this->queue_.empty()) { if (!this->queue_.empty()) {
std::deque<float> median_queue = this->queue_; // Copy queue without NaN values
std::vector<float> median_queue;
for (auto v : this->queue_) {
if (!std::isnan(v)) {
median_queue.push_back(v);
}
}
sort(median_queue.begin(), median_queue.end()); sort(median_queue.begin(), median_queue.end());
size_t queue_size = median_queue.size(); size_t queue_size = median_queue.size();
if (queue_size) {
if (queue_size % 2) { if (queue_size % 2) {
median = median_queue[queue_size / 2]; median = median_queue[queue_size / 2];
} else { } else {
median = (median_queue[queue_size / 2] + median_queue[(queue_size / 2) - 1]) / 2.0f; median = (median_queue[queue_size / 2] + median_queue[(queue_size / 2) - 1]) / 2.0f;
} }
} }
}
ESP_LOGVV(TAG, "MedianFilter(%p)::new_value(%f) SENDING", this, median); ESP_LOGVV(TAG, "MedianFilter(%p)::new_value(%f) SENDING %f", this, value, median);
return median; return median;
} }
return {}; return {};
@ -74,29 +81,36 @@ void QuantileFilter::set_send_every(size_t send_every) { this->send_every_ = sen
void QuantileFilter::set_window_size(size_t window_size) { this->window_size_ = window_size; } void QuantileFilter::set_window_size(size_t window_size) { this->window_size_ = window_size; }
void QuantileFilter::set_quantile(float quantile) { this->quantile_ = quantile; } void QuantileFilter::set_quantile(float quantile) { this->quantile_ = quantile; }
optional<float> QuantileFilter::new_value(float value) { optional<float> QuantileFilter::new_value(float value) {
if (!std::isnan(value)) {
while (this->queue_.size() >= this->window_size_) { while (this->queue_.size() >= this->window_size_) {
this->queue_.pop_front(); this->queue_.pop_front();
} }
this->queue_.push_back(value); this->queue_.push_back(value);
ESP_LOGVV(TAG, "QuantileFilter(%p)::new_value(%f), quantile:%f", this, value, this->quantile_); ESP_LOGVV(TAG, "QuantileFilter(%p)::new_value(%f), quantile:%f", this, value, this->quantile_);
}
if (++this->send_at_ >= this->send_every_) { if (++this->send_at_ >= this->send_every_) {
this->send_at_ = 0; this->send_at_ = 0;
float result = 0.0f; float result = NAN;
if (!this->queue_.empty()) { if (!this->queue_.empty()) {
std::deque<float> quantile_queue = this->queue_; // Copy queue without NaN values
std::vector<float> quantile_queue;
for (auto v : this->queue_) {
if (!std::isnan(v)) {
quantile_queue.push_back(v);
}
}
sort(quantile_queue.begin(), quantile_queue.end()); sort(quantile_queue.begin(), quantile_queue.end());
size_t queue_size = quantile_queue.size(); size_t queue_size = quantile_queue.size();
if (queue_size) {
size_t position = ceilf(queue_size * this->quantile_) - 1; size_t position = ceilf(queue_size * this->quantile_) - 1;
ESP_LOGVV(TAG, "QuantileFilter(%p)::position: %d/%d", this, position, queue_size); ESP_LOGVV(TAG, "QuantileFilter(%p)::position: %d/%d", this, position + 1, queue_size);
result = quantile_queue[position]; result = quantile_queue[position];
} }
}
ESP_LOGVV(TAG, "QuantileFilter(%p)::new_value(%f) SENDING", this, result); ESP_LOGVV(TAG, "QuantileFilter(%p)::new_value(%f) SENDING %f", this, value, result);
return result; return result;
} }
return {}; return {};
@ -108,24 +122,23 @@ MinFilter::MinFilter(size_t window_size, size_t send_every, size_t send_first_at
void MinFilter::set_send_every(size_t send_every) { this->send_every_ = send_every; } void MinFilter::set_send_every(size_t send_every) { this->send_every_ = send_every; }
void MinFilter::set_window_size(size_t window_size) { this->window_size_ = window_size; } void MinFilter::set_window_size(size_t window_size) { this->window_size_ = window_size; }
optional<float> MinFilter::new_value(float value) { optional<float> MinFilter::new_value(float value) {
if (!std::isnan(value)) {
while (this->queue_.size() >= this->window_size_) { while (this->queue_.size() >= this->window_size_) {
this->queue_.pop_front(); this->queue_.pop_front();
} }
this->queue_.push_back(value); this->queue_.push_back(value);
ESP_LOGVV(TAG, "MinFilter(%p)::new_value(%f)", this, value); ESP_LOGVV(TAG, "MinFilter(%p)::new_value(%f)", this, value);
}
if (++this->send_at_ >= this->send_every_) { if (++this->send_at_ >= this->send_every_) {
this->send_at_ = 0; this->send_at_ = 0;
float min = 0.0f; float min = NAN;
if (!this->queue_.empty()) { for (auto v : this->queue_) {
std::deque<float>::iterator it = std::min_element(queue_.begin(), queue_.end()); if (!std::isnan(v)) {
min = *it; min = std::isnan(min) ? v : std::min(min, v);
}
} }
ESP_LOGVV(TAG, "MinFilter(%p)::new_value(%f) SENDING", this, min); ESP_LOGVV(TAG, "MinFilter(%p)::new_value(%f) SENDING %f", this, value, min);
return min; return min;
} }
return {}; return {};
@ -137,24 +150,23 @@ MaxFilter::MaxFilter(size_t window_size, size_t send_every, size_t send_first_at
void MaxFilter::set_send_every(size_t send_every) { this->send_every_ = send_every; } void MaxFilter::set_send_every(size_t send_every) { this->send_every_ = send_every; }
void MaxFilter::set_window_size(size_t window_size) { this->window_size_ = window_size; } void MaxFilter::set_window_size(size_t window_size) { this->window_size_ = window_size; }
optional<float> MaxFilter::new_value(float value) { optional<float> MaxFilter::new_value(float value) {
if (!std::isnan(value)) {
while (this->queue_.size() >= this->window_size_) { while (this->queue_.size() >= this->window_size_) {
this->queue_.pop_front(); this->queue_.pop_front();
} }
this->queue_.push_back(value); this->queue_.push_back(value);
ESP_LOGVV(TAG, "MaxFilter(%p)::new_value(%f)", this, value); ESP_LOGVV(TAG, "MaxFilter(%p)::new_value(%f)", this, value);
}
if (++this->send_at_ >= this->send_every_) { if (++this->send_at_ >= this->send_every_) {
this->send_at_ = 0; this->send_at_ = 0;
float max = 0.0f; float max = NAN;
if (!this->queue_.empty()) { for (auto v : this->queue_) {
std::deque<float>::iterator it = std::max_element(queue_.begin(), queue_.end()); if (!std::isnan(v)) {
max = *it; max = std::isnan(max) ? v : std::max(max, v);
}
} }
ESP_LOGVV(TAG, "MaxFilter(%p)::new_value(%f) SENDING", this, max); ESP_LOGVV(TAG, "MaxFilter(%p)::new_value(%f) SENDING %f", this, value, max);
return max; return max;
} }
return {}; return {};
@ -167,33 +179,30 @@ SlidingWindowMovingAverageFilter::SlidingWindowMovingAverageFilter(size_t window
void SlidingWindowMovingAverageFilter::set_send_every(size_t send_every) { this->send_every_ = send_every; } void SlidingWindowMovingAverageFilter::set_send_every(size_t send_every) { this->send_every_ = send_every; }
void SlidingWindowMovingAverageFilter::set_window_size(size_t window_size) { this->window_size_ = window_size; } void SlidingWindowMovingAverageFilter::set_window_size(size_t window_size) { this->window_size_ = window_size; }
optional<float> SlidingWindowMovingAverageFilter::new_value(float value) { optional<float> SlidingWindowMovingAverageFilter::new_value(float value) {
if (!std::isnan(value)) { while (this->queue_.size() >= this->window_size_) {
if (this->queue_.size() == this->window_size_) {
this->sum_ -= this->queue_[0];
this->queue_.pop_front(); this->queue_.pop_front();
} }
this->queue_.push_back(value); this->queue_.push_back(value);
this->sum_ += value; ESP_LOGVV(TAG, "SlidingWindowMovingAverageFilter(%p)::new_value(%f)", this, value);
}
float average;
if (this->queue_.empty()) {
average = 0.0f;
} else {
average = this->sum_ / this->queue_.size();
}
ESP_LOGVV(TAG, "SlidingWindowMovingAverageFilter(%p)::new_value(%f) -> %f", this, value, average);
if (++this->send_at_ % this->send_every_ == 0) { if (++this->send_at_ >= this->send_every_) {
if (this->send_at_ >= 10000) {
// Recalculate to prevent floating point error accumulating
this->sum_ = 0;
for (auto v : this->queue_)
this->sum_ += v;
average = this->sum_ / this->queue_.size();
this->send_at_ = 0; this->send_at_ = 0;
float sum = 0;
size_t valid_count = 0;
for (auto v : this->queue_) {
if (!std::isnan(v)) {
sum += v;
valid_count++;
}
} }
ESP_LOGVV(TAG, "SlidingWindowMovingAverageFilter(%p)::new_value(%f) SENDING", this, value); float average = NAN;
if (valid_count) {
average = sum / valid_count;
}
ESP_LOGVV(TAG, "SlidingWindowMovingAverageFilter(%p)::new_value(%f) SENDING %f", this, value, average);
return average; return average;
} }
return {}; return {};
@ -206,17 +215,17 @@ optional<float> ExponentialMovingAverageFilter::new_value(float value) {
if (!std::isnan(value)) { if (!std::isnan(value)) {
if (this->first_value_) { if (this->first_value_) {
this->accumulator_ = value; this->accumulator_ = value;
this->first_value_ = false;
} else { } else {
this->accumulator_ = (this->alpha_ * value) + (1.0f - this->alpha_) * this->accumulator_; this->accumulator_ = (this->alpha_ * value) + (1.0f - this->alpha_) * this->accumulator_;
} }
this->first_value_ = false;
} }
float average = this->accumulator_; const float average = std::isnan(value) ? value : this->accumulator_;
ESP_LOGVV(TAG, "ExponentialMovingAverageFilter(%p)::new_value(%f) -> %f", this, value, average); ESP_LOGVV(TAG, "ExponentialMovingAverageFilter(%p)::new_value(%f) -> %f", this, value, average);
if (++this->send_at_ >= this->send_every_) { if (++this->send_at_ >= this->send_every_) {
ESP_LOGVV(TAG, "ExponentialMovingAverageFilter(%p)::new_value(%f) SENDING", this, value); ESP_LOGVV(TAG, "ExponentialMovingAverageFilter(%p)::new_value(%f) SENDING %f", this, value, average);
this->send_at_ = 0; this->send_at_ = 0;
return average; return average;
} }
@ -308,8 +317,13 @@ optional<float> ThrottleFilter::new_value(float value) {
// DeltaFilter // DeltaFilter
DeltaFilter::DeltaFilter(float min_delta) : min_delta_(min_delta), last_value_(NAN) {} DeltaFilter::DeltaFilter(float min_delta) : min_delta_(min_delta), last_value_(NAN) {}
optional<float> DeltaFilter::new_value(float value) { optional<float> DeltaFilter::new_value(float value) {
if (std::isnan(value)) if (std::isnan(value)) {
if (std::isnan(this->last_value_)) {
return {}; return {};
} else {
return this->last_value_ = value;
}
}
if (std::isnan(this->last_value_)) { if (std::isnan(this->last_value_)) {
return this->last_value_ = value; return this->last_value_ = value;
} }

View file

@ -180,7 +180,6 @@ class SlidingWindowMovingAverageFilter : public Filter {
void set_window_size(size_t window_size); void set_window_size(size_t window_size);
protected: protected:
float sum_{0.0};
std::deque<float> queue_; std::deque<float> queue_;
size_t send_every_; size_t send_every_;
size_t send_at_; size_t send_at_;
@ -203,7 +202,7 @@ class ExponentialMovingAverageFilter : public Filter {
protected: protected:
bool first_value_{true}; bool first_value_{true};
float accumulator_{0.0f}; float accumulator_{NAN};
size_t send_every_; size_t send_every_;
size_t send_at_; size_t send_at_;
float alpha_; float alpha_;