From d645ab767580bf89d362a8288c2f0c7130e5060e Mon Sep 17 00:00:00 2001 From: Markus Bullmann Date: Wed, 13 Nov 2019 15:08:40 +0100 Subject: [PATCH] Use kalman to predict missing measurements --- code/Eval.cpp | 57 +++++++++++++++++++++++++++++++++++++++++++--- code/FtmKalman.cpp | 49 ++++++++++++++++++++++++++++++++++++--- code/FtmKalman.h | 17 ++++++++++---- code/filter.h | 2 +- 4 files changed, 114 insertions(+), 11 deletions(-) diff --git a/code/Eval.cpp b/code/Eval.cpp index 7e12d5e..8ef243a 100644 --- a/code/Eval.cpp +++ b/code/Eval.cpp @@ -1,5 +1,8 @@ #include "Eval.h" +#include +#include + #include "Settings.h" #include @@ -8,6 +11,8 @@ double ftmEval(SensorMode UseSensor, const Timestamp& currentTime, const Point3& { double result = 1.0; + std::array 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::getProbability(ftmDist, sigma, apDist); + + // if (x > 1e-80) + // { + // Assert::isNot0(x, ""); + + // volatile double oldResult = result; + + // result *= x; + + + // Assert::isNot0(result, ""); + + // printf(""); + // } + + // } + // } + // } + //} + return result; } \ No newline at end of file diff --git a/code/FtmKalman.cpp b/code/FtmKalman.cpp index 2e077cd..db9cd5e 100644 --- a/code/FtmKalman.cpp +++ b/code/FtmKalman.cpp @@ -1,12 +1,22 @@ #include "FtmKalman.h" +#include #include -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> x(this->x); Eigen::Map> 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); -} \ No newline at end of file +} + +KalmanPrediction Kalman::predict(const Timestamp timestamp) +{ + if (isnan(lastTimestamp)) + { + // Kalman has no data => nothing to predict + return KalmanPrediction{ NAN, NAN }; + } + else + { + Eigen::Map> x(this->x); + Eigen::Map> 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) }; + } +} diff --git a/code/FtmKalman.h b/code/FtmKalman.h index 810af5a..e99619c 100644 --- a/code/FtmKalman.h +++ b/code/FtmKalman.h @@ -2,14 +2,22 @@ #include +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); }; diff --git a/code/filter.h b/code/filter.h index d2ebd66..9d5a20c 100644 --- a/code/filter.h +++ b/code/filter.h @@ -183,7 +183,7 @@ struct MyPFTransRandom : public SMC::ParticleFilterTransition 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>& particles, const MyControl* control) override {