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

330
code/mainTrilat.cpp
View File

@@ -35,20 +35,29 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
{ {
// reading file // reading file
Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(setup.map); Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(setup.map);
Offline::FileReader fr(setup.training[walkIdx]); Offline::FileReader fr(setup.training[walkIdx], setup.HasNanoSecondTimestamps);
// ground truth // 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; CombinedStats<float> errorStats;
//calculate distance of path //calculate distance of path
std::vector<Interpolator<uint64_t, Point3>::InterpolatorEntry> gtEntries = gtInterpolator.getEntries(); 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) { 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 // debug show
//MeshPlotter dbg; //MeshPlotter dbg;
@@ -60,138 +69,139 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
Plotty plot(map); Plotty plot(map);
plot.buildFloorplan(); plot.buildFloorplan();
plot.setGroundTruth(setup.gtPath); plot.setGroundTruth(gtPath);
plot.setView(30, 0); 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(); plot.plot();
std::vector<WiFiMeasurement> obs;
Timestamp lastTimestamp = Timestamp::fromMS(0);
Plotta::Plotta plotta("test", "C:\\Temp\\Plotta\\dataTrilat.py"); 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; const int movAvgWnd = 10;
std::array<MovingAVG<float>, 4> movAvgsFtm { {movAvgWnd,movAvgWnd,movAvgWnd,movAvgWnd} }; std::unordered_map<MACAddress, MovingAVG<float>> movAvgsFtm;
std::array<MovingAVG<float>, 4> movAvgsRssi { {movAvgWnd,movAvgWnd,movAvgWnd,movAvgWnd} }; 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<float> errorValuesFtm, errorValuesRssi;
std::vector<int> timestamps; 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) {
plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1)); continue;
gtPath.push_back(gtPos); }
float distErrorFtm = 0; // TIME
float distErrorRssi = 0; const Timestamp ts = Timestamp::fromMS(e.ts);
//if (wifi.numSucessMeas() == 4) auto wifiFtm = fr.getWifiFtm()[e.idx].data;
{ obs.push_back(wifiFtm);
// FTM
{ if (ts - lastTimestamp >= Timestamp::fromMS(500))
std::vector<float> avgDists; {
// Do update
for (size_t i = 0; i < 4; i++) Point2 gtPos = gtInterpolator.get(static_cast<uint64_t>(ts.ms())).xy();
{ plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1));
float dist = wifi.ftmDists[i];
std::unordered_map<MACAddress, std::pair<float, float>> apPosDistMap;
if (!isnan(dist))
{ for (const WiFiMeasurement& wifi : obs)
movAvgsFtm[i].add(dist); {
} if (wifi.getNumSuccessfulMeasurements() < 3)
continue;
if (movAvgsFtm[i].getNumUsed() == 0) const MACAddress& mac = wifi.getAP().getMAC();
{ float ftm_offset = setup.NUCs.at(mac).ftm_offset;
avgDists.push_back(0); float ftmDist = wifi.getFtmDist() + ftm_offset;
}
else float rssi_pathloss = setup.NUCs.at(mac).rssi_pathloss;
{ float rssiDist = LogDistanceModel::rssiToDistance(-40, rssi_pathloss, wifi.getRSSI());
avgDists.push_back(movAvgsFtm[i].get());
} movAvgsFtm[mac].add(ftmDist);
} movAvgsRssi[mac].add(rssiDist);
Point2 estPos = Trilateration::calculateLocation2d(apPositions, avgDists); apPosDistMap[mac] = { movAvgsFtm[mac].get(), movAvgsRssi[mac].get() };
}
plot.setCurEst(Point3(estPos.x, estPos.y, 0.1));
plot.addEstimationNode(Point3(estPos.x, estPos.y, 0.1)); if (apPosDistMap.size() > 3)
{
// draw wifi ranges // Do update for real
for (size_t i = 0; i < 4; i++) std::vector<Point2> apPositions;
{ std::vector<float> ftmDists;
plot.addCircle(i + 1, apPositions[i], avgDists[i]); std::vector<float> rssiDists;
}
for (const auto& kvp : apPosDistMap)
// Error {
distErrorFtm = gtPos.getDistance(estPos); apPositions.push_back(setup.NUCs.at(kvp.first).position.xy());
errorStats.ftm.add(distErrorFtm); ftmDists.push_back(kvp.second.first);
estPathFtm.push_back(estPos); rssiDists.push_back(kvp.second.second);
} }
Point2 estFtmPos = Trilateration::levenbergMarquardt(apPositions, ftmDists);
// RSSI Point2 estRssiPos = Trilateration::levenbergMarquardt(apPositions, rssiDists);
{
std::vector<float> avgDists; // Error
float distErrorFtm = gtPos.getDistance(estFtmPos);
for (size_t i = 0; i < 4; i++) errorStats.ftm.add(distErrorFtm);
{ estPathFtm.push_back(estFtmPos);
float dist = wifi.rssiDists[i];
float distErrorRssi = gtPos.getDistance(estRssiPos);
if (!isnan(dist)) errorStats.rssi.add(distErrorRssi);
{ estPathRssi.push_back(estRssiPos);
movAvgsRssi[i].add(dist);
} errorValuesFtm.push_back(distErrorFtm);
errorValuesRssi.push_back(distErrorRssi);
timestamps.push_back(ts.ms());
if (movAvgsRssi[i].getNumUsed() == 0)
{ plotta.add("t", timestamps);
avgDists.push_back(0); plotta.add("errorFtm", errorValuesFtm);
} plotta.add("errorRssi", errorValuesRssi);
else plotta.frame();
{
avgDists.push_back(movAvgsRssi[i].get()); // 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));
Point2 estPos = Trilateration::calculateLocation2d(apPositions, avgDists);
// draw wifi ranges
plot.addEstimationNode2(Point3(estPos.x, estPos.y, 0.1)); if (Settings::PlotCircles)
{
// Error plot.clearDistanceCircles();
distErrorRssi = gtPos.getDistance(estPos);
errorStats.rssi.add(distErrorRssi); for (size_t i = 0; i < ftmDists.size(); i++)
estPathRssi.push_back(estPos); {
} plot.addDistanceCircle(apPositions[i], ftmDists[i], K::GnuplotColor::fromRGB(255, 0, 0));
plot.addDistanceCircle(apPositions[i], rssiDists[i], K::GnuplotColor::fromRGB(0, 255, 0));
//std::cout << wifi.ts.ms() << " " << distError << "\n"; }
}
errorValuesFtm.push_back(distErrorFtm);
errorValuesRssi.push_back(distErrorRssi);
timestamps.push_back(wifi.ts.ms()); plot.plot();
Sleep(100);
plotta.add("t", timestamps); }
plotta.add("errorFtm", errorValuesFtm);
plotta.add("errorRssi", errorValuesRssi); obs.clear();
plotta.frame(); lastTimestamp = ts;
} }
plot.plot();
//Sleep(250);
printf(""); printf("");
} }
plotta.add("gtPath", gtPath);
plotta.add("estPathFtm", estPathFtm); plotta.add("estPathFtm", estPathFtm);
plotta.add("estPathRssi", estPathRssi); plotta.add("estPathRssi", estPathRssi);
plotta.frame(); plotta.frame();
@@ -211,46 +221,64 @@ int mainTrilat(int argc, char** argv)
CombinedStats<float> statsQuantil; CombinedStats<float> statsQuantil;
CombinedStats<float> tmp; CombinedStats<float> tmp;
std::string evaluationName = "prologic/tmp"; std::string evaluationName = "prologic/trilat";
for (size_t walkIdx = 0; walkIdx < Settings::CurrentPath.training.size(); walkIdx++) 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)
{ {
std::cout << "Executing walk " << walkIdx << "\n"; Settings::CurrentPath = setupToRun;
for (int i = 0; i < 1; ++i)
for (size_t walkIdx = 0; walkIdx < Settings::CurrentPath.training.size(); walkIdx++)
{ {
std::cout << "Start of iteration " << i << "\n"; std::cout << "Executing walk " << walkIdx << "\n";
for (int i = 0; i < 1; ++i)
{
std::cout << "Start of iteration " << i << "\n";
tmp = run(Settings::CurrentPath, walkIdx, evaluationName); tmp = run(Settings::CurrentPath, walkIdx, evaluationName);
statsAVG.ftm.add(tmp.ftm.getAvg()); statsAVG.ftm.add(tmp.ftm.getAvg());
statsMedian.ftm.add(tmp.ftm.getMedian()); statsMedian.ftm.add(tmp.ftm.getMedian());
statsSTD.ftm.add(tmp.ftm.getStdDev()); statsSTD.ftm.add(tmp.ftm.getStdDev());
statsQuantil.ftm.add(tmp.ftm.getQuantile(0.75)); statsQuantil.ftm.add(tmp.ftm.getQuantile(0.75));
statsAVG.rssi.add(tmp.rssi.getAvg()); statsAVG.rssi.add(tmp.rssi.getAvg());
statsMedian.rssi.add(tmp.rssi.getMedian()); statsMedian.rssi.add(tmp.rssi.getMedian());
statsSTD.rssi.add(tmp.rssi.getStdDev()); statsSTD.rssi.add(tmp.rssi.getStdDev());
statsQuantil.rssi.add(tmp.rssi.getQuantile(0.75)); statsQuantil.rssi.add(tmp.rssi.getQuantile(0.75));
std::cout << "Iteration " << i << " completed" << std::endl; std::cout << "Iteration " << i << " completed" << std::endl;
}
} }
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;
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;
} }
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; 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 #pragma once
#include <cmath>
#include <vector> #include <vector>
#include <eigen3/Eigen/Eigen>
#include <Indoor/geo/Point2.h> #include <Indoor/geo/Point2.h>
#include <Indoor/geo/Point3.h> #include <Indoor/geo/Point3.h>
namespace Trilateration 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) Point2 levenbergMarquardt(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());
}
}