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

parameter for normal distirbuation approximation are okay

Browse Source
This commit is contained in:
toni
2017-04-19 14:40:51 +02:00
parent fdbd984584
commit afc253aebf
4 changed files with 94 additions and 65 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
code/filter/KLB.h
View File

@@ -23,6 +23,7 @@
#include <Indoor/sensors/imu/MotionDetection.h>
#include <Indoor/sensors/pressure/RelativePressure.h>
#include <Indoor/sensors/radio/WiFiGridEstimator.h>
#include <Indoor/sensors/radio/WiFiQualityAnalyzer.h>
#include <Indoor/sensors/beacon/model/BeaconModelLogDistCeiling.h>
#include <Indoor/math/MovingAVG.h>
@@ -55,6 +56,7 @@
#include "../Settings.h"
double __KLD = 0.0;
double __QUALITY = 0.0;
//todo function return the transition prob matrix for markov chain!
//getKernelDensityProbability should work fine for first shot! nevertheless we need to do 2 kernel density estimations for both filters :( :( :(
@@ -67,7 +69,7 @@ struct ModeProbabilityTransition : public K::MarkovTransitionProbability<MyState
ModeProbabilityTransition(Grid<MyNode>& grid, double lambda) : grid(grid), lambda(lambda) {;}
virtual Eigen::MatrixXd update(std::vector<K::ParticleFilterMixing<MyState, MyControl, MyObs>>& modes) override {
virtual Eigen::MatrixXd update(std::vector<K::ParticleFilterMixing<MyState, MyControl, MyObs>>& modes, const MyObs& obs) override {
std::vector<double> probsWifiV;
std::vector<double> probsParticleV;
@@ -121,6 +123,7 @@ struct ModeProbabilityTransition : public K::MarkovTransitionProbability<MyState
struct ModeProbabilityTransitionNormal : public K::MarkovTransitionProbability<MyState, MyControl, MyObs>{
const double lambda;
WiFiQualityAnalyzer analyzer;
//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
@@ -129,7 +132,7 @@ struct ModeProbabilityTransitionNormal : public K::MarkovTransitionProbability<M
/** ctor */
ModeProbabilityTransitionNormal(double lambda) : lambda(lambda) {;}
virtual Eigen::MatrixXd update(std::vector<K::ParticleFilterMixing<MyState, MyControl, MyObs>>& modes) override {
virtual Eigen::MatrixXd update(std::vector<K::ParticleFilterMixing<MyState, MyControl, MyObs>>& modes, const MyObs& obs) override {
Assert::equal(modes[0].getParticles().size(), modes[1].getParticles().size(), "Particle.size() differs!");
@@ -159,25 +162,37 @@ struct ModeProbabilityTransitionNormal : public K::MarkovTransitionProbability<M
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;
//calc wi-fi metrik
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(obs.wifi);
if(!wifiObs.entries.empty()){
analyzer.add(wifiObs);
}
float qualityWifi = analyzer.getQuality();
if(std::isnan(qualityWifi)){
qualityWifi = 1.0;
} else if(qualityWifi == 0) {
qualityWifi = 0.00000001;
}
// debugging global variable
__QUALITY = qualityWifi;
// get kld
double kld = Divergence::KullbackLeibler<double>::getMultivariateGauss(normParticle, normWifi);
// debugging global variable
__KLD = kld;
//exp. distribution
double expKld = std::exp(-lambda * kld);
double expKld = std::exp(-lambda * (kld * qualityWifi));
Assert::isTrue(expKld < 1.0, "exp. distribution greater 1!");
//create the matrix
Eigen::MatrixXd m(2,2);
m << expKld, 1- expKld, 0, 1;
m << expKld, 1.0 - expKld, 1 - qualityWifi, qualityWifi;
return m;

78
code/filter/Logic.h
View File

@@ -100,9 +100,10 @@ struct PFInitFixed : public K::ParticleFilterInitializer<MyState> {
Grid<MyNode>& grid;
GridPoint startPos;
float headingDeg;
int mode;
PFInitFixed(Grid<MyNode>& grid, GridPoint startPos, float headingDeg) :
grid(grid), startPos(startPos), headingDeg(headingDeg) {;}
PFInitFixed(Grid<MyNode>& grid, GridPoint startPos, float headingDeg, int mode) :
grid(grid), startPos(startPos), headingDeg(headingDeg), mode(mode) {;}
virtual void initialize(std::vector<K::Particle<MyState>>& particles) override {
@@ -118,6 +119,9 @@ struct PFInitFixed : public K::ParticleFilterInitializer<MyState> {
p.state.heading.error = 0;
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;
}
}
@@ -130,7 +134,7 @@ struct PFTransSimple : public K::ParticleFilterTransition<MyState, MyControl>{
// 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);
Distribution::Normal<float> height_m = Distribution::Normal<float>(0.0, 6.0);
// draw randomly from a vector
random_selector<> rand;
@@ -149,55 +153,35 @@ struct PFTransSimple : public K::ParticleFilterTransition<MyState, MyControl>{
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);
double diffHeight = p.state.position.inMeter().z + height_m.draw();
double newHeight_cm = p.state.position.z_cm;
if(diffHeight > 9.1){
newHeight_cm = 10.8 * 100.0;
} else if (diffHeight < 9.1 && diffHeight > 5.7){
newHeight_cm = 7.4 * 100.0;
} else if (diffHeight < 5.7 && diffHeight > 2.0) {
newHeight_cm = 4.0 * 100.0;
} else {
newHeight_cm = 0.0;
}
// 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);
// }
// }
// 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
// }
// }
double diffHeight = p.state.position.z_cm + height.draw();
if()
GridPoint noisePt(noise_cm.draw(), noise_cm.draw(), height.draw());
GridPoint noisePt(noise_cm.draw(), noise_cm.draw(), 0.0);
GridPoint newPosition = p.state.position + noisePt;
newPosition.z_cm = newHeight_cm;
p.state.position = grid.getNearestNode(newPosition);
// p.state.position = grid.getNearestNode(newPosition);
// if(grid.hasNodeFor(newPosition)){
// p.state.position = newPosition;
// }else{
// //no new position!
// #pragma omp atomic
// noNewPositionCounter++;
// }
if(grid.hasNodeFor(newPosition)){
p.state.position = newPosition;
}else{
//no new position!
#pragma omp atomic
noNewPositionCounter++;
}
}
// std::cout << noNewPositionCounter << std::endl;
std::cout << noNewPositionCounter << std::endl;
}
};
@@ -339,7 +323,7 @@ struct PFEval : public K::ParticleFilterEvaluation<MyState, MyObs> {
// Point3 posOld_m = p.state.positionOld.inMeter();
const double pWifi = getWIFI(observation, wifiObs, p.state.position);
//const double pBaroPressure = getStairProb(p, observation.activity);
const double pBaroPressure = getStairProb(p, observation.activity);
//const double pBaroPressure = getBaroPressure(observation, p.state.relativePressure);
//const double pBeacon = getBEACON(observation, p.state.position);
@@ -347,7 +331,7 @@ struct PFEval : public K::ParticleFilterEvaluation<MyState, MyObs> {
_assertNotNAN(pWifi, "Wifi prob is nan");
//_assertNot0(pBaroPressure,"pBaroPressure is null");
const double prob = pWifi;
const double prob = pWifi * pBaroPressure;
p.weight = prob;