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

code/CMakeLists.txt

@@ -58,7 +58,7 @@ ADD_DEFINITIONS(
 	-fstack-protector-all
 
 	-g3
-        #-O2
+        -O2
 	-march=native
 
 	-DWITH_TESTS

code/Settings.h

@@ -5,12 +5,27 @@
 #include <Indoor/data/Timestamp.h>
 #include <Indoor/sensors/radio/VAPGrouper.h>
 
+#include <eigen3/Eigen/Dense>
+
 namespace Settings {
 
     bool useKLB = true;
 
     const int numParticles = 5000;
 
+    namespace Mode1 {
+        const double modeProbability = 0.5;
+    }
+
+    namespace Mode2 {
+        const double modeProbability = 0.5;
+    }
+
+    namespace Mixing {
+        //Eigen::Matrix2d transitionProbabilityMatrix(1,0,0,1);
+        const double lambda = 0.05;
+    }
+
 	namespace IMU {
         const float turnSigma = 2.5;		// 3.5
         const float stepLength = 1.00;

code/filter/KLB.h

@@ -32,8 +32,7 @@
 #include <Indoor/data/Timestamp.h>
 
 #include <KLib/math/filter/particles/Particle.h>
-#include <KLib/math/filter/particles/ParticleFilter.h>
+#include <KLib/math/filter/particles/ParticleFilterMixing.h>
-#include <KLib/math/filter/particles/ParticleFilterHistory.h>
 #include <KLib/math/filter/particles/ParticleFilterInitializer.h>
 
 #include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverage.h>
@@ -45,12 +44,81 @@
 #include <KLib/math/filter/particles/resampling/ParticleFilterResamplingPercent.h>
 #include <KLib/math/filter/particles/resampling/ParticleFilterResamplingDivergence.h>
 
+#include <KLib/math/filter/merging/MarkovTransitionProbability.h>
+#include <KLib/math/filter/merging/mixing/MixingSamplerDivergency.h>
+#include <KLib/math/filter/merging/estimation/JointEstimationPosteriorOnly.h>
+
 #include "Structs.h"
 
 #include "../Plotti.h"
 #include "Logic.h"
 #include "../Settings.h"
 
+double __KLD = 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 :( :( :(
+
+
+struct ModeProbabilityTransition : public K::MarkovTransitionProbability<MyState, MyControl, MyObs>{
+
+    Grid<MyNode>& grid;
+    const double lambda;
+
+    ModeProbabilityTransition(Grid<MyNode>& grid, double lambda) : grid(grid), lambda(lambda) {;}
+
+    virtual Eigen::MatrixXd update(std::vector<K::ParticleFilterMixing<MyState, MyControl, MyObs>>& modes) override {
+
+        std::vector<double> probsWifiV;
+        std::vector<double> probsParticleV;
+
+        // mode[0] -> Posterior & mode[1] -> Wifi ---- i know what im doing :)
+        for(MyNode node : grid.getNodes()){
+            double probParzenPosterior = calcKernelDensity(node, modes[0].getParticles());
+            probsParticleV.push_back(probParzenPosterior);
+
+            double probParzenWifi = calcKernelDensity(node, modes[1].getParticles());
+            probsWifiV.push_back(probParzenWifi);
+        }
+
+        // make vectors
+        Eigen::Map<Eigen::VectorXd> probsWifi(&probsWifiV[0], probsWifiV.size());
+        Eigen::Map<Eigen::VectorXd> probsParticle(&probsParticleV[0], probsParticleV.size());
+
+        // get kld
+        double kld = Divergence::KullbackLeibler<double>::getGeneralFromSamples(probsParticle, probsWifi, Divergence::LOGMODE::NATURALIS);
+
+        // debugging global variable
+        __KLD = kld;
+
+        //exp. distribution
+        double expKld = std::exp(-lambda * kld);
+
+        //create the matrix
+        Eigen::MatrixXd m(2,2);
+        m << 1-expKld, expKld, 0, 1;
+
+        return m;
+
+    }
+
+    double calcKernelDensity(const MyNode node, const std::vector<K::Particle<MyState>> particles){
+
+        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(node);
+            prob += Distribution::Normal<double>::getProbability(0, 100, distance) * particles[i].weight;
+        }
+
+        return prob;
+    }
+};
+
+
+
 static double getKernelDensityProbability(std::vector<K::Particle<MyState>>& particles, MyState state, std::vector<K::Particle<MyState>>& samplesWifi){
 
     Distribution::KernelDensity<double, MyState> parzen([&](MyState state){
@@ -92,8 +160,8 @@ static double getKernelDensityProbability(std::vector<K::Particle<MyState>>& par
     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::KullbackLeibler<double>::getGeneralFromSamples(probsParticle, probsWifi, Divergence::LOGMODE::NATURALIS);
-    double kld = Divergence::JensenShannon<double>::getGeneralFromSamples(probsParticle, probsWifi, Divergence::LOGMODE::NATURALIS);
+    //double kld = Divergence::JensenShannon<double>::getGeneralFromSamples(probsParticle, probsWifi, Divergence::LOGMODE::NATURALIS);
 
     //plotti
     //plot.debugDistribution1(samplesWifi);
@@ -101,9 +169,9 @@ static double getKernelDensityProbability(std::vector<K::Particle<MyState>>& par
 
 
     //estimate the mean
-    //K::ParticleFilterEstimationOrderedWeightedAverage<MyState> estimateWifi(0.95);
+//    K::ParticleFilterEstimationOrderedWeightedAverage<MyState> estimateWifi(0.95);
-    //const MyState estWifi = estimateWifi.estimate(samplesWifi);
+//    const MyState estWifi = estimateWifi.estimate(samplesWifi);
-    //plot.addEstimationNodeSmoothed(estWifi.position.inMeter());
+//    plot.addEstimationNodeSmoothed(estWifi.position.inMeter());
 
     return kld;
 }

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);
+       //const MyNode& node = grid.getNodeFor(point);
-       return wiFiProbability.getProbability(node, observation.currentTime, vapWifi);
+       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;
 

code/filter/Structs.h

@@ -108,6 +108,8 @@ struct MyNode : public GridPoint, public GridNode, public GridNodeImportance, pu
     /** ctor */
     MyNode(const int x, const int y, const int z) : GridPoint(x,y,z) {;}
 
+    MyNode(const GridPoint point) : GridPoint(point.x_cm, point.y_cm, point.z_cm) {;}
+
     static void staticDeserialize(std::istream& inp) {
         WiFiGridNode::staticDeserialize(inp);
     }

code/main.cpp

@@ -9,6 +9,7 @@
 #include <sys/types.h>
 #include <sys/stat.h>
 
+#include <KLib/math/filter/merging/InteractingMultipleModelParticleFilter.h>
 
 //frank
 //const std::string mapDir = "/mnt/data/workspaces/IPIN2016/IPIN2016/competition/maps/";
@@ -162,32 +163,41 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
     ctrl.resetAfterTransition();
     MyObs obs;
 
-    //random start position
-    std::unique_ptr<K::ParticleFilterInitializer<MyState>> init(new PFInit(grid)); std::move(init);
 
-    //filter init
+    //init the mode filters
-    //std::unique_ptr<PFInit> init =
+    std::vector<K::ParticleFilterMixing<MyState, MyControl, MyObs>> modes;
-    K::ParticleFilterHistory<MyState, MyControl, MyObs> pf(Settings::numParticles, std::unique_ptr<PFInit>(new PFInit(grid)));
-    //K::ParticleFilterHistory<MyState, MyControl, MyObs> pf(Settings::numParticles, std::unique_ptr<PFInitFixed>(new PFInitFixed(grid, GridPoint(1120.0f, 750.0f, 740.0f), 90.0f)));
-    pf.setTransition(std::unique_ptr<PFTrans>(new PFTrans(grid, &ctrl)));
-    pf.setEvaluation(std::unique_ptr<PFEval>(new PFEval(WiFiModel, beaconModel, grid)));
 
-    //resampling
+    std::shared_ptr<K::ParticleFilterInitializer<MyState>> init(new PFInit(grid));
-    if(Settings::useKLB){
+    //std::shared_ptr<K::ParticleFilterInitializer<MyState>> init(new PFInitFixed(grid, GridPoint(1120.0f, 750.0f, 740.0f), 90.0f));
-        pf.setResampling(std::unique_ptr<K::ParticleFilterResamplingDivergence<MyState>>(new K::ParticleFilterResamplingDivergence<MyState>()));
-    } else {
-        //pf.setResampling(std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()));
-        pf.setResampling(std::unique_ptr<K::ParticleFilterResamplingPercent<MyState>>(new K::ParticleFilterResamplingPercent<MyState>(0.4)));
-        //pf.setResampling(std::unique_ptr<NodeResampling<MyState, MyNode>>(new NodeResampling<MyState, MyNode>(*grid)););
-    }
 
-    pf.setNEffThreshold(0.95);
+    // mode 1
+    K::ParticleFilterMixing<MyState, MyControl, MyObs> mode1(Settings::numParticles, init, Settings::Mode1::modeProbability);
+    mode1.setTransition(std::shared_ptr<PFTrans>(new PFTrans(grid, &ctrl)));
+    mode1.setEvaluation(std::shared_ptr<PFEval>(new PFEval(WiFiModel, beaconModel, grid)));
+    mode1.setResampling(std::shared_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()));
+    mode1.setNEffThreshold(0.95);
+    mode1.setEstimator(std::shared_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
 
-    //estimation
+    modes.push_back(mode1);
-    //pf.setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
+
-    //pf.setEstimation(std::unique_ptr<K::ParticleFilterEstimationRegionalWeightedAverage<MyState>>(new K::ParticleFilterEstimationRegionalWeightedAverage<MyState>()));
+    // mode 2
-    pf.setEstimation(std::unique_ptr<K::ParticleFilterEstimationOrderedWeightedAverage<MyState>>(new K::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.5)));
+    K::ParticleFilterMixing<MyState, MyControl, MyObs> mode2(Settings::numParticles, init, Settings::Mode2::modeProbability);
-    //pf.setEstimation(std::unique_ptr<K::ParticleFilterEstimationKernelDensity<MyState, 3>>(new K::ParticleFilterEstimationKernelDensity<MyState, 3>()));
+    mode2.setTransition(std::shared_ptr<PFTransSimple>(new PFTransSimple(grid)));
+    mode2.setEvaluation(std::shared_ptr<PFEval>(new PFEval(WiFiModel, beaconModel, grid)));
+    mode2.setResampling(std::shared_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()));
+    mode2.setNEffThreshold(0.95);
+    mode2.setEstimator(std::shared_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
+
+    modes.push_back(mode2);
+
+    //init mixing
+    Eigen::MatrixXd transitionProbabilityMatrix(2,2);
+    transitionProbabilityMatrix << 1,0,0,1;
+
+    K::InteractingMultipleModelParticleFilter<MyState, MyControl, MyObs> IMMAPF(modes, transitionProbabilityMatrix);
+    IMMAPF.setMixingSampler(std::unique_ptr<K::MixingSamplerDivergency<MyState, MyControl, MyObs>>(new K::MixingSamplerDivergency<MyState, MyControl, MyObs>()));
+    IMMAPF.setJointEstimation(std::unique_ptr<K::JointEstimationPosteriorOnly<MyState, MyControl, MyObs>>(new K::JointEstimationPosteriorOnly<MyState, MyControl, MyObs>()));
+    IMMAPF.setMarkovTransitionProbability(std::unique_ptr<ModeProbabilityTransition>(new ModeProbabilityTransition(grid, Settings::Mixing::lambda)));
 
 
     Timestamp lastTimestamp = Timestamp::fromMS(0);
@@ -201,9 +211,6 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
 
     K::Statistics<float> errorStats;
 
-    //calc wi-fi prob for every node and get mean vector
-    WiFiObserverFree wiFiProbability(Settings::WiFiModel::sigma, WiFiModel);
-
 
     //file writing for error data
     const long int t = static_cast<long int>(time(NULL));
@@ -256,7 +263,6 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
 
         } else if (e.type == Offline::Sensor::GRAVITY) {
             md.addGravity(ts, fr.getGravity()[e.idx].data);
-            Eigen::Vector2f curVec = md.getCurrentMotionAxis();
             ctrl.motionDeltaAngle_rad = md.getMotionChangeInRad();
         }
 
@@ -265,34 +271,14 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
             obs.currentTime = ts;
 
             MyState est;
-            if(Settings::useKLB){
 
-                const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(obs.wifi);
+            //update filter
+            est = IMMAPF.update(&ctrl, obs);
 
-                std::vector<MyNode> allNodes = grid.getNodes();
+            if(kld_data.empty()){
-                std::vector<K::Particle<MyState>> particleWifi;
+                kld_data.push_back(0.0);
-                for(MyNode node : allNodes){
+            } else{
-                    double prob = wiFiProbability.getProbability(node, ts, wifiObs);
+                kld_data.push_back(__KLD);
-                    K::Particle<MyState> tmp (MyState(GridPoint(node.x_cm, node.y_cm, node.z_cm)), prob);
-                    particleWifi.push_back(tmp);
-                }
-
-                if(kld_data.empty()){
-                    kld_data.push_back(0.0);
-                }
-
-                double kld = 0.0;
-
-                //set probability distributions.
-                //std::function<double(std::vector<K::Particle<MyState>>&, MyState, std::vector<K::Particle<MyState>>&)> kldFunc = getKernelDensityProbability;
-                std::function<double(std::vector<K::Particle<MyState>>&, MyState, std::vector<K::Particle<MyState>>&)> kldFunc = kldFromMultivariatNormal;
-
-                //update filter
-                est = pf.update(&ctrl, obs, particleWifi, kldFunc, kld);
-
-                kld_data.push_back(kld);
-            } else {
-                est = pf.update(&ctrl, obs);
             }
 
             Point3 estPos = est.position.inMeter();
@@ -314,7 +300,8 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
             plot.setEst(estPos);
             plot.setGT(gtPos);
             plot.addEstimationNode(estPos);
-            plot.addParticles(pf.getParticles());
+            plot.addParticles(IMMAPF.getModes()[0].getParticles());
+            plot.addEstimationNodeSmoothed(IMMAPF.getModes()[1].getEstimation().position.inMeter());
 
             //plot.gp << "set arrow 919 from " << tt.pos.x << "," << tt.pos.y << "," << tt.pos.z << " to "<< tt.pos.x << "," << tt.pos.y << "," << tt.pos.z+1 << "lw 3\n";
 
@@ -404,13 +391,13 @@ int main(int argc, char** argv) {
     //run(data.BERKWERK, 6, "EVALBERGWERK");    // Nexus vor
 
     //for(int i = 0; i < 5; ++i){
-        Settings::useKLB = false;
+//        Settings::useKLB = false;
-        //run(data.IPIN2017, 0, "ipin2017", Settings::Paths_IPIN2017::path1);
+//        //run(data.IPIN2017, 0, "ipin2017", Settings::Paths_IPIN2017::path1);
-        run(data.IPIN2017, 1, "ipin2017", Settings::Paths_IPIN2017::path1);
+//        run(data.IPIN2017, 1, "ipin2017", Settings::Paths_IPIN2017::path1);
-        run(data.IPIN2017, 2, "ipin2017", Settings::Paths_IPIN2017::path2);
+//        run(data.IPIN2017, 2, "ipin2017", Settings::Paths_IPIN2017::path2);
-        //run(data.IPIN2017, 3, "ipin2017", Settings::Paths_IPIN2017::path2);
+//        //run(data.IPIN2017, 3, "ipin2017", Settings::Paths_IPIN2017::path2);
-        run(data.IPIN2017, 5, "ipin2017", Settings::Paths_IPIN2017::path3);
+//        run(data.IPIN2017, 5, "ipin2017", Settings::Paths_IPIN2017::path3);
-        //run(data.IPIN2017, 4, "ipin2017", Settings::Paths_IPIN2017::path3);
+//        //run(data.IPIN2017, 4, "ipin2017", Settings::Paths_IPIN2017::path3);
 
         Settings::useKLB = true;
         //run(data.IPIN2017, 0, "ipin2017", Settings::Paths_IPIN2017::path1);