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

Reworked trilat code

Browse Source
This commit is contained in:
Markus Bullmann
2019-11-27 16:59:03 +01:00
parent 992b8edc60
commit 3433bdaf66
5 changed files with 394 additions and 238 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
code/CMakeLists.txt
View File

@@ -22,6 +22,8 @@ INCLUDE_DIRECTORIES(
../../
../../../
../../../../
../../eigen3
)
@@ -49,6 +51,7 @@ FILE(GLOB SOURCES
mainProb.cpp
Eval.cpp
FtmKalman.cpp
trilateration.cpp
)

3
code/main.cpp
View File

@@ -737,7 +737,7 @@ int main(int argc, char** argv)
//Settings::data.Path10,
//Settings::data.Path11
//Settings::data.Path20,
Settings::data.Path21,
//Settings::data.Path21,
Settings::data.Path22,
};
@@ -745,7 +745,6 @@ int main(int argc, char** argv)
{
Settings::CurrentPath = setupToRun;
for (size_t walkIdx = 0; walkIdx < Settings::CurrentPath.training.size(); walkIdx++)
{
std::cout << "Executing walk " << walkIdx << "\n";

218
code/mainTrilat.cpp
View File

@@ -35,20 +35,29 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
{
// 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
Interpolator<uint64_t, Point3> gtInterpolator = fr.getGroundTruthPath(map, setup.gtPath);
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;
double gtTotalDistance = 0;
Stats::Statistics<double> gtWalkingSpeed;
for (int i = 1; i < gtEntries.size(); ++i) {
distance += gtEntries[i].value.getDistance(gtEntries[i - 1].value);
double distance = gtEntries[i].value.getDistance(gtEntries[i - 1].value);
double timeDiff = static_cast<double>(gtEntries[i].key - gtEntries[i - 1].key);
double walkingSpeed = distance / (timeDiff / 1000.0f); // m / s
gtWalkingSpeed.add(walkingSpeed);
gtTotalDistance += distance;
}
std::cout << "Distance of Path: " << distance << std::endl;
std::cout << "Distance of Path: " << gtTotalDistance << std::endl;
std::cout << "GT walking speed: " << gtWalkingSpeed.getAvg() << "m/s (" << gtWalkingSpeed.getAvg()*3.6f << "km/h)" << std::endl;
// debug show
//MeshPlotter dbg;
@@ -60,138 +69,139 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
Plotty plot(map);
plot.buildFloorplan();
plot.setGroundTruth(setup.gtPath);
plot.setGroundTruth(gtPath);
plot.setView(30, 0);
// APs Positions
for (auto& nucConfig : setup.NUCs)
{
plot.addCircle(10000 + nucConfig.second.ID, nucConfig.second.position.xy(), 0.1);
}
plot.plot();
std::vector<WiFiMeasurement> obs;
Timestamp lastTimestamp = Timestamp::fromMS(0);
Plotta::Plotta plotta("test", "C:\\Temp\\Plotta\\dataTrilat.py");
//plotta.add("apPos", apPositions);
std::vector<Point2> apPositions{
Settings::CurrentPath.NUCs.at(Settings::NUC1).position.xy(),
Settings::CurrentPath.NUCs.at(Settings::NUC2).position.xy(),
Settings::CurrentPath.NUCs.at(Settings::NUC3).position.xy(),
Settings::CurrentPath.NUCs.at(Settings::NUC4).position.xy(),
};
plotta.add("apPos", apPositions);
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::unordered_map<MACAddress, MovingAVG<float>> movAvgsFtm;
std::unordered_map<MACAddress, MovingAVG<float>> movAvgsRssi;
for (auto& nucConfig : setup.NUCs)
{
movAvgsFtm.insert({ nucConfig.first, MovingAVG<float>(movAvgWnd) });
movAvgsRssi.insert({ nucConfig.first, MovingAVG<float>(movAvgWnd) });
}
std::vector<float> errorValuesFtm, errorValuesRssi;
std::vector<int> timestamps;
std::vector<Point2> gtPath, estPathFtm, estPathRssi;
std::vector<Point2> estPathFtm, estPathRssi;
for (const WifiMeas& wifi : data)
for (const Offline::Entry& e : fr.getEntries())
{
Point2 gtPos = gtInterpolator.get(static_cast<uint64_t>(wifi.ts.ms())).xy();
if (e.type != Offline::Sensor::WIFI_FTM) {
continue;
}
// TIME
const Timestamp ts = Timestamp::fromMS(e.ts);
auto wifiFtm = fr.getWifiFtm()[e.idx].data;
obs.push_back(wifiFtm);
if (ts - lastTimestamp >= Timestamp::fromMS(500))
{
// Do update
Point2 gtPos = gtInterpolator.get(static_cast<uint64_t>(ts.ms())).xy();
plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1));
gtPath.push_back(gtPos);
float distErrorFtm = 0;
float distErrorRssi = 0;
std::unordered_map<MACAddress, std::pair<float, float>> apPosDistMap;
//if (wifi.numSucessMeas() == 4)
for (const WiFiMeasurement& wifi : obs)
{
// FTM
{
std::vector<float> avgDists;
if (wifi.getNumSuccessfulMeasurements() < 3)
continue;
for (size_t i = 0; i < 4; i++)
{
float dist = wifi.ftmDists[i];
const MACAddress& mac = wifi.getAP().getMAC();
float ftm_offset = setup.NUCs.at(mac).ftm_offset;
float ftmDist = wifi.getFtmDist() + ftm_offset;
if (!isnan(dist))
{
movAvgsFtm[i].add(dist);
float rssi_pathloss = setup.NUCs.at(mac).rssi_pathloss;
float rssiDist = LogDistanceModel::rssiToDistance(-40, rssi_pathloss, wifi.getRSSI());
movAvgsFtm[mac].add(ftmDist);
movAvgsRssi[mac].add(rssiDist);
apPosDistMap[mac] = { movAvgsFtm[mac].get(), movAvgsRssi[mac].get() };
}
if (movAvgsFtm[i].getNumUsed() == 0)
if (apPosDistMap.size() > 3)
{
avgDists.push_back(0);
}
else
// Do update for real
std::vector<Point2> apPositions;
std::vector<float> ftmDists;
std::vector<float> rssiDists;
for (const auto& kvp : apPosDistMap)
{
avgDists.push_back(movAvgsFtm[i].get());
}
apPositions.push_back(setup.NUCs.at(kvp.first).position.xy());
ftmDists.push_back(kvp.second.first);
rssiDists.push_back(kvp.second.second);
}
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]);
}
Point2 estFtmPos = Trilateration::levenbergMarquardt(apPositions, ftmDists);
Point2 estRssiPos = Trilateration::levenbergMarquardt(apPositions, rssiDists);
// Error
distErrorFtm = gtPos.getDistance(estPos);
float distErrorFtm = gtPos.getDistance(estFtmPos);
errorStats.ftm.add(distErrorFtm);
estPathFtm.push_back(estPos);
}
estPathFtm.push_back(estFtmPos);
// 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);
float distErrorRssi = gtPos.getDistance(estRssiPos);
errorStats.rssi.add(distErrorRssi);
estPathRssi.push_back(estPos);
}
//std::cout << wifi.ts.ms() << " " << distError << "\n";
estPathRssi.push_back(estRssiPos);
errorValuesFtm.push_back(distErrorFtm);
errorValuesRssi.push_back(distErrorRssi);
timestamps.push_back(wifi.ts.ms());
timestamps.push_back(ts.ms());
plotta.add("t", timestamps);
plotta.add("errorFtm", errorValuesFtm);
plotta.add("errorRssi", errorValuesRssi);
plotta.frame();
// Plot
plot.setCurEst(Point3(estFtmPos.x, estFtmPos.y, 0.1));
plot.addEstimationNode(Point3(estFtmPos.x, estFtmPos.y, 0.1));
plot.addEstimationNode2(Point3(estRssiPos.x, estRssiPos.y, 0.1));
// draw wifi ranges
if (Settings::PlotCircles)
{
plot.clearDistanceCircles();
for (size_t i = 0; i < ftmDists.size(); i++)
{
plot.addDistanceCircle(apPositions[i], ftmDists[i], K::GnuplotColor::fromRGB(255, 0, 0));
plot.addDistanceCircle(apPositions[i], rssiDists[i], K::GnuplotColor::fromRGB(0, 255, 0));
}
}
plot.plot();
//Sleep(250);
Sleep(100);
}
obs.clear();
lastTimestamp = ts;
}
printf("");
}
plotta.add("gtPath", gtPath);
plotta.add("estPathFtm", estPathFtm);
plotta.add("estPathRssi", estPathRssi);
plotta.frame();
@@ -211,7 +221,23 @@ int mainTrilat(int argc, char** argv)
CombinedStats<float> statsQuantil;
CombinedStats<float> tmp;
std::string evaluationName = "prologic/tmp";
std::string evaluationName = "prologic/trilat";
std::vector<Settings::DataSetup> setupsToRun = {
//Settings::data.Path5,
//Settings::data.Path7,
//Settings::data.Path8,
//Settings::data.Path9,
//Settings::data.Path10,
//Settings::data.Path11
//Settings::data.Path20,
Settings::data.Path21,
//Settings::data.Path22,
};
for (Settings::DataSetup setupToRun : setupsToRun)
{
Settings::CurrentPath = setupToRun;
for (size_t walkIdx = 0; walkIdx < Settings::CurrentPath.training.size(); walkIdx++)
{
@@ -236,6 +262,7 @@ int mainTrilat(int argc, char** argv)
}
}
std::cout << "Results for path " << Settings::CurrentPath.name << std::endl;
std::cout << "==========================================================" << std::endl;
std::cout << "Average of all statistical data FTM: " << std::endl;
std::cout << "Median: " << statsMedian.ftm.getAvg() << std::endl;
@@ -251,6 +278,7 @@ int mainTrilat(int argc, char** argv)
std::cout << "Standard Deviation: " << statsSTD.rssi.getAvg() << std::endl;
std::cout << "75 Quantil: " << statsQuantil.rssi.getAvg() << std::endl;
std::cout << "==========================================================" << std::endl;
}
return 0;
}

207
code/trilateration.cpp Normal file
View File

@@ -0,0 +1,207 @@
#include "trilateration.h"
#include <cmath>
#include <iostream>
#include <Eigen/Eigen>
#include <unsupported/Eigen/NonLinearOptimization>
#include <unsupported/Eigen/NumericalDiff>
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);
Eigen::Vector2d pseudoInv = (m.transpose()*m).inverse() * m.transpose() *b;
return Point2(pseudoInv.x(), pseudoInv.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());
}
// Generic functor
// See http://eigen.tuxfamily.org/index.php?title=Functors
// C++ version of a function pointer that stores meta-data about the function
template<typename _Scalar, int NX = Eigen::Dynamic, int NY = Eigen::Dynamic>
struct Functor
{
// Information that tells the caller the numeric type (eg. double) and size (input / output dim)
typedef _Scalar Scalar;
enum { // Required by numerical differentiation module
InputsAtCompileTime = NX,
ValuesAtCompileTime = NY
};
// Tell the caller the matrix sizes associated with the input, output, and jacobian
typedef Eigen::Matrix<Scalar, InputsAtCompileTime, 1> InputType;
typedef Eigen::Matrix<Scalar, ValuesAtCompileTime, 1> ValueType;
typedef Eigen::Matrix<Scalar, ValuesAtCompileTime, InputsAtCompileTime> JacobianType;
// Local copy of the number of inputs
int m_inputs, m_values;
// Two constructors:
Functor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {}
Functor(int inputs, int values) : m_inputs(inputs), m_values(values) {}
// Get methods for users to determine function input and output dimensions
int inputs() const { return m_inputs; }
int values() const { return m_values; }
};
struct DistanceFunction : Functor<double>
{
private:
const std::vector<Point2>& positions;
const std::vector<float>& distances;
public:
DistanceFunction(const std::vector<Point2>& positions, const std::vector<float>& distances)
: Functor<double>(positions.size(), positions.size()), positions(positions), distances(distances)
{}
int operator()(const Eigen::VectorXd &x, Eigen::VectorXd &fvec) const
{
const Point2 p(x(0), x(1));
for (size_t i = 0; i < positions.size(); i++)
{
fvec(i) = p.getDistance(positions[i]) - distances[i];
}
return 0;
}
};
struct DistanceFunctionDiff : public Eigen::NumericalDiff<DistanceFunction>
{
DistanceFunctionDiff(const DistanceFunction& functor)
: Eigen::NumericalDiff<DistanceFunction>(functor, 1.0e-6)
{}
};
Point2 levenbergMarquardt(const std::vector<Point2>& positions, const std::vector<float>& distances)
{
Point2 pseudoInvApprox = calculateLocation2d(positions, distances);
Eigen::Vector2d initVal;
initVal << pseudoInvApprox.x, pseudoInvApprox.y;
Eigen::Vector2d startVal;
//startVal << pseudoInvApprox.x, pseudoInvApprox.y;
startVal << 0, 0;
DistanceFunction functor(positions, distances);
DistanceFunctionDiff numDiff(functor);
Eigen::LevenbergMarquardt<DistanceFunctionDiff, double> lm(numDiff);
lm.parameters.maxfev = 2000;
lm.parameters.xtol = 1.0e-10;
std::cout << lm.parameters.maxfev << std::endl;
Eigen::VectorXd z = startVal;
int ret = lm.minimize(z);
std::cout << "iter count: " << lm.iter << std::endl;
std::cout << "return status: " << ret << std::endl;
std::cout << "zSolver: " << z.transpose() << std::endl;
std::cout << "pseudoInv: " << initVal.transpose() << std::endl;
Point2 bla(z(0), z(1));
double errPseudo = 0;
double errLeven = 0;
for (size_t i = 0; i < positions.size(); i++)
{
double d1 = pseudoInvApprox.getDistance(positions[i]) - distances[i];
errPseudo += d1 * d1;
double d2 = bla.getDistance(positions[i]) - distances[i];
errLeven += d2 * d2;
}
//assert(errLeven <= errPseudo);
std::cout << "err pseud: " << errPseudo << std::endl;
std::cout << "err leven: " << errLeven << std::endl << std::endl;
return Point2(z(0), z(1));
}
}

89
code/trilateration.h
View File

@@ -1,95 +1,14 @@
#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);
Point3 calculateLocation3d(const std::vector<Point3>& positions, const std::vector<float>& distances);
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());
}
}
Point2 levenbergMarquardt(const std::vector<Point2>& positions, const std::vector<float>& distances);
};