Initial project version

code/FtmKalman.h

@@ -0,0 +1,76 @@
+#pragma once
+
+#include <eigen3/Eigen/Eigen>
+
+#include <Indoor/data/Timestamp.h>
+
+
+struct Kalman
+{  
+    int nucID = 0; // debug only
+   
+    Eigen::Matrix<float, 2, 1> x; // predicted state
+    Eigen::Matrix<float, 2, 2> P; // Covariance
+
+    float R = 30;  //  measurement noise covariance   
+
+    float lastTimestamp = NAN; // in sec
+
+    Kalman(): nucID(0) { }
+
+    Kalman(int nucID)
+        : nucID(nucID)
+    {}
+
+    Kalman(int nucID, float measStdDev)
+        : nucID(nucID), R(measStdDev*measStdDev)
+    {}
+
+    float predict(const Timestamp timestamp, const float measurment)
+    {
+        constexpr auto square = [](float x) { return x * x; };
+        const auto I = Eigen::Matrix2f::Identity();
+
+        // init kalman filter
+        if (isnan(lastTimestamp))
+        {
+            P << 10, 0,
+                  0, 10; // Initial Uncertainty   
+
+            x << measurment,
+                 0;
+        }
+
+        const float dt = isnan(lastTimestamp) ? 1 : timestamp.sec() - lastTimestamp;
+        lastTimestamp = timestamp.sec();
+
+        Eigen::Matrix<float, 1, 2> H; // Measurement function
+        H << 1, 0;
+
+        Eigen::Matrix2f A;   // Transition Matrix
+        A << 1, dt,
+             0, 1;
+
+        Eigen::Matrix2f Q;  // Process Noise Covariance
+        Q << 0, 0,
+             0, square(0.3);
+
+        // Prediction
+        x = A * x;                    // Pr<50>dizierter Zustand aus Bisherigem und System
+        P = A * P*A.transpose()+Q;    // Pr<50>dizieren der Kovarianz
+
+        // Correction
+        float Z = measurment;
+        auto y = Z - (H*x);                // Innovation aus Messwertdifferenz
+        auto S = (H*P*H.transpose()+R);    // Innovationskovarianz
+        auto K = P * H.transpose()* (1/S); //Filter-Matrix (Kalman-Gain)
+
+        x = x + (K*y);     // aktualisieren des Systemzustands
+        P = (I - (K*H))*P; // aktualisieren der Kovarianz
+
+        return x(0);
+    }
+};
+
+
+