#39 #40 git add for last commit

smc/merging/InteractingMultipleModelParticleFilter.h

@@ -0,0 +1,135 @@
+#ifndef INTERACTINGMULTIPLEMODELPARTICLEFILTER_H
+#define INTERACTINGMULTIPLEMODELPARTICLEFILTER_H
+
+#include <vector>
+
+#include "mixing/MixingSampler.h"
+#include "MarkovTransitionProbability.h"
+#include "estimation/JointEstimation.h"
+
+#include "../filtering/ParticleFilterMixing.h"
+#include "../../Assertions.h"
+
+namespace SMC {
+
+    /**
+     * the main-class for IMMPF based on Driessen and Boers
+     * @param vector of particle filters as modes
+     */
+    template <typename State, typename Control, typename Observation>
+    class InteractingMultipleModelParticleFilter {
+
+    private:
+
+        /** the used particle filters */
+        std::vector<ParticleFilterMixing<State, Control, Observation>> modes;
+
+        /** the mixing function to use */
+        std::unique_ptr<MixingSampler<State, Control, Observation>> mixing;
+
+        /** the function for calculating markov chain transition*/
+        std::unique_ptr<MarkovTransitionProbability<State, Control, Observation>> transition;
+
+        /** the function for calculating a joint estimation */
+        std::unique_ptr<JointEstimation<State, Control, Observation>> estimation;
+
+        /** the transition probability matrix */
+        Eigen::MatrixXd transitionProbabilityMatrix;
+
+    public:
+
+        /** ctor
+        * NOTE: The single rows of the transition matrix need to sum to 1!!!
+        */
+        InteractingMultipleModelParticleFilter(std::vector<ParticleFilterMixing<State, Control, Observation>>& modes, Eigen::MatrixXd transitionProbabilityMatrix) {
+
+            // TODO: this is a deep copy... we could or should change that later.. since slooooooooow =)
+            this->modes = modes;
+
+            //init transmatrix
+            this->transitionProbabilityMatrix = transitionProbabilityMatrix;
+        }
+
+        /** dtor */
+        ~InteractingMultipleModelParticleFilter() {
+            ;
+        }
+
+        /** set mixing function */
+        void setMixingSampler(std::unique_ptr<MixingSampler<State, Control, Observation>> mixing) {
+            Assert::isNotNull(mixing, "setMixingSampler() MUST not be called with a nullptr!");
+            this->mixing = std::move(mixing);
+        }
+
+        /** set mode transition function */
+        void setMarkovTransitionProbability(std::unique_ptr<MarkovTransitionProbability<State, Control, Observation>> transition) {
+            Assert::isNotNull(transition, "setMarkovTransitionProbability() MUST not be called with a nullptr!");
+            this->transition = std::move(transition);
+        }
+
+        /** set joint estimation function */
+        void setJointEstimation(std::unique_ptr<JointEstimation<State, Control, Observation>> estimation) {
+            Assert::isNotNull(estimation, "setJointEstimation() MUST not be called with a nullptr!");
+            this->estimation = std::move(estimation);
+        }
+
+        const std::vector<ParticleFilterMixing<State, Control, Observation>>& getModes() const{
+            return modes;
+        }
+
+        /** perform the mixed update -> update particle filters -> estimation -> mixing*/
+        State update(const Control* control, const Observation& observation){
+
+            // sanity checks (if enabled)
+            Assert::isNotNull(mixing, "mixingsampler MUST not be null! call setResampler() first!");
+            //Assert::isNotNull(transition, "transition MUST not be null! call setResampler() first!");
+            Assert::isNotNull(estimation, "estimation MUST not be null! call setResampler() first!");
+
+            // mix both modes depending on the kld divergency and their calculated
+            // impact (mode posterior prob) to draw new particles
+            mixing->mixAndSample(modes, this->transitionProbabilityMatrix);
+
+            for(ParticleFilterMixing<State, Control, Observation>& filter : modes){
+
+                //update the particle filter and save estimation
+                filter.update(control, observation);
+            }
+
+            // calculate the transition probability matrix for the markov chain based on
+            // kld divergency.
+            this->transitionProbabilityMatrix = transition->update(modes, observation);
+
+            // calc posterior probability
+            this->calcPosteriorProbability();
+
+            // calculate current estimation
+            const State jointEst = estimation->estimate(modes);
+
+            //done
+            return jointEst;
+        }
+
+
+    private:
+
+        void calcPosteriorProbability(){
+
+            // calculate the likelihood Sum lambda for all modes
+            // the Posterior is init with P(m_0 | Z_0)
+            double likelihoodSum;
+            for(ParticleFilterMixing<State, Control, Observation>& filter : modes){
+
+                likelihoodSum += filter.getWeightSum() * filter.getPredictedModeProbability();
+            }
+
+            // set the last posterior probability p(m_t-1 | Z_t-1) for all modes
+            for(ParticleFilterMixing<State, Control, Observation>& filter : modes){
+
+                filter.setModePosteriorProbability(likelihoodSum);
+            }
+        }
+    };
+
+}
+
+#endif // INTERACTINGMULTIPLEMODELPARTICLEFILTER_H

smc/merging/MarkovTransitionProbability.h

@@ -0,0 +1,30 @@
+#ifndef MARKOVTRANSITIONPROBABILITY_H
+#define MARKOVTRANSITIONPROBABILITY_H
+
+#include "../filtering/ParticleFilterMixing.h"
+#include <eigen3/Eigen/Dense>
+
+namespace SMC {
+
+
+    /**
+     * interface for all available transition probability calculations
+     * within the IMMPF
+     */
+    template <typename State, typename Control, typename Observation>
+    class MarkovTransitionProbability {
+
+    public:
+
+        /**
+         * perform the calculation of the transition matrix
+         * @param vector of modes / particle filters
+         */
+         virtual Eigen::MatrixXd update(std::vector<ParticleFilterMixing<State, Control, Observation>>& modes, const Observation& obs) = 0;
+
+    };
+
+}
+
+
+#endif // MARKOVTRANSITIONPROBABILITY_H

smc/merging/estimation/JointEstimation.h

@@ -0,0 +1,25 @@
+#ifndef JOINTESTIMATION_H
+#define JOINTESTIMATION_H
+
+#include "../../filtering/ParticleFilterMixing.h"
+
+namespace SMC {
+
+    /**
+     * interface for all available joint estimations
+     * within the IMMPF we have multiple particle filters
+     * the "true" estimation is a joint state of all
+     */
+    template <typename State, typename Control, typename Observation>
+    class JointEstimation {
+
+    public:
+
+        // get the current state estimation for the given particle set
+        virtual const State estimate(std::vector<ParticleFilterMixing<State, Control, Observation>>& modes) = 0;
+
+    };
+
+}
+
+#endif // JOINTESTIMATION_H

smc/merging/estimation/JointEstimationPosteriorOnly.h

@@ -0,0 +1,28 @@
+#ifndef JOINTESTIMATIONPOSTERIORONLY_H
+#define JOINTESTIMATIONPOSTERIORONLY_H
+
+#include "JointEstimation.h"
+
+#include "../../filtering/ParticleFilterMixing.h"
+
+namespace SMC {
+
+    /**
+     * Use only the posterior distribution (first mode entry) for the joint estimaton
+     */
+    template <typename State, typename Control, typename Observation>
+    class JointEstimationPosteriorOnly
+    : public JointEstimation<State, Control, Observation> {
+
+    public:
+
+        // get the current state estimation for the given particle set
+        const State estimate(std::vector<ParticleFilterMixing<State, Control, Observation>>& modes) override {
+            return modes[0].getEstimation();
+        }
+
+    };
+
+}
+
+#endif // JOINTESTIMATIONPOSTERIORONLY_H

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