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

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];