Use kalman to predict missing measurements

code/Eval.cpp

@@ -1,5 +1,8 @@
 #include "Eval.h"
 
+#include <array>
+#include <vector>
+
 #include "Settings.h"
 
 #include <Indoor/math/distribution/Normal.h>
@@ -8,6 +11,8 @@ double ftmEval(SensorMode UseSensor, const Timestamp& currentTime, const Point3&
 {
     double result = 1.0;
 
+    std::array<bool, 4> hadMeas = {false};
+
     for (WiFiMeasurement wifi : measurements)
     {
         if (wifi.getNumSuccessfulMeasurements() < 3)
@@ -34,12 +39,12 @@ double ftmEval(SensorMode UseSensor, const Timestamp& currentTime, const Point3&
             if (ftmDist > 0)
             {
                 //double sigma = wifi.getFtmDistStd()*wifi.getFtmDistStd(); // 3.5; // TODO
-                double sigma = 5;
+                double sigma = 3;
 
-                if (ftmKalmanFilters != nullptr)
+                if (false && ftmKalmanFilters != nullptr)
                 {
                     Kalman& kalman = ftmKalmanFilters->at(mac);
-                    ftmDist = kalman.predict(currentTime, ftmDist);
+                    ftmDist = kalman.predictAndUpdate(currentTime, ftmDist);
                     //sigma = std::sqrt(kalman.P(0, 0));
 
                     Assert::isTrue(sigma > 0, "sigma");
@@ -54,6 +59,8 @@ double ftmEval(SensorMode UseSensor, const Timestamp& currentTime, const Point3&
 
                     result *= x;
                 }
+
+                hadMeas[nucIndex] = true;
             }
         }
         else
@@ -66,5 +73,49 @@ double ftmEval(SensorMode UseSensor, const Timestamp& currentTime, const Point3&
         }
     }
 
+    // Use kalman to predict missing measurments
+    //if (UseSensor == SensorMode::FTM && ftmKalmanFilters != nullptr)
+    //{
+    //    for (size_t i = 0; i < 4; i++)
+    //    {
+    //        if (!hadMeas[i])
+    //        {
+    //            double sigma = 5;
+
+    //            Kalman& kalman = ftmKalmanFilters->at(Settings::nucFromIndex(i));
+
+    //            if (!isnan(kalman.lastTimestamp))
+    //            {
+    //                KalmanPrediction prediction = kalman.predict(currentTime);
+    //                sigma = std::sqrt(prediction.P[0]);
+
+    //                sigma = sigma > 0 ? sigma : 5;
+
+    //                const Point3 apPos = Settings::CurrentPath.nucInfo(i).position;
+    //                // particlePos.z = 1.3; // smartphone höhe
+    //                const float apDist = particlePos.getDistance(apPos);
+
+    //                float ftmDist = prediction.distance;
+    //                double x = Distribution::Normal<double>::getProbability(ftmDist, sigma, apDist);
+
+    //                if (x > 1e-80)
+    //                {
+    //                    Assert::isNot0(x, "");
+
+    //                    volatile double oldResult = result;
+
+    //                    result *= x;
+
+
+    //                    Assert::isNot0(result, "");
+
+    //                    printf("");
+    //                }
+
+    //            }
+    //        }
+    //    }
+    //}
+
     return result;
 }

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) };
+    }
+}

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 x[2] = {NAN};   // predicted state  [m, m/s]
-    float P[4];   // Covariance
+    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);
 };
 
 

code/filter.h

@@ -183,7 +183,7 @@ struct MyPFTransRandom : public SMC::ParticleFilterTransition<MyState, MyControl
         Distribution::Uniform<float> dHeading;
 
         MyPFTransRandom()
-            : dStepSize(2.0f, 0.2f), dHeading(0, 2*M_PI)
+            : dStepSize(1.5f, 0.2f), dHeading(0, 2*M_PI)
         {}
 
         void transition(std::vector<SMC::Particle<MyState>>& particles, const MyControl* control) override {