From b406c6403c055f089aaa3978e269e7d28a471dbc Mon Sep 17 00:00:00 2001 From: Valentin Ochs Date: Sun, 9 Jan 2022 23:44:36 +0100 Subject: [PATCH] Create new kalman_combinator component (#2965) --- CODEOWNERS | 1 + .../components/kalman_combinator/__init__.py | 1 + .../kalman_combinator/kalman_combinator.cpp | 82 +++++++++++++++++ .../kalman_combinator/kalman_combinator.h | 46 ++++++++++ .../components/kalman_combinator/sensor.py | 87 +++++++++++++++++++ esphome/core/entity_helpers.py | 42 ++++++--- tests/test1.yaml | 11 +++ 7 files changed, 260 insertions(+), 10 deletions(-) create mode 100644 esphome/components/kalman_combinator/__init__.py create mode 100644 esphome/components/kalman_combinator/kalman_combinator.cpp create mode 100644 esphome/components/kalman_combinator/kalman_combinator.h create mode 100644 esphome/components/kalman_combinator/sensor.py diff --git a/CODEOWNERS b/CODEOWNERS index 62e49055a2..deecd094dc 100644 --- a/CODEOWNERS +++ b/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 diff --git a/esphome/components/kalman_combinator/__init__.py b/esphome/components/kalman_combinator/__init__.py new file mode 100644 index 0000000000..3356e61bb2 --- /dev/null +++ b/esphome/components/kalman_combinator/__init__.py @@ -0,0 +1 @@ +CODEOWNERS = ["@Cat-Ion"] diff --git a/esphome/components/kalman_combinator/kalman_combinator.cpp b/esphome/components/kalman_combinator/kalman_combinator.cpp new file mode 100644 index 0000000000..d55f26126f --- /dev/null +++ b/esphome/components/kalman_combinator/kalman_combinator.cpp @@ -0,0 +1,82 @@ +#include "kalman_combinator.h" +#include "esphome/core/hal.h" +#include +#include + +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 const &stddev) { + this->sensors_.emplace_back(sensor, stddev); +} + +void KalmanCombinatorComponent::add_source(Sensor *sensor, float stddev) { + this->add_source(sensor, std::function{[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 diff --git a/esphome/components/kalman_combinator/kalman_combinator.h b/esphome/components/kalman_combinator/kalman_combinator.h new file mode 100644 index 0000000000..afbe3ece92 --- /dev/null +++ b/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 +#include + +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 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>> 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 diff --git a/esphome/components/kalman_combinator/sensor.py b/esphome/components/kalman_combinator/sensor.py new file mode 100644 index 0000000000..9223f883b2 --- /dev/null +++ b/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)) diff --git a/esphome/core/entity_helpers.py b/esphome/core/entity_helpers.py index b2dbe2116e..7f7d78aaa2 100644 --- a/esphome/core/entity_helpers.py +++ b/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 - parent_id_property -- the name of the property that holds the ID of the parent, e.g. CONF_POWER_ID + property_to_inherit -- the name or path of the property to inherit, e.g. CONF_ICON or [CONF_SENSOR, 0, CONF_ICON] + (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_config = fconf.get_config_for_path(path) + parent_id = _walk_config(config, parent_id_property) + 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[property_to_inherit] = parent_config[property_to_inherit] + this_config = _walk_config( + fconf.get_config_for_path(path), property_path + ) + this_config[property] = parent_config[property] return config diff --git a/tests/test1.yaml b/tests/test1.yaml index 7494146d1e..2836a97e4f 100644 --- a/tests/test1.yaml +++ b/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"