Create new kalman_combinator component (#2965)

esphome/components/kalman_combinator/__init__.py

@@ -0,0 +1 @@
+CODEOWNERS = ["@Cat-Ion"]

esphome/components/kalman_combinator/kalman_combinator.cpp

@@ -0,0 +1,82 @@
+#include "kalman_combinator.h"
+#include "esphome/core/hal.h"
+#include <cmath>
+#include <functional>
+
+namespace esphome {
+namespace kalman_combinator {
+
+void KalmanCombinatorComponent::dump_config() {
+  ESP_LOGCONFIG("kalman_combinator", "Kalman Combinator:");
+  ESP_LOGCONFIG("kalman_combinator", "  Update variance: %f per ms", this->update_variance_value_);
+  ESP_LOGCONFIG("kalman_combinator", "  Sensors:");
+  for (const auto &sensor : this->sensors_) {
+    auto &entity = *sensor.first;
+    ESP_LOGCONFIG("kalman_combinator", "    - %s", entity.get_name().c_str());
+  }
+}
+
+void KalmanCombinatorComponent::setup() {
+  for (const auto &sensor : this->sensors_) {
+    const auto stddev = sensor.second;
+    sensor.first->add_on_state_callback([this, stddev](float x) -> void { this->correct_(x, stddev(x)); });
+  }
+}
+
+void KalmanCombinatorComponent::add_source(Sensor *sensor, std::function<float(float)> const &stddev) {
+  this->sensors_.emplace_back(sensor, stddev);
+}
+
+void KalmanCombinatorComponent::add_source(Sensor *sensor, float stddev) {
+  this->add_source(sensor, std::function<float(float)>{[stddev](float) -> float { return stddev; }});
+}
+
+void KalmanCombinatorComponent::update_variance_() {
+  uint32_t now = millis();
+
+  // Variance increases by update_variance_ each millisecond
+  auto dt = now - this->last_update_;
+  auto dv = this->update_variance_value_ * dt;
+  this->variance_ += dv;
+  this->last_update_ = now;
+}
+
+void KalmanCombinatorComponent::correct_(float value, float stddev) {
+  if (std::isnan(value) || std::isinf(stddev)) {
+    return;
+  }
+
+  if (std::isnan(this->state_) || std::isinf(this->variance_)) {
+    this->state_ = value;
+    this->variance_ = stddev * stddev;
+    if (this->std_dev_sensor_ != nullptr) {
+      this->std_dev_sensor_->publish_state(stddev);
+    }
+    return;
+  }
+
+  this->update_variance_();
+
+  // Combine two gaussian distributions mu1+-var1, mu2+-var2 to a new one around mu
+  // Use the value with the smaller variance as mu1 to prevent precision errors
+  const bool this_first = this->variance_ < (stddev * stddev);
+  const float mu1 = this_first ? this->state_ : value;
+  const float mu2 = this_first ? value : this->state_;
+
+  const float var1 = this_first ? this->variance_ : stddev * stddev;
+  const float var2 = this_first ? stddev * stddev : this->variance_;
+
+  const float mu = mu1 + var1 * (mu2 - mu1) / (var1 + var2);
+  const float var = var1 - (var1 * var1) / (var1 + var2);
+
+  // Update and publish state
+  this->state_ = mu;
+  this->variance_ = var;
+
+  this->publish_state(mu);
+  if (this->std_dev_sensor_ != nullptr) {
+    this->std_dev_sensor_->publish_state(std::sqrt(var));
+  }
+}
+}  // namespace kalman_combinator
+}  // namespace esphome

esphome/components/kalman_combinator/kalman_combinator.h

@@ -0,0 +1,46 @@
+#pragma once
+#include "esphome/core/component.h"
+#include "esphome/components/sensor/sensor.h"
+#include <cmath>
+#include <vector>
+
+namespace esphome {
+namespace kalman_combinator {
+
+class KalmanCombinatorComponent : public Component, public sensor::Sensor {
+ public:
+  KalmanCombinatorComponent() = default;
+
+  float get_setup_priority() const override { return esphome::setup_priority::DATA; }
+
+  void dump_config() override;
+  void setup() override;
+
+  void add_source(Sensor *sensor, std::function<float(float)> const &stddev);
+  void add_source(Sensor *sensor, float stddev);
+  void set_process_std_dev(float process_std_dev) {
+    this->update_variance_value_ = process_std_dev * process_std_dev * 0.001f;
+  }
+  void set_std_dev_sensor(Sensor *sensor) { this->std_dev_sensor_ = sensor; }
+
+ private:
+  void update_variance_();
+  void correct_(float value, float stddev);
+
+  // Source sensors and their error functions
+  std::vector<std::pair<Sensor *, std::function<float(float)>>> sensors_;
+
+  // Optional sensor for publishing the current error
+  sensor::Sensor *std_dev_sensor_{nullptr};
+
+  // Tick of the last update
+  uint32_t last_update_{0};
+  // Change of the variance, per ms
+  float update_variance_value_{0.f};
+
+  // Best guess for the state and its variance
+  float state_{NAN};
+  float variance_{INFINITY};
+};
+}  // namespace kalman_combinator
+}  // namespace esphome

esphome/components/kalman_combinator/sensor.py

@@ -0,0 +1,87 @@
+import esphome.codegen as cg
+import esphome.config_validation as cv
+from esphome.components import sensor
+from esphome.const import (
+    CONF_ID,
+    CONF_SOURCE,
+    CONF_ACCURACY_DECIMALS,
+    CONF_DEVICE_CLASS,
+    CONF_ENTITY_CATEGORY,
+    CONF_ICON,
+    CONF_UNIT_OF_MEASUREMENT,
+)
+from esphome.core.entity_helpers import inherit_property_from
+
+kalman_combinator_ns = cg.esphome_ns.namespace("kalman_combinator")
+KalmanCombinatorComponent = kalman_combinator_ns.class_(
+    "KalmanCombinatorComponent", cg.Component, sensor.Sensor
+)
+
+CONF_ERROR = "error"
+CONF_SOURCES = "sources"
+CONF_PROCESS_STD_DEV = "process_std_dev"
+CONF_STD_DEV = "std_dev"
+
+
+CONFIG_SCHEMA = sensor.SENSOR_SCHEMA.extend(cv.COMPONENT_SCHEMA).extend(
+    {
+        cv.GenerateID(): cv.declare_id(KalmanCombinatorComponent),
+        cv.Required(CONF_PROCESS_STD_DEV): cv.positive_float,
+        cv.Required(CONF_SOURCES): cv.ensure_list(
+            cv.Schema(
+                {
+                    cv.Required(CONF_SOURCE): cv.use_id(sensor.Sensor),
+                    cv.Required(CONF_ERROR): cv.templatable(cv.positive_float),
+                }
+            ),
+        ),
+        cv.Optional(CONF_STD_DEV): sensor.SENSOR_SCHEMA,
+    }
+)
+
+# Inherit some sensor values from the first source, for both the state and the error value
+properties_to_inherit = [
+    CONF_ACCURACY_DECIMALS,
+    CONF_DEVICE_CLASS,
+    CONF_ENTITY_CATEGORY,
+    CONF_ICON,
+    CONF_UNIT_OF_MEASUREMENT,
+    # CONF_STATE_CLASS could also be inherited, but might lead to unexpected behaviour with "total_increasing"
+]
+inherit_schema_for_state = [
+    inherit_property_from(property, [CONF_SOURCES, 0, CONF_SOURCE])
+    for property in properties_to_inherit
+]
+inherit_schema_for_std_dev = [
+    inherit_property_from([CONF_STD_DEV, property], [CONF_SOURCES, 0, CONF_SOURCE])
+    for property in properties_to_inherit
+]
+
+FINAL_VALIDATE_SCHEMA = cv.All(
+    CONFIG_SCHEMA.extend(
+        {cv.Required(CONF_ID): cv.use_id(KalmanCombinatorComponent)},
+        extra=cv.ALLOW_EXTRA,
+    ),
+    *inherit_schema_for_state,
+    *inherit_schema_for_std_dev,
+)
+
+
+async def to_code(config):
+    var = cg.new_Pvariable(config[CONF_ID])
+    await cg.register_component(var, config)
+    await sensor.register_sensor(var, config)
+
+    cg.add(var.set_process_std_dev(config[CONF_PROCESS_STD_DEV]))
+    for source_conf in config[CONF_SOURCES]:
+        source = await cg.get_variable(source_conf[CONF_SOURCE])
+        error = await cg.templatable(
+            source_conf[CONF_ERROR],
+            [(float, "x")],
+            cg.float_,
+        )
+        cg.add(var.add_source(source, error))
+
+    if CONF_STD_DEV in config:
+        sens = await sensor.new_sensor(config[CONF_STD_DEV])
+        cg.add(var.set_std_dev_sensor(sens))