From 704b64bfcfd55dd3ad5765a84298298e5c61763d Mon Sep 17 00:00:00 2001 From: Markus Bullmann Date: Tue, 8 Oct 2019 13:31:59 +0200 Subject: [PATCH] Moved eval into own cpp to allow shorter compile times --- code/CMakeLists.txt | 2 ++ code/Eval.cpp | 70 +++++++++++++++++++++++++++++++++++++++++++++ code/Eval.h | 23 +++++++++++++++ code/filter.h | 64 ++++------------------------------------- 4 files changed, 101 insertions(+), 58 deletions(-) create mode 100644 code/Eval.cpp create mode 100644 code/Eval.h diff --git a/code/CMakeLists.txt b/code/CMakeLists.txt index c33101e..b3d15b9 100644 --- a/code/CMakeLists.txt +++ b/code/CMakeLists.txt @@ -38,6 +38,7 @@ FILE(GLOB HEADERS trilateration.h Plotta.h misc.h + Eval.h ) @@ -48,6 +49,7 @@ FILE(GLOB SOURCES mainFtm.cpp mainTrilat.cpp mainProb.cpp + Eval.cpp ) diff --git a/code/Eval.cpp b/code/Eval.cpp new file mode 100644 index 0000000..11b0172 --- /dev/null +++ b/code/Eval.cpp @@ -0,0 +1,70 @@ +#include "Eval.h" + +#include "Settings.h" + +#include + +double ftmEval(SensorMode UseSensor, const Timestamp& currentTime, const Point3& particlePos, const std::vector& measurements, std::shared_ptr> ftmKalmanFilters) +{ + double result = 1.0; + + for (WiFiMeasurement wifi : measurements) + { + if (wifi.getNumSuccessfulMeasurements() < 3) + { + continue; + } + + const MACAddress& mac = wifi.getAP().getMAC(); + int nucIndex = Settings::nucIndex(mac); + + const Point3 apPos = Settings::data.CurrentPath.nucInfo(nucIndex).position; + // particlePos.z = 1.3; // smartphone höhe + const float apDist = particlePos.getDistance(apPos); + + // compute ftm distance + float ftm_offset = Settings::data.CurrentPath.NUCs.at(mac).ftm_offset; + float ftmDist = wifi.getFtmDist() + ftm_offset; // in m; plus static offset + + float rssi_pathloss = Settings::data.CurrentPath.NUCs.at(mac).rssi_pathloss; + float rssiDist = LogDistanceModel::rssiToDistance(-40, rssi_pathloss, wifi.getRSSI()); + + if (UseSensor == SensorMode::FTM) + { + if (ftmDist > 0) + { + //double sigma = wifi.getFtmDistStd()*wifi.getFtmDistStd(); // 3.5; // TODO + double sigma = 5; + + if (ftmKalmanFilters != nullptr) + { + Kalman& kalman = ftmKalmanFilters->at(mac); + ftmDist = kalman.predict(currentTime, ftmDist); + //sigma = std::sqrt(kalman.P(0, 0)); + + Assert::isTrue(sigma > 0, "sigma"); + + double x = Distribution::Normal::getProbability(ftmDist, sigma, apDist); + + result *= x; + } + else + { + double x = Distribution::Normal::getProbability(ftmDist, sigma, apDist); + + result *= x; + } + } + } + else + { + // RSSI + double sigma = 5; + + double x = Distribution::Normal::getProbability(rssiDist, sigma, apDist); + result *= x; + } + } + + return result; +} \ No newline at end of file diff --git a/code/Eval.h b/code/Eval.h new file mode 100644 index 0000000..7bc62c7 --- /dev/null +++ b/code/Eval.h @@ -0,0 +1,23 @@ +#pragma once + +#include +#include + +#include + +#include "FtmKalman.h" +#include +#include + + +enum class SensorMode +{ + FTM, + RSSI +}; + +double ftmEval(SensorMode UseSensor, + const Timestamp& currentTime, + const Point3& particlePos, + const std::vector& measurements, + std::shared_ptr> ftmKalmanFilters); diff --git a/code/filter.h b/code/filter.h index 9105d76..fdff44d 100644 --- a/code/filter.h +++ b/code/filter.h @@ -34,6 +34,7 @@ #include #include "FtmKalman.h" +#include "Eval.h" struct MyState { @@ -269,72 +270,19 @@ struct MyPFEval : public SMC::ParticleFilterEvaluation { virtual double evaluation(std::vector>& particles, const MyObservation& observation) override { double sum = 0; - + #pragma omp parallel for num_threads(3) for (int i = 0; i < particles.size(); ++i) { SMC::Particle& p = particles[i]; - double pFtm = 1.0; + auto kalmanFilters = ftmKalmanFilters; - for (WiFiMeasurement wifi : observation.ftm) + if (!Settings::UseKalman) { - if (wifi.getNumSuccessfulMeasurements() < 3) - { - continue; - } - - const MACAddress& mac = wifi.getAP().getMAC(); - int nucIndex = Settings::nucIndex(mac); - - const Point3 apPos = Settings::data.CurrentPath.nucInfo(nucIndex).position; - Point3 particlePos = p.state.pos.pos; - particlePos.z = 1.3; // smartphone höhe - const float apDist = particlePos.getDistance(apPos); - - // compute ftm distance - float ftm_offset = Settings::data.CurrentPath.NUCs.at(mac).ftm_offset; - float ftmDist = wifi.getFtmDist() + ftm_offset; // in m; plus static offset - - float rssi_pathloss = Settings::data.CurrentPath.NUCs.at(mac).rssi_pathloss; - float rssiDist = LogDistanceModel::rssiToDistance(-40, rssi_pathloss, wifi.getRSSI()); - - - if (ftmDist > 0) - { - double sigma = wifi.getFtmDistStd()*wifi.getFtmDistStd(); // 3.5; // TODO - - if (sigma == 0) - { - sigma = 38*38; - } - - if (Settings::UseKalman) - { - Kalman& kalman = ftmKalmanFilters->at(mac); - ftmDist = kalman.predict(observation.currentTime, ftmDist); - sigma = kalman.P(0, 0); - - //Assert::isTrue(sigma > 0, "sigma"); - if (sigma <= 0) - { - sigma = 38 * 38; - } - - double x = Distribution::Normal::getProbability(ftmDist, std::sqrt(sigma), apDist); - - pFtm *= x; - } - else - { - double x = Distribution::Normal::getProbability(ftmDist, std::sqrt(sigma), apDist); - - pFtm *= x; - } - } + kalmanFilters = nullptr; } - - double prob = pFtm; + double prob = ftmEval(SensorMode::FTM, observation.currentTime, p.state.pos.pos, observation.ftm, kalmanFilters); if (assignProps) p.weight = prob;