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

37
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> {
//using MyNavMeshWalk = NM::NavMeshWalkSimple<MyNavMeshTriangle>;
@@ -190,8 +208,6 @@ class MyPFTrans : public SMC::ParticleFilterTransition<MyState, MyControl> {
MyNavMeshWalk walker;
const double lambda = 0.03;
public:
//std::vector<double> listRadiusSub;
@@ -208,9 +224,9 @@ public:
void transition(std::vector<SMC::Particle<MyState>>& particles, const MyControl* control) override {
// walking and heading random
Distribution::Normal<float> dStepSizeFloor(0.60, 0.1);
Distribution::Normal<float> dStepSizeStair(0.35, 0.1);
Distribution::Normal<float> dHeading(0.0, 0.1);
Distribution::Normal<float> dStepSizeFloor(0.60f, 0.1f);
Distribution::Normal<float> dStepSizeStair(0.35f, 0.1f);
Distribution::Normal<float> dHeading(0.0, 0.1f);
#pragma omp parallel for num_threads(3)
for (int i = 0; i < particles.size(); ++i) {
@@ -225,16 +241,13 @@ public:
params.stepSizes.stepSizeFloor_m = dStepSizeFloor.draw();
params.stepSizes.stepSizeStair_m = dStepSizeStair.draw();
if(params.stepSizes.stepSizeFloor_m < 0.1 || params.stepSizes.stepSizeStair_m < 0.1){
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.1f;
params.stepSizes.stepSizeStair_m = 0.1f;
}
double deltaUnblockable = 0.01;
// walk
MyNavMeshWalk::ResultEntry res = walker.getOne(params);
//MyNavMeshWalk::ResultEntry res = walker.getOne(params, kld, lambda, qualityWifi);
// assign back to particle's state
p.weight *= res.probability;
@@ -303,7 +316,7 @@ public:
for (auto& wifi : observation.wifi) {
if ( (true && wifi.second.getAP().getMAC() == Settings::NUC1)
|| (true && wifi.second.getAP().getMAC() == Settings::NUC2)
|| (true && wifi.second.getAP().getMAC() == Settings::NUC2)
|| (true && wifi.second.getAP().getMAC() == Settings::NUC3)
|| (true && wifi.second.getAP().getMAC() == Settings::NUC4)
)

4
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>();
eval->kalmanMap = kalmanMap;
auto trans = std::make_unique<MyPFTrans>(mesh);
auto trans = std::make_unique<MyPFTransRandom>();
//auto trans = std::make_unique<MyPFTransStatic>();
auto resample = std::make_unique<SMC::ParticleFilterResamplingSimple<MyState>>();
@@ -380,7 +380,7 @@ static Stats::Statistics<float> run(Settings::DataSetup setup, int walkIdx, std:
ctrl.lastEstimate = est.pos.pos;
// draw wifi ranges
// draw wifi ranges
for (auto& ftm : obs.wifi)
{
int nucid = Settings::data.CurrentPath.NUCs.at(ftm.second.getAP().getMAC()).ID;