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

change simple transition model

Browse Source 
added klb transition models
added debugging output
This commit is contained in:
toni
2017-04-18 11:18:37 +02:00
parent 1f6df67010
commit fdbd984584
7 changed files with 190 additions and 226 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
code/CMakeLists.txt
View File

@@ -58,7 +58,7 @@ ADD_DEFINITIONS(
-fstack-protector-all -fstack-protector-all
-g3 -g3
-O2 #-O2
-march=native -march=native
-DWITH_TESTS -DWITH_TESTS

22
code/Plotti.h
View File

@@ -34,7 +34,8 @@ struct Plotti {
K::GnuplotSplotElementLines pStairs; K::GnuplotSplotElementLines pStairs;
K::GnuplotSplotElementPoints pAPs; K::GnuplotSplotElementPoints pAPs;
K::GnuplotSplotElementPoints pInterest; K::GnuplotSplotElementPoints pInterest;
K::GnuplotSplotElementPoints pParticles; K::GnuplotSplotElementPoints pParticles1;
K::GnuplotSplotElementPoints pParticles2;
K::GnuplotSplotElementPoints pNormal1; K::GnuplotSplotElementPoints pNormal1;
K::GnuplotSplotElementPoints pNormal2; K::GnuplotSplotElementPoints pNormal2;
K::GnuplotSplotElementColorPoints pDistributation1; K::GnuplotSplotElementColorPoints pDistributation1;
@@ -51,7 +52,8 @@ struct Plotti {
splot.add(&pColorPoints); pColorPoints.setPointSize(0.6); splot.add(&pColorPoints); pColorPoints.setPointSize(0.6);
splot.add(&pDistributation1); pDistributation1.setPointSize(0.6); splot.add(&pDistributation1); pDistributation1.setPointSize(0.6);
splot.add(&pDistributation2); pDistributation2.setPointSize(0.6); splot.add(&pDistributation2); pDistributation2.setPointSize(0.6);
splot.add(&pParticles); pParticles.setColorHex("#0000ff"); pParticles.setPointSize(0.4f); splot.add(&pParticles1); pParticles1.setColorHex("#0000ff"); pParticles1.setPointSize(0.4f);
splot.add(&pParticles2); pParticles2.setColorHex("#ff00ff"); pParticles2.setPointSize(0.4f);
splot.add(&pNormal1); pNormal1.setColorHex("#ff00ff"); pNormal1.setPointSize(0.4f); splot.add(&pNormal1); pNormal1.setColorHex("#ff00ff"); pNormal1.setPointSize(0.4f);
splot.add(&pNormal2); pNormal2.setColorHex("#00aaff"); pNormal2.setPointSize(0.4f); splot.add(&pNormal2); pNormal2.setColorHex("#00aaff"); pNormal2.setPointSize(0.4f);
splot.add(&pFloor); splot.add(&pFloor);
@@ -295,16 +297,26 @@ struct Plotti {
} }
} }
template <typename State> void addParticles(const std::vector<K::Particle<State>>& particles) { template <typename State> void addParticles1(const std::vector<K::Particle<State>>& particles) {
pParticles.clear(); pParticles1.clear();
int i = 0; int i = 0;
for (const K::Particle<State>& p : particles) { for (const K::Particle<State>& p : particles) {
if (++i % 25 != 0) {continue;} if (++i % 25 != 0) {continue;}
K::GnuplotPoint3 pos(p.state.position.x_cm, p.state.position.y_cm, p.state.position.z_cm); K::GnuplotPoint3 pos(p.state.position.x_cm, p.state.position.y_cm, p.state.position.z_cm);
pParticles.add(pos / 100.0f); pParticles1.add(pos / 100.0f);
} }
} }
template <typename State> void addParticles2(const std::vector<K::Particle<State>>& particles) {
pParticles2.clear();
int i = 0;
for (const K::Particle<State>& p : particles) {
if (++i % 25 != 0) {continue;}
K::GnuplotPoint3 pos(p.state.position.x_cm, p.state.position.y_cm, p.state.position.z_cm);
pParticles2.add(pos / 100.0f);
}
}
void show() { void show() {
gp.draw(splot); gp.draw(splot);
gp.flush(); gp.flush();

2
code/Settings.h
View File

@@ -23,7 +23,7 @@ namespace Settings {
namespace Mixing { namespace Mixing {
//Eigen::Matrix2d transitionProbabilityMatrix(1,0,0,1); //Eigen::Matrix2d transitionProbabilityMatrix(1,0,0,1);
const double lambda = 0.05; const double lambda = 0.01;
} }
namespace IMU { namespace IMU {

219
code/filter/KLB.h
View File

@@ -118,172 +118,71 @@ struct ModeProbabilityTransition : public K::MarkovTransitionProbability<MyState
}; };
struct ModeProbabilityTransitionNormal : public K::MarkovTransitionProbability<MyState, MyControl, MyObs>{
static double getKernelDensityProbability(std::vector<K::Particle<MyState>>& particles, MyState state, std::vector<K::Particle<MyState>>& samplesWifi){ const double lambda;
Distribution::KernelDensity<double, MyState> parzen([&](MyState state){
int size = particles.size();
double prob = 0;
#pragma omp parallel for reduction(+:prob) num_threads(6)
for(int i = 0; i < size; ++i){
double distance = particles[i].state.position.getDistanceInCM(state.position);
prob += Distribution::Normal<double>::getProbability(0, 100, distance) * particles[i].weight;
}
return prob;
;});
std::vector<double> probsWifiV;
std::vector<double> probsParticleV;
//just for plottingstuff
std::vector<K::Particle<MyState>> samplesParticles;
const int step = 4;
int i = 0;
for(K::Particle<MyState> particle : samplesWifi){
if(++i % step != 0){continue;}
MyState state(GridPoint(particle.state.position.x_cm, particle.state.position.y_cm, particle.state.position.z_cm));
double probiParticle = parzen.getProbability(state);
probsParticleV.push_back(probiParticle);
double probiwifi = particle.weight;
probsWifiV.push_back(probiwifi);
//samplesParticles.push_back(K::Particle<MyState>(state, probiParticle));
}
//make vectors
Eigen::Map<Eigen::VectorXd> probsWifi(&probsWifiV[0], probsWifiV.size());
Eigen::Map<Eigen::VectorXd> probsParticle(&probsParticleV[0], probsParticleV.size());
//get divergence
double kld = Divergence::KullbackLeibler<double>::getGeneralFromSamples(probsParticle, probsWifi, Divergence::LOGMODE::NATURALIS);
//double kld = Divergence::JensenShannon<double>::getGeneralFromSamples(probsParticle, probsWifi, Divergence::LOGMODE::NATURALIS);
//plotti
//plot.debugDistribution1(samplesWifi);
//plot.debugDistribution1(samplesParticles);
//estimate the mean
// K::ParticleFilterEstimationOrderedWeightedAverage<MyState> estimateWifi(0.95);
// const MyState estWifi = estimateWifi.estimate(samplesWifi);
// plot.addEstimationNodeSmoothed(estWifi.position.inMeter());
return kld;
}
static double kldFromMultivariatNormal(std::vector<K::Particle<MyState>>& particles, MyState state, std::vector<K::Particle<MyState>>& particleWifi){
//kld: particle die resampling hatten nehmen und nv daraus schätzen. vergleiche mit wi-fi
//todo put this in depletionhelper.h
Point3 estPos = state.position.inMeter();
//this is a hack! it is possible that the sigma of z is getting 0 and therefore the rank decreases to 2 and //this is a hack! it is possible that the sigma of z is getting 0 and therefore the rank decreases to 2 and
//no inverse matrix is possible //no inverse matrix is possible
std::mt19937_64 rng; Distribution::Uniform<float> uniRand = Distribution::Uniform<float>(-0.1, 0.1);
// initialize the random number generator with time-dependent seed
uint64_t timeSeed = std::chrono::high_resolution_clock::now().time_since_epoch().count(); /** ctor */
std::seed_seq ss{uint32_t(timeSeed & 0xffffffff), uint32_t(timeSeed>>32)}; ModeProbabilityTransitionNormal(double lambda) : lambda(lambda) {;}
rng.seed(ss);
// initialize a uniform distribution between -0.0001 and 0.0001 virtual Eigen::MatrixXd update(std::vector<K::ParticleFilterMixing<MyState, MyControl, MyObs>>& modes) override {
std::uniform_real_distribution<double> unif(-0.0001, 0.0001);
Assert::equal(modes[0].getParticles().size(), modes[1].getParticles().size(), "Particle.size() differs!");
// create eigen matrix for posterior and wifi
Eigen::MatrixXd mParticle(modes[0].getParticles().size(), 3);
Eigen::MatrixXd mWifi(modes[1].getParticles().size(), 3);
#pragma omp parallel for num_threads(6)
for(int i = 0; i < modes[0].getParticles().size(); ++i){
mParticle(i,0) = (modes[0].getParticles()[i].state.position.x_cm / 100.0) + uniRand.draw();
mParticle(i,1) = (modes[0].getParticles()[i].state.position.y_cm / 100.0) + uniRand.draw();
mParticle(i,2) = (modes[0].getParticles()[i].state.position.z_cm / 100.0) + uniRand.draw();
mWifi(i,0) = (modes[1].getParticles()[i].state.position.x_cm / 100.0) + uniRand.draw();
mWifi(i,1) = (modes[1].getParticles()[i].state.position.y_cm / 100.0) + uniRand.draw();
mWifi(i,2) = (modes[1].getParticles()[i].state.position.z_cm / 100.0) + uniRand.draw();
}
// create normal distributions
Eigen::VectorXd meanParticle(3);
Point3 estParticle = modes[0].getEstimation().position.inMeter();
meanParticle << estParticle.x, estParticle.y, estParticle.z;
Distribution::NormalDistributionN normParticle = Distribution::NormalDistributionN::getNormalNFromSamplesAndMean(mParticle, meanParticle);
Eigen::VectorXd meanWifi(3);
Point3 estWifi = modes[1].getEstimation().position.inMeter();
meanWifi << estWifi.x, estWifi.y, estWifi.z;
Distribution::NormalDistributionN normWifi = Distribution::NormalDistributionN::getNormalNFromSamplesAndMean(mWifi, meanWifi);
// get kld
double kld = Divergence::KullbackLeibler<double>::getMultivariateGauss(normParticle, normWifi);
if(kld > 20){
std::cout << "STTTTTOOOOOOP" << std::endl;
}
// debugging global variable
__KLD = kld;
//exp. distribution
double expKld = std::exp(-lambda * kld);
Assert::isTrue(expKld < 1.0, "exp. distribution greater 1!");
//create the matrix
Eigen::MatrixXd m(2,2);
m << expKld, 1- expKld, 0, 1;
return m;
//create a gauss dist for the current particle approx.
Eigen::MatrixXd m(particles.size(), 3);
for(int i = 0; i < particles.size(); ++i){
m(i,0) = (particles[i].state.position.x_cm / 100.0) + unif(rng);
m(i,1) = (particles[i].state.position.y_cm / 100.0) + unif(rng);
m(i,2) = (particles[i].state.position.z_cm / 100.0) + unif(rng);
} }
};
Eigen::VectorXd mean(3);
mean << estPos.x, estPos.y, estPos.z;
Distribution::NormalDistributionN normParticle = Distribution::NormalDistributionN::getNormalNFromSamplesAndMean(m, mean);
//create a gauss dist for wifi
Eigen::MatrixXd covWifi(3,3);
covWifi << Settings::WiFiModel::sigma, 0, 0,
0, Settings::WiFiModel::sigma, 0,
0, 0, 0.01;
// //calc wi-fi prob for every node and get mean vector
// WiFiObserverFree wiFiProbability(Settings::WiFiModel::sigma, model);
// const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(obs.wifi);
// std::vector<MyNode> allNodes = grid.getNodes();
// std::vector<K::Particle<MyState>> particleWifi;
// //problem! dadurch das ich nur die nodes nehme, verschiebt sich der mittelwert natürlich in die mitte des gebäudes und nicht an den rand
// //muss also die verteilung über mehr nodes oder sampling erstellen!! mittelwert fehler!!!!
// //#pragma omp parallel for num_threads(6)
// for(MyNode node : allNodes){
// double prob = wiFiProbability.getProbability(node, ts, wifiObs);
// K::Particle<MyState> tmp (MyState(GridPoint(node.x_cm, node.y_cm, node.z_cm)), prob);
// //#pragma omp critical
// particleWifi.push_back(tmp);
// }
// std::vector<double> floors;
// floors.push_back(0.0);
// floors.push_back(4.0);
// floors.push_back(7.4);
// floors.push_back(10.8);
// #pragma omp parallel for num_threads(6)
// for(int x = -20; x < 100; ++x){
// for(int y = -20; y < 75; ++y){
// for(double z : floors){
// double X = x;// / 10.0;
// double Y = y;// / 10.0;
// double Z = z;// / 10.0;
// Point3 pt(X,Y,Z);
// double prob = wiFiProbability.getProbability(pt + Point3(0,0,1.3), ts, wifiObs);
// K::Particle<MyState> tmp (MyState(GridPoint(X * 100.0, Y * 100.0, Z * 100.0)), prob);
// #pragma omp critical
// particleWifi.push_back(tmp);
// }
// }
// }
//estimate the mean
K::ParticleFilterEstimationOrderedWeightedAverage<MyState> estimateWifi(0.95);
const MyState estWifi = estimateWifi.estimate(particleWifi);
//get matrix with wifi particles
// Eigen::MatrixXd mW(particleWifi.size(), 3);
// for(int i = 0; i < particleWifi.size(); ++i){
// mW(i,0) = particleWifi[i].state.position.x_cm / 100.0;
// mW(i,1) = particleWifi[i].state.position.y_cm / 100.0;
// mW(i,2) = estWifi.position.z_cm / 100.0;
// }
Eigen::VectorXd meanWifi(3);
meanWifi << estWifi.position.x_cm / 100.0, estWifi.position.y_cm / 100.0, estWifi.position.z_cm / 100.0;
Distribution::NormalDistributionN normWifi(meanWifi, covWifi);
//Distribution::NormalDistributionN normWifi = Distribution::NormalDistributionN::getNormalNFromSamplesAndMean(mW, meanWifi);
//get the kld distance
double kld = Divergence::KullbackLeibler<double>::getMultivariateGauss(normParticle, normWifi);
//plot.debugDistribution1(particleWifi);
//plot.drawNormalN1(normParticle);
//plot.drawNormalN2(normWifi);
//plot.addEstimationNodeSmoothed(estWifi.position.inMeter());
return kld;
}
#endif // KLB_H #endif // KLB_H

93
code/filter/Logic.h
View File

@@ -72,8 +72,9 @@ private:
struct PFInit : public K::ParticleFilterInitializer<MyState> { struct PFInit : public K::ParticleFilterInitializer<MyState> {
Grid<MyNode>& grid; Grid<MyNode>& grid;
int mode;
PFInit(Grid<MyNode>& grid) : grid(grid) {;} PFInit(Grid<MyNode>& grid, int mode) : grid(grid), mode(mode) {;}
virtual void initialize(std::vector<K::Particle<MyState>>& particles) override { virtual void initialize(std::vector<K::Particle<MyState>>& particles) override {
for (K::Particle<MyState>& p : particles) { for (K::Particle<MyState>& p : particles) {
@@ -85,6 +86,9 @@ struct PFInit : public K::ParticleFilterInitializer<MyState> {
p.state.relativePressure = 0; // start with a relative pressure of 0 p.state.relativePressure = 0; // start with a relative pressure of 0
p.weight = 1.0 / particles.size(); // equal weight p.weight = 1.0 / particles.size(); // equal weight
//for debugging
p.state.curMode = mode;
} }
} }
@@ -123,56 +127,77 @@ struct PFInitFixed : public K::ParticleFilterInitializer<MyState> {
struct PFTransSimple : public K::ParticleFilterTransition<MyState, MyControl>{ struct PFTransSimple : public K::ParticleFilterTransition<MyState, MyControl>{
Grid<MyNode>& grid; Grid<MyNode>& grid;
std::minstd_rand gen;
// define the noise
Distribution::Normal<float> noise_cm = Distribution::Normal<float>(0.0, Settings::IMU::stepLength * 2.0 * 100.0);
Distribution::Normal<float> height = Distribution::Normal<float>(0.0, 600.0);
// draw randomly from a vector
random_selector<> rand; random_selector<> rand;
// draw from 0 - 1
Distribution::Uniform<float> uniRand = Distribution::Uniform<float>(0,1); Distribution::Uniform<float> uniRand = Distribution::Uniform<float>(0,1);
/** ctor */
PFTransSimple(Grid<MyNode>& grid) : grid(grid) {} PFTransSimple(Grid<MyNode>& grid) : grid(grid) {}
virtual void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* control) override { virtual void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* control) override {
std::normal_distribution<float> noise_cm(0.0, Settings::IMU::stepLength * 2.0 * 100.0);
int noNewPositionCounter = 0;
#pragma omp parallel for num_threads(6) #pragma omp parallel for num_threads(6)
for (int i = 0; i < Settings::numParticles; ++i) { for (int i = 0; i < Settings::numParticles; ++i) {
K::Particle<MyState>& p = particles[i]; K::Particle<MyState>& p = particles[i];
// if neighboring node is a staircase, we have a 0.8 chance to walk them. // // if neighboring node is a staircase, we have a 0.8 chance to walk them.
GridPoint tmp = grid.getNodeFor(p.state.position); // GridPoint tmp = grid.getNodeFor(p.state.position);
MyNode tmpNode(tmp); // MyNode tmpNode(tmp);
int numNeigbors = grid.getNumNeighbors(tmpNode); // int numNeigbors = grid.getNumNeighbors(tmpNode);
std::vector<MyNode> zNodes; // std::vector<MyNode> zNodes;
for(int i = 0; i < numNeigbors; ++i){ // for(int i = 0; i < numNeigbors; ++i){
//if neighbor is stair (1) or elevator (2) // //if neighbor is stair (1) or elevator (2)
MyNode curNode = grid.getNeighbor(tmpNode, i); // MyNode curNode = grid.getNeighbor(tmpNode, i);
if(curNode.getType() == 1 || curNode.getType() == 2){ // if(curNode.getType() == 1 || curNode.getType() == 2){
zNodes.push_back(curNode); // zNodes.push_back(curNode);
} // }
} // }
float height = 0.0; // float height = 0.0;
if(!zNodes.empty()){ // if(!zNodes.empty()){
if(uniRand.draw() > 0.3){ // if(uniRand.draw() > 0.3){
//get a random height from all the neighbors on stairs or elevators // //get a random height from all the neighbors on stairs or elevators
height = rand(zNodes).z_cm - p.state.position.z_cm; // height = rand(zNodes).z_cm - p.state.position.z_cm;
}else{ // }else{
//do nothin // //do nothin
} // }
} // }
GridPoint noisePt(noise_cm(gen), noise_cm(gen), height);
double diffHeight = p.state.position.z_cm + height.draw();
if()
GridPoint noisePt(noise_cm.draw(), noise_cm.draw(), height.draw());
GridPoint newPosition = p.state.position + noisePt; GridPoint newPosition = p.state.position + noisePt;
if(grid.hasNodeFor(newPosition)){ p.state.position = grid.getNearestNode(newPosition);
p.state.position = newPosition;
}else{ // if(grid.hasNodeFor(newPosition)){
//no new position! // p.state.position = newPosition;
} // }else{
// //no new position!
// #pragma omp atomic
// noNewPositionCounter++;
// }
} }
// std::cout << noNewPositionCounter << std::endl;
} }
}; };
@@ -214,7 +239,9 @@ struct PFTrans : public K::ParticleFilterTransition<MyState, MyControl> {
std::normal_distribution<float> noise(0, Settings::IMU::stepSigma); std::normal_distribution<float> noise(0, Settings::IMU::stepSigma);
for (K::Particle<MyState>& p : particles) { #pragma omp parallel for num_threads(6)
for (int i = 0; i < Settings::numParticles; ++i) {
K::Particle<MyState>& p = particles[i];
// save old position // save old position
p.state.positionOld = p.state.position; //GridPoint(p.state.position.x_cm, p.state.position.y_cm, p.state.position.z_cm); p.state.positionOld = p.state.position; //GridPoint(p.state.position.x_cm, p.state.position.y_cm, p.state.position.z_cm);
@@ -259,7 +286,7 @@ struct PFEval : public K::ParticleFilterEvaluation<MyState, MyObs> {
inline double getWIFI(const MyObs& observation, const WiFiMeasurements& vapWifi, const GridPoint& point) const { inline double getWIFI(const MyObs& observation, const WiFiMeasurements& vapWifi, const GridPoint& point) const {
//const MyNode& node = grid.getNodeFor(point); //const MyNode& node = grid.getNodeFor(point);
return wiFiProbability.getProbability(point.inMeter(), observation.currentTime, vapWifi); return wiFiProbability.getProbability(point.inMeter() + Point3(0,0,1.3), observation.currentTime, vapWifi);
} }
/** probability for BEACONS */ /** probability for BEACONS */
@@ -304,7 +331,7 @@ struct PFEval : public K::ParticleFilterEvaluation<MyState, MyObs> {
double sum = 0; double sum = 0;
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi); const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi);
#pragma omp parallel for num_threads(3) #pragma omp parallel for num_threads(6)
for (int i = 0; i < Settings::numParticles; ++i) { for (int i = 0; i < Settings::numParticles; ++i) {
K::Particle<MyState>& p = particles[i]; K::Particle<MyState>& p = particles[i];

9
code/filter/Structs.h
View File

@@ -28,10 +28,13 @@ struct MyState : public WalkState, public WalkStateHeading, public WalkStateSpre
GridPoint positionOld; GridPoint positionOld;
MyState() : WalkState(GridPoint(0,0,0)), WalkStateHeading(Heading(0), 0), positionOld(0,0,0), relativePressure(0) {;} int curMode;
MyState() : WalkState(GridPoint(0,0,0)), WalkStateHeading(Heading(0), 0), positionOld(0,0,0), relativePressure(0) {;}
MyState(GridPoint pos) : WalkState(pos), WalkStateHeading(Heading(0), 0), positionOld(0,0,0), relativePressure(0) {;} MyState(GridPoint pos) : WalkState(pos), WalkStateHeading(Heading(0), 0), positionOld(0,0,0), relativePressure(0) {;}
MyState& operator += (const MyState& o) { MyState& operator += (const MyState& o) {
this->position += o.position; this->position += o.position;
return *this; return *this;
@@ -40,8 +43,8 @@ struct MyState : public WalkState, public WalkStateHeading, public WalkStateSpre
this->position /= d; this->position /= d;
return *this; return *this;
} }
MyState operator * (const double d) const { MyState operator * (const double d) const {
return MyState(this->position*d); return MyState(this->position*d);
} }
bool belongsToRegion(const MyState& o) const { bool belongsToRegion(const MyState& o) const {
return position.inMeter().getDistance(o.position.inMeter()) < 3.0; return position.inMeter().getDistance(o.position.inMeter()) < 3.0;

69
code/main.cpp
View File

@@ -166,12 +166,11 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
//init the mode filters //init the mode filters
std::vector<K::ParticleFilterMixing<MyState, MyControl, MyObs>> modes; std::vector<K::ParticleFilterMixing<MyState, MyControl, MyObs>> modes;
std::shared_ptr<K::ParticleFilterInitializer<MyState>> init(new PFInit(grid));
//std::shared_ptr<K::ParticleFilterInitializer<MyState>> init(new PFInitFixed(grid, GridPoint(1120.0f, 750.0f, 740.0f), 90.0f)); //std::shared_ptr<K::ParticleFilterInitializer<MyState>> init(new PFInitFixed(grid, GridPoint(1120.0f, 750.0f, 740.0f), 90.0f));
// mode 1 // mode 1
K::ParticleFilterMixing<MyState, MyControl, MyObs> mode1(Settings::numParticles, init, Settings::Mode1::modeProbability); std::shared_ptr<K::ParticleFilterInitializer<MyState>> initMode1(new PFInit(grid, 1));
K::ParticleFilterMixing<MyState, MyControl, MyObs> mode1(Settings::numParticles, initMode1, Settings::Mode1::modeProbability);
mode1.setTransition(std::shared_ptr<PFTrans>(new PFTrans(grid, &ctrl))); mode1.setTransition(std::shared_ptr<PFTrans>(new PFTrans(grid, &ctrl)));
mode1.setEvaluation(std::shared_ptr<PFEval>(new PFEval(WiFiModel, beaconModel, grid))); mode1.setEvaluation(std::shared_ptr<PFEval>(new PFEval(WiFiModel, beaconModel, grid)));
mode1.setResampling(std::shared_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>())); mode1.setResampling(std::shared_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()));
@@ -181,7 +180,8 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
modes.push_back(mode1); modes.push_back(mode1);
// mode 2 // mode 2
K::ParticleFilterMixing<MyState, MyControl, MyObs> mode2(Settings::numParticles, init, Settings::Mode2::modeProbability); std::shared_ptr<K::ParticleFilterInitializer<MyState>> initMode2(new PFInit(grid, 2));
K::ParticleFilterMixing<MyState, MyControl, MyObs> mode2(Settings::numParticles, initMode2, Settings::Mode2::modeProbability);
mode2.setTransition(std::shared_ptr<PFTransSimple>(new PFTransSimple(grid))); mode2.setTransition(std::shared_ptr<PFTransSimple>(new PFTransSimple(grid)));
mode2.setEvaluation(std::shared_ptr<PFEval>(new PFEval(WiFiModel, beaconModel, grid))); mode2.setEvaluation(std::shared_ptr<PFEval>(new PFEval(WiFiModel, beaconModel, grid)));
mode2.setResampling(std::shared_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>())); mode2.setResampling(std::shared_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()));
@@ -197,7 +197,8 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
K::InteractingMultipleModelParticleFilter<MyState, MyControl, MyObs> IMMAPF(modes, transitionProbabilityMatrix); K::InteractingMultipleModelParticleFilter<MyState, MyControl, MyObs> IMMAPF(modes, transitionProbabilityMatrix);
IMMAPF.setMixingSampler(std::unique_ptr<K::MixingSamplerDivergency<MyState, MyControl, MyObs>>(new K::MixingSamplerDivergency<MyState, MyControl, MyObs>())); IMMAPF.setMixingSampler(std::unique_ptr<K::MixingSamplerDivergency<MyState, MyControl, MyObs>>(new K::MixingSamplerDivergency<MyState, MyControl, MyObs>()));
IMMAPF.setJointEstimation(std::unique_ptr<K::JointEstimationPosteriorOnly<MyState, MyControl, MyObs>>(new K::JointEstimationPosteriorOnly<MyState, MyControl, MyObs>())); IMMAPF.setJointEstimation(std::unique_ptr<K::JointEstimationPosteriorOnly<MyState, MyControl, MyObs>>(new K::JointEstimationPosteriorOnly<MyState, MyControl, MyObs>()));
IMMAPF.setMarkovTransitionProbability(std::unique_ptr<ModeProbabilityTransition>(new ModeProbabilityTransition(grid, Settings::Mixing::lambda))); //IMMAPF.setMarkovTransitionProbability(std::unique_ptr<ModeProbabilityTransition>(new ModeProbabilityTransition(grid, Settings::Mixing::lambda)));
IMMAPF.setMarkovTransitionProbability(std::unique_ptr<ModeProbabilityTransitionNormal>(new ModeProbabilityTransitionNormal(Settings::Mixing::lambda)));
Timestamp lastTimestamp = Timestamp::fromMS(0); Timestamp lastTimestamp = Timestamp::fromMS(0);
@@ -299,8 +300,9 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
plot.setEst(estPos); plot.setEst(estPos);
plot.setGT(gtPos); plot.setGT(gtPos);
plot.addParticles1(IMMAPF.getModes()[0].getParticles());
plot.addParticles2(IMMAPF.getModes()[1].getParticles());
plot.addEstimationNode(estPos); plot.addEstimationNode(estPos);
plot.addParticles(IMMAPF.getModes()[0].getParticles());
plot.addEstimationNodeSmoothed(IMMAPF.getModes()[1].getEstimation().position.inMeter()); plot.addEstimationNodeSmoothed(IMMAPF.getModes()[1].getEstimation().position.inMeter());
//plot.gp << "set arrow 919 from " << tt.pos.x << "," << tt.pos.y << "," << tt.pos.z << " to "<< tt.pos.x << "," << tt.pos.y << "," << tt.pos.z+1 << "lw 3\n"; //plot.gp << "set arrow 919 from " << tt.pos.x << "," << tt.pos.y << "," << tt.pos.z << " to "<< tt.pos.x << "," << tt.pos.y << "," << tt.pos.z+1 << "lw 3\n";
@@ -308,10 +310,34 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
//plot.gp << "set label 1001 at screen 0.02, 0.98 'base:" << relBaro.getBaseAvg() << " sigma:" << relBaro.getSigma() << " cur:" << relBaro.getPressureRealtiveToStart() << " hPa " << -relBaro.getPressureRealtiveToStart()/0.10/4.0f << " floor'\n"; //plot.gp << "set label 1001 at screen 0.02, 0.98 'base:" << relBaro.getBaseAvg() << " sigma:" << relBaro.getSigma() << " cur:" << relBaro.getPressureRealtiveToStart() << " hPa " << -relBaro.getPressureRealtiveToStart()/0.10/4.0f << " floor'\n";
int minutes = static_cast<int>(ts.sec()) / 60; int minutes = static_cast<int>(ts.sec()) / 60;
plot.gp << "set label 1002 at screen 0.02, 0.94 'Time: " << minutes << ":" << static_cast<int>(static_cast<int>(ts.sec())%60) << "'\n"; plot.gp << "set label 1002 at screen 0.02, 0.94 'Time: " << minutes << ":" << static_cast<int>(static_cast<int>(ts.sec())%60) << "'\n";
if(Settings::useKLB){ plot.gp << "set label 1003 at screen 0.02, 0.92 'KLD: " << ":" << kld_data.back() << "'\n";
plot.gp << "set label 1002 at screen 0.04, 0.94 'KLD: " << ":" << kld_data.back() << "'\n"; plot.gp << "set label 1004 at screen 0.90, 0.98 'act:" << obs.activity << "'\n";
plot.gp << "set label 1005 at screen 0.90, 0.08 'Prob. Mode1:" << IMMAPF.getModes()[0].getModePosteriorProbability() << "'\n";
plot.gp << "set label 1006 at screen 0.90, 0.06 'Prob. Mode2:" << IMMAPF.getModes()[1].getModePosteriorProbability() << "'\n";
int ones = 0;
int twos = 0;
for(int i = 0; i < IMMAPF.getModes()[0].getParticles().size(); ++i){
int mode1 = IMMAPF.getModes()[0].getParticles()[i].state.curMode;
int mode2 = IMMAPF.getModes()[1].getParticles()[i].state.curMode;
if(mode1 == 1){
++ones;
} else {
++twos;
}
if(mode2 == 1){
++ones;
} else {
++twos;
}
} }
plot.gp << "set label 1002 at screen 0.98, 0.98 'act:" << obs.activity << "'\n";
plot.gp << "set label 1007 at screen 0.90, 0.04 'Part. Mode1:" << ones << "'\n";
plot.gp << "set label 1008 at screen 0.90, 0.02 'Part. Mode2:" << twos << "'\n";
// error between GT and estimation // error between GT and estimation
float err_m = gtPos.getDistance(estPos); float err_m = gtPos.getDistance(estPos);
@@ -362,24 +388,22 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
K::GnuplotPlotElementLines lines; K::GnuplotPlotElementLines lines;
//save as screenshot for klb //save as screenshot for klb
if(Settings::useKLB){ std::string path = evalDir + "/image" + std::to_string(numFile) + "_" + std::to_string(t);
std::string path = evalDir + "/image" + std::to_string(numFile) + "_" + std::to_string(t); gp << "set terminal png size 1280,720\n";
gp << "set terminal png size 1280,720\n"; gp << "set output '" << path << "_shennendistance.png'\n";
gp << "set output '" << path << "_shennendistance.png'\n";
for(int i=0; i < kld_data.size()-1; ++i){ for(int i=0; i < kld_data.size()-1; ++i){
K::GnuplotPoint2 p1(i, kld_data[i]); K::GnuplotPoint2 p1(i, kld_data[i]);
K::GnuplotPoint2 p2(i+1, kld_data[i+1]); K::GnuplotPoint2 p2(i+1, kld_data[i+1]);
lines.addSegment(p1, p2); lines.addSegment(p1, p2);
}
plotkld.add(&lines);
gp.draw(plotkld);
gp.flush();
} }
plotkld.add(&lines);
gp.draw(plotkld);
gp.flush();
std::cout << "finished" << std::endl; std::cout << "finished" << std::endl;
sleep(1); sleep(1);
@@ -399,7 +423,6 @@ int main(int argc, char** argv) {
// run(data.IPIN2017, 5, "ipin2017", Settings::Paths_IPIN2017::path3); // run(data.IPIN2017, 5, "ipin2017", Settings::Paths_IPIN2017::path3);
// //run(data.IPIN2017, 4, "ipin2017", Settings::Paths_IPIN2017::path3); // //run(data.IPIN2017, 4, "ipin2017", Settings::Paths_IPIN2017::path3);
Settings::useKLB = true;
//run(data.IPIN2017, 0, "ipin2017", Settings::Paths_IPIN2017::path1); //run(data.IPIN2017, 0, "ipin2017", Settings::Paths_IPIN2017::path1);
run(data.IPIN2017, 1, "ipin2017", Settings::Paths_IPIN2017::path1); run(data.IPIN2017, 1, "ipin2017", Settings::Paths_IPIN2017::path1);
run(data.IPIN2017, 2, "ipin2017", Settings::Paths_IPIN2017::path2); run(data.IPIN2017, 2, "ipin2017", Settings::Paths_IPIN2017::path2);