Add calibrate_polynomial sensor filter (#642)

* Add calibrate_polynomial sensor filter

* Fix

* Lint

* Format

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

esphome/components/sensor/filter.cpp

@@ -228,5 +228,15 @@ float HeartbeatFilter::get_setup_priority() const { return setup_priority::HARDW
 optional<float> CalibrateLinearFilter::new_value(float value) { return value * this->slope_ + this->bias_; }
 CalibrateLinearFilter::CalibrateLinearFilter(float slope, float bias) : slope_(slope), bias_(bias) {}
 
+optional<float> 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

esphome/components/sensor/filter.h

@@ -243,5 +243,14 @@ class CalibrateLinearFilter : public Filter {
   float bias_;
 };
 
+class CalibratePolynomialFilter : public Filter {
+ public:
+  CalibratePolynomialFilter(const std::vector<float> &coefficients) : coefficients_(coefficients) {}
+  optional<float> new_value(float value) override;
+
+ protected:
+  std::vector<float> coefficients_;
+};
+
 }  // namespace sensor
 }  // namespace esphome

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