Added moving average

code/FtmKalman.h

@@ -13,6 +13,8 @@ struct Kalman
     Eigen::Matrix<float, 2, 2> P; // Covariance
 
     float R = 30;  //  measurement noise covariance   
+    float processNoiseDistance; // stdDev
+    float processNoiseVelocity; // stdDev
 
     float lastTimestamp = NAN; // in sec
 
@@ -22,8 +24,8 @@ struct Kalman
         : nucID(nucID)
     {}
 
-    Kalman(int nucID, float measStdDev)
-        : nucID(nucID), R(measStdDev*measStdDev)
+    Kalman(int nucID, float measStdDev, float processNoiseDistance = 0.2, float processNoiseVelocity = 0.4)
+        : nucID(nucID), R(measStdDev*measStdDev), processNoiseDistance(processNoiseDistance), processNoiseVelocity(processNoiseVelocity)
     {}
 
     float predict(const Timestamp timestamp, const float measurment)
@@ -52,8 +54,8 @@ struct Kalman
              0, 1;
 
         Eigen::Matrix2f Q;  // Process Noise Covariance
-        Q << 0, 0,
-             0, square(0.3);
+        Q << square(processNoiseDistance), 0,
+            0, square(processNoiseVelocity);
 
         // Prediction
         x = A * x;                    // Pr<50>dizierter Zustand aus Bisherigem und System