BluetoothDispatcher was added & reformatting

Filters/kalman.hpp

@@ -0,0 +1,46 @@
+// упрощённый Калман для одномерного случая
+
+#ifndef _GKalman_h
+#define _GKalman_h
+
+class GKalman {
+public:
+    // разброс измерения, разброс оценки, скорость изменения значений
+    GKalman(float mea_e, float est_e, float q) {
+        setParameters(mea_e, est_e, q);
+    }
+    
+    // разброс измерения, скорость изменения значений (разброс измерения принимается равным разбросу оценки)
+    GKalman(float mea_e, float q) {
+        setParameters(mea_e, mea_e, q);
+    }
+    
+    // разброс измерения, разброс оценки, скорость изменения значений
+    void setParameters(float mea_e, float est_e, float q) {
+        _err_measure = mea_e;
+        _err_estimate = est_e;
+        _q = q;
+    }
+    
+    // разброс измерения, скорость изменения значений (разброс измерения принимается равным разбросу оценки)
+    void setParameters(float mea_e, float q) {
+        setParameters(mea_e, mea_e, q);
+    }
+    
+    // возвращает фильтрованное значение
+    float filtered(float value) {		
+        float _kalman_gain, _current_estimate;
+        _kalman_gain = _err_estimate / (_err_estimate + _err_measure);
+        _current_estimate = _last_estimate + _kalman_gain * (value - _last_estimate);
+        _err_estimate =  (1.0 - _kalman_gain)*_err_estimate + fabs(_last_estimate-_current_estimate)*_q;
+        _last_estimate=_current_estimate;
+        return _current_estimate;
+    }
+
+private:
+    float _err_measure = 0.0;
+    float _err_estimate = 0.0;
+    float _q = 0.0;
+    float _last_estimate = 0.0;
+};
+#endif