added the mixing particle filter model with all is might and failures :)

code/filter/Logic.h

@@ -23,7 +23,9 @@
 #include <Indoor/sensors/beacon/model/BeaconModelLogDistCeiling.h>
 #include <Indoor/sensors/beacon/BeaconProbabilityFree.h>
 
-#include <KLib/math/filter/particles/ParticleFilter.h>
+#include <Indoor/math/distribution/Uniform.h>
+
+#include <KLib/math/filter/particles/ParticleFilterMixing.h>
 #include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
 #include <KLib/math/filter/particles/resampling/ParticleFilterResamplingPercent.h>
 
@@ -32,6 +34,40 @@
 #include "../Settings.h"
 
 
+#include  <random>
+#include  <iterator>
+
+template <typename RandomGenerator = std::default_random_engine>
+struct random_selector
+{
+    //On most platforms, you probably want to use std::random_device("/dev/urandom")()
+    random_selector(RandomGenerator g = RandomGenerator(std::random_device()()))
+        : gen(g) {}
+
+    template <typename Iter>
+    Iter select(Iter start, Iter end) {
+        std::uniform_int_distribution<> dis(0, std::distance(start, end) - 1);
+        std::advance(start, dis(gen));
+        return start;
+    }
+
+    //convenience function
+    template <typename Iter>
+    Iter operator()(Iter start, Iter end) {
+        return select(start, end);
+    }
+
+    //convenience function that works on anything with a sensible begin() and end(), and returns with a ref to the value type
+    template <typename Container>
+    auto operator()(const Container& c) -> decltype(*begin(c))& {
+        return *select(begin(c), end(c));
+    }
+
+private:
+    RandomGenerator gen;
+};
+
+
 /** particle-filter init randomly distributed within the building*/
 struct PFInit : public K::ParticleFilterInitializer<MyState> {
 
@@ -83,6 +119,63 @@ struct PFInitFixed : public K::ParticleFilterInitializer<MyState> {
 
 };
 
+/** very simple transition model, just scatter normal distributed  */
+struct PFTransSimple : public K::ParticleFilterTransition<MyState, MyControl>{
+
+    Grid<MyNode>& grid;
+    std::minstd_rand gen;
+    random_selector<> rand;
+    Distribution::Uniform<float> uniRand = Distribution::Uniform<float>(0,1);
+
+    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);
+
+        #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);
+
+            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
+                }
+
+            }
+
+            GridPoint noisePt(noise_cm(gen), noise_cm(gen), height);
+            GridPoint newPosition = p.state.position + noisePt;
+
+            if(grid.hasNodeFor(newPosition)){
+                p.state.position = newPosition;
+            }else{
+                //no new position!
+            }
+
+        }
+    }
+};
+
 /** particle-filter transition */
 struct PFTrans : public K::ParticleFilterTransition<MyState, MyControl> {
 
@@ -165,8 +258,8 @@ struct PFEval : public K::ParticleFilterEvaluation<MyState, MyObs> {
 	/** probability for WIFI */
     inline double getWIFI(const MyObs& observation, const WiFiMeasurements& vapWifi, const GridPoint& point) const {
 
-       const MyNode& node = grid.getNodeFor(point);
-       return wiFiProbability.getProbability(node, observation.currentTime, vapWifi);
+       //const MyNode& node = grid.getNodeFor(point);
+       return wiFiProbability.getProbability(point.inMeter(), observation.currentTime, vapWifi);
     }
 
     /** probability for BEACONS */
@@ -219,15 +312,15 @@ 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);
 
             //small checks
             _assertNotNAN(pWifi, "Wifi prob is nan");
-            _assertNot0(pBaroPressure,"pBaroPressure is null");
+            //_assertNot0(pBaroPressure,"pBaroPressure is null");
 
-            const double prob = pBaroPressure * pWifi;
+            const double prob = pWifi;
 
             p.weight = prob;