quantile filter support (#2900)

Co-authored-by: Oxan van Leeuwen <oxan@oxanvanleeuwen.nl>
Co-authored-by: pvranik <petr.vranik@mgm-tp.com>

esphome/components/sensor/__init__.py

@@ -19,6 +19,7 @@ from esphome.const import (
     CONF_ON_RAW_VALUE,
     CONF_ON_VALUE,
     CONF_ON_VALUE_RANGE,
+    CONF_QUANTILE,
     CONF_SEND_EVERY,
     CONF_SEND_FIRST_AT,
     CONF_STATE_CLASS,
@@ -151,6 +152,7 @@ SensorPublishAction = sensor_ns.class_("SensorPublishAction", automation.Action)
 
 # Filters
 Filter = sensor_ns.class_("Filter")
+QuantileFilter = sensor_ns.class_("QuantileFilter", Filter)
 MedianFilter = sensor_ns.class_("MedianFilter", Filter)
 MinFilter = sensor_ns.class_("MinFilter", Filter)
 MaxFilter = sensor_ns.class_("MaxFilter", Filter)
@@ -285,6 +287,30 @@ async def filter_out_filter_to_code(config, filter_id):
     return cg.new_Pvariable(filter_id, config)
 
 
+QUANTILE_SCHEMA = cv.All(
+    cv.Schema(
+        {
+            cv.Optional(CONF_WINDOW_SIZE, default=5): cv.positive_not_null_int,
+            cv.Optional(CONF_SEND_EVERY, default=5): cv.positive_not_null_int,
+            cv.Optional(CONF_SEND_FIRST_AT, default=1): cv.positive_not_null_int,
+            cv.Optional(CONF_QUANTILE, default=0.9): cv.zero_to_one_float,
+        }
+    ),
+    validate_send_first_at,
+)
+
+
+@FILTER_REGISTRY.register("quantile", QuantileFilter, QUANTILE_SCHEMA)
+async def quantile_filter_to_code(config, filter_id):
+    return cg.new_Pvariable(
+        filter_id,
+        config[CONF_WINDOW_SIZE],
+        config[CONF_SEND_EVERY],
+        config[CONF_SEND_FIRST_AT],
+        config[CONF_QUANTILE],
+    )
+
+
 MEDIAN_SCHEMA = cv.All(
     cv.Schema(
         {

esphome/components/sensor/filter.cpp

@@ -1,7 +1,8 @@
 #include "filter.h"
-#include "sensor.h"
-#include "esphome/core/log.h"
 #include "esphome/core/hal.h"
+#include "esphome/core/log.h"
+#include "sensor.h"
+#include <cmath>
 
 namespace esphome {
 namespace sensor {
@@ -66,6 +67,41 @@ optional<float> MedianFilter::new_value(float value) {
   return {};
 }
 
+// QuantileFilter
+QuantileFilter::QuantileFilter(size_t window_size, size_t send_every, size_t send_first_at, float quantile)
+    : send_every_(send_every), send_at_(send_every - send_first_at), window_size_(window_size), quantile_(quantile) {}
+void QuantileFilter::set_send_every(size_t send_every) { this->send_every_ = send_every; }
+void QuantileFilter::set_window_size(size_t window_size) { this->window_size_ = window_size; }
+void QuantileFilter::set_quantile(float quantile) { this->quantile_ = quantile; }
+optional<float> QuantileFilter::new_value(float value) {
+  if (!std::isnan(value)) {
+    while (this->queue_.size() >= this->window_size_) {
+      this->queue_.pop_front();
+    }
+    this->queue_.push_back(value);
+    ESP_LOGVV(TAG, "QuantileFilter(%p)::new_value(%f), quantile:%f", this, value, this->quantile_);
+  }
+
+  if (++this->send_at_ >= this->send_every_) {
+    this->send_at_ = 0;
+
+    float result = 0.0f;
+    if (!this->queue_.empty()) {
+      std::deque<float> quantile_queue = this->queue_;
+      sort(quantile_queue.begin(), quantile_queue.end());
+
+      size_t queue_size = quantile_queue.size();
+      size_t position = ceilf(queue_size * this->quantile_) - 1;
+      ESP_LOGVV(TAG, "QuantileFilter(%p)::position: %d/%d", this, position, queue_size);
+      result = quantile_queue[position];
+    }
+
+    ESP_LOGVV(TAG, "QuantileFilter(%p)::new_value(%f) SENDING", this, result);
+    return result;
+  }
+  return {};
+}
+
 // MinFilter
 MinFilter::MinFilter(size_t window_size, size_t send_every, size_t send_first_at)
     : send_every_(send_every), send_at_(send_every - send_first_at), window_size_(window_size) {}

esphome/components/sensor/filter.h

@@ -42,6 +42,37 @@ class Filter {
   Sensor *parent_{nullptr};
 };
 
+/** Simple quantile filter.
+ *
+ * Takes the quantile of the last <send_every> values and pushes it out every <send_every>.
+ */
+class QuantileFilter : public Filter {
+ public:
+  /** Construct a QuantileFilter.
+   *
+   * @param window_size The number of values that should be used in quantile calculation.
+   * @param send_every After how many sensor values should a new one be pushed out.
+   * @param send_first_at After how many values to forward the very first value. Defaults to the first value
+   *   on startup being published on the first *raw* value, so with no filter applied. Must be less than or equal to
+   *   send_every.
+   * @param quantile float 0..1 to pick the requested quantile. Defaults to 0.9.
+   */
+  explicit QuantileFilter(size_t window_size, size_t send_every, size_t send_first_at, float quantile);
+
+  optional<float> new_value(float value) override;
+
+  void set_send_every(size_t send_every);
+  void set_window_size(size_t window_size);
+  void set_quantile(float quantile);
+
+ protected:
+  std::deque<float> queue_;
+  size_t send_every_;
+  size_t send_at_;
+  size_t window_size_;
+  float quantile_;
+};
+
 /** Simple median filter.
  *
  * Takes the median of the last <send_every> values and pushes it out every <send_every>.

esphome/const.py

@@ -518,6 +518,7 @@ CONF_PULLDOWN = "pulldown"
 CONF_PULLUP = "pullup"
 CONF_PULSE_LENGTH = "pulse_length"
 CONF_QOS = "qos"
+CONF_QUANTILE = "quantile"
 CONF_RADON = "radon"
 CONF_RADON_LONG_TERM = "radon_long_term"
 CONF_RANDOM = "random"

tests/test3.yaml

@@ -358,6 +358,11 @@ sensor:
       - filter_out: NAN
       - sliding_window_moving_average:
       - exponential_moving_average:
+      - quantile:
+          window_size: 5
+          send_every: 5
+          send_first_at: 3
+          quantile: .8
       - lambda: 'return 0;'
       - delta: 100
       - throttle: 100ms