Moved kalman computation into cpp to improve compile speed

code/FtmKalman.h

@@ -1,17 +1,14 @@
 #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 x[2];   // predicted state
+    float P[4];   // Covariance
+    
     float R = 30;  //  measurement noise covariance   
     float processNoiseDistance; // stdDev
     float processNoiseVelocity; // stdDev
@@ -28,50 +25,7 @@ struct Kalman
         : nucID(nucID), R(measStdDev*measStdDev), processNoiseDistance(processNoiseDistance), processNoiseVelocity(processNoiseVelocity)
     {}
 
-    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 << square(processNoiseDistance), 0,
-            0, square(processNoiseVelocity);
-
-        // Prediction
-        x = A * x;                    // Prädizierter Zustand aus Bisherigem und System
-        P = A * P*A.transpose()+Q;    // Prä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);
-    }
+    float predict(const Timestamp timestamp, const float measurment);
 };