#39 #40 git add for last commit

smc/merging/mixing/MixingSampler.h

@@ -0,0 +1,29 @@
+#ifndef MIXINGSAMPLER_H
+#define MIXINGSAMPLER_H
+
+#include "../../filtering/ParticleFilterMixing.h"
+#include <eigen3/Eigen/Dense>
+
+namespace SMC {
+
+
+    /**
+     * interface for all available resampling methods
+     * within the particle filter
+     */
+    template <typename State, typename Control, typename Observation>
+    class MixingSampler {
+
+    public:
+
+        /**
+         * perform mixing of modes and sample according to the modes probability
+         * @param particles the vector of all particles to resample
+         */
+        virtual void mixAndSample(std::vector<ParticleFilterMixing<State, Control, Observation>>& modes, Eigen::MatrixXd transitionProbabilityMatrix) = 0;
+
+    };
+
+}
+
+#endif // MIXINGSAMPLER_H

smc/merging/mixing/MixingSamplerDivergency.h

@@ -0,0 +1,157 @@
+#ifndef MIXINGSAMPLERDIVERGENCY_H
+#define MIXINGSAMPLERDIVERGENCY_H
+
+#include "MixingSampler.h"
+
+#include <algorithm>
+#include <random>
+#include <eigen3/Eigen/Dense>
+
+namespace SMC {
+
+    /**
+     * Using the direct sampling approach of Driessen and Boers in
+     * "Efficient particle filter for jump Markov nonlinear systems".
+     * as transition probability matrix for the markov chain we use
+     * a divergence based on Jensen–Shannon divergence
+     */
+    template <typename State, typename Control, typename Observation> class MixingSamplerDivergency
+        : public MixingSampler<State, Control, Observation> {
+
+    /** random number generator */
+    std::minstd_rand genNorm;
+    std::minstd_rand genPart;
+
+    /** copy of the modes with cumulative probabilities for easy drawing*/
+    std::vector<ParticleFilterMixing<State, Control, Observation>> copyModes;
+
+    public:
+
+         void mixAndSample(std::vector<ParticleFilterMixing<State, Control, Observation>>& modes, Eigen::MatrixXd transitionProbabilityMatrix) override{
+
+            genNorm.seed(std::chrono::system_clock::now().time_since_epoch().count());
+            genPart.seed(std::chrono::system_clock::now().time_since_epoch().count() + 233);
+
+            // set copyModes for later drawing
+            copyModes = modes;
+
+            // create cumulative particlesets for the copy
+            // Note: in most cases, the particles are already resampled within the filtering stage
+            // but in some cases they are not and therefore we need to draw cumulatively and not equally
+            for(ParticleFilterMixing<State, Control, Observation>& copyFilter : copyModes){
+                double cumWeight = 0;
+                std::vector<Particle<State>> copyParticles;
+                copyParticles = copyFilter.getParticles();
+
+                for(int i = 0; i < copyParticles.size(); ++i){
+                    cumWeight += copyParticles[i].weight;
+                    copyParticles[i].weight = cumWeight;
+                }
+
+                copyFilter.setParticles(copyParticles);
+            }
+
+            // calculate the new predicted mode prob P(m_t|Z_t-1) and P(m_t-1 | m_t, Z_t-1)
+            int m = 0; //this is m_t
+            for(ParticleFilterMixing<State, Control, Observation>& focusedFilter : modes){
+
+                // P(m_t|Z_t-1) = sum(P(m_t | m_t-1) P(m_t-1 | Z_t-1))
+                int i = 0; //this are all possible m_t-1
+                double predictedModeProbability = 0;
+                for(ParticleFilterMixing<State, Control, Observation>& filter : modes){
+                    predictedModeProbability += transitionProbabilityMatrix(m,i) * filter.getModePosteriorProbability();
+                    ++i;
+                }
+
+                //Assert::isNotNull(predictedModeProbability, "predictedModeProbability is zero.. thats not possible!");
+                focusedFilter.setPredictedModeProbability(predictedModeProbability);
+
+                // cumulative sum of TransitionModeProbabilities for drawing modes from the perspective of ONE filter!
+                // this means, the transition mode probabilities are calculated for each filter NEW!
+                // calculate P(m_t-1 | m_t, Z_t-1) = P(m_t | m_t-1) * p(m_t-1 | Z_t-1) / P(m_t|Z_t-1)
+                double cumTransitionModeProbability = 0;
+                i = 0;
+                for(ParticleFilterMixing<State, Control, Observation>& filter : modes){
+
+                    double prob = (transitionProbabilityMatrix(m,i) * filter.getModePosteriorProbability()) / focusedFilter.getPredictedModeProbability();
+                    filter.setTransitionModeProbability(prob);
+
+                    cumTransitionModeProbability += prob;
+                    copyModes[i].setTransitionModeProbability(cumTransitionModeProbability);
+
+                    //std::cout << "Draw Mode Probability from mode " << m << i << " : " << prob << std::endl;
+                    ++i;
+                }
+
+                // draw new modes and particles
+                // Note: in most cases, the particles are already resampled within the filtering stage
+                // but in some cases they are not and therefore we need to draw cumulatively and not equally
+
+                // todo: make the particle size dynamic depending on the kld or something else
+                // number of particles for this timestep
+                int numParticles = focusedFilter.getParticles().size();
+
+                std::vector<Particle<State>> newParticles;
+                newParticles.resize(numParticles);
+                double equalWeight = 1.0 / numParticles;
+
+                // draw modes cumulative
+                for(int k = 0; k < numParticles; ++k){
+
+                    auto mode = drawMode(cumTransitionModeProbability);
+                    newParticles[k] = drawParticle(mode);
+                    newParticles[k].weight = equalWeight;
+                }
+
+                focusedFilter.setParticles(newParticles);
+
+                //iter
+                ++m;
+
+            }
+        }
+
+    private:
+
+         /** draw a mode depending upon the transition mode probabilities */
+         ParticleFilterMixing<State, Control, Observation>& drawMode(const double cumTransitionModeProbabilities){
+
+             // generate random values between [0:cumWeight]
+             std::uniform_real_distribution<float> dist(0, cumTransitionModeProbabilities);
+
+             // draw a random value between [0:cumWeight]
+             const float rand = dist(genNorm);
+
+             // search comparator (cumWeight is ordered -> use binary search)
+             auto comp = [] (ParticleFilterMixing<State, Control, Observation>& filter, const float d) {return filter.getTransitionModeProbability() < d;};
+             auto it = std::lower_bound(copyModes.begin(), copyModes.end(), rand, comp);
+             return *it;
+
+         }
+
+
+        /** draw one particle according to its weight from the copy vector of a given mode */
+        const Particle<State>& drawParticle(ParticleFilterMixing<State, Control, Observation>& filter) {
+
+            double weights = filter.getParticles().back().weight;
+
+            // generate random values between [0:cumWeight]
+            std::uniform_real_distribution<float> dist(0, filter.getParticles().back().weight);
+
+            // draw a random value between [0:cumWeight]
+            const float rand = dist(genPart);
+
+            // search comparator (cumWeight is ordered -> use binary search)
+            auto comp = [] (const Particle<State>& s, const float d) {return s.weight < d;};
+            auto it = std::lower_bound(filter.getParticles().begin(), filter.getParticles().end(), rand, comp);
+            return *it;
+
+        }
+
+    };
+
+
+}
+
+
+#endif // MIXINGSAMPLERDIVERGENCY_H