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

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;