Create new kalman_combinator component (#2965)

CODEOWNERS

@@ -83,6 +83,7 @@ esphome/components/inkplate6/* @jesserockz
 esphome/components/integration/* @OttoWinter
 esphome/components/interval/* @esphome/core
 esphome/components/json/* @OttoWinter
+esphome/components/kalman_combinator/* @Cat-Ion
 esphome/components/ledc/* @OttoWinter
 esphome/components/light/* @esphome/core
 esphome/components/logger/* @esphome/core

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))

esphome/core/entity_helpers.py

@@ -5,27 +5,49 @@ from esphome.const import CONF_ID
 
 def inherit_property_from(property_to_inherit, parent_id_property):
     """Validator that inherits a configuration property from another entity, for use with FINAL_VALIDATE_SCHEMA.
-
     If a property is already set, it will not be inherited.
-
     Keyword arguments:
-    property_to_inherit -- the name of the property to inherit, e.g. CONF_ICON
+    property_to_inherit -- the name or path of the property to inherit, e.g. CONF_ICON or [CONF_SENSOR, 0, CONF_ICON]
-    parent_id_property -- the name of the property that holds the ID of the parent, e.g. CONF_POWER_ID
+                           (the parent must exist, otherwise nothing is done).
+    parent_id_property -- the name or path of the property that holds the ID of the parent, e.g. CONF_POWER_ID or
+                          [CONF_SENSOR, 1, CONF_POWER_ID].
     """
 
+    def _walk_config(config, path):
+        walk = [path] if not isinstance(path, list) else path
+        for item_or_index in walk:
+            config = config[item_or_index]
+        return config
+
     def inherit_property(config):
-        if property_to_inherit not in config:
+        # Split the property into its path and name
+        if not isinstance(property_to_inherit, list):
+            property_path, property = [], property_to_inherit
+        else:
+            property_path, property = property_to_inherit[:-1], property_to_inherit[-1]
+
+        # Check if the property to inherit is accessible
+        try:
+            config_part = _walk_config(config, property_path)
+        except KeyError:
+            return config
+
+        # Only inherit the property if it does not exist yet
+        if property not in config_part:
             fconf = fv.full_config.get()
 
             # Get config for the parent entity
-            path = fconf.get_path_for_id(config[parent_id_property])[:-1]
+            parent_id = _walk_config(config, parent_id_property)
-            parent_config = fconf.get_config_for_path(path)
+            parent_path = fconf.get_path_for_id(parent_id)[:-1]
+            parent_config = fconf.get_config_for_path(parent_path)
 
             # If parent sensor has the property set, inherit it
-            if property_to_inherit in parent_config:
+            if property in parent_config:
                 path = fconf.get_path_for_id(config[CONF_ID])[:-1]
-                this_config = fconf.get_config_for_path(path)
+                this_config = _walk_config(
-                this_config[property_to_inherit] = parent_config[property_to_inherit]
+                    fconf.get_config_for_path(path), property_path
+                )
+                this_config[property] = parent_config[property]
 
         return config
 

tests/test1.yaml

@@ -657,6 +657,15 @@ sensor:
         name: 'INA3221 Channel 1 Shunt Voltage'
     update_interval: 15s
     i2c_id: i2c_bus
+  - platform: kalman_combinator
+    name: "Kalman-filtered temperature"
+    process_std_dev: 0.00139
+    sources:
+      - source: scd30_temperature
+        error: !lambda |-
+          return 0.4 + std::abs(x - 25) * 0.023;
+      - source: scd4x_temperature
+        error: 1.5
   - platform: htu21d
     temperature:
       name: 'Living Room Temperature 6'
@@ -820,6 +829,7 @@ sensor:
     co2:
       name: 'Living Room CO2 9'
     temperature:
+      id: scd30_temperature
       name: 'Living Room Temperature 9'
     humidity:
       name: 'Living Room Humidity 9'
@@ -834,6 +844,7 @@ sensor:
     co2:
       name: "SCD4X CO2"
     temperature:
+      id: scd4x_temperature
       name: "SCD4X Temperature"
     humidity:
       name: "SCD4X Humidity"