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

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){
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();
const double lambda;
//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
std::mt19937_64 rng;
// initialize the random number generator with time-dependent seed
uint64_t timeSeed = std::chrono::high_resolution_clock::now().time_since_epoch().count();
std::seed_seq ss{uint32_t(timeSeed & 0xffffffff), uint32_t(timeSeed>>32)};
rng.seed(ss);
// initialize a uniform distribution between -0.0001 and 0.0001
std::uniform_real_distribution<double> unif(-0.0001, 0.0001);
Distribution::Uniform<float> uniRand = Distribution::Uniform<float>(-0.1, 0.1);
/** ctor */
ModeProbabilityTransitionNormal(double lambda) : lambda(lambda) {;}
virtual Eigen::MatrixXd update(std::vector<K::ParticleFilterMixing<MyState, MyControl, MyObs>>& modes) override {
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

93
code/filter/Logic.h
View File

@@ -72,8 +72,9 @@ private:
struct PFInit : public K::ParticleFilterInitializer<MyState> {
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 {
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.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>{
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;
// draw from 0 - 1
Distribution::Uniform<float> uniRand = Distribution::Uniform<float>(0,1);
/** ctor */
PFTransSimple(Grid<MyNode>& grid) : grid(grid) {}
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)
for (int i = 0; i < Settings::numParticles; ++i) {
K::Particle<MyState>& p = particles[i];
// if neighboring node is a staircase, we have a 0.8 chance to walk them.
GridPoint tmp = grid.getNodeFor(p.state.position);
MyNode tmpNode(tmp);
int numNeigbors = grid.getNumNeighbors(tmpNode);
// // if neighboring node is a staircase, we have a 0.8 chance to walk them.
// GridPoint tmp = grid.getNodeFor(p.state.position);
// MyNode tmpNode(tmp);
// int numNeigbors = grid.getNumNeighbors(tmpNode);
std::vector<MyNode> zNodes;
for(int i = 0; i < numNeigbors; ++i){
// std::vector<MyNode> zNodes;
// for(int i = 0; i < numNeigbors; ++i){
//if neighbor is stair (1) or elevator (2)
MyNode curNode = grid.getNeighbor(tmpNode, i);
if(curNode.getType() == 1 || curNode.getType() == 2){
zNodes.push_back(curNode);
}
}
// //if neighbor is stair (1) or elevator (2)
// MyNode curNode = grid.getNeighbor(tmpNode, i);
// if(curNode.getType() == 1 || curNode.getType() == 2){
// zNodes.push_back(curNode);
// }
// }
float height = 0.0;
if(!zNodes.empty()){
// float height = 0.0;
// if(!zNodes.empty()){
if(uniRand.draw() > 0.3){
//get a random height from all the neighbors on stairs or elevators
height = rand(zNodes).z_cm - p.state.position.z_cm;
}else{
//do nothin
}
// if(uniRand.draw() > 0.3){
// //get a random height from all the neighbors on stairs or elevators
// height = rand(zNodes).z_cm - p.state.position.z_cm;
// }else{
// //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;
if(grid.hasNodeFor(newPosition)){
p.state.position = newPosition;
}else{
//no new position!
}
p.state.position = grid.getNearestNode(newPosition);
// if(grid.hasNodeFor(newPosition)){
// 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);
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
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 {
//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 */
@@ -304,7 +331,7 @@ struct PFEval : public K::ParticleFilterEvaluation<MyState, MyObs> {
double sum = 0;
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) {
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;
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& operator += (const MyState& o) {
this->position += o.position;
return *this;
@@ -40,8 +43,8 @@ struct MyState : public WalkState, public WalkStateHeading, public WalkStateSpre
this->position /= d;
return *this;
}
MyState operator * (const double d) const {
return MyState(this->position*d);
MyState operator * (const double d) const {
return MyState(this->position*d);
}
bool belongsToRegion(const MyState& o) const {
return position.inMeter().getDistance(o.position.inMeter()) < 3.0;