FHWS/Indoor
Archived
4
0
Fork 0
Code Issues 19 Pull Requests Releases 1 Wiki Activity

added stuff for bluetooth

Browse Source 
worked on resampling methods
This commit is contained in:
mail@toni-fetzer.de
2019-06-05 18:09:15 +02:00
parent cb61e0fe68
commit 8d37e94647
12 changed files with 472 additions and 320 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
smc/filtering/ParticleFilter.h
View File

@@ -134,8 +134,7 @@ namespace SMC {
Assert::isNotNull(evaluation, "evaluation MUST not be null! call setEvaluation() first!");
Assert::isNotNull(estimation, "estimation MUST not be null! call setEstimation() first!");
// if the number of efficient particles is too low, perform resampling
if (lastNEff < particles.size() * nEffThresholdPercent) {resampler->resample(particles); }
// perform the transition step
transition->transition(particles, control);
@@ -143,6 +142,9 @@ namespace SMC {
// perform the evaluation step and calculate the sum of all particle weights
evaluation->evaluation(particles, observation);
// perform an additional step to prevent impoverishment of the particles
//enrichment->enrichment(particles, observation);
// normalize the particle weights and thereby calculate N_eff
lastNEff = normalize();
@@ -151,6 +153,9 @@ namespace SMC {
// estimate the current state
const State est = estimation->estimate(particles);
// if the number of efficient particles is too low, perform resampling
if (lastNEff < particles.size() * nEffThresholdPercent) {resampler->resample(particles); }
// done
return est;

338
smc/filtering/ParticleFilterMixing.h
View File

@@ -16,225 +16,225 @@
namespace SMC {
/**
* the main-class for the particle filter
* @param State the (user-defined) state for each particle
* @param Observation the observation (sensor) data
*/
template <typename State, typename Control, typename Observation>
class ParticleFilterMixing {
/**
* the main-class for the particle filter
* @param State the (user-defined) state for each particle
* @param Observation the observation (sensor) data
*/
template <typename State, typename Control, typename Observation>
class ParticleFilterMixing {
private:
private:
/** all used particles */
std::vector<Particle<State>> particles;
/** all used particles */
std::vector<Particle<State>> particles;
/** the current calculated estimation */
State estimation;
/** the current calculated estimation */
State estimation;
/** the resampler to use */
std::shared_ptr<ParticleFilterResampling<State>> resampler;
/** the resampler to use */
std::shared_ptr<ParticleFilterResampling<State>> resampler;
/** the estimation function to use */
std::shared_ptr<ParticleFilterEstimation<State>> estimator;
/** the estimation function to use */
std::shared_ptr<ParticleFilterEstimation<State>> estimator;
/** the transition function to use */
std::shared_ptr<ParticleFilterTransition<State, Control>> transition;
/** the transition function to use */
std::shared_ptr<ParticleFilterTransition<State, Control>> transition;
/** the evaluation function to use */
std::shared_ptr<ParticleFilterEvaluation<State, Observation>> evaluation;
/** the evaluation function to use */
std::shared_ptr<ParticleFilterEvaluation<State, Observation>> evaluation;
/** the initialization function to use */
std::shared_ptr<ParticleFilterInitializer<State>> initializer;
/** the initialization function to use */
std::shared_ptr<ParticleFilterInitializer<State>> initializer;
/** the percentage-of-efficient-particles-threshold for resampling */
double nEffThresholdPercent = 0.25;
/** the percentage-of-efficient-particles-threshold for resampling */
double nEffThresholdPercent = 0.25;
/** the current sum of all weights NOT normalized*/
double weightSum = 1.0;
/** the current sum of all weights NOT normalized*/
double weightSum = 1.0;
/** the predicted mode probability P(m_t|Z_t-1) */
double predictedModeProbability = 1.0;
/** the predicted mode probability P(m_t|Z_t-1) */
double predictedModeProbability = 1.0;
/** the posterior probability of the mode p(m_t | Z_t)*/
double modePosteriorProbability = 1.0;
/** the posterior probability of the mode p(m_t | Z_t)*/
double modePosteriorProbability = 1.0;
/** the transition mode probability P(m_t-1 | m_t, Z_t-1)*/
double transitionModeProbability = 1.0;
/** the transition mode probability P(m_t-1 | m_t, Z_t-1)*/
double transitionModeProbability = 1.0;
public:
public:
/** ctor
* NOTE: The modePosteriorProbability needs the be normalized depending on the number of filters within the IMMPF!!
*/
ParticleFilterMixing(const uint32_t numParticles, std::shared_ptr<ParticleFilterInitializer<State>> initializer, double modePosteriorProbability) {
this->modePosteriorProbability = modePosteriorProbability;
/** ctor
* NOTE: The modePosteriorProbability needs the be normalized depending on the number of filters within the IMMPF!!
*/
ParticleFilterMixing(const uint32_t numParticles, std::shared_ptr<ParticleFilterInitializer<State>> initializer, double modePosteriorProbability) {
this->modePosteriorProbability = modePosteriorProbability;
particles.resize(numParticles);
setInitializier(std::move(initializer));
init();
}
particles.resize(numParticles);
setInitializier(std::move(initializer));
init();
}
/** dtor */
~ParticleFilterMixing() {
;
}
/** dtor */
~ParticleFilterMixing() {
;
}
/** access to all particles */
const std::vector<Particle<State>>& getParticles() const {
return this->particles;
}
/** access to all particles */
const std::vector<Particle<State>>& getParticles() const {
return this->particles;
}
void setParticles(const std::vector<Particle<State>>& newParticles){
this->particles = newParticles;
}
void setParticles(const std::vector<Particle<State>>& newParticles){
this->particles = newParticles;
}
/** get the current estimation */
const State getEstimation() const {
return estimation;
}
/** get the current estimation */
const State getEstimation() const {
return estimation;
}
/** initialize/re-start the particle filter */
void init() {
Assert::isNotNull(initializer, "initializer MUST not be null! call setInitializer() first!");
initializer->initialize(particles);
}
/** initialize/re-start the particle filter */
void init() {
Assert::isNotNull(initializer, "initializer MUST not be null! call setInitializer() first!");
initializer->initialize(particles);
}
/** set the resampling method to use */
void setResampling(std::shared_ptr<ParticleFilterResampling<State>> resampler) {
Assert::isNotNull(resampler, "setResampling() MUST not be called with a nullptr!");
this->resampler = std::move(resampler);
}
/** set the resampling method to use */
void setResampling(std::shared_ptr<ParticleFilterResampling<State>> resampler) {
Assert::isNotNull(resampler, "setResampling() MUST not be called with a nullptr!");
this->resampler = std::move(resampler);
}
/** set the estimation method to use */
void setEstimator(std::shared_ptr<ParticleFilterEstimation<State>> estimator) {
Assert::isNotNull(estimator, "setEstimation() MUST not be called with a nullptr!");
this->estimator = std::move(estimator);
}
/** set the estimation method to use */
void setEstimator(std::shared_ptr<ParticleFilterEstimation<State>> estimator) {
Assert::isNotNull(estimator, "setEstimation() MUST not be called with a nullptr!");
this->estimator = std::move(estimator);
}
/** set the transition method to use */
void setTransition(std::shared_ptr<ParticleFilterTransition<State, Control>> transition) {
Assert::isNotNull(transition, "setTransition() MUST not be called with a nullptr!");
this->transition = std::move(transition);
}
/** set the transition method to use */
void setTransition(std::shared_ptr<ParticleFilterTransition<State, Control>> transition) {
Assert::isNotNull(transition, "setTransition() MUST not be called with a nullptr!");
this->transition = std::move(transition);
}
/** get the used transition method */
ParticleFilterTransition<State, Control>* getTransition() {
return this->transition.get();
}
/** get the used transition method */
ParticleFilterTransition<State, Control>* getTransition() {
return this->transition.get();
}
/** set the evaluation method to use */
void setEvaluation(std::shared_ptr<ParticleFilterEvaluation<State, Observation>> evaluation) {
Assert::isNotNull(evaluation, "setEvaluation() MUST not be called with a nullptr!");
this->evaluation = std::move(evaluation);
}
/** set the evaluation method to use */
void setEvaluation(std::shared_ptr<ParticleFilterEvaluation<State, Observation>> evaluation) {
Assert::isNotNull(evaluation, "setEvaluation() MUST not be called with a nullptr!");
this->evaluation = std::move(evaluation);
}
/** set the initialization method to use */
void setInitializier(std::shared_ptr<ParticleFilterInitializer<State>> initializer) {
Assert::isNotNull(initializer, "setInitializer() MUST not be called with a nullptr!");
this->initializer = std::move(initializer);
}
/** set the initialization method to use */
void setInitializier(std::shared_ptr<ParticleFilterInitializer<State>> initializer) {
Assert::isNotNull(initializer, "setInitializer() MUST not be called with a nullptr!");
this->initializer = std::move(initializer);
}
/** set the resampling threshold as the percentage of efficient particles */
void setNEffThreshold(const double thresholdPercent) {
this->nEffThresholdPercent = thresholdPercent;
}
/** set the resampling threshold as the percentage of efficient particles */
void setNEffThreshold(const double thresholdPercent) {
this->nEffThresholdPercent = thresholdPercent;
}
/** get the unormalized weight sum of all particles */
double getWeightSum() const
{
return this->weightSum;
}
/** get the unormalized weight sum of all particles */
double getWeightSum() const
{
return this->weightSum;
}
/** get the predicted mode probability P(m_t|Z_t-1)*/
double getPredictedModeProbability() const
{
return this->predictedModeProbability;
}
/** get the predicted mode probability P(m_t|Z_t-1)*/
double getPredictedModeProbability() const
{
return this->predictedModeProbability;
}
/** set the predicted mode probability P(m_t|Z_t-1)*/
void setPredictedModeProbability(const double val) {
this->predictedModeProbability = val;
}
/** set the predicted mode probability P(m_t|Z_t-1)*/
void setPredictedModeProbability(const double val) {
this->predictedModeProbability = val;
}
/** get the posterior mode probability P(m_t|Z_t)*/
double getModePosteriorProbability() const
{
return this->modePosteriorProbability;
}
/** get the posterior mode probability P(m_t|Z_t)*/
double getModePosteriorProbability() const
{
return this->modePosteriorProbability;
}
/** set the posterior mode probability P(m_t|Z_t)*/
void setModePosteriorProbability(const double likelihoodSum) {
/** set the posterior mode probability P(m_t|Z_t)*/
void setModePosteriorProbability(const double likelihoodSum) {
Assert::isNotNull(likelihoodSum, "likelihoodsum is zero.. thats not possible");
Assert::isNotNull(this->weightSum, "weightSum is zero.. thats not possible");
//Assert::isNotNull(this->predictedModeProbability, "predictedModeProbability is zero.. thats not possible");
Assert::isNotNull(likelihoodSum, "likelihoodsum is zero.. thats not possible");
Assert::isNotNull(this->weightSum, "weightSum is zero.. thats not possible");
//Assert::isNotNull(this->predictedModeProbability, "predictedModeProbability is zero.. thats not possible");
this->modePosteriorProbability = (this->weightSum * this->predictedModeProbability) / likelihoodSum;
this->modePosteriorProbability = (this->weightSum * this->predictedModeProbability) / likelihoodSum;
//Assert::isNotNull(this->modePosteriorProbability, "modePosteriorProbability is zero.. thats not possible");
}
//Assert::isNotNull(this->modePosteriorProbability, "modePosteriorProbability is zero.. thats not possible");
}
/** get the transition mode probability P(m_t|Z_t)
* NOTE: Dont use this value! It is only needed for more beatiful mixed sampling!
*/
double getTransitionModeProbability() const
{
return this->transitionModeProbability;
}
/** get the transition mode probability P(m_t|Z_t)
* NOTE: Dont use this value! It is only needed for more beautiful mixed sampling!
*/
double getTransitionModeProbability() const
{
return this->transitionModeProbability;
}
/** set the transition mode probability P(m_t|Z_t)*/
void setTransitionModeProbability(const double val) {
this->transitionModeProbability = val;
}
/** set the transition mode probability P(m_t|Z_t)*/
void setTransitionModeProbability(const double val) {
this->transitionModeProbability = val;
}
/** perform resampling -> transition -> evaluation -> estimation */
void update(const Control* control, const Observation& observation) {
/** perform resampling -> transition -> evaluation -> estimation */
void update(const Control* control, const Observation& observation) {
// sanity checks (if enabled)
Assert::isNotNull(resampler, "resampler MUST not be null! call setResampler() first!");
Assert::isNotNull(transition, "transition MUST not be null! call setTransition() first!");
Assert::isNotNull(evaluation, "evaluation MUST not be null! call setEvaluation() first!");
Assert::isNotNull(estimator, "estimation MUST not be null! call setEstimation() first!");
// sanity checks (if enabled)
Assert::isNotNull(resampler, "resampler MUST not be null! call setResampler() first!");
Assert::isNotNull(transition, "transition MUST not be null! call setTransition() first!");
Assert::isNotNull(evaluation, "evaluation MUST not be null! call setEvaluation() first!");
Assert::isNotNull(estimator, "estimation MUST not be null! call setEstimation() first!");
// perform the transition step
transition->transition(particles, control);
// perform the transition step
transition->transition(particles, control);
// perform the evaluation step and calculate the sum of all particle weights
this->weightSum = evaluation->evaluation(particles, observation);
// perform the evaluation step and calculate the sum of all particle weights
this->weightSum = evaluation->evaluation(particles, observation);
// normalize the particle weights and thereby calculate N_eff
const double neff = normalize(weightSum);
// normalize the particle weights and thereby calculate N_eff
const double neff = normalize(weightSum);
// estimate the current state
this->estimation = estimator->estimate(particles);
// estimate the current state
this->estimation = estimator->estimate(particles);
// if the number of efficient particles is too low, perform resampling
if (neff < particles.size() * nEffThresholdPercent) { resampler->resample(particles); }
// if the number of efficient particles is too low, perform resampling
if (neff < particles.size() * nEffThresholdPercent) { resampler->resample(particles); }
// done
}
// done
}
private:
private:
/** normalize the weight of all particles to one */
double normalize(const double weightSum) {
double sum = 0.0;
for (auto& p : particles) {
p.weight /= weightSum;
sum += (p.weight * p.weight);
}
return 1.0 / sum;
}
/** normalize the weight of all particles to one */
double normalize(const double weightSum) {
double sum = 0.0;
for (auto& p : particles) {
p.weight /= weightSum;
sum += (p.weight * p.weight);
}
return 1.0 / sum;
}
/** calculate the number of efficient particles (N_eff) */
double getNeff() const {
double sum = 0.0;
for (auto& p : particles) {sum += (p.weight * p.weight);}
return 1.0 / sum;
}
};
/** calculate the number of efficient particles (N_eff) */
double getNeff() const {
double sum = 0.0;
for (auto& p : particles) {sum += (p.weight * p.weight);}
return 1.0 / sum;
}
};
}

147
smc/filtering/resampling/ParticleFilterResamplingKDE.h
View File

@@ -1,4 +1,4 @@
#ifndef PARTICLEFILTERRESAMPLINGKDE_H
#ifndef PARTICLEFILTERRESAMPLINGKDE_H
#define PARTICLEFILTERRESAMPLINGKDE_H
#include <algorithm>
@@ -19,103 +19,102 @@
namespace SMC {
/**
* Resample based on rapid KDE
*/
template <typename State, typename Tria>
class ParticleFilterResamplingKDE : public ParticleFilterResampling<State> {
/**
* Resample based on rapid KDE
*/
template <typename State, typename Tria>
class ParticleFilterResamplingKDE : public ParticleFilterResampling<State> {
private:
private:
/** this is a copy of the particle-set to draw from it */
std::vector<Particle<State>> particlesCopy;
/** this is a copy of the particle-set to draw from it */
std::vector<Particle<State>> particlesCopy;
/** random number generator */
std::minstd_rand gen;
/** random number generator */
std::minstd_rand gen;
/** boundingBox for the boxKDE */
_BBox3<float> bb;
/** boundingBox for the boxKDE */
_BBox3<float> bb;
/** histogram/grid holding the particles*/
std::unique_ptr<Grid3D<float>> grid;
/** histogram/grid holding the particles*/
std::unique_ptr<Grid3D<float>> grid;
/** bandwith for KDE */
Point3 bandwith;
/** bandwith for KDE */
Point3 bandwith;
/** the current mesh */
const NM::NavMesh<Tria>* mesh;
/** the current mesh */
const NM::NavMesh<Tria>* mesh;
public:
public:
/** ctor */
ParticleFilterResamplingKDE(const NM::NavMesh<Tria>* mesh, const Point3 gridsize_m, const Point3 bandwith) {
/** ctor */
ParticleFilterResamplingKDE(const NM::NavMesh<Tria>* mesh, const Point3 gridsize_m, const Point3 bandwith) {
this->mesh = mesh;
this->bandwith = bandwith;
this->bb = mesh->getBBox();
this->bb.grow(10);
this->mesh = mesh;
this->bandwith = bandwith;
this->bb = mesh->getBBox();//_BBox3<float>(mesh->getBBox().getMin() * 100.f, mesh->getBBox().getMax() * 100.f);
this->bb.grow(10);
// Create histogram
size_t nBinsX = (size_t)((this->bb.getMax().x - this->bb.getMin().x) / gridsize_m.x);
size_t nBinsY = (size_t)((this->bb.getMax().y - this->bb.getMin().y) / gridsize_m.y);
size_t nBinsZ = (size_t)((this->bb.getMax().z - this->bb.getMin().z) / gridsize_m.z);
// Create histogram
size_t nBinsX = (size_t)((this->bb.getMax().x - this->bb.getMin().x) / gridsize_m.x);
size_t nBinsY = (size_t)((this->bb.getMax().y - this->bb.getMin().y) / gridsize_m.y);
size_t nBinsZ = (size_t)((this->bb.getMax().z - this->bb.getMin().z) / gridsize_m.z);
this->grid = std::make_unique<Grid3D<float>>(bb, nBinsX, nBinsY, nBinsZ);
//to centimeter
//bb.add(bb.getMin() * 100.0f);
//bb.add(bb.getMax() * 100.0f);
gen.seed(1234);
}
this->grid = std::make_unique<Grid3D<float>>(bb, nBinsX, nBinsY, nBinsZ);
void resample(std::vector<Particle<State>>& particles) override {
gen.seed(1234);
}
// compile-time sanity checks
static_assert( HasOperatorPlusEq<State>::value, "your state needs a += operator!" );
static_assert( HasOperatorDivEq<State>::value, "your state needs a /= operator!" );
static_assert( HasOperatorMul<State>::value, "your state needs a * operator!" );
//static_assert( std::is_constructible<State, Point3>::value, "your state needs a constructor with Point3!");
//todo: static assert for getx, gety, getz, setposition
void resample(std::vector<Particle<State>>& particles) override {
grid->clear();
for (Particle<State> p : particles){
//grid.add receives position in meter!
// compile-time sanity checks
static_assert( HasOperatorPlusEq<State>::value, "your state needs a += operator!" );
static_assert( HasOperatorDivEq<State>::value, "your state needs a /= operator!" );
static_assert( HasOperatorMul<State>::value, "your state needs a * operator!" );
//static_assert( std::is_constructible<State, Point3>::value, "your state needs a constructor with Point3!");
//todo: static assert for getx, gety, getz, setposition
//if weight is to low, remove.
if((float) p.weight > 0 ){
grid->add(p.state.getX(), p.state.getY(), p.state.getZ(), p.weight);
}
}
grid->clear();
for (Particle<State> p : particles){
//grid.add receives position in meter!
int nFilt = 3;
float sigmaX = bandwith.x / grid->binSizeX;
float sigmaY = bandwith.y / grid->binSizeY;
float sigmaZ = bandwith.z / grid->binSizeZ;
//if weight is to low, remove.
if((float) p.weight > 0 ){
grid->add(p.state.getX(), p.state.getY(), p.state.getZ(), p.weight);
}
}
BoxGaus3D<float> boxGaus;
boxGaus.approxGaus(grid->image(), Point3(sigmaX, sigmaY, sigmaZ), nFilt);
int nFilt = 3;
float sigmaX = bandwith.x / grid->binSizeX;
float sigmaY = bandwith.y / grid->binSizeY;
float sigmaZ = bandwith.z / grid->binSizeZ;
// fill a drawlist with N equal distributed particles
// assign them a weight based on the KDE density.
DrawList<Point3> dl;
for (int i = 0; i < 10000; ++i){
NM::NavMeshLocation<Tria> tmpPos = mesh->getRandom().draw();
float weight = grid->fetch(tmpPos.pos.x, tmpPos.pos.y, tmpPos.pos.z);
BoxGaus3D<float> boxGaus;
boxGaus.approxGaus(grid->image(), Point3(sigmaX, sigmaY, sigmaZ), nFilt);
dl.add(tmpPos.pos, weight);
}
// fill a drawlist with N equal distributed particles
// assign them a weight based on the KDE density.
DrawList<Point3> dl;
for (int i = 0; i < 10000; ++i){
NM::NavMeshLocation<Tria> tmpPos = mesh->getRandom().draw(); //TODO: Hier kommen oft seltsame "Randomwerte" raus, die nur innerhalb eines Dreiecks sind. Woran liegt das?
float weight = grid->fetch(tmpPos.pos.x, tmpPos.pos.y, tmpPos.pos.z);
// used the same particles to not lose the heading.
dl.add(tmpPos.pos, weight);
}
// TODO: Summe aller Partikel wird relativ schnell 0! Ich vermute da am Anfang ein einzelner Partikel stark gewichtet ist und alleine
// die Dichte repräsentiert über die KDE. Jetzt wird beim nächsten zufälligen ziehen an dieser Stelle keiner der 10k partikel dort gezogen, d.h. alle
// haben ein Gewicht von 0 und ciao.
// Lösung: erstmal equal weight versuchen. ansonten: warum nehmen wir nochmal diese 10k? und nciht einfach die partikel die wir eh schon haben?
for (int i = 0; i < particles.size(); ++i){
// used the same particles to not lose the heading.
for (int i = 0; i < particles.size(); ++i){
double tmpWeight = 1;
particles[i].state.pos = mesh->getLocation(dl.get(tmpWeight));
particles[i].weight = tmpWeight;
}
}
};
double tmpWeight = 1;
particles[i].state.pos = mesh->getLocation(dl.get(tmpWeight));
particles[i].weight = tmpWeight;
}
}
};
}

4
smc/merging/mixing/MixingSamplerDivergency.h
View File

@@ -100,7 +100,7 @@ namespace SMC {
auto mode = drawMode(cumTransitionModeProbability);
newParticles[k] = drawParticle(mode);
newParticles[k].weight = equalWeight;
newParticles[k].weight = equalWeight; //todo: why equal weight? i guess if the different filters have different representations of probability?
}
focusedFilter.setParticles(newParticles);
@@ -133,7 +133,7 @@ namespace SMC {
/** draw one particle according to its weight from the copy vector of a given mode */
const Particle<State>& drawParticle(ParticleFilterMixing<State, Control, Observation>& filter) {
double weights = filter.getParticles().back().weight;
//double weights = filter.getParticles().back().weight;
// generate random values between [0:cumWeight]
std::uniform_real_distribution<float> dist(0, filter.getParticles().back().weight);