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

Refactored probabilistic code

Browse Source
This commit is contained in:
Markus Bullmann
2019-10-01 17:23:59 +02:00
parent f8a9739daf
commit 17140f2dbe
1 changed files with 122 additions and 95 deletions
Download Patch File Download Diff File Expand all files Collapse all files

217
code/mainProb.cpp
View File

@@ -30,7 +30,7 @@
static float kalman_procNoiseDistStdDev = 1.2f; // standard deviation of distance for process noise
static float kalman_procNoiseVelStdDev = 0.1f; // standard deviation of velocity for process noise
static void poorMansKDE(const BBox3& bbox, float sigma, std::array<float, 4> dist, std::array<Point2, 4> apPos, std::vector<std::pair<Point2, float>>& density, std::pair<Point2, float>& maxElem)
static void poorMansKDE(const BBox3& bbox, std::vector<std::pair<Point2, double>>& density, std::pair<Point2, double>& maxElem, const std::function<double(Point2 pt)>& evalProc)
{
density.clear();
@@ -48,35 +48,99 @@ static void poorMansKDE(const BBox3& bbox, float sigma, std::array<float, 4> dis
{
const Point2 pos(x, y);
float P = 1.0f;
for (size_t i = 0; i < 4; i++)
{
// TODO: Was mit nan machen?
if (!isnan(dist[i]))
{
float d = pos.getDistance(apPos[i]) - dist[i];
float p = Distribution::Normal<float>::getProbability(0, sigma, d);
P *= p;
}
}
double P = evalProc(pos);
density.push_back({ pos, P });
}
}
auto maxElement = std::max_element(density.begin(), density.end(), [](std::pair<Point2, float> a, std::pair<Point2, float> b) {
auto maxElement = std::max_element(density.begin(), density.end(), [](std::pair<Point2, double> a, std::pair<Point2, double> b) {
return a.second < b.second;
});
maxElem = *maxElement;
}
static void computeDensity(const BBox3& bbox, std::vector<std::pair<Point2, double>>& density, std::pair<Point2, double>& maxElem, const std::vector<WiFiMeasurement>& obs, bool useFtm, double sigma)
{
poorMansKDE(bbox, density, maxElem, [&obs, useFtm, sigma](Point2 pt) {
double p = 1.0;
for (const WiFiMeasurement& wifi : obs)
{
if (wifi.getNumSuccessfulMeasurements() < 3)
continue;
const MACAddress& mac = wifi.getAP().getMAC();
int nucIndex = Settings::nucIndex(mac);
// compute AP distance
const Point3 apPos = Settings::data.CurrentPath.nucInfo(nucIndex).position;
Point3 pos = Point3(pt.x, pt.y, 1.3); // smartphone h<>he
const float apDist = pos.getDistance(apPos);
double dist = 0;
if (useFtm)
{
// 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
dist = ftmDist;
}
else
{
// compute rssi distance
float rssi_pathloss = Settings::data.CurrentPath.NUCs.at(mac).rssi_pathloss;
float rssiDist = LogDistanceModel::rssiToDistance(-40, 2.25 /*rssi_pathloss*/, wifi.getRSSI());
dist = rssiDist;
}
if (dist > 0)
{
double d = apDist - dist;
double x = Distribution::Normal<double>::getProbability(0, 3.5, d);
p *= x;
}
}
return p;
});
}
static void plotDensity(Plotty& plot, std::vector<std::pair<Point2, double>>& density)
{
plot.particles.clear();
double min = std::numeric_limits<double>::max();
double max = std::numeric_limits<double>::min();
for (auto it = density.begin(); ; std::advance(it, 2))
{
if (it > density.end())
break;
auto p = *it;
const K::GnuplotPoint3 p3(p.first.x, p.first.y, 0.1);
const double prob = std::pow(p.second, 0.25);
plot.particles.add(p3, prob);
if (prob > max) { max = prob; }
if (prob < min) { min = prob; }
}
plot.splot.getAxisCB().setRange(min, max + 0.000001);
}
static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::string folder)
{
// reading file
Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(setup.map);
Offline::FileReader fr(setup.training[walkIdx]);
Offline::FileReader fr(setup.training[walkIdx], setup.HasNanoSecondTimestamps);
// ground truth
std::vector<int> gtPath = setup.gtPath;
@@ -113,7 +177,7 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
plot.setGroundTruth(gtPath);
plot.setView(30, 0);
plot.plot();
// wifi
std::array<Kalman, 4> ftmKalmanFilters{
Kalman(1, setup.NUCs.at(Settings::NUC1).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev),
@@ -129,7 +193,9 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
Settings::data.CurrentPath.NUCs.at(Settings::NUC4).position.xy(),
};
std::vector<WifiMeas> data = filterOfflineData(fr);
std::vector<WiFiMeasurement> obs;
Timestamp lastTimestamp = Timestamp::fromMS(0);
const float sigma = 3.5;
const int movAvgWnd = 15;
@@ -139,124 +205,85 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
std::vector<float> errorValuesFtm, errorValuesRssi;
std::vector<int> timestamps;
for (const WifiMeas& wifi : data)
for (const Offline::Entry& e : fr.getEntries())
{
Plotta::Plotta test("test", "C:\\Temp\\Plotta\\probData.py");
if (e.type != Offline::Sensor::WIFI_FTM) {
continue;
}
Point2 gtPos = gtInterpolator.get(static_cast<uint64_t>(wifi.ts.ms())).xy();
plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1));
// TIME
const Timestamp ts = Timestamp::fromMS(e.ts);
float distErrorFtm = 0;
float distErrorRssi = 0;
auto wifiFtm = fr.getWifiFtm()[e.idx].data;
obs.push_back(wifiFtm);
//if (wifi.numSucessMeas() == 4)
if (ts - lastTimestamp >= Timestamp::fromMS(500))
{
// Do update
Plotta::Plotta test("test", "C:\\Temp\\Plotta\\probData.py");
Point2 gtPos = gtInterpolator.get(static_cast<uint64_t>(ts.ms())).xy();
plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1));
float distErrorFtm = 0;
float distErrorRssi = 0;
// FTM
{
std::array<float, 4> dists = wifi.ftmDists;
if (Settings::UseKalman)
{
std::cout << "Using Kalman" << "\n";
for (size_t i = 0; i < 4; i++)
{
if (!isnan(dists[i]))
{
dists[i] = ftmKalmanFilters[i].predict(wifi.ts, dists[i]);
}
}
}
BBox3 bbox = FloorplanHelper::getBBox(map);
std::vector<std::pair<Point2, float>> density;
std::pair<Point2, float> maxElement;
std::vector<std::pair<Point2, double>> density;
std::pair<Point2, double> maxElement;
poorMansKDE(bbox, sigma, dists, apPositions, density, maxElement);
computeDensity(bbox, density, maxElement, obs, true, 3.5);
Point2 estPos = maxElement.first;
plot.setCurEst(Point3(estPos.x, estPos.y, 0.1));
plot.addEstimationNode(Point3(estPos.x, estPos.y, 0.1));
plot.setCurEst(Point3(estPos.x, estPos.y, 0.1));
// Plot density
//plotDensity(plot, density);
// Error
distErrorFtm = gtPos.getDistance(estPos);
errorStats.ftm.add(distErrorFtm);
//std::vector<float> densityX, densityY, densityZ;
//for (const auto& item : density)
//{
// densityX.push_back(item.first.x);
// densityY.push_back(item.first.y);
// densityZ.push_back(item.second);
//}
//test.add("densityX", densityX);
//test.add("densityY", densityY);
//test.add("densityZ", densityZ);
}
// RSSI
{
std::array<float, 4> dists = wifi.rssiDists;
if (Settings::UseKalman)
{
for (size_t i = 0; i < 4; i++)
{
if (!isnan(dists[i]))
{
dists[i] = ftmKalmanFilters[i].predict(wifi.ts, dists[i]);
}
}
}
BBox3 bbox = FloorplanHelper::getBBox(map);
std::vector<std::pair<Point2, float>> density;
std::pair<Point2, float> maxElement;
std::vector<std::pair<Point2, double>> density;
std::pair<Point2, double> maxElement;
poorMansKDE(bbox, sigma, dists, apPositions, density, maxElement);
computeDensity(bbox, density, maxElement, obs, false, 8);
Point2 estPos = maxElement.first;
plot.setCurEst(Point3(estPos.x, estPos.y, 0.1));
plot.addEstimationNode2(Point3(estPos.x, estPos.y, 0.1));
// Plot density
//plotDensity(plot, density);
// Error
distErrorRssi = gtPos.getDistance(estPos);
errorStats.rssi.add(distErrorRssi);
//std::vector<float> densityX, densityY, densityZ;
//for (const auto& item : density)
//{
// densityX.push_back(item.first.x);
// densityY.push_back(item.first.y);
// densityZ.push_back(item.second);
//}
//test.add("densityX", densityX);
//test.add("densityY", densityY);
//test.add("densityZ", densityZ);
}
//std::cout << wifi.ts.ms() << " " << distError << "\n";
errorValuesFtm.push_back(distErrorFtm);
errorValuesRssi.push_back(distErrorRssi);
timestamps.push_back(wifi.ts.ms());
timestamps.push_back(ts.ms());
test.add("t", timestamps);
test.add("errorFtm", errorValuesFtm);
test.add("errorRssi", errorValuesRssi);
test.frame();
}
plot.plot();
//Sleep(250);
printf("");
plot.plot();
//Sleep(250);
printf("");
lastTimestamp = ts;
obs.clear();
}
}
printErrorStats(errorStats);