Project structure redefined

src/Filters/Kalman2DFilter.hpp

@@ -0,0 +1,43 @@
+#include <BasicLinearAlgebra.h>
+
+class Kalman2DFilter {
+  public:
+    Kalman2DFilter(float dt = 4.f, float accelUncertainty = 10.f, float barometerUncertainty = 100.f) {
+        dt /= 1000.f;
+        F = { 1, dt, 0, 1 };
+        G = { 0.5f * dt * dt, dt };
+        H = { 1, 0 };
+        I = { 1, 0, 0, 1 };
+        Q = G * ~G * accelUncertainty * accelUncertainty;
+        R = { barometerUncertainty * barometerUncertainty };
+        P = { 0, 0, 0, 0 };
+        S = { 0, 0 };
+    }
+
+    void filter(const float &AccZInertial, const float &AltitudeBarometer, float &AltitudeKalman, float &VelocityVerticalKalman) {
+        Acc = { AccZInertial };
+        S = F * S + G * Acc;
+        P = F * P * ~F + Q;
+        L = H * P * ~H + R;
+        K = P * ~H * Inverse(L);
+        M = { AltitudeBarometer };
+        S = S + K * (M - H * S);
+        AltitudeKalman = S(0, 0);
+        VelocityVerticalKalman = S(1, 0);
+        P = (I - K * H) * P;
+    }
+
+  private:
+    BLA::Matrix<2, 2> F;
+    BLA::Matrix<2, 1> G;
+    BLA::Matrix<2, 2> P;
+    BLA::Matrix<2, 2> Q;
+    BLA::Matrix<2, 1> S;
+    BLA::Matrix<1, 2> H;
+    BLA::Matrix<2, 2> I;
+    BLA::Matrix<1, 1> Acc;
+    BLA::Matrix<2, 1> K;
+    BLA::Matrix<1, 1> R;
+    BLA::Matrix<1, 1> L;
+    BLA::Matrix<1, 1> M;
+};

src/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

src/Filters/median3.hpp

@@ -0,0 +1,19 @@
+// быстрый медианный фильтр 3-го порядка
+
+#ifndef _GMedian3_h
+#define _GMedian3_h
+
+template < typename TYPE >
+class GMedian3 {
+public:
+    TYPE filtered(TYPE value) {	// возвращает фильтрованное значение
+        buf[_counter] = value;
+        if (++_counter > 2) _counter = 0;
+        return (max(buf[0], buf[1]) == max(buf[1], buf[2])) ? max(buf[0], buf[2]) : max(buf[1], min(buf[0], buf[2]));
+    }
+    
+private:
+    TYPE buf[3];
+    uint8_t _counter = 0;
+};
+#endif

src/Filters/runningAverage.hpp

@@ -0,0 +1,49 @@
+// экспоненциальное бегущее среднее
+
+#ifndef _GFilterRA_h
+#define _GFilterRA_h
+
+class GFilterRA {
+public:
+    GFilterRA(){}
+    
+    GFilterRA(float coef, uint16_t interval) {
+        _coef = coef;
+        _prd = interval;
+    }
+
+    GFilterRA(float coef) {
+        _coef = coef;
+    }
+
+    void setCoef(float coef) {
+        _coef = coef;
+    }
+    
+    void setPeriod(uint16_t interval) {
+        _prd = interval;
+    }
+
+    float filteredTime(float value) {
+        if (millis() - _tmr >= _prd) {
+            _tmr += _prd;
+            filtered(value);
+        }
+        return _fil;
+    }
+
+    float filtered(float value) {
+        return _fil += (value - _fil) * _coef;
+    }
+    
+    //
+    void setStep(uint16_t interval) {
+        _prd = interval;
+    }
+    
+private:
+    float _coef = 0.0, _fil = 0.0;
+    uint32_t _tmr = 0;
+    uint16_t _prd = 0;
+};
+#endif