FHWS/museumLoc
Archived
3
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

aktuell spielerein

Browse Source
This commit is contained in:
toni
2018-08-01 10:08:33 +02:00
parent 1e52b377dc
commit 38042f8dad
5 changed files with 145 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@@ -75,7 +75,7 @@ endif()
add_definitions( add_definitions(
# -DWITH_TESTS # -DWITH_TESTS
-DWITH_ASSERTIONS -DWITH_ASSERTIONS
# -DWITH_DEBUG_LOG -DWITH_DEBUG_LOG
# -DWITH_DEBUG_PLOT # -DWITH_DEBUG_PLOT
# -D_GLIBCXX_DEBUG # -D_GLIBCXX_DEBUG
) )

4
Plotty.h
View File

@@ -526,6 +526,10 @@ public:
gp.flush(); gp.flush();
} }
void closeStream(){
gp.close();
}
void saveToFile(std::ofstream& stream){ void saveToFile(std::ofstream& stream){
gp.draw(splot); gp.draw(splot);
stream << "set terminal x11 size 2000,1500\n"; stream << "set terminal x11 size 2000,1500\n";

25
Settings.h
View File

@@ -9,7 +9,7 @@ namespace Settings {
bool useKLB = false; bool useKLB = false;
const int numParticles = 5000; const int numParticles = 1000;
const int numBSParticles = 50; const int numBSParticles = 50;
namespace IMU { namespace IMU {
@@ -41,6 +41,11 @@ namespace Settings {
const Point2 bandwidth(100,100); const Point2 bandwidth(100,100);
} }
namespace KDE3D {
const Point3 bandwidth(1, 1, 1);
const Point3 gridSize(0.2, 0.2, 1); // in meter
}
//const GridPoint destination = GridPoint(70*100, 35*100, 0*100); // use destination //const GridPoint destination = GridPoint(70*100, 35*100, 0*100); // use destination
const GridPoint destination = GridPoint(0,0,0); // do not use destination const GridPoint destination = GridPoint(0,0,0); // do not use destination
@@ -56,7 +61,7 @@ namespace Settings {
constexpr float WAF = -11.0; constexpr float WAF = -11.0;
const bool optimize = true; const bool optimize = true;
const bool useRegionalOpt = true; const bool useRegionalOpt = false;
// how to perform VAP grouping. see // how to perform VAP grouping. see
// - calibration in Controller.cpp // - calibration in Controller.cpp
@@ -84,13 +89,13 @@ namespace Settings {
constexpr bool useMainThread = false; // perform filtering in the main thread constexpr bool useMainThread = false; // perform filtering in the main thread
} }
// const std::string mapDir = "../map/"; const std::string mapDir = "../map/";
// const std::string dataDir = "../measurements/"; const std::string dataDir = "../measurements/";
// const std::string errorDir = dataDir + "results/"; const std::string errorDir = dataDir + "results/";
const std::string mapDir = "/apps/museum/maps/"; // const std::string mapDir = "/apps/museum/maps/";
const std::string dataDir = "/apps/"; // const std::string dataDir = "/apps/";
const std::string errorDir = dataDir + "museum/results/"; // const std::string errorDir = dataDir + "museum/results/";
/** describes one dataset (map, training, parameter-estimation, ...) */ /** describes one dataset (map, training, parameter-estimation, ...) */
struct DataSetup { struct DataSetup {
@@ -164,7 +169,7 @@ namespace Settings {
DataSetup Path3 = { DataSetup Path3 = {
mapDir + "map42_ap_path3.xml", mapDir + "map47_ap_path3.xml",
{ {
dataDir + "museum/Nexus/Path3_4519.csv", dataDir + "museum/Nexus/Path3_4519.csv",
@@ -178,7 +183,7 @@ namespace Settings {
dataDir + "museum/Nexus/fingerprints/wifi_fp.dat", dataDir + "museum/Nexus/fingerprints/wifi_fp.dat",
dataDir + "museum/wifimodel.dat", dataDir + "museum/wifimodel.dat",
45 46
}; };
} data; } data;

106
main.cpp
View File

@@ -28,6 +28,7 @@
#include <Indoor/math/stats/Statistics.h> #include <Indoor/math/stats/Statistics.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingSimpleImpoverishment.h> #include <Indoor/smc/filtering/resampling/ParticleFilterResamplingSimpleImpoverishment.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingKDE.h>
#include <sys/stat.h> #include <sys/stat.h>
@@ -45,6 +46,15 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
Interpolator<uint64_t, Point3> gtInterpolator = fr.getGroundTruthPath(map, gtPath); Interpolator<uint64_t, Point3> gtInterpolator = fr.getGroundTruthPath(map, gtPath);
Stats::Statistics<float> errorStats; Stats::Statistics<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;
// error file // error file
const long int t = static_cast<long int>(time(NULL)); const long int t = static_cast<long int>(time(NULL));
auto evalDir = std::experimental::filesystem::path(Settings::errorDir); auto evalDir = std::experimental::filesystem::path(Settings::errorDir);
@@ -63,7 +73,7 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
// WiFiModelPerFloor WiFiModelPerFloor(map); // WiFiModelPerFloor WiFiModelPerFloor(map);
// WiFiModelPerBBox WiFiModelPerBBox(map); // WiFiModelPerBBox WiFiModelPerBBox(map);
WiFiModel* WiFiModel = nullptr; WiFiModel* wifiModel = nullptr;
// with optimization // with optimization
if(Settings::WiFiModel::optimize){ if(Settings::WiFiModel::optimize){
@@ -76,22 +86,23 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
// use a regional optimization scheme (one per floor) // use a regional optimization scheme (one per floor)
WiFiOptimizerPerFloor opt(map); WiFiOptimizer::PerFloor opt(map, Settings::WiFiModel::vg_calib, WiFiOptimizer::Mode::QUALITY);
WiFiOptimizer::LogDistCeiling ldc(map, Settings::WiFiModel::vg_calib, WiFiOptimizer::Mode::QUALITY);
// add all fingerprints to the optimizer (optimizer will add them to the correct floor/model) // add all fingerprints to the optimizer (optimizer will add them to the correct floor/model)
for (const WiFiFingerprint& fp : fingerprints.getFingerprints()) { for (const WiFiFingerprint& fp : fingerprints.getFingerprints()) {
opt.addFingerprint(fp); opt.addFingerprint(fp);
} }
WiFiModel = opt.optimizeAll(); wifiModel = opt.optimizeAll(ldc.MIN_2_FPS);
wifiModel->saveXML(setup.wifiModel);
WiFiModel->saveXML(setup.wifiModel);
} else { } else {
/** NOTE: Funktioniert fürs Museum VIEL VIEL Besser */
// use one model per AP for the whole map // use one model per AP for the whole map
WiFiModel = new WiFiModelLogDistCeiling(map); wifiModel = new WiFiModelLogDistCeiling(map);
WiFiOptimizer::LogDistCeiling opt(map, Settings::WiFiModel::vg_calib); WiFiOptimizer::LogDistCeiling opt(map, Settings::WiFiModel::vg_calib);
for (const WiFiFingerprint& fp : fingerprints.getFingerprints()) { for (const WiFiFingerprint& fp : fingerprints.getFingerprints()) {
opt.addFingerprint(fp); opt.addFingerprint(fp);
@@ -99,10 +110,10 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
const WiFiOptimizer::LogDistCeiling::APParamsList res = opt.optimizeAll(opt.NONE); const WiFiOptimizer::LogDistCeiling::APParamsList res = opt.optimizeAll(opt.NONE);
for (const WiFiOptimizer::LogDistCeiling::APParamsMAC& ap : res.get()) { for (const WiFiOptimizer::LogDistCeiling::APParamsMAC& ap : res.get()) {
const WiFiModelLogDistCeiling::APEntry entry(ap.params.getPos(), ap.params.txp, ap.params.exp, ap.params.waf); const WiFiModelLogDistCeiling::APEntry entry(ap.params.getPos(), ap.params.txp, ap.params.exp, ap.params.waf);
((WiFiModelLogDistCeiling*)WiFiModel)->addAP(ap.mac, entry); ((WiFiModelLogDistCeiling*)wifiModel)->addAP(ap.mac, entry);
} }
WiFiModel->saveXML(setup.wifiModel); wifiModel->saveXML(setup.wifiModel);
} }
@@ -111,15 +122,15 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
// load WiFiModel from file. The factory will create the correct instance // load WiFiModel from file. The factory will create the correct instance
//WiFiModel->loadXML(setup.wifiModel); //WiFiModel->loadXML(setup.wifiModel);
WiFiModelFactory fac(map); WiFiModelFactory fac(map);
WiFiModel = fac.loadXML(setup.wifiModel); wifiModel = fac.loadXML(setup.wifiModel);
} }
} else { } else {
// without optimization // without optimization
WiFiModel = new WiFiModelLogDistCeiling(map); wifiModel = new WiFiModelLogDistCeiling(map);
((WiFiModelLogDistCeiling*)WiFiModel)->loadAPs(map, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF); ((WiFiModelLogDistCeiling*)wifiModel)->loadAPs(map, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF);
Assert::isFalse(WiFiModel->getAllAPs().empty(), "no AccessPoints stored within the map.xml"); Assert::isFalse(wifiModel->getAllAPs().empty(), "no AccessPoints stored within the map.xml");
} }
@@ -156,15 +167,16 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
const int numParticles = 5000; const int numParticles = 5000;
//auto init = std::make_unique<MyPFInitFixed>(&mesh, srcPath1); // known position //auto init = std::make_unique<MyPFInitFixed>(&mesh, srcPath1); // known position
auto init = std::make_unique<MyPFInitUniform>(&mesh); // uniform distribution auto init = std::make_unique<MyPFInitUniform>(&mesh); // uniform distribution
auto eval = std::make_unique<MyPFEval>(*WiFiModel); auto eval = std::make_unique<MyPFEval>(*wifiModel);
auto trans = std::make_unique<MyPFTrans>(mesh, *WiFiModel); auto trans = std::make_unique<MyPFTrans>(mesh, *wifiModel);
//auto resample = std::make_unique<SMC::ParticleFilterResamplingSimple<MyState>>(); //auto resample = std::make_unique<SMC::ParticleFilterResamplingSimple<MyState>>();
auto resample = std::make_unique<SMC::ParticleFilterResamplingKDE<MyState, MyNavMeshTriangle>>(mesh, Settings::KDE3D::gridSize, Settings::KDE3D::bandwidth);
//auto resample = std::make_unique<SMC::ParticleFilterResamplingSimpleImpoverishment<MyState, MyNavMeshTriangle>>(); //auto resample = std::make_unique<SMC::ParticleFilterResamplingSimpleImpoverishment<MyState, MyNavMeshTriangle>>();
auto resample = std::make_unique<SMC::ParticleFilterResamplingKLD<MyState>>(); //auto resample = std::make_unique<SMC::ParticleFilterResamplingKLD<MyState>>();
auto estimate = std::make_unique<SMC::ParticleFilterEstimationBoxKDE<MyState>>(map, 0.2, Point2(1,1)); //auto estimate = std::make_unique<SMC::ParticleFilterEstimationBoxKDE<MyState>>(map, 0.2, Point2(1,1));
//auto estimate = std::make_unique<SMC::ParticleFilterEstimationWeightedAverage<MyState>>(); auto estimate = std::make_unique<SMC::ParticleFilterEstimationWeightedAverage<MyState>>();
//auto estimate = std::make_unique<SMC::ParticleFilterEstimationMax<MyState>>(); //auto estimate = std::make_unique<SMC::ParticleFilterEstimationMax<MyState>>();
// setup // setup
@@ -187,6 +199,8 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
relBaro.setCalibrationTimeframe( Timestamp::fromMS(5000) ); relBaro.setCalibrationTimeframe( Timestamp::fromMS(5000) );
Timestamp lastTimestamp = Timestamp::fromMS(0); Timestamp lastTimestamp = Timestamp::fromMS(0);
int i = 0;
// parse each sensor-value within the offline data // parse each sensor-value within the offline data
for (const Offline::Entry& e : fr.getEntries()) { for (const Offline::Entry& e : fr.getEntries()) {
@@ -248,19 +262,35 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
plot.showParticles(pf.getParticles()); plot.showParticles(pf.getParticles());
plot.setCurEst(est.pos.pos); plot.setCurEst(est.pos.pos);
plot.setGroundTruth(gtPos); //plot.setGroundTruth(gtPos);
plot.addEstimationNode(est.pos.pos); plot.addEstimationNode(est.pos.pos);
plot.setActivity((int) act.get()); plot.setActivity((int) act.get());
plot.plot(); //plot.plot();
// error calc // error calc
float err_m = gtPos.getDistance(est.pos.pos); // float err_m = gtPos.getDistance(est.pos.pos);
// errorStats.add(err_m);
// errorFile << ts.ms() << " " << err_m << "\n";
//error calc with penalty for wrong floor
double errorFactor = 3.0;
Point3 gtPosError = Point3(gtPos.x, gtPos.y, errorFactor * gtPos.z);
Point3 estError = Point3(est.pos.pos.x, est.pos.pos.y, errorFactor * est.pos.pos.z);
float err_m = gtPosError.getDistance(estError);
errorStats.add(err_m); errorStats.add(err_m);
errorFile << ts.ms() << " " << err_m << "\n"; errorFile << ts.ms() << " " << err_m << "\n";
//dbg.gp.setOutput("/tmp/123/" + std::to_string(i) + ".png");
//dbg.gp.setTerminal("pngcairo", K::GnuplotSize(60, 30)); plot.gp << "set terminal png size 1280,720\n";
plot.gp.setOutput("/tmp/videoSparkasse/" + std::to_string(i++) + ".png");
int degree = ((30 - i) % 360);
if (degree < 0){degree += 360;}
plot.gp << "set view 63,"<< degree << "\n";
plot.gp << "set autoscale xy\n";
plot.gp << "set autoscale z\n";
plot.plot();
} }
} }
@@ -289,6 +319,33 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
plot.printOverview(evalDir.string() + "/" + std::to_string(numFile) + "_" + std::to_string(t)); plot.printOverview(evalDir.string() + "/" + std::to_string(numFile) + "_" + std::to_string(t));
plot.plot(); plot.plot();
plot.closeStream();
// //save file for radius_sub
// std::ofstream radiusFile;
// radiusFile.open(evalDir.string() + "/" + std::to_string(numFile) + "_" + std::to_string(t) + "_radius.csv");
// //provide the maximum value for radius_sub (kld * wifiquality)
// double max = *std::max_element(((MyPFTrans*) pf.getTransition())->listRadiusSub.begin() , ((MyPFTrans*) pf.getTransition())->listRadiusSub.end());
// std::cout << "Max Value radius_sub: " << max << std::endl;
// double sum = 0;
// int pos = ((MyPFTrans*) pf.getTransition())->listRadiusSub.size() * 0.75;
// double quantilValue = 0;
// int num = 0;
// for(double val : ((MyPFTrans*) pf.getTransition())->listRadiusSub){
// sum += val;
// if(num == pos){quantilValue = val;}
// ++num;
// radiusFile << val << "\n";
// }
// double avg = sum / num;
// std::cout << "Average radius_sub: " << avg << std::endl;
// std::cout << "75 Quantil radius_sub: " << quantilValue << std::endl;
// radiusFile.close();
return errorStats; return errorStats;
} }
@@ -300,7 +357,8 @@ int main(int argc, char** argv) {
Stats::Statistics<float> statsQuantil; Stats::Statistics<float> statsQuantil;
Stats::Statistics<float> tmp; Stats::Statistics<float> tmp;
std::string evaluationName = "museum/tmp"; //Path2_Museum_KDE_SimpleResampling
std::string evaluationName = "museum/videoSparkasse";
for(int i = 0; i < 1; ++i){ for(int i = 0; i < 1; ++i){
@@ -330,7 +388,7 @@ int main(int argc, char** argv) {
// statsQuantil.add(tmp.getQuantile(0.75)); // statsQuantil.add(tmp.getQuantile(0.75));
// } // }
for(int j = 0; j < Settings::data.Path3.training.size(); ++j){ for(int j = 0; j < 1; ++j){
tmp = run(Settings::data.Path3, j, evaluationName); tmp = run(Settings::data.Path3, j, evaluationName);
statsMedian.add(tmp.getMedian()); statsMedian.add(tmp.getMedian());
statsAVG.add(tmp.getAvg()); statsAVG.add(tmp.getAvg());

74
navMesh/filter.h
View File

@@ -23,6 +23,7 @@
#include <Indoor/navMesh/walk/NavMeshWalkWifiRegional.h> #include <Indoor/navMesh/walk/NavMeshWalkWifiRegional.h>
#include <Indoor/navMesh/walk/NavMeshWalkUnblockable.h> #include <Indoor/navMesh/walk/NavMeshWalkUnblockable.h>
#include <Indoor/navMesh/walk/NavMeshWalkKLD.h> #include <Indoor/navMesh/walk/NavMeshWalkKLD.h>
#include <Indoor/navMesh/walk/NavMeshWalkSinkOrSwim.h>
#include <Indoor/navMesh/NavMeshRandom.h> #include <Indoor/navMesh/NavMeshRandom.h>
#include <Indoor/sensors/radio/WiFiMeasurements.h> #include <Indoor/sensors/radio/WiFiMeasurements.h>
@@ -71,6 +72,11 @@ struct MyState {
return pos.pos.y; return pos.pos.y;
} }
float getZ() {
return pos.pos.z;
}
float getBinValue(const int dim) const { float getBinValue(const int dim) const {
switch (dim) { switch (dim) {
case 0: return this->pos.pos.x; case 0: return this->pos.pos.x;
@@ -172,27 +178,31 @@ public:
class MyPFTrans : public SMC::ParticleFilterTransition<MyState, MyControl> { class MyPFTrans : public SMC::ParticleFilterTransition<MyState, MyControl> {
//using MyNavMeshWalk = NM::NavMeshWalkSimple<MyNavMeshTriangle>; //using MyNavMeshWalk = NM::NavMeshWalkSimple<MyNavMeshTriangle>;
//using MyNavMeshWalk = NM::NavMeshWalkWifiRegional<MyNavMeshTriangle>; //using MyNavMeshWalk = NM::NavMeshWalkWifiRegional<MyNavMeshTriangle>;
//using MyNavMeshWalk = NM::NavMeshWalkUnblockable<MyNavMeshTriangle>; //using MyNavMeshWalk = NM::NavMeshWalkUnblockable<MyNavMeshTriangle>;
using MyNavMeshWalk = NM::NavMeshWalkKLD<MyNavMeshTriangle>; //using MyNavMeshWalk = NM::NavMeshWalkKLD<MyNavMeshTriangle>;
using MyNavMeshWalk = NM::NavMeshWalkSinkOrSwim<MyNavMeshTriangle>;
MyNavMeshWalk walker; MyNavMeshWalk walker;
WiFiQualityAnalyzer analyzer; WiFiQualityAnalyzer analyzer;
WiFiObserverFree wifiProbability; WiFiObserverFree wifiProbability;
const double lambda = 0.0003; const double lambda = 0.03;
SMC::ParticleFilterEstimationBoxKDE<MyState> estimator; SMC::ParticleFilterEstimationBoxKDE<MyState> estimator;
public: public:
//std::vector<double> listRadiusSub;
MyPFTrans(MyNavMesh& mesh, WiFiModel& wifiModel) : MyPFTrans(MyNavMesh& mesh, WiFiModel& wifiModel) :
walker(mesh), walker(mesh),
wifiProbability(Settings::WiFiModel::sigma, wifiModel){ wifiProbability(Settings::WiFiModel::sigma, wifiModel){
// how to evaluate drawn points // how to evaluate drawn points
//walker.addEvaluator(new NM::WalkEvalHeadingStartEndNormal<MyNavMeshTriangle>(0.04)); walker.addEvaluator(new NM::WalkEvalHeadingStartEndNormal<MyNavMeshTriangle>(0.04));
//walker.addEvaluator(new NM::WalkEvalDistance<MyNavMeshTriangle>(0.1)); walker.addEvaluator(new NM::WalkEvalDistance<MyNavMeshTriangle>(0.1));
//walker.addEvaluator(new NM::WalkEvalApproachesTarget<MyNavMeshTriangle>(0.9)); // 90% for particles moving towards the target //walker.addEvaluator(new NM::WalkEvalApproachesTarget<MyNavMeshTriangle>(0.9)); // 90% for particles moving towards the target
estimator = SMC::ParticleFilterEstimationBoxKDE<MyState>(&mesh, 0.2, Point2(1,1)); estimator = SMC::ParticleFilterEstimationBoxKDE<MyState>(&mesh, 0.2, Point2(1,1));
@@ -203,36 +213,36 @@ public:
// walking and heading random // walking and heading random
Distribution::Normal<float> dStepSizeFloor(0.70, 0.1); Distribution::Normal<float> dStepSizeFloor(0.70, 0.1);
Distribution::Normal<float> dStepSizeStair(0.35, 0.1); Distribution::Normal<float> dStepSizeStair(0.35, 0.1);
Distribution::Normal<float> dHeading(0.0, 0.1); Distribution::Normal<float> dHeading(0.0, 0.1);
// wifi and quality of wifi // // wifi and quality of wifi
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(control->wifi); // const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(control->wifi);
if(!wifiObs.entries.empty()){ // if(!wifiObs.entries.empty()){
analyzer.add(wifiObs); // analyzer.add(wifiObs);
} // }
float qualityWifi = analyzer.getQuality(); // float qualityWifi = analyzer.getQuality();
if(std::isnan(qualityWifi)){ // if(std::isnan(qualityWifi)){
qualityWifi = 1.0; // qualityWifi = 1.0;
} else if(qualityWifi == 0) { // } else if(qualityWifi == 0) {
qualityWifi = 0.00000001; // qualityWifi = 0.00000001;
} // }
// divergence between eval and transition // // divergence between eval and transition
std::vector<SMC::Particle<MyState>> wifiParticles; // std::vector<SMC::Particle<MyState>> wifiParticles;
NM::NavMeshRandom<MyNavMeshTriangle> rnd = walker.getMesh().getRandom(); // NM::NavMeshRandom<MyNavMeshTriangle> rnd = walker.getMesh().getRandom();
for(int i = 0; i < 10000; ++i){ // for(int i = 0; i < 10000; ++i){
NM::NavMeshLocation<MyNavMeshTriangle> tmpLocation = rnd.draw(); // NM::NavMeshLocation<MyNavMeshTriangle> tmpLocation = rnd.draw();
double weight = wifiProbability.getProbability(tmpLocation.pos, control->currentTime, wifiObs); // double weight = wifiProbability.getProbability(tmpLocation.pos, control->currentTime, wifiObs);
SMC::Particle<MyState> tmpParticle(MyState(tmpLocation.pos), weight); // SMC::Particle<MyState> tmpParticle(MyState(tmpLocation.pos), weight);
wifiParticles.push_back(tmpParticle); // wifiParticles.push_back(tmpParticle);
} // }
MyState wifiEstimate = estimator.estimate(wifiParticles); // MyState wifiEstimate = estimator.estimate(wifiParticles);
// fake kld // // fake kld
const double kld = control->lastEstimate.getDistance(wifiEstimate.pos.pos); // const double kld = control->lastEstimate.getDistance(wifiEstimate.pos.pos);
//const double kld = Divergence::KullbackLeibler<double>::getMultivariateGauss(normParticle, normWifi);; //const double kld = Divergence::KullbackLeibler<double>::getMultivariateGauss(normParticle, normWifi);;
//std::cout << "KLD: " << kld << std::endl; //std::cout << "KLD: " << kld << std::endl;
@@ -241,6 +251,8 @@ public:
//update wifi //update wifi
//walker.updateWiFi(wifiObs, control->currentTime, control->lastEstimate); //walker.updateWiFi(wifiObs, control->currentTime, control->lastEstimate);
// listRadiusSub.push_back(kld * qualityWifi);
#pragma omp parallel for num_threads(3) #pragma omp parallel for num_threads(3)
for (int i = 0; i < particles.size(); ++i) { for (int i = 0; i < particles.size(); ++i) {
SMC::Particle<MyState>& p = particles[i]; SMC::Particle<MyState>& p = particles[i];
@@ -262,8 +274,8 @@ public:
double deltaUnblockable = 0.01; double deltaUnblockable = 0.01;
// walk // walk
//MyNavMeshWalk::ResultEntry res = walker.getOne(params); MyNavMeshWalk::ResultEntry res = walker.getOne(params);
MyNavMeshWalk::ResultEntry res = walker.getOne(params, kld, lambda, qualityWifi); //MyNavMeshWalk::ResultEntry res = walker.getOne(params, kld, lambda, qualityWifi);
// assign back to particle's state // assign back to particle's state
p.weight *= res.probability; p.weight *= res.probability;