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

Particle filter is now using a fixed time interval as step

Browse Source
This commit is contained in:
Markus Bullmann
2019-09-25 16:49:57 +02:00
parent 24bbc56f28
commit deb318e115
5 changed files with 185 additions and 83 deletions
Download Patch File Download Diff File Expand all files Collapse all files

80
code/filter.h
View File

@@ -5,6 +5,7 @@
#include <omp.h>
#include <array>
#include <Indoor/Assertions.h>
#include <Indoor/geo/Heading.h>
#include <Indoor/math/distribution/Uniform.h>
#include <Indoor/math/distribution/Normal.h>
@@ -114,8 +115,8 @@ struct MyObservation {
//wifi
std::unordered_map<MACAddress, WiFiMeasurement> wifi; // deprecated
std::array<float, 4> dists;
std::array<float, 4> sigmas; // from kalman
std::vector<WiFiMeasurement> ftm;
//time
Timestamp currentTime;
@@ -263,34 +264,85 @@ struct MyPFEval : public SMC::ParticleFilterEvaluation<MyState, MyObservation> {
MyPFEval() { };
bool assignProps = false;
std::shared_ptr<std::unordered_map<MACAddress, Kalman>> ftmKalmanFilters;
virtual double evaluation(std::vector<SMC::Particle<MyState>>& particles, const MyObservation& observation) override {
double sum = 0;
//#pragma omp parallel for num_threads(3)
#pragma omp parallel for num_threads(3)
for (int i = 0; i < particles.size(); ++i) {
SMC::Particle<MyState>& p = particles[i];
double pFtm = 1.0;
for (size_t i = 0; i < 4; i++)
for (WiFiMeasurement wifi : observation.ftm)
{
float dist = observation.dists[i];
const float sigma = isnan(observation.sigmas[i]) ? 3.5 : observation.sigmas[i];
if (!isnan(dist))
if (wifi.getNumSuccessfulMeasurements() < 3)
{
Point3 apPos = Settings::data.CurrentPath.nucInfo(i).position;
Point3 particlePos = p.state.pos.pos;
particlePos.z = 1.3; // smartphone höhe
float apDist = particlePos.getDistance(apPos);
continue;
}
double x = Distribution::Normal<double>::getProbability(dist, std::sqrt(sigma), apDist);
pFtm *= x;
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);
if (x > 1e-90)
{
pFtm *= x;
Assert::isTrue(pFtm != 0, "zero prob");
}
}
else
{
double x = Distribution::Normal<double>::getProbability(ftmDist, std::sqrt(sigma), apDist);
if (x > 1e-90)
{
pFtm *= x;
Assert::isTrue(pFtm != 0, "zero prob");
}
}
}
}
double prob = pFtm;
if (assignProps)