#39 #40 git add for last commit

tests/smc/filtering/TestParticles.cpp

@@ -0,0 +1,44 @@
+/*
+ * TestParticles.cpp
+ *
+ *  Created on: Sep 18, 2013
+ *      Author: Frank Ebner
+ */
+
+#ifdef FIXME
+
+#ifdef WITH_TESTS
+
+#include "../../../Tests.h"
+
+#include "../particles/ParticleFilter.h"
+#include "../particles/ParticleModel.h"
+#include "../particles/ParticleSensor.h"
+#include "../wifi/math/WiFiMath.h"
+#include "../lib/gnuplot/Gnuplot.h"
+#include <iostream>
+#include "../floorplan/FloorPlan.h"
+#include "../floorplan/FloorPlanFactory.h"
+#include "../lib/misc/File.h"
+#include "../particles/ParticleMath.h"
+#include "../particles/resampling/ParticleResamplingSimple.h"
+#include "../particles/resampling/ParticleResamplingNone.h"
+
+#include "../wifi/factory/WiFiHelper.h"
+
+#include <chrono>
+#include <thread>
+
+typedef Point TestState;
+
+
+TEST(Particles, init) {
+
+    //create filter
+
+
+}
+
+#endif
+
+#endif

tests/smc/merging/mixing/TestMixingSamplerDivergency.cpp

@@ -0,0 +1,146 @@
+#ifdef WITH_TESTS
+
+#include "../../../../smc/merging/mixing/MixingSamplerDivergency.h"
+#include "../../../../smc/filtering/ParticleFilterMixing.h"
+
+#include "../../../Tests.h"
+
+namespace K {
+
+    struct MyState {
+        double x;
+        double y;
+        MyState() : x(0), y(0) {;}
+        MyState(double x, double y) : x(x), y(y) {;}
+        MyState& operator += (const MyState& other) {
+            x += other.x;
+            y += other.y;
+            return *this;
+        }
+        MyState& operator /= (double d) {
+            x /= d;
+            y /= d;
+            return *this;
+        }
+        MyState operator * (double d) const {
+            return MyState(x*d, y*d);
+        }
+        MyState& operator = (const MyState& other) {
+            this->x = other.x;
+            this->y = other.y;
+            return *this;
+        }
+        double distance(const MyState& o) const {
+            return std::sqrt( (x-o.x)*(x-o.x) + (y-o.y)*(y-o.y) ) / 4.9;
+        }
+        bool belongsToRegion(const MyState& o) const {
+            return distance(o) <= 1.0;
+        }
+
+    };
+
+    struct MyControl {
+
+    };
+
+    struct MyObservation {
+        double x;
+        double y;
+        MyObservation() : x(0), y(0) {;}
+        void set(double x, double y) {this->x = x; this->y = y;}
+    };
+
+    class MyInitializer1 : public SMC::ParticleFilterInitializer<MyState> {
+        void initialize(std::vector<SMC::Particle<MyState>>& particles) override {
+            for (SMC::Particle<MyState>& p : particles) {
+                p.state.x = 0;
+                p.state.y = 0;
+                p.weight = 1.0 / particles.size();
+            }
+        }
+    };
+
+    class MyInitializer2 : public SMC::ParticleFilterInitializer<MyState> {
+        void initialize(std::vector<SMC::Particle<MyState>>& particles) override {
+            for (SMC::Particle<MyState>& p : particles) {
+                p.state.x = 1;
+                p.state.y = 1;
+                p.weight = 1.0 / particles.size();
+            }
+        }
+    };
+
+
+    TEST(Mixing, standard) {
+
+        //init particle filters
+        int numParticles = 1000;
+        Eigen::MatrixXd transition(2,2);
+        transition << 0.8, 0.2, 0.2, 0.8;
+
+        SMC::ParticleFilterMixing<MyState, MyControl, MyObservation> mode1(numParticles, std::unique_ptr<MyInitializer1>(new MyInitializer1()), 0.5);
+        SMC::ParticleFilterMixing<MyState, MyControl, MyObservation> mode2(numParticles, std::unique_ptr<MyInitializer2>(new MyInitializer2()), 0.5);
+
+        std::vector<SMC::ParticleFilterMixing<MyState, MyControl, MyObservation>> modes;
+        modes.push_back(mode1);
+        modes.push_back(mode2);
+
+
+        SMC::MixingSamplerDivergency<MyState, MyControl, MyObservation> mixer;
+
+        //run the mixing and check results
+        for(int t = 0; t < 100; ++t){
+            mixer.mixAndSample(modes, transition);
+
+            int cnt_zero = 0;
+            int cnt_ones = 0;
+            for(int i = 0; i < modes[0].getParticles().size(); ++i){
+
+                if(modes[0].getParticles()[i].state.x == 0){
+                    cnt_zero++;
+                }else{
+                    cnt_ones++;
+                }
+            }
+
+            std::cout << "MODE1 Zeros: " << (double) cnt_zero /  numParticles<< std::endl;
+            std::cout << "MODE1 Ones: " << (double) cnt_ones/ numParticles << std::endl;
+
+
+            cnt_zero = 0;
+            cnt_ones = 0;
+            for(int i = 0; i < modes[0].getParticles().size(); ++i){
+
+                if(modes[1].getParticles()[i].state.x == 0){
+                    cnt_zero++;
+                }else{
+                    cnt_ones++;
+                }
+            }
+
+            std::cout << "MODE2 Zeros: " << (double) cnt_zero /  numParticles<< std::endl;
+            std::cout << "MODE2 Ones: " << (double) cnt_ones/ numParticles << std::endl;
+        }
+
+
+
+    }
+
+    TEST(Mixing, differentParticleSize) {
+
+    }
+
+
+    TEST(Mixing, dynamicTransitionMatrix) {
+
+    }
+
+    TEST(Mixing, ThreeFilters) {
+
+    }
+
+}
+
+
+
+#endif