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

9
code/Plotta.h
View File

@@ -6,6 +6,7 @@
#include <vector>
#include <unordered_map>
#include <iostream>
#include <array>
namespace Plotta
{
@@ -39,6 +40,8 @@ namespace Plotta
{
// send data
std::ofstream stream;
stream.exceptions(stream.exceptions() | std::ios::failbit);
stream.open(dataFile);
std::time_t t = std::time(nullptr);
@@ -126,6 +129,12 @@ namespace Plotta
return writeNumpyArray(stream, list.begin(), list.end());
}
template<typename T, size_t S>
plottastream& operator<<(plottastream& stream, const std::array<T, S>& list)
{
return writeNumpyArray(stream, list.begin(), list.end());
}
template<typename T>
static plottastream& operator<<(plottastream& stream, const T& value)

14
code/Plotty.h
View File

@@ -184,6 +184,20 @@ public:
splot.getObjects().set(id, obj);
}
void addCircle(int id, const Point2& center, float radius, const K::GnuplotColor& strokeColor)
{
auto c = K::GnuplotCoordinate2(center.x, center.y, K::GnuplotCoordinateSystem::FIRST);
auto r = K::GnuplotCoordinate1(radius, K::GnuplotCoordinateSystem::FIRST);
K::GnuplotFill fill(K::GnuplotFillStyle::EMPTY, K::GnuplotColor::fromRGB(0, 0, 0));
K::GnuplotStroke stroke(K::GnuplotDashtype::SOLID, 1, strokeColor);
K::GnuplotObjectCircle* obj = new K::GnuplotObjectCircle(c, r, fill, stroke);
splot.getObjects().set(id, obj);
}
void addBBoxes(const BBoxes3& boxes, const K::GnuplotColor& c) {

6
code/Settings.h
View File

@@ -94,6 +94,7 @@ namespace Settings {
const std::string mapDir = "../map/";
const std::string dataDir = "../measurements/data/";
const std::string errorDir = "../measurements/error/";
const std::string plotDataDir = "../plots/data/";
const bool UseKalman = true;
@@ -147,6 +148,11 @@ namespace Settings {
{
return NUCs.at(nucFromIndex(idx));
}
NUCSettings nuc(const MACAddress& mac) const
{
return NUCs.at(mac);
}
};
/** all configured datasets */

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)

159
code/main.cpp
View File

@@ -5,6 +5,7 @@
#include "Settings.h"
#include "meshPlotter.h"
#include "Plotty.h"
#include "Plotta.h"
#include <array>
#include <memory>
@@ -216,12 +217,11 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
// wifi
std::array<Kalman, 4> ftmKalmanFilters{
Kalman(1, setup.NUCs.at(Settings::NUC1).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev),
Kalman(2, setup.NUCs.at(Settings::NUC2).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev),
Kalman(3, setup.NUCs.at(Settings::NUC3).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev),
Kalman(4, setup.NUCs.at(Settings::NUC4).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev)
};
auto kalmanMap = std::make_shared<std::unordered_map<MACAddress, Kalman>>();
kalmanMap->insert({ Settings::NUC1, Kalman(1, setup.NUCs.at(Settings::NUC1).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev) });
kalmanMap->insert({ Settings::NUC2, Kalman(2, setup.NUCs.at(Settings::NUC2).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev) });
kalmanMap->insert({ Settings::NUC3, Kalman(3, setup.NUCs.at(Settings::NUC3).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev) });
kalmanMap->insert({ Settings::NUC4, Kalman(4, setup.NUCs.at(Settings::NUC4).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev) });
std::cout << "Optimal wifi parameters for " << setup.training[walkIdx] << "\n";
optimizeWifiParameters(fr, gtInterpolator);
@@ -257,12 +257,14 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
//auto init = std::make_unique<MyPFInitFixed>(&mesh, srcPath0); // known position
auto init = std::make_unique<MyPFInitUniform>(&mesh); // uniform distribution
auto eval = std::make_unique<MyPFEval>();
eval->ftmKalmanFilters = kalmanMap;
auto trans = std::make_unique<MyPFTransRandom>();
//auto trans = std::make_unique<MyPFTransStatic>();
auto resample = std::make_unique<SMC::ParticleFilterResamplingSimple<MyState>>();
auto estimate = std::make_unique<SMC::ParticleFilterEstimationWeightedAverage<MyState>>();
//auto estimate = std::make_unique<SMC::ParticleFilterEstimationWeightedAverage<MyState>>();
auto estimate = std::make_unique<SMC::ParticleFilterEstimationMax<MyState>>();
// setup
MyFilter pf(numParticles, std::move(init));
@@ -277,60 +279,51 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
MyObservation obs;
Timestamp lastTimestamp = Timestamp::fromMS(0);
std::vector<WifiMeas> data = filterOfflineData(fr);
std::vector<float> errorValuesFtm, errorValuesRssi;
std::vector<int> timestamps;
std::vector<std::array<float, 4>> gtDistances, ftmDistances, rssiDistances; // distance per AP
Plotta::Plotta errorPlot("errorPlot", Settings::plotDataDir + "errorData.py");
Plotta::Plotta distsPlot("distsPlot", Settings::plotDataDir + "distances.py");
for (const WifiMeas& wifi : data)
for (const Offline::Entry& e : fr.getEntries())
{
Point2 gtPos = gtInterpolator.get(static_cast<uint64_t>(wifi.ts.ms())).xy();
plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1));
if (e.type != Offline::Sensor::WIFI_FTM) {
continue;
}
Point3 estPos;
float distErrorFtm = 0;
float distErrorRssi = 0;
// TIME
const Timestamp ts = Timestamp::fromMS(e.ts);
// FTM
auto wifiFtm = fr.getWifiFtm()[e.idx].data;
obs.ftm.push_back(wifiFtm);
if (ts - lastTimestamp >= Timestamp::fromMS(500))
{
std::array<float, 4> dists = wifi.ftmDists;
std::array<float, 4> sigmas = {NAN, NAN, NAN, NAN };
// Do update step
Point2 gtPos = gtInterpolator.get(static_cast<uint64_t>(ts.ms())).xy();
plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1));
for (size_t i = 0; i < 4; i++)
{
if (dists[i] <= 0)
{
dists[i] = NAN;
}
}
gtDistances.push_back({ gtPos.getDistance(Settings::data.CurrentPath.nucInfo(0).position.xy()),
gtPos.getDistance(Settings::data.CurrentPath.nucInfo(1).position.xy()),
gtPos.getDistance(Settings::data.CurrentPath.nucInfo(2).position.xy()),
gtPos.getDistance(Settings::data.CurrentPath.nucInfo(3).position.xy()) });
if (Settings::UseKalman)
{
for (size_t i = 0; i < 4; i++)
{
if (!isnan(dists[i]))
{
dists[i] = ftmKalmanFilters[i].predict(wifi.ts, dists[i]);
sigmas[i] = ftmKalmanFilters[i].P(0, 0);
}
}
}
obs.dists = dists;
obs.sigmas = sigmas;
Point3 estPos;
float distErrorFtm = 0;
float distErrorRssi = 0;
// Run PF
obs.currentTime = wifi.ts;
ctrl.currentTime = wifi.ts;
obs.currentTime = ts;
ctrl.currentTime = ts;
MyState est = pf.update(&ctrl, obs);
ctrl.afterEval();
lastTimestamp = wifi.ts;
lastTimestamp = ts;
estPos = est.pos.pos;
ctrl.lastEstimate = estPos;
plot.setCurEst(Point3(estPos.x, estPos.y, 0.1));
@@ -341,35 +334,63 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
errorStats.ftm.add(distErrorFtm);
// draw wifi ranges
for (size_t i = 0; i < 4; i++)
for (size_t i = 0; i < obs.ftm.size(); i++)
{
Point3 apPos = Settings::data.CurrentPath.nucInfo(i).position;
plot.addCircle(1000+i, apPos.xy(), dists[i]);
WiFiMeasurement wifi2 = obs.ftm[i];
Point3 apPos = Settings::data.CurrentPath.nuc(wifi2.getAP().getMAC()).position;
K::GnuplotColor color;
switch (Settings::data.CurrentPath.nuc(wifi2.getAP().getMAC()).ID)
{
case 1: color = K::GnuplotColor::fromRGB(0, 255, 0); break;
case 2: color = K::GnuplotColor::fromRGB(0, 0, 255); break;
case 3: color = K::GnuplotColor::fromRGB(255, 255, 0); break;
default: color = K::GnuplotColor::fromRGB(255, 0, 0); break;
}
plot.addCircle(1000 + i, apPos.xy(), wifi2.getFtmDist(), color);
}
obs.wifi.clear();
obs.ftm.clear();
errorValuesFtm.push_back(distErrorFtm);
errorValuesRssi.push_back(distErrorRssi);
timestamps.push_back(ts.ms());
// Error plot
errorPlot.add("t", timestamps);
errorPlot.add("errorFtm", errorValuesFtm);
errorPlot.add("errorRssi", errorValuesRssi);
errorPlot.frame();
// Distances plot
//distsPlot.add("t", timestamps);
//distsPlot.add("gtDists", gtDistances);
//distsPlot.add("ftmDists", ftmDistances);
//distsPlot.frame();
// Plotting
plot.showParticles(pf.getParticles());
plot.setCurEst(estPos);
plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1));
plot.addEstimationNode(estPos);
//plot.setActivity((int)act.get());
//plot.splot.getView().setEnabled(false);
//plot.splot.getView().setCamera(0, 0);
//plot.splot.getView().setEqualXY(true);
plot.plot();
}
errorValuesFtm.push_back(distErrorFtm);
errorValuesRssi.push_back(distErrorRssi);
timestamps.push_back(wifi.ts.ms());
// Plotting
//plot.showParticles(pf.getParticles());
plot.setCurEst(estPos);
plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1));
plot.addEstimationNode(estPos);
//plot.setActivity((int)act.get());
//plot.splot.getView().setEnabled(false);
//plot.splot.getView().setCamera(0, 0);
//plot.splot.getView().setEqualXY(true);
plot.plot();
//std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
printErrorStats(errorStats);
//system("pause");
return errorStats;
}
@@ -398,10 +419,10 @@ int main(int argc, char** argv)
std::string evaluationName = "prologic/tmp";
for (size_t walkIdx = 0; walkIdx < Settings::data.CurrentPath.training.size(); walkIdx++)
for (size_t walkIdx = 0; walkIdx < 1 /*Settings::data.CurrentPath.training.size()*/; walkIdx++)
{
std::cout << "Executing walk " << walkIdx << "\n";
for (int i = 0; i < 1; ++i)
for (int i = 0; i < 5; ++i)
{
std::cout << "Start of iteration " << i << "\n";