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

Added moving average

Browse Source
This commit is contained in:
Markus Bullmann
2019-07-02 10:36:24 +02:00
parent 2c4d0beacc
commit 4415288cda
5 changed files with 263 additions and 181 deletions
Download Patch File Download Diff File Expand all files Collapse all files

150
code/mainFtm.cpp
View File

@@ -29,150 +29,6 @@
#include <sys/stat.h>
using namespace std::chrono_literals;
std::vector<std::tuple<float, float, float>> getFtmValues(Offline::FileReader& fr, Interpolator<uint64_t, Point3>& gtInterpolator, const MACAddress nuc)
{
std::vector<std::tuple<float, float, float>> result;
for (const Offline::Entry& e : fr.getEntries())
{
if (e.type == Offline::Sensor::WIFI_FTM)
{
const Timestamp ts = Timestamp::fromMS(e.ts);
Point3 gtPos = gtInterpolator.get(static_cast<uint64_t>(ts.ms())) + Point3(0, 0, 1.3);
auto wifi = fr.getWifiFtm()[e.idx].data;
if (wifi.getAP().getMAC() == nuc)
{
Point3 apPos = Settings::data.CurrentPath.NUCs.find(wifi.getAP().getMAC())->second.position;
float apDist = gtPos.getDistance(apPos);
float ftmDist = wifi.getFtmDist();
float rssi = wifi.getRSSI();
result.push_back({ apDist, ftmDist, rssi });
}
}
}
return result;
}
std::pair<float, float> optimizeFtm(std::vector<std::tuple<float, float, float>>& values)
{
std::vector<std::pair<float, float>> error;
for (float offset = 0; offset < 10.0f; offset += 0.25)
{
Stats::Statistics<float> diffs;
for (const auto& data : values)
{
float apDist = std::get<0>(data);
float ftmDist = std::get<1>(data);
ftmDist += offset;
float diff = (apDist - ftmDist);
diffs.add(diff);
}
error.push_back({ offset, diffs.getSquaredSumAvg() });
}
auto minElement = std::min_element(error.begin(), error.end(), [](std::pair<float, float> a, std::pair<float, float> b) {
return a.second < b.second;
});
std::cout << "Min ftm offset \t" << minElement->first << "\t" << minElement->second << "\n";
return *minElement;
}
std::pair<float, float> optimizeRssi(std::vector<std::tuple<float, float, float>>& values)
{
std::vector<std::pair<float, float>> error;
for (float pathLoss = 2.0f; pathLoss < 4.0f; pathLoss += 0.125)
{
Stats::Statistics<float> diffs;
for (const auto& data : values)
{
float apDist = std::get<0>(data);
float rssi = std::get<2>(data);
float rssiDist = LogDistanceModel::rssiToDistance(-40, pathLoss, rssi);
float diff = (apDist - rssiDist);
diffs.add(diff);
}
error.push_back({ pathLoss, diffs.getSquaredSumAvg() });
}
auto minElement = std::min_element(error.begin(), error.end(), [](std::pair<float, float> a, std::pair<float, float> b) {
return a.second < b.second;
});
std::cout << "Min path loss \t" << minElement->first << "\t" << minElement->second << "\n";
return *minElement;
}
void optimize(Offline::FileReader& fr, Interpolator<uint64_t, Point3>& gtInterpolator)
{
int i = 1;
for (auto nuc : {Settings::NUC1, Settings::NUC2, Settings::NUC3, Settings::NUC4})
{
auto values = getFtmValues(fr, gtInterpolator, nuc);
std::cout << "NUC" << i++ << "\n";
optimizeFtm(values);
optimizeRssi(values);
}
}
void exportFtmValues(Offline::FileReader& fr, Interpolator<uint64_t, Point3>& gtInterpolator)
{
std::fstream fs;
fs.open("test.txt", std::fstream::out);
fs << "timestamp;nucid;dist;rssiDist;ftmDist;ftmStdDev;numMeas;numSuccesMeas" << "\n";
for (const Offline::Entry& e : fr.getEntries())
{
if (e.type == Offline::Sensor::WIFI_FTM)
{
const Timestamp ts = Timestamp::fromMS(e.ts);
Point3 gtPos = gtInterpolator.get(static_cast<uint64_t>(ts.ms())) + Point3(0, 0, 1.3);
auto wifi = fr.getWifiFtm()[e.idx].data;
int nucid = Settings::data.CurrentPath.NUCs.at(wifi.getAP().getMAC()).ID;
float ftm_offset = Settings::data.CurrentPath.NUCs.at(wifi.getAP().getMAC()).ftm_offset;
float rssi_pathloss = Settings::data.CurrentPath.NUCs.at(wifi.getAP().getMAC()).rssi_pathloss;
float rssiDist = LogDistanceModel::rssiToDistance(-40, rssi_pathloss, wifi.getRSSI());
float ftmDist = wifi.getFtmDist() + ftm_offset; //in m; plus static offset
float ftmStdDev = wifi.getFtmDistStd();
int numMeas = wifi.getNumAttemptedMeasurements();
int numSuccessMeas = wifi.getNumSuccessfulMeasurements();
Point3 apPos = Settings::data.CurrentPath.NUCs.find(wifi.getAP().getMAC())->second.position;
float apDist = gtPos.getDistance(apPos);
fs << ts.ms() << ";" << nucid << ";" << apDist << ";" << rssiDist << ";" << ftmDist << ";" << ftmStdDev << ";" << numMeas << ";" << numSuccessMeas << "\n";
}
}
fs.close();
}
static Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string folder) {
// reading file
@@ -276,12 +132,6 @@ static Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std:
relBaro.setCalibrationTimeframe( Timestamp::fromMS(5000) );
Timestamp lastTimestamp = Timestamp::fromMS(0);
//optimize(fr, gtInterpolator);
//return errorStats;
int i = 0;
//exportFtmValues(fr, gtInterpolator);
// parse each sensor-value within the offline data
for (const Offline::Entry& e : fr.getEntries()) {