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

added simple transition with normal distribution

Browse Source
This commit is contained in:
toni
2019-09-18 12:18:56 +02:00
parent e4054a9206
commit bb1d141088
2 changed files with 27 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

35
code/filter.h
View File

@@ -180,6 +180,24 @@ class MyPFTransStatic : public SMC::ParticleFilterTransition<MyState, MyControl>
} }
}; };
struct MyPFTransRandom : public SMC::ParticleFilterTransition<MyState, MyControl>{
Distribution::Normal<float> dStepSize;
MyPFTransRandom()
: dStepSize(0.0f, 0.6f)
{}
void transition(std::vector<SMC::Particle<MyState>>& particles, const MyControl* control) override {
#pragma omp parallel for num_threads(3)
for (int i = 0; i < particles.size(); ++i) {
SMC::Particle<MyState>& p = particles[i];
p.state.pos.pos.x += dStepSize.draw();
p.state.pos.pos.y += dStepSize.draw();
}
}
};
class MyPFTrans : public SMC::ParticleFilterTransition<MyState, MyControl> { class MyPFTrans : public SMC::ParticleFilterTransition<MyState, MyControl> {
//using MyNavMeshWalk = NM::NavMeshWalkSimple<MyNavMeshTriangle>; //using MyNavMeshWalk = NM::NavMeshWalkSimple<MyNavMeshTriangle>;
@@ -190,8 +208,6 @@ class MyPFTrans : public SMC::ParticleFilterTransition<MyState, MyControl> {
MyNavMeshWalk walker; MyNavMeshWalk walker;
const double lambda = 0.03;
public: public:
//std::vector<double> listRadiusSub; //std::vector<double> listRadiusSub;
@@ -208,9 +224,9 @@ public:
void transition(std::vector<SMC::Particle<MyState>>& particles, const MyControl* control) override { void transition(std::vector<SMC::Particle<MyState>>& particles, const MyControl* control) override {
// walking and heading random // walking and heading random
Distribution::Normal<float> dStepSizeFloor(0.60, 0.1); Distribution::Normal<float> dStepSizeFloor(0.60f, 0.1f);
Distribution::Normal<float> dStepSizeStair(0.35, 0.1); Distribution::Normal<float> dStepSizeStair(0.35f, 0.1f);
Distribution::Normal<float> dHeading(0.0, 0.1); Distribution::Normal<float> dHeading(0.0, 0.1f);
#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) {
@@ -225,16 +241,13 @@ public:
params.stepSizes.stepSizeFloor_m = dStepSizeFloor.draw(); params.stepSizes.stepSizeFloor_m = dStepSizeFloor.draw();
params.stepSizes.stepSizeStair_m = dStepSizeStair.draw(); params.stepSizes.stepSizeStair_m = dStepSizeStair.draw();
if(params.stepSizes.stepSizeFloor_m < 0.1 || params.stepSizes.stepSizeStair_m < 0.1){ if(params.stepSizes.stepSizeFloor_m < 0.1f || params.stepSizes.stepSizeStair_m < 0.1f){
params.stepSizes.stepSizeFloor_m = 0.1; params.stepSizes.stepSizeFloor_m = 0.1f;
params.stepSizes.stepSizeStair_m = 0.1; params.stepSizes.stepSizeStair_m = 0.1f;
} }
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);
// assign back to particle's state // assign back to particle's state
p.weight *= res.probability; p.weight *= res.probability;

2
code/main.cpp
View File

@@ -265,7 +265,7 @@ static Stats::Statistics<float> run(Settings::DataSetup setup, int walkIdx, std:
auto eval = std::make_unique<MyPFEval>(); auto eval = std::make_unique<MyPFEval>();
eval->kalmanMap = kalmanMap; eval->kalmanMap = kalmanMap;
auto trans = std::make_unique<MyPFTrans>(mesh); auto trans = std::make_unique<MyPFTransRandom>();
//auto trans = std::make_unique<MyPFTransStatic>(); //auto trans = std::make_unique<MyPFTransStatic>();
auto resample = std::make_unique<SMC::ParticleFilterResamplingSimple<MyState>>(); auto resample = std::make_unique<SMC::ParticleFilterResamplingSimple<MyState>>();