FHWS/FtmPrologic
Archived
4
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Moved eval into own cpp to allow shorter compile times

Browse Source
This commit is contained in:
Markus Bullmann
2019-10-08 13:31:59 +02:00
parent 17140f2dbe
commit 704b64bfcf
4 changed files with 101 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
code/CMakeLists.txt
View File

@@ -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
)

70
code/Eval.cpp Normal file
View File

@@ -0,0 +1,70 @@
#include "Eval.h"
#include "Settings.h"
#include <Indoor/math/distribution/Normal.h>
double ftmEval(SensorMode UseSensor, const Timestamp& currentTime, const Point3& particlePos, const std::vector<WiFiMeasurement>& measurements, std::shared_ptr<std::unordered_map<MACAddress, Kalman>> 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<double>::getProbability(ftmDist, sigma, apDist);
result *= x;
}
else
{
double x = Distribution::Normal<double>::getProbability(ftmDist, sigma, apDist);
result *= x;
}
}
}
else
{
// RSSI
double sigma = 5;
double x = Distribution::Normal<double>::getProbability(rssiDist, sigma, apDist);
result *= x;
}
}
return result;
}

23
code/Eval.h Normal file
View File

@@ -0,0 +1,23 @@
#pragma once
#include <unordered_map>
#include <vector>
#include <Indoor/geo/Point3.h>
#include "FtmKalman.h"
#include <Indoor/sensors/radio/WiFiMeasurements.h>
#include <Indoor/sensors/radio/model/LogDistanceModel.h>
enum class SensorMode
{
FTM,
RSSI
};
double ftmEval(SensorMode UseSensor,
const Timestamp& currentTime,
const Point3& particlePos,
const std::vector<WiFiMeasurement>& measurements,
std::shared_ptr<std::unordered_map<MACAddress, Kalman>> ftmKalmanFilters);

62
code/filter.h
View File

@@ -34,6 +34,7 @@
#include <Indoor/sensors/activity/ActivityDetector.h>
#include "FtmKalman.h"
#include "Eval.h"
struct MyState {
@@ -274,67 +275,14 @@ struct MyPFEval : public SMC::ParticleFilterEvaluation<MyState, MyObservation> {
for (int i = 0; i < particles.size(); ++i) {
SMC::Particle<MyState>& 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<double>::getProbability(ftmDist, std::sqrt(sigma), apDist);
pFtm *= x;
}
else
{
double x = Distribution::Normal<double>::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;