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

Added Path 1

Browse Source
This commit is contained in:
Markus Bullmann
2019-06-11 17:28:33 +02:00
parent c973909580
commit 2aa7d682b3
8 changed files with 95561 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
code/Plotty.h
View File

@@ -622,6 +622,7 @@ public:
splot.getObjects().add(gpol);
}
break;
}
// plot obstacles?

64
code/Settings.h
View File

@@ -11,25 +11,6 @@ namespace Settings {
const int numParticles = 5000;
const int numBSParticles = 50;
const MACAddress NUC1("38:de:ad:6d:77:25");
const MACAddress NUC2("38:de:ad:6d:60:ff");
const MACAddress NUC3("1c:1b:b5:ef:a2:9a");
const MACAddress NUC4("1c:1b:b5:ec:d1:82");
struct NUCSettings
{
int ID;
float ftm_offset;
float rssi_pathloss;
};
const std::unordered_map<MACAddress, NUCSettings> NUCS = {
{ NUC1, { 1, 1.25, 3.375 }},
{ NUC2, { 2, 2.00, 3.000 }},
{ NUC3, { 3, 1.75, 3.375 }},
{ NUC4, { 4, 2.75, 2.750 }}
};
namespace IMU {
const float turnSigma = 2.5; // 3.5
const float stepLength = 1.00;
@@ -113,11 +94,26 @@ namespace Settings {
const std::string errorDir = "../measurements/error/";
/** describes one dataset (map, training, parameter-estimation, ...) */
const MACAddress NUC1("38:de:ad:6d:77:25");
const MACAddress NUC2("38:de:ad:6d:60:ff");
const MACAddress NUC3("1c:1b:b5:ef:a2:9a");
const MACAddress NUC4("1c:1b:b5:ec:d1:82");
struct NUCSettings
{
int ID = 0;
Point3 position = { 0,0,0 };
float ftm_offset = 0.0f;
float rssi_pathloss = 0.0f;
float kalman_measStdDev = 0.0f;
};
struct DataSetup {
std::string map;
std::vector<std::string> training;
std::unordered_map<MACAddress, Point3> APs;
int numGTPoints;
std::unordered_map<MACAddress, NUCSettings> NUCs;
std::vector<int> gtPath;
};
/** all configured datasets */
@@ -129,13 +125,29 @@ namespace Settings {
dataDir + "Pixel2/Path0_0605.csv",
},
{
{ NUC1, { 7.5, 18.7, 0.8} }, // NUC 1
{ NUC2, { 8.6, 26.8, 0.8} }, // NUC 2
{ NUC3, {21.6, 19.1, 0.8} }, // NUC 3
{ NUC4, {20.8, 27.1, 0.8} }, // NUC 4
{ NUC1, {1, { 7.5, 18.7, 0.8}, 1.25, 3.375, 6.156} }, // NUC 1
{ NUC2, {2, { 8.6, 26.8, 0.8}, 2.00, 3.000, 5.650} }, // NUC 2
{ NUC3, {3, {21.6, 19.1, 0.8}, 1.75, 3.375, 6.107} }, // NUC 3
{ NUC4, {4, {20.8, 27.1, 0.8}, 2.75, 2.750, 3.985} }, // NUC 4
},
4
{ 0, 1, 2, 3 }
};
const DataSetup Path1 = {
mapDir + "map2_ap_path1.xml",
{
dataDir + "Pixel2/Path1_7208.csv",
},
{
{ NUC1, {1, { 8.1, 18.7, 0.8}, 0, 0, 0} }, // NUC 1
{ NUC2, {2, { 8.4, 27.3, 0.8}, 0, 0, 0} }, // NUC 2
{ NUC3, {3, {21.3, 19.3, 0.8}, 0, 0, 0} }, // NUC 3
{ NUC4, {4, {20.6, 26.8, 0.8}, 0, 0, 0} }, // NUC 4
},
{ 1, 2, 6, 7, 6, 2, 1 }
};
const DataSetup CurrentPath = Path1;
} data;
}

4
code/filter.h
View File

@@ -308,12 +308,12 @@ public:
|| (true && wifi.second.getAP().getMAC() == Settings::NUC4)
)
{
float rssi_pathloss = Settings::NUCS.at(wifi.second.getAP().getMAC()).rssi_pathloss;
float rssi_pathloss = Settings::data.CurrentPath.NUCs.at(wifi.second.getAP().getMAC()).rssi_pathloss;
float rssiDist = LogDistanceModel::rssiToDistance(-40, rssi_pathloss, wifi.second.getRSSI());
float ftmDist = wifi.second.getFtmDist();
Point3 apPos = Settings::data.Path0.APs.find(wifi.first)->second;
Point3 apPos = Settings::data.CurrentPath.NUCs.find(wifi.first)->second.position;
Point3 particlePos = p.state.pos.pos;
particlePos.z = 1.3; // smartphone h<>he
float apDist = particlePos.getDistance(apPos);

19
code/main.cpp
View File

@@ -41,8 +41,7 @@ static Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std:
Offline::FileReader fr(setup.training[numFile]);
// ground truth
std::vector<int> gtPath;
for(int i = 0; i < setup.numGTPoints; ++i){gtPath.push_back(i);}
std::vector<int> gtPath = setup.gtPath;
Interpolator<uint64_t, Point3> gtInterpolator = fr.getGroundTruthPath(map, gtPath);
Stats::Statistics<float> errorStats;
@@ -70,10 +69,10 @@ static Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std:
// wifi
auto kalmanMap = std::make_shared<std::unordered_map<MACAddress, Kalman>>();
kalmanMap->insert({ Settings::NUC1, Kalman(1, 6.156) });
kalmanMap->insert({ Settings::NUC2, Kalman(2, 5.650) });
kalmanMap->insert({ Settings::NUC3, Kalman(3, 6.107) });
kalmanMap->insert({ Settings::NUC4, Kalman(4, 3.985) });
kalmanMap->insert({ Settings::NUC1, Kalman(1, setup.NUCs.at(Settings::NUC1).kalman_measStdDev) });
kalmanMap->insert({ Settings::NUC2, Kalman(2, setup.NUCs.at(Settings::NUC2).kalman_measStdDev) });
kalmanMap->insert({ Settings::NUC3, Kalman(3, setup.NUCs.at(Settings::NUC3).kalman_measStdDev) });
kalmanMap->insert({ Settings::NUC4, Kalman(4, setup.NUCs.at(Settings::NUC4).kalman_measStdDev) });
// mesh
@@ -144,7 +143,7 @@ static Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std:
if (e.type == Offline::Sensor::WIFI_FTM) {
auto ftm = fr.getWifiFtm()[e.idx].data;
float ftm_offset = Settings::NUCS.at(ftm.getAP().getMAC()).ftm_offset;
float ftm_offset = Settings::data.CurrentPath.NUCs.at(ftm.getAP().getMAC()).ftm_offset;
float ftmDist = ftm.getFtmDist() + ftm_offset; // in m; plus static offset
auto& kalman = kalmanMap->at(ftm.getAP().getMAC());
@@ -204,11 +203,11 @@ static Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std:
// draw wifi ranges
for (auto& ftm : obs.wifi)
{
int nucid = Settings::NUCS.at(ftm.second.getAP().getMAC()).ID;
int nucid = Settings::data.CurrentPath.NUCs.at(ftm.second.getAP().getMAC()).ID;
if (nucid == 1)
{
Point3 apPos = Settings::data.Path0.APs.find(ftm.first)->second;
Point3 apPos = Settings::data.CurrentPath.NUCs.find(ftm.first)->second.position;
//plot.addCircle(nucid, apPos.xy(), ftm.second.getFtmDist());
}
}
@@ -289,7 +288,7 @@ int main(int argc, char** argv)
for(int i = 0; i < 1; ++i){
for(int j = 0; j < 1; ++j){
tmp = run(Settings::data.Path0, j, evaluationName);
tmp = run(Settings::data.CurrentPath, j, evaluationName);
statsMedian.add(tmp.getMedian());
statsAVG.add(tmp.getAvg());
statsSTD.add(tmp.getStdDev());

29
code/mainFtm.cpp
View File

@@ -47,7 +47,7 @@ std::vector<std::tuple<float, float, float>> getFtmValues(Offline::FileReader& f
if (wifi.getAP().getMAC() == nuc)
{
Point3 apPos = Settings::data.Path0.APs.find(wifi.getAP().getMAC())->second;
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();
@@ -152,15 +152,15 @@ void exportFtmValues(Offline::FileReader& fr, Interpolator<uint64_t, Point3>& gt
auto wifi = fr.getWifiFtm()[e.idx].data;
int nucid = Settings::NUCS.at(wifi.getAP().getMAC()).ID;
float ftm_offset = Settings::NUCS.at(wifi.getAP().getMAC()).ftm_offset;
float rssi_pathloss = Settings::NUCS.at(wifi.getAP().getMAC()).rssi_pathloss;
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();
Point3 apPos = Settings::data.Path0.APs.find(wifi.getAP().getMAC())->second;
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 << "\n";
@@ -180,8 +180,7 @@ static Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std:
Offline::FileReader fr(setup.training[numFile]);
// ground truth
std::vector<int> gtPath;
for(int i = 0; i < setup.numGTPoints; ++i){gtPath.push_back(i);}
std::vector<int> gtPath = setup.gtPath;
Interpolator<uint64_t, Point3> gtInterpolator = fr.getGroundTruthPath(map, gtPath);
Stats::Statistics<float> errorStats;
@@ -209,10 +208,10 @@ static Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std:
// wifi
auto kalmanMap = std::make_shared<std::unordered_map<MACAddress, Kalman>>();
kalmanMap->insert({ Settings::NUC1, Kalman(1, 6.156) });
kalmanMap->insert({ Settings::NUC2, Kalman(2, 5.650) });
kalmanMap->insert({ Settings::NUC3, Kalman(3, 6.107) });
kalmanMap->insert({ Settings::NUC4, Kalman(4, 3.985) });
kalmanMap->insert({ Settings::NUC1, Kalman(1, setup.NUCs.at(Settings::NUC1).kalman_measStdDev) });
kalmanMap->insert({ Settings::NUC2, Kalman(2, setup.NUCs.at(Settings::NUC2).kalman_measStdDev) });
kalmanMap->insert({ Settings::NUC3, Kalman(3, setup.NUCs.at(Settings::NUC3).kalman_measStdDev) });
kalmanMap->insert({ Settings::NUC4, Kalman(4, setup.NUCs.at(Settings::NUC4).kalman_measStdDev) });
// mesh
NM::NavMeshSettings set;
@@ -290,7 +289,7 @@ static Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std:
if (e.type == Offline::Sensor::WIFI_FTM) {
auto ftm = fr.getWifiFtm()[e.idx].data;
float ftm_offset = Settings::NUCS.at(ftm.getAP().getMAC()).ftm_offset;
float ftm_offset = Settings::data.CurrentPath.NUCs.at(ftm.getAP().getMAC()).ftm_offset;
float ftmDist = ftm.getFtmDist() + ftm_offset; // in m; plus static offset
auto& kalman = kalmanMap->at(ftm.getAP().getMAC());
@@ -323,11 +322,11 @@ static Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std:
// draw wifi ranges
for (auto& ftm : obs.wifi)
{
int nucid = Settings::NUCS.at(ftm.second.getAP().getMAC()).ID;
int nucid = Settings::data.CurrentPath.NUCs.at(ftm.second.getAP().getMAC()).ID;
if (nucid == 1)
{
Point3 apPos = Settings::data.Path0.APs.find(ftm.first)->second;
Point3 apPos = Settings::data.CurrentPath.NUCs.find(ftm.first)->second.position;
// plot.addCircle(nucid, apPos.xy(), ftm.second.getFtmDist());
}
}
@@ -402,7 +401,7 @@ int mainFtm(int argc, char** argv) {
for(int i = 0; i < 1; ++i){
for(int j = 0; j < 1; ++j){
tmp = run(Settings::data.Path0, j, evaluationName);
tmp = run(Settings::data.CurrentPath, j, evaluationName);
statsMedian.add(tmp.getMedian());
statsAVG.add(tmp.getAvg());
statsSTD.add(tmp.getStdDev());