Use kalman to predict missing measurements

code/FtmKalman.h

@@ -2,14 +2,22 @@
 
 #include <Indoor/data/Timestamp.h>
 
+struct KalmanPrediction
+{
+    float distance;
+    float speed;
+
+    float P[4];   // Covariance
+};
+
 struct Kalman
 {  
     int nucID = 0; // debug only
    
-    float x[2];   // predicted state
-    float P[4];   // Covariance
+    float x[2] = {NAN};   // predicted state  [m, m/s]
+    float P[4] = {NAN};   // Covariance
     
-    float R = 30;  //  measurement noise covariance   
+    float R = 30;  //  measurement noise covariance 
     float processNoiseDistance; // stdDev
     float processNoiseVelocity; // stdDev
 
@@ -25,7 +33,8 @@ struct Kalman
         : nucID(nucID), R(measStdDev*measStdDev), processNoiseDistance(processNoiseDistance), processNoiseVelocity(processNoiseVelocity)
     {}
 
-    float predict(const Timestamp timestamp, const float measurment);
+    float predictAndUpdate(const Timestamp timestamp, const float measurment);
+    KalmanPrediction predict(const Timestamp timestamp);
 };