From 5b20b67e5f496dc75cd67b2c4e19d79aad3d58f2 Mon Sep 17 00:00:00 2001
From: Markus Bullmann <markus.bullmann@fhws.de>
Date: Tue, 8 Oct 2019 13:59:58 +0200
Subject: [PATCH] Moved kalman computation into cpp to improve compile speed

---
 code/CMakeLists.txt |  1 +
 code/FtmKalman.cpp  | 51 ++++++++++++++++++++++++++++++++++++++++++
 code/FtmKalman.h    | 54 ++++-----------------------------------------
 3 files changed, 56 insertions(+), 50 deletions(-)
 create mode 100644 code/FtmKalman.cpp

diff --git a/code/CMakeLists.txt b/code/CMakeLists.txt
index b3d15b9..3afaa25 100644
--- a/code/CMakeLists.txt
+++ b/code/CMakeLists.txt
@@ -50,6 +50,7 @@ FILE(GLOB SOURCES
     mainTrilat.cpp
     mainProb.cpp
     Eval.cpp
+    FtmKalman.cpp
 )
 
 
diff --git a/code/FtmKalman.cpp b/code/FtmKalman.cpp
new file mode 100644
index 0000000..2e077cd
--- /dev/null
+++ b/code/FtmKalman.cpp
@@ -0,0 +1,51 @@
+#include "FtmKalman.h"
+
+#include <eigen3/Eigen/Eigen>
+
+float Kalman::predict(const Timestamp timestamp, const float measurment)
+{
+    constexpr auto square = [](float x) { return x * x; };
+    const auto I = Eigen::Matrix2f::Identity();
+
+    Eigen::Map<Eigen::Matrix<float, 2, 1>> x(this->x);
+    Eigen::Map<Eigen::Matrix<float, 2, 2>> P(this->P);
+
+    // 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);
+}
\ No newline at end of file
diff --git a/code/FtmKalman.h b/code/FtmKalman.h
index 384cd38..810af5a 100644
--- a/code/FtmKalman.h
+++ b/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);
 };