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

current state

Browse Source
This commit is contained in:
FrankE
2017-04-18 18:03:31 +02:00
parent 3e244118cc
commit 11cb939758
18 changed files with 1809 additions and 516 deletions
Download Patch File Download Diff File Expand all files Collapse all files

54
pf/EvalWalk.h
View File

@@ -108,7 +108,7 @@ public:
pf->setTransition( std::unique_ptr<PFTrans>( new PFTrans(grid)) );
// resampling step?
//pf->setNEffThreshold(0.35);
//pf->setNEffThreshold(0.15);
//pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
//pf->setNEffThreshold(0.75);
@@ -117,22 +117,26 @@ public:
//pf->setNEffThreshold(0.75);
//pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingPercent<MyState>>(new K::ParticleFilterResamplingPercent<MyState>(0.05)) );
K::ParticleFilterResamplingNEff<MyState>* res = new K::ParticleFilterResamplingNEff<MyState>(0.50, 0.05);
K::ParticleFilterResamplingNEff<MyState>* res = new K::ParticleFilterResamplingNEff<MyState>(0.75, 0.05);
pf->setNEffThreshold(1.0);
pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingNEff<MyState>>(res) );
res->setDrawCallback([&] (K::Particle<MyState>& p) {
static std::minstd_rand gen;
const MyGridNode* n = grid->getNodePtrFor(p.state.position);
std::normal_distribution<float> distTurn(-0.3, +0.3);
for (int j = 0; j < 2; ++j) {
std::uniform_int_distribution<int> distIdx(0, n->getNumNeighbors()-1);
const int idx = distIdx(gen);
n = &(grid->getNeighbor(*n, idx));
}
p.state.position = *n;
p.state.heading.direction += distTurn(gen);
});
// move during resampling. NOT ALLOWED!
// res->setDrawCallback([&] (K::Particle<MyState>& p) {
// static std::minstd_rand gen;
// static int cnt = 0; ++cnt;
// bool init = cnt < pf->getParticles().size() * 50;
// const MyGridNode* n = grid->getNodePtrFor(p.state.position);
// std::normal_distribution<float> distTurn(0, (init) ? (0.5) : (0.05));
// for (int j = 0; j < 2; ++j) {
// std::uniform_int_distribution<int> distIdx(0, n->getNumNeighbors()-1);
// const int idx = distIdx(gen);
// n = &(grid->getNeighbor(*n, idx));
// }
// p.state.position = *n;
// p.state.heading.direction += distTurn(gen);
// });
// state estimation step
//pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
@@ -144,15 +148,20 @@ public:
void walk1() {
// path1
absHead = M_PI/2;
const std::string path = Settings::path1b;
const std::vector<int> pathPoints = Settings::GroundTruth::path1;
// absHead = M_PI/2;
// const std::string path = Settings::path1b;
// const std::vector<int> pathPoints = Settings::GroundTruth::path1;
// path2
// absHead = 0;
// const std::string path = Settings::path2a;
// const std::string path = Settings::path2b;
// const std::vector<int> pathPoints = Settings::GroundTruth::path2;
// path_toni_inst_2
absHead = M_PI;
const std::string path = Settings::path_toni_inst_2b;
const std::vector<int> pathPoints = Settings::GroundTruth::path_toni_inst_2;
runName = "";
// get ground-truth
@@ -162,8 +171,12 @@ public:
//WiFiModelLogDistCeiling wifiModel(map);
//wifiModel.loadXML(Settings::wifiAllFixed);
//wifiModel.loadXML(Settings::wifiEachOptParPos);
WiFiModelPerFloor wifiModel(map);
wifiModel.loadXML(Settings::wifiEachOptParPos_multimodel);
//WiFiModelPerFloor wifiModel(map);
//wifiModel.loadXML(Settings::wifiEachOptParPos_multimodel);
WiFiModelPerBBox wifiModel(map);
wifiModel.loadXML(Settings::wifiEachOptParPos_perBBox);
// eval
std::unique_ptr<PFEval> eval = std::unique_ptr<PFEval>( new PFEval(grid, wifiModel, em) );
@@ -226,6 +239,7 @@ public:
const float diff = Angle::getSignedDiffRAD_2PI(curCtrl.compassAzimuth_rad, newAzimuth_safe);
curCtrl.compassAzimuth_rad += diff * 0.01;
curCtrl.compassAzimuth_rad = Angle::makeSafe_2PI(curCtrl.compassAzimuth_rad);
curObs.compassAzimuth_rad = curCtrl.compassAzimuth_rad;
//curCtrl.compassAzimuth_rad = curCtrl.compassAzimuth_rad * 0.99 + newAzimuth * 0.01;
}