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

Added trilateration code

Browse Source
This commit is contained in:
Markus Bullmann
2019-08-06 11:31:45 +02:00
parent a7571862c6
commit 52eac3a28a
3 changed files with 353 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
code/Plotty.h
View File

@@ -98,6 +98,7 @@ public:
K::GnuplotSplotElementLines pathReal;
K::GnuplotSplotElementLines pathEst;
K::GnuplotSplotElementLines pathEst2;
K::GnuplotSplotElementColorPoints particles;
K::GnuplotSplotElementLines mapOutlineGlass;
@@ -155,6 +156,7 @@ public:
splot.add(&pathReal); pathReal.getStroke().setWidth(2); pathReal.getStroke().getColor().setHexStr("#000000");
splot.add(&pathEst); pathEst.getStroke().setWidth(2); pathEst.getStroke().getColor().setHexStr("#0000ff");
splot.add(&pathEst2); pathEst2.getStroke().setWidth(2); pathEst2.getStroke().getColor().setHexStr("#ff0000");
splot.add(&pm3doutline);
@@ -478,6 +480,11 @@ public:
pathEst.add(est);
}
void addEstimationNode2(const Point3 pos) {
K::GnuplotPoint3 est(pos.x, pos.y, std::round(pos.z * 10) / 10);
pathEst2.add(est);
}
void setTitle(const std::string& title) {
gp << "set title '" << title << "'\n";

251
code/mainTrilat.cpp Normal file
View File

@@ -0,0 +1,251 @@
#include "main.h"
#include <array>
#include <memory>
#include <thread>
#include <filesystem>
#include <chrono>
#include <iostream>
#include <Indoor/math/stats/Statistics.h>
#include <Indoor/floorplan/v2/FloorplanReader.h>
#include <Indoor/sensors/offline/FileReader.h>
#include <Indoor/sensors/offline/Sensors.h>
#include <Indoor/sensors/radio/model/LogDistanceModel.h>
#include <Indoor/geo/Heading.h>
#include <Indoor/geo/Point2.h>
#include <Indoor/data/Timestamp.h>
#include <Indoor/math/MovingAVG.h>
#include "Settings.h"
#include "Plotty.h"
#include "Plotta.h"
#include "trilateration.h"
#include "misc.h"
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]);
// ground truth
std::vector<int> gtPath = setup.gtPath;
Interpolator<uint64_t, Point3> gtInterpolator = fr.getGroundTruthPath(map, gtPath);
CombinedStats<float> errorStats;
//calculate distance of path
std::vector<Interpolator<uint64_t, Point3>::InterpolatorEntry> gtEntries = gtInterpolator.getEntries();
double distance = 0;
for (int i = 1; i < gtEntries.size(); ++i) {
distance += gtEntries[i].value.getDistance(gtEntries[i - 1].value);
}
std::cout << "Distance of Path: " << distance << std::endl;
// debug show
//MeshPlotter dbg;
//dbg.addFloors(map);
//dbg.addOutline(map);
//dbg.addMesh(mesh);
////dbg.addDijkstra(mesh);
//dbg.draw();
Plotty plot(map);
plot.buildFloorplan();
plot.setGroundTruth(gtPath);
plot.setView(30, 0);
plot.plot();
std::vector<Point2> apPositions{
Settings::data.CurrentPath.NUCs.at(Settings::NUC1).position.xy(),
Settings::data.CurrentPath.NUCs.at(Settings::NUC2).position.xy(),
Settings::data.CurrentPath.NUCs.at(Settings::NUC3).position.xy(),
Settings::data.CurrentPath.NUCs.at(Settings::NUC4).position.xy(),
};
std::vector<WifiMeas> data = filterOfflineData(fr);
const bool UseFTM = false;
const int movAvgWnd = 10;
std::array<MovingAVG<float>, 4> movAvgsFtm { {movAvgWnd,movAvgWnd,movAvgWnd,movAvgWnd} };
std::array<MovingAVG<float>, 4> movAvgsRssi { {movAvgWnd,movAvgWnd,movAvgWnd,movAvgWnd} };
std::vector<float> errorValuesFtm, errorValuesRssi;
std::vector<int> timestamps;
for (const WifiMeas& wifi : data)
{
Point2 gtPos = gtInterpolator.get(static_cast<uint64_t>(wifi.ts.ms())).xy();
plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1));
float distErrorFtm = 0;
float distErrorRssi = 0;
//if (wifi.numSucessMeas() == 4)
{
// FTM
{
std::vector<float> avgDists;
for (size_t i = 0; i < 4; i++)
{
float dist = wifi.ftmDists[i];
if (!isnan(dist))
{
movAvgsFtm[i].add(dist);
}
if (movAvgsFtm[i].getNumUsed() == 0)
{
avgDists.push_back(0);
}
else
{
avgDists.push_back(movAvgsFtm[i].get());
}
}
Point2 estPos = Trilateration::calculateLocation2d(apPositions, avgDists);
plot.setCurEst(Point3(estPos.x, estPos.y, 0.1));
plot.addEstimationNode(Point3(estPos.x, estPos.y, 0.1));
// draw wifi ranges
for (size_t i = 0; i < 4; i++)
{
plot.addCircle(i + 1, apPositions[i], avgDists[i]);
}
// Error
distErrorFtm = gtPos.getDistance(estPos);
errorStats.ftm.add(distErrorFtm);
}
// RSSI
{
std::vector<float> avgDists;
for (size_t i = 0; i < 4; i++)
{
float dist = wifi.rssiDists[i];
if (!isnan(dist))
{
movAvgsRssi[i].add(dist);
}
if (movAvgsRssi[i].getNumUsed() == 0)
{
avgDists.push_back(0);
}
else
{
avgDists.push_back(movAvgsRssi[i].get());
}
}
Point2 estPos = Trilateration::calculateLocation2d(apPositions, avgDists);
plot.addEstimationNode2(Point3(estPos.x, estPos.y, 0.1));
// Error
distErrorRssi = gtPos.getDistance(estPos);
errorStats.rssi.add(distErrorRssi);
}
//std::cout << wifi.ts.ms() << " " << distError << "\n";
errorValuesFtm.push_back(distErrorFtm);
errorValuesRssi.push_back(distErrorRssi);
timestamps.push_back(wifi.ts.ms());
Plotta::Plotta test("test", "C:\\Temp\Plotta\\data.py");
test.add("t", timestamps);
test.add("errorFtm", errorValuesFtm);
test.add("errorRssi", errorValuesRssi);
test.frame();
}
plot.plot();
//Sleep(250);
printf("");
}
std::cout << "Walk error:" << "\n";
std::cout << "[m] " << " mean \t stdDev median" << "\n";
std::cout << "FTM " << errorStats.ftm.getAvg() << "\t" << errorStats.ftm.getStdDev() << "\t" << errorStats.ftm.getMedian() << "\n";
std::cout << "RSSI " << errorStats.rssi.getAvg() << "\t" << errorStats.rssi.getStdDev() << "\t" << errorStats.rssi.getMedian() << "\n";
std::cout << std::endl;
return errorStats;
}
int mainTrilat(int argc, char** argv)
{
// global stats
CombinedStats<float> statsAVG;
CombinedStats<float> statsMedian;
CombinedStats<float> statsSTD;
CombinedStats<float> statsQuantil;
CombinedStats<float> tmp;
std::string evaluationName = "prologic/tmp";
for (size_t walkIdx = 0; walkIdx < 6; walkIdx++)
{
std::cout << "Executing walk " << walkIdx << "\n";
for (int i = 0; i < 1; ++i)
{
std::cout << "Start of iteration " << i << "\n";
tmp = run(Settings::data.CurrentPath, walkIdx, evaluationName);
statsAVG.ftm.add(tmp.ftm.getAvg());
statsMedian.ftm.add(tmp.ftm.getMedian());
statsSTD.ftm.add(tmp.ftm.getStdDev());
statsQuantil.ftm.add(tmp.ftm.getQuantile(0.75));
statsAVG.rssi.add(tmp.rssi.getAvg());
statsMedian.rssi.add(tmp.rssi.getMedian());
statsSTD.rssi.add(tmp.rssi.getStdDev());
statsQuantil.rssi.add(tmp.rssi.getQuantile(0.75));
std::cout << "Iteration " << i << " completed" << std::endl;
}
}
std::cout << "==========================================================" << std::endl;
std::cout << "Average of all statistical data FTM: " << std::endl;
std::cout << "Median: " << statsMedian.ftm.getAvg() << std::endl;
std::cout << "Average: " << statsAVG.ftm.getAvg() << std::endl;
std::cout << "Standard Deviation: " << statsSTD.ftm.getAvg() << std::endl;
std::cout << "75 Quantil: " << statsQuantil.ftm.getAvg() << std::endl;
std::cout << "==========================================================" << std::endl;
std::cout << "==========================================================" << std::endl;
std::cout << "Average of all statistical data RSSI: " << std::endl;
std::cout << "Median: " << statsMedian.rssi.getAvg() << std::endl;
std::cout << "Average: " << statsAVG.rssi.getAvg() << std::endl;
std::cout << "Standard Deviation: " << statsSTD.rssi.getAvg() << std::endl;
std::cout << "75 Quantil: " << statsQuantil.rssi.getAvg() << std::endl;
std::cout << "==========================================================" << std::endl;
return 0;
}

95
code/trilateration.h Normal file
View File

@@ -0,0 +1,95 @@
#pragma once
#include <cmath>
#include <vector>
#include <eigen3/Eigen/Eigen>
#include "..\Indoor\geo\Point2.h"
#include "..\Indoor\geo\Point3.h"
namespace Trilateration
{
// see: https://github.com/Wayne82/Trilateration/blob/master/source/Trilateration.cpp
Point2 calculateLocation2d(const std::vector<Point2>& positions, const std::vector<float>& distances)
{
// To locate position on a 2d plan, have to get at least 3 becaons,
// otherwise return false.
if (positions.size() < 3)
assert(false);
if (positions.size() != distances.size())
assert(false);
// Define the matrix that we are going to use
size_t count = positions.size();
size_t rows = count * (count - 1) / 2;
Eigen::MatrixXd m(rows, 2);
Eigen::VectorXd b(rows);
// Fill in matrix according to the equations
size_t row = 0;
double x1, x2, y1, y2, r1, r2;
for (size_t i = 0; i < count; ++i) {
for (size_t j = i + 1; j < count; ++j) {
x1 = positions[i].x, y1 = positions[i].y;
x2 = positions[j].x, y2 = positions[j].y;
r1 = distances[i];
r2 = distances[j];
m(row, 0) = x1 - x2;
m(row, 1) = y1 - y2;
b(row) = ((pow(x1, 2) - pow(x2, 2)) +
(pow(y1, 2) - pow(y2, 2)) -
(pow(r1, 2) - pow(r2, 2))) / 2;
row++;
}
}
// Then calculate to solve the equations, using the least square solution
Eigen::Vector2d location = m.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
return Point2(location.x(), location.y());
}
Point3 calculateLocation3d(const std::vector<Point3>& positions, const std::vector<float>& distances)
{
// To locate position in a 3D space, have to get at least 4 becaons
if (positions.size() < 4)
assert(false);
if (positions.size() != distances.size())
assert(false);
// Define the matrix that we are going to use
size_t count = positions.size();
size_t rows = count * (count - 1) / 2;
Eigen::MatrixXd m(rows, 3);
Eigen::VectorXd b(rows);
// Fill in matrix according to the equations
size_t row = 0;
double x1, x2, y1, y2, z1, z2, r1, r2;
for (size_t i = 0; i < count; ++i) {
for (size_t j = i + 1; j < count; ++j) {
x1 = positions[i].x, y1 = positions[i].y, z1 = positions[i].z;
x2 = positions[j].x, y2 = positions[j].y, z2 = positions[j].z;
r1 = distances[i];
r2 = distances[j];
m(row, 0) = x1 - x2;
m(row, 1) = y1 - y2;
m(row, 2) = z1 - z2;
b(row) = ((pow(x1, 2) - pow(x2, 2)) +
(pow(y1, 2) - pow(y2, 2)) +
(pow(z1, 2) - pow(z2, 2)) -
(pow(r1, 2) - pow(r2, 2))) / 2;
row++;
}
}
// Then calculate to solve the equations, using the least square solution
Eigen::Vector3d location = m.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
return Point3(location.x(), location.y(), location.z());
}
}