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

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";