diff --git a/esphome/components/sensor/__init__.py b/esphome/components/sensor/__init__.py index 43f0cefd56..c006dfad57 100644 --- a/esphome/components/sensor/__init__.py +++ b/esphome/components/sensor/__init__.py @@ -73,6 +73,7 @@ HeartbeatFilter = sensor_ns.class_('HeartbeatFilter', Filter, cg.Component) DeltaFilter = sensor_ns.class_('DeltaFilter', Filter) OrFilter = sensor_ns.class_('OrFilter', Filter) CalibrateLinearFilter = sensor_ns.class_('CalibrateLinearFilter', Filter) +CalibratePolynomialFilter = sensor_ns.class_('CalibratePolynomialFilter', Filter) SensorInRangeCondition = sensor_ns.class_('SensorInRangeCondition', Filter) unit_of_measurement = cv.string_strict @@ -194,6 +195,32 @@ def calibrate_linear_filter_to_code(config, filter_id): yield cg.new_Pvariable(filter_id, k, b) +CONF_DATAPOINTS = 'datapoints' +CONF_DEGREE = 'degree' + + +def validate_calibrate_polynomial(config): + if config[CONF_DEGREE] >= len(config[CONF_DATAPOINTS]): + raise cv.Invalid("Degree is too high! Maximum possible degree with given datapoints is " + "{}".format(len(config[CONF_DATAPOINTS]) - 1), [CONF_DEGREE]) + return config + + +@FILTER_REGISTRY.register('calibrate_polynomial', CalibratePolynomialFilter, cv.All(cv.Schema({ + cv.Required(CONF_DATAPOINTS): cv.All(cv.ensure_list(validate_datapoint), cv.Length(min=1)), + cv.Required(CONF_DEGREE): cv.positive_int, +}), validate_calibrate_polynomial)) +def calibrate_polynomial_filter_to_code(config, filter_id): + x = [conf[CONF_FROM] for conf in config[CONF_DATAPOINTS]] + y = [conf[CONF_TO] for conf in config[CONF_DATAPOINTS]] + degree = config[CONF_DEGREE] + a = [[1] + [x_**(i+1) for i in range(degree)] for x_ in x] + # Column vector + b = [[v] for v in y] + res = [v[0] for v in _lstsq(a, b)] + yield cg.new_Pvariable(filter_id, res) + + @coroutine def build_filters(config): yield cg.build_registry_list(FILTER_REGISTRY, config) @@ -303,6 +330,66 @@ def fit_linear(x, y): return k, b +def _mat_copy(m): + return [list(row) for row in m] + + +def _mat_transpose(m): + return _mat_copy(zip(*m)) + + +def _mat_identity(n): + return [[int(i == j) for j in range(n)] for i in range(n)] + + +def _mat_dot(a, b): + b_t = _mat_transpose(b) + return [[sum(x*y for x, y in zip(row_a, col_b)) for col_b in b_t] for row_a in a] + + +def _mat_inverse(m): + n = len(m) + m = _mat_copy(m) + id = _mat_identity(n) + + for diag in range(n): + # If diag element is 0, swap rows + if m[diag][diag] == 0: + for i in range(diag+1, n): + if m[i][diag] != 0: + break + else: + raise ValueError("Singular matrix, inverse cannot be calculated!") + + # Swap rows + m[diag], m[i] = m[i], m[diag] + id[diag], id[i] = id[i], id[diag] + + # Scale row to 1 in diagonal + scaler = 1.0 / m[diag][diag] + for j in range(n): + m[diag][j] *= scaler + id[diag][j] *= scaler + + # Subtract diag row + for i in range(n): + if i == diag: + continue + scaler = m[i][diag] + for j in range(n): + m[i][j] -= scaler * m[diag][j] + id[i][j] -= scaler * id[diag][j] + + return id + + +def _lstsq(a, b): + # min_x ||b - ax||^2_2 => x = (a^T a)^{-1} a^T b + a_t = _mat_transpose(a) + x = _mat_inverse(_mat_dot(a_t, a)) + return _mat_dot(_mat_dot(x, a_t), b) + + @coroutine_with_priority(40.0) def to_code(config): cg.add_define('USE_SENSOR') diff --git a/esphome/components/sensor/filter.cpp b/esphome/components/sensor/filter.cpp index 306607dfda..79323bd8ab 100644 --- a/esphome/components/sensor/filter.cpp +++ b/esphome/components/sensor/filter.cpp @@ -228,5 +228,15 @@ float HeartbeatFilter::get_setup_priority() const { return setup_priority::HARDW optional CalibrateLinearFilter::new_value(float value) { return value * this->slope_ + this->bias_; } CalibrateLinearFilter::CalibrateLinearFilter(float slope, float bias) : slope_(slope), bias_(bias) {} +optional CalibratePolynomialFilter::new_value(float value) { + float res = 0.0f; + float x = 1.0f; + for (float coefficient : this->coefficients_) { + res += x * coefficient; + x *= value; + } + return res; +} + } // namespace sensor } // namespace esphome diff --git a/esphome/components/sensor/filter.h b/esphome/components/sensor/filter.h index 6bd22be230..17a583e40e 100644 --- a/esphome/components/sensor/filter.h +++ b/esphome/components/sensor/filter.h @@ -243,5 +243,14 @@ class CalibrateLinearFilter : public Filter { float bias_; }; +class CalibratePolynomialFilter : public Filter { + public: + CalibratePolynomialFilter(const std::vector &coefficients) : coefficients_(coefficients) {} + optional new_value(float value) override; + + protected: + std::vector coefficients_; +}; + } // namespace sensor } // namespace esphome diff --git a/tests/test3.yaml b/tests/test3.yaml index 05fec27c80..797153bb12 100644 --- a/tests/test3.yaml +++ b/tests/test3.yaml @@ -133,6 +133,14 @@ sensor: - calibrate_linear: - 0 -> 0 - 100 -> 100 + - calibrate_polynomial: + degree: 3 + datapoints: + - 0 -> 0 + - 100 -> 200 + - 400 -> 500 + - -50 -> -1000 + - -100 -> -10000 - platform: resistance sensor: my_sensor configuration: DOWNSTREAM