Use kalman to predict missing measurements

code/FtmKalman.cpp

@@ -1,12 +1,22 @@
 #include "FtmKalman.h"
 
+#include <iostream>
 #include <eigen3/Eigen/Eigen>
 
-float Kalman::predict(const Timestamp timestamp, const float measurment)
+constexpr auto square(float x) { return x * x; };
+
+
+float Kalman::predictAndUpdate(const Timestamp timestamp, const float measurment)
 {
-    constexpr auto square = [](float x) { return x * x; };
     const auto I = Eigen::Matrix2f::Identity();
 
+    {
+    // hack
+        processNoiseDistance = 1.2f; //1.2f;
+        processNoiseVelocity = 1.5f; //1.5;
+        R = 300;// 200;
+    }
+
     Eigen::Map<Eigen::Matrix<float, 2, 1>> x(this->x);
     Eigen::Map<Eigen::Matrix<float, 2, 2>> P(this->P);
 
@@ -48,4 +58,37 @@ float Kalman::predict(const Timestamp timestamp, const float measurment)
     P = (I - (K*H))*P; // aktualisieren der Kovarianz
 
     return x(0);
-}
+}
+
+KalmanPrediction Kalman::predict(const Timestamp timestamp)
+{
+    if (isnan(lastTimestamp))
+    {
+        // Kalman has no data => nothing to predict
+        return KalmanPrediction{ NAN, NAN };
+    }
+    else 
+    {
+        Eigen::Map<Eigen::Matrix<float, 2, 1>> x(this->x);
+        Eigen::Map<Eigen::Matrix<float, 2, 2>> P(this->P);
+
+        const float dt = timestamp.sec() - lastTimestamp;
+        lastTimestamp = timestamp.sec();
+
+        Eigen::Matrix2f A;   // Transition Matrix
+        A << 1, dt,
+             0, 1;
+
+
+        Eigen::Matrix2f Q;  // Process Noise Covariance
+        Q << square(processNoiseDistance), 0,
+             0, square(processNoiseVelocity);
+
+
+        x = A * x;
+        P = A * P*A.transpose() + Q;
+
+
+        return KalmanPrediction{ x(0), x(1) };
+    }
+}