#39 #40 git add for last commit

smc/smoothing/ArtificialDistribution.h

@@ -0,0 +1,24 @@
+#ifndef ARTIFICIALDISTRIBUTION_H
+#define ARTIFICIALDISTRIBUTION_H
+
+#include <vector>
+#include "../Particle.h"
+
+namespace SMC {
+
+	/**
+     * interface for artificial distributions
+	 */
+    template <typename State>
+    class ArtificialDistribution {
+
+	public:
+
+        /** calculate the probability/density*/
+        virtual double calculate(Particle<State> const& particle) = 0;
+
+	};
+
+}
+
+#endif // ARTIFICIALDISTRIBUTION_H

smc/smoothing/BackwardFilter.h

@@ -0,0 +1,57 @@
+#ifndef BACKWARDFILTER_H
+#define BACKWARDFILTER_H
+
+
+#include <vector>
+#include <memory>
+
+#include "BackwardFilterTransition.h"
+
+#include "../sampling/ParticleTrajectorieSampler.h"
+
+#include "../Particle.h"
+
+#include "../filtering/resampling/ParticleFilterResampling.h"
+#include "../filtering/estimation/ParticleFilterEstimation.h"
+#include "../filtering/ParticleFilterEvaluation.h"
+#include "../filtering/ParticleFilterInitializer.h"
+
+#include "../../Assertions.h"
+
+
+namespace SMC {
+
+        template <typename State, typename Control, typename Observation>
+        class BackwardFilter {
+
+        public:
+            virtual State update(std::vector<Particle<State>> const& forwardParticles) = 0;
+
+            /** access to all backward / smoothed particles */
+            virtual const std::vector<std::vector<Particle<State>>>& getbackwardParticles() = 0;
+
+            /** set the estimation method to use */
+            virtual void setEstimation(std::unique_ptr<ParticleFilterEstimation<State>> estimation) = 0;
+
+            /** set the transition method to use */
+            virtual void setTransition(std::unique_ptr<BackwardFilterTransition<State, Control>> transition) = 0;
+
+            /** set the resampling method to use */
+            virtual void setResampling(std::unique_ptr<ParticleFilterResampling<State>> resampler) = 0;
+
+            /** set the resampling threshold as the percentage of efficient particles */
+            virtual void setNEffThreshold(const double thresholdPercent) = 0;
+
+            /** set sampler */
+            virtual void setSampler(std::unique_ptr<ParticleTrajectorieSampler<State>> sampler) { (void) sampler; };
+
+            /** get the used transition method */
+            virtual BackwardFilterTransition<State, Control>* getTransition() = 0;
+
+            /** reset */
+            virtual void reset() {};
+
+    };
+}
+
+#endif // BACKWARDFILTER_H

smc/smoothing/BackwardFilterTransition.h

@@ -0,0 +1,33 @@
+#ifndef BACKWARDFILTERTRANSITION_H
+#define BACKWARDFILTERTRANSITION_H
+
+#include <vector>
+#include "../Particle.h"
+
+namespace SMC {
+
+    /**
+     * interface for the user-defined backward filter transition.
+     * the transition describes the probability of a state change during the transition phase p(q_t+1 | q_t)
+     */
+    template <typename State, typename Control>
+    class BackwardFilterTransition {
+
+    public:
+
+        /**
+         * @brief perform the transition p(q_t+1 | q_t) for all particles and possibilities
+         * if you do not use this abstract function, do not forget to throw an error if the user does
+         */
+        virtual std::vector<std::vector<double>> transition(std::vector<Particle<State>> const& toBeSmoothedParticles_qt, std::vector<Particle<State>>const& alreadySmoothedParticles_qt1) = 0;
+
+        /**
+         * @brief perform a forward transition based on the to be smoothed particles at time q_t and sample particles at time q_t+1, also gets an vector with controls c_1:T
+         * if you do not use this abstract function, do not forget to throw an error if the user does
+        */
+        virtual std::vector<Particle<State>> transition(std::vector<Particle<State>> const& toBeSmoothedParticles_qt, std::vector<Control> const& controls_1T) = 0;
+    };
+
+}
+
+#endif // BACKWARDFILTERTRANSITION_H

smc/smoothing/BackwardSimulation.h

@@ -0,0 +1,258 @@
+/*
+ * CondensationBackwardFilter.h
+ *
+ *  Created on: Jun 23, 2015
+ *      Author: Toni Fetzer
+ */
+
+#ifndef BACKWARDSIMULATION_H_
+#define BACKWARDSIMULATION_H_
+
+
+#include <vector>
+#include <memory>
+#include <algorithm>
+
+#include "BackwardFilterTransition.h"
+#include "BackwardFilter.h"
+
+#include "../Particle.h"
+
+#include "../filtering/resampling/ParticleFilterResampling.h"
+#include "../filtering/estimation/ParticleFilterEstimation.h"
+#include "../filtering/ParticleFilterEvaluation.h"
+#include "../filtering/ParticleFilterInitializer.h"
+
+#include "../sampling/ParticleTrajectorieSampler.h"
+
+#include "../../Assertions.h"
+
+namespace SMC {
+
+    /**
+     * the main-class for the Backward Simulation Filter
+     * running "backwards" in time, generates multiple backwards trajectories
+     * (Realizations) by repeating the backward simulation M time.
+     * it can be started at a random time T of any forward particle filter
+     * [Monte Carlo smoothing for non-linear time series Godsill et al. '03]
+     * @param State the (user-defined) state for each particle
+     * @param numRealizations is the number of backward trajectories starting
+     */
+    template <typename State, typename Control, typename Observation>
+    class BackwardSimulation : public BackwardFilter<State, Control, Observation>{
+
+    private:
+
+        /** all smoothed particles T -> 1*/
+        std::vector<std::vector<Particle<State>>> backwardParticles;
+
+        /** container for particles */
+        std::vector<Particle<State>> smoothedParticles;
+
+        /** the estimation function to use */
+        std::unique_ptr<ParticleFilterEstimation<State>> estimation;
+
+        /** the transition function to use */
+        std::unique_ptr<BackwardFilterTransition<State, Control>> transition;
+
+        /** the resampler to use */
+        std::unique_ptr<ParticleFilterResampling<State>> resampler;
+
+        /** the sampler for drawing trajectories */
+        std::unique_ptr<ParticleTrajectorieSampler<State>> sampler;
+
+        /** the percentage-of-efficient-particles-threshold for resampling */
+        double nEffThresholdPercent = 0.25;
+
+        /** number of realizations to be calculated */
+        int numRealizations;
+
+        /** is update called the first time? */
+        bool firstFunctionCall;
+
+
+    public:
+
+        /** ctor */
+        BackwardSimulation(int numRealizations) {
+            this->numRealizations = numRealizations;
+            backwardParticles.reserve(numRealizations);
+            smoothedParticles.reserve(numRealizations);
+            firstFunctionCall = true;
+        }
+
+        /** dtor */
+        ~BackwardSimulation() {
+            ;
+        }
+
+        /** reset **/
+        void reset(){
+            this->numRealizations = numRealizations;
+
+            backwardParticles.clear();
+            backwardParticles.reserve(numRealizations);
+
+            smoothedParticles.clear();
+            smoothedParticles.reserve(numRealizations);
+
+            firstFunctionCall = true;
+        }
+
+        /** access to all backward / smoothed particles */
+        const std::vector<std::vector<Particle<State>>>& getbackwardParticles() {
+            return backwardParticles;
+        }
+
+        /** set the estimation method to use */
+        void setEstimation(std::unique_ptr<ParticleFilterEstimation<State>> estimation) {
+            Assert::isNotNull(estimation, "setEstimation() MUST not be called with a nullptr!");
+            this->estimation = std::move(estimation);
+        }
+
+        /** set the transition method to use */
+        void setTransition(std::unique_ptr<BackwardFilterTransition<State, Control>> transition) {
+            Assert::isNotNull(transition, "setTransition() MUST not be called with a nullptr!");
+            this->transition = std::move(transition);
+        }
+
+        /** set the resampling method to use */
+        void setResampling(std::unique_ptr<ParticleFilterResampling<State>> resampler) {
+            Assert::isNotNull(resampler, "setResampling() MUST not be called with a nullptr!");
+            this->resampler = std::move(resampler);
+        }
+
+        /** set the sampler method to use */
+        void setSampler(std::unique_ptr<ParticleTrajectorieSampler<State>> sampler){
+            Assert::isNotNull(sampler, "setSampler() MUST not be called with a nullptr!");
+            this->sampler = std::move(sampler);
+        }
+
+
+        /** set the resampling threshold as the percentage of efficient particles */
+        void setNEffThreshold(const double thresholdPercent) {
+            this->nEffThresholdPercent = thresholdPercent;
+        }
+
+        /** get the used transition method */
+        BackwardFilterTransition<State, Control>* getTransition() {
+            return this->transition.get();
+        }
+
+        /**
+         * perform update: transition -> correction -> approximation
+         * gets the weighted sample set of a standard condensation
+         * particle filter in REVERSED order!
+         */
+        State update(std::vector<Particle<State>> const& forwardParticles) {
+
+            // sanity checks (if enabled)
+            Assert::isNotNull(transition, "transition MUST not be null! call setTransition() first!");
+            Assert::isNotNull(estimation, "estimation MUST not be null! call setEstimation() first!");
+
+            //storage for single trajectories / smoothed particles
+            smoothedParticles.clear();
+
+            // Choose \tilde x_T = x^(i)_T with probability w^(i)_T
+            // Therefore sample independently from the categorical distribution of weights.
+            if(firstFunctionCall){
+
+                smoothedParticles = sampler->drawTrajectorie(forwardParticles, numRealizations);
+
+                firstFunctionCall = false;
+                backwardParticles.push_back(smoothedParticles);
+
+                const State es = estimation->estimate(smoothedParticles);
+                return es;
+            }
+
+            // compute weights using the transition model
+            // transitionWeigths[numRealizations][numParticles]
+            std::vector<std::vector<double>> transitionWeights = transition->transition(forwardParticles, backwardParticles.back());
+
+            //get the next trajectorie for a realisation
+            for(int j = 0; j < numRealizations; ++j){
+
+                //vector for the current smoothedWeights at time t
+                std::vector<Particle<State>> smoothedWeights;
+                smoothedWeights.resize(forwardParticles.size());
+                smoothedWeights = forwardParticles;
+
+                //check if all transitionWeights are zero
+                double weightSumTransition = std::accumulate(transitionWeights[j].begin(), transitionWeights[j].end(), 0.0);
+                Assert::isNot0(weightSumTransition, "all transition weights for smoothing are zero");
+
+                int i = 0;
+                for (auto& w : transitionWeights.at(j)) {
+
+                    // multiply the weight of the particles at time t and normalize
+                    smoothedWeights.at(i).weight = (smoothedWeights.at(i).weight * w);
+                    if(smoothedWeights.at(i).weight != smoothedWeights.at(i).weight) {throw "detected NaN";}
+
+                    // iter
+                    ++i;
+                }
+
+                //get the sum of all weights
+                auto lambda = [](double current, const Particle<State>& a){return current + a.weight; };
+                double weightSumSmoothed = std::accumulate(smoothedWeights.begin(), smoothedWeights.end(), 0.0, lambda);
+
+                //normalize the weights
+                if(weightSumSmoothed != 0.0){
+                    for (int i = 0; i < smoothedWeights.size(); ++i){
+                        smoothedWeights.at(i).weight /= weightSumSmoothed;
+                    }
+
+                    //check if normalization worked
+                    double normWeightSum = std::accumulate(smoothedWeights.begin(), smoothedWeights.end(), 0.0, lambda);
+                    Assert::isNear(normWeightSum, 1.0, 0.001, "Smoothed weights do not sum to 1");
+                }
+
+
+                //draw the next trajectorie at time t for a realization and save them
+                smoothedParticles.push_back(sampler->drawSingleParticle(smoothedWeights));
+
+                //throw if weight of smoothedParticle is zero
+                //in practice this is possible, if a particle is completely separated from the rest and is therefore
+                //weighted zero or very very low.
+                Assert::isNot0(smoothedParticles.back().weight, "smoothed particle has zero weight");
+            }
+
+
+            if(resampler)
+            {
+
+                //TODO - does this even make sense?
+                std::cout << "Warning - Resampling is not yet implemented!" << std::endl;
+//                //resampling if necessery
+//                double sum = 0.0;
+//                double weightSum = std::accumulate(smoothedParticles.begin().weight, smoothedParticles.end().weight, 0.0);
+//                for (auto& p : smoothedParticles) {
+//                    p.weight /= weightSum;
+//                    sum += (p.weight * p.weight);
+//                }
+
+//                const double neff = 1.0/sum;
+//                if (neff != neff) {throw "detected NaN";}
+
+//                // if the number of efficient particles is too low, perform resampling
+//               if (neff < smoothedParticles.size() * nEffThresholdPercent) { resampler->resample(smoothedParticles); }
+            }
+
+            // push_back the smoothedParticles
+            backwardParticles.push_back(smoothedParticles);
+
+            // estimate the current state
+            const State est = estimation->estimate(smoothedParticles);
+
+            // done
+            return est;
+
+        }
+
+    };
+
+}
+
+
+#endif /* BACKWARDSIMULATION_H_ */

smc/smoothing/CondensationBackwardFilter.h

@@ -0,0 +1,226 @@
+/*
+ * CondensationBackwardFilter.h
+ *
+ *  Created on: Jun 23, 2015
+ *      Author: Toni Fetzer
+ */
+
+#ifndef CONDENSATIONBACKWARDFILTER_H_
+#define CONDENSATIONBACKWARDFILTER_H_
+
+
+#include <vector>
+#include <memory>
+
+#include "BackwardFilterTransition.h"
+#include "BackwardFilter.h"
+
+#include "../Particle.h"
+
+#include "../filtering/resampling/ParticleFilterResampling.h"
+#include "../filtering/estimation/ParticleFilterEstimation.h"
+#include "../filtering/ParticleFilterEvaluation.h"
+#include "../filtering/ParticleFilterInitializer.h"
+
+#include "../../Assertions.h"
+
+long long count = 0.0;
+
+namespace SMC {
+
+    /**
+     * the main-class for the Condensation Backward Filter
+     * running "backwards" in time, updating every timestep, no resampling
+     * it can be started at a random time T of an forward particle filter
+     * [Smoothing filter for condensation by Isard and Blake '98]
+     * @param State the (user-defined) state for each particle
+     */
+    template <typename State>
+    class CondensationBackwardFilter : public BackwardFilter<State> {
+
+    private:
+
+        /** all smoothed particles 1 -> T*/
+        std::vector<std::vector<Particle<State>>> backwardParticles;
+
+        /** the estimation function to use */
+        std::unique_ptr<ParticleFilterEstimation<State>> estimation;
+
+        /** the transition function to use */
+        std::unique_ptr<BackwardFilterTransition<State>> transition;
+
+        /** the resampler to use */
+        std::unique_ptr<ParticleFilterResampling<State>> resampler;
+
+        /** the percentage-of-efficient-particles-threshold for resampling */
+        double nEffThresholdPercent = 0.25;
+
+
+    public:
+
+        /** ctor */
+        CondensationBackwardFilter() {
+        }
+
+        /** dtor */
+        ~CondensationBackwardFilter() {
+            ;
+        }
+
+        /** access to all backward / smoothed particles */
+        const std::vector<std::vector<Particle<State>>>& getbackwardParticles() {
+            return backwardParticles;
+        }
+
+        /** set the estimation method to use */
+        void setEstimation(std::unique_ptr<ParticleFilterEstimation<State>> estimation) {
+            Assert::isNotNull(estimation, "setEstimation() MUST not be called with a nullptr!");
+            this->estimation = std::move(estimation);
+        }
+
+        /** set the transition method to use */
+        void setTransition(std::unique_ptr<BackwardFilterTransition<State>> transition) {
+            Assert::isNotNull(transition, "setTransition() MUST not be called with a nullptr!");
+            this->transition = std::move(transition);
+        }
+
+        /** set the resampling method to use */
+        void setResampling(std::unique_ptr<ParticleFilterResampling<State>> resampler) {
+                Assert::isNotNull(resampler, "setResampling() MUST not be called with a nullptr!");
+                this->resampler = std::move(resampler);
+        }
+
+
+        /** set the resampling threshold as the percentage of efficient particles */
+        void setNEffThreshold(const double thresholdPercent) {
+                this->nEffThresholdPercent = thresholdPercent;
+        }
+
+        /** get the used transition method */
+        BackwardFilterTransition<State>* getTransition() {
+            return this->transition.get();
+        }
+
+        /**
+         * perform update: transition -> correction -> approximation
+         * gets the weighted sample set of a standard condensation
+         * particle filter in REVERSED order!
+         */
+        State update(std::vector<Particle<State>> const& forwardParticles) {
+
+            // sanity checks (if enabled)
+            Assert::isNotNull(transition, "transition MUST not be null! call setTransition() first!");
+            Assert::isNotNull(estimation, "estimation MUST not be null! call setEstimation() first!");
+
+            // since the algorithm starts at T-1 we need to initialize with the first set of forwardParticels
+            // psi_T = pi_T
+            static bool firstFunctionCall = true;
+            if(firstFunctionCall){
+                backwardParticles.push_back(forwardParticles);
+                firstFunctionCall = false;
+
+                std::vector<Particle<State>> tt = forwardParticles;
+                const State es = estimation->estimate(tt);
+                return es;
+            }
+
+            //weightsume for normalization
+            double weightSum = 0.0;
+
+            // perform the transition step p(x_t+1|x_t)
+            std::vector<std::vector<double>> predictionProbabilities = transition->transition(forwardParticles, backwardParticles.back());
+
+            // calculate the correction factors
+            std::vector<double> correctionFactors;
+            for(int m = 0; m < forwardParticles.size(); ++m){
+
+                double gamma = 0.0;
+                for(int k = 0; k < forwardParticles.size(); ++k){
+                    // gamma(m) = sum(pi(k) * alpha(m,k))
+                    gamma += forwardParticles[k].weight * predictionProbabilities[m][k];
+
+                    if (gamma != gamma) {throw "detected NaN";}
+                }
+                correctionFactors.push_back(gamma);
+            }
+
+            // approximate backward variables
+            std::vector<Particle<State>> smoothedParticles = forwardParticles;
+            for(int n = 0; n < forwardParticles.size(); ++n){
+
+                double delta = 0.0;
+                for(int m = 0; m < forwardParticles.size(); ++m){
+                    // delta(n) = sum(psi(m) * alpha(m,n) / gamma(m))
+
+                    //!! THIS IS A HACK !! Gamma is getting zero if the prob is to damn low. This would results in NaN for gamma
+                    //!! Therefore we set alpha(m,n) / gamma(m) = 1.0;
+                    if(correctionFactors[m] == 0.0){
+                        delta += backwardParticles.back().at(m).weight;
+                        std::cout << "Gamma is Zero" << count ++ << std::endl;
+                    }
+                    else
+                        delta += backwardParticles.back().at(m).weight * (predictionProbabilities[m][n] / correctionFactors[m]);
+
+                    if (delta != delta) {throw "detected NaN";}
+                }
+
+                // Evaluate smoothing weights
+                // psi(n) = pi(n) * delta(n)
+                double weight = delta * forwardParticles[n].weight;
+                smoothedParticles[n].weight = weight;
+
+                // fill weightsum
+                weightSum += weight;
+
+                if (forwardParticles[n].weight != forwardParticles[n].weight) {throw "detected NaN";}
+                if (delta != delta) {throw "detected NaN";}
+                if (weight != weight) {throw "detected NaN";}
+                if (weightSum != weightSum) {throw "detected NaN";}
+            }
+
+            // normalize the particle weights and thereby calculate N_eff       
+            double sum = 0.0;
+            for (auto& p : smoothedParticles) {
+                p.weight /= weightSum;
+                sum += (p.weight * p.weight);
+
+                // sanity check
+//                if (p.state.heading != p.state.heading) {throw "detected NaN";}
+//                if (p.state.z_nr != p.state.z_nr) {throw "detected NaN";}
+//                if (p.state.x_cm != p.state.x_cm) {throw "detected NaN";}
+//                if (p.state.y_cm != p.state.y_cm) {throw "detected NaN";}
+//                if (p.weight != p.weight) {throw "detected NaN";}
+            }
+
+            const double neff = 1.0/sum;
+            if (neff != neff) {throw "detected NaN";}
+
+            // estimate the current state
+            const State est = estimation->estimate(smoothedParticles);
+
+//            if (est.heading != est.heading) {throw "detected NaN";}
+//            if (est.z_nr != est.z_nr) {throw "detected NaN";}
+//            if (est.x_cm != est.x_cm) {throw "detected NaN";}
+//            if (est.y_cm != est.y_cm) {throw "detected NaN";}
+
+            if(resampler)
+            {
+                // if the number of efficient particles is too low, perform resampling
+                if (neff < smoothedParticles.size() * nEffThresholdPercent) { resampler->resample(smoothedParticles); }
+            }
+
+
+            // push_back the smoothedParticles
+            backwardParticles.push_back(smoothedParticles);
+
+            // done
+            return est;
+
+        }
+
+    };
+
+}
+
+
+#endif /* CONDENSATIONBACKWARDFILTER_H_ */

smc/smoothing/ForwardFilterHistory.h

@@ -0,0 +1,142 @@
+#ifndef FORWARDFILTERHISTORY_H
+#define FORWARDFILTERHISTORY_H
+
+#include <vector>
+#include "../Particle.h"
+#include "../../data/Timestamp.h"
+
+#include "../../Assertions.h"
+
+namespace SMC {
+
+    /**
+     * @brief Provides a data structur for the data available at a specific timestamp of the forward filtering procedure.
+     * @brief Timestamp; ParticleSet (After Transition and Update); Controls; Observations
+     */
+    template <typename State, typename Control, typename Observation>
+    class ForwardFilterHistory {
+
+
+    private:
+
+        // NOTE: it would be possible to make some kind of struct for this, but in many upcoming functions and methods, i am not able
+        // to use all this informations. sometimes if have something like p(q_t+1| q_t, o_t) or p(o_t | q_t, c_t). So keep it flexible!
+        std::vector<Timestamp> timestamps;
+        std::vector<std::vector<Particle<State>>> particleSets;
+        std::vector<Control> controls;
+        std::vector<Observation> observations;
+
+    public:
+
+        ForwardFilterHistory(){
+            //empty ctor
+        }
+
+        void add(Timestamp time, std::vector<std::vector<Particle<State>>> set, Control control, Observation obs){
+
+            // Is empty? Null? etc.
+            Assert::isNotNull(time, "Timestamp is Null");
+            Assert::isNotNull(set, "Particle Set is Null");
+            Assert::isNotNull(control, "Control is Null");
+            Assert::isNotNull(obs, "Observation is Null");
+
+            timestamps.push_back(time);
+            particleSets.push_back(set);
+            controls.push_back(control);
+            observations.push_back(obs);
+        }
+
+        void removeLatest(){
+
+            particleSets.pop_back();
+            controls.pop_back();
+            observations.pop_back();
+        }
+
+        void removeFirst(){
+
+            particleSets.erase(particleSets.begin());
+            controls.erase(controls.begin());
+            observations.erase(observations.begin());
+        }
+
+        /**
+         * @brief Return the particles from [latestFilterUpdate - @param idx]
+         * @return returns vector of particles. note: c11 makes a std::move here
+         */
+        std::vector<Particle<State>> getParticleSet(idx = 0){
+            return particleSets.at(particleSets.end() - idx);
+        }
+
+        /**
+         * @brief getControl from [latestFilterUpdate - @param idx]
+         * @return const controls object
+         */
+        const Control getControl(idx = 0){
+            return controls.at(controls.end() - idx);
+        }
+
+        /**
+         * @brief getObservationf rom [latestFilterUpdate - @param idx]
+         * @return const obervations object
+         */
+        const Observation getObservation (idx = 0){
+            return observations.at(observations.end() - idx);
+        }
+
+        /**
+         * @brief Return the timestamp from [latestFilterUpdate - @param idx]
+         * @return returns a Timstampf object
+         */
+        std::vector<Particle<State>> getTimestamp(idx = 0){
+            return timestamps.at(particleSets.end() - idx);
+        }
+
+        /**
+         * @brief getLatestFilterUpdateNum
+         * @return num of particleSets size
+         */
+        const int getLatestFilterUpdateNum(){
+            return particleSets.size();
+        }
+
+        /**
+         * @brief getLatestParticleSet Reference
+         * @return return particle set Note: c11 std::move by vector
+         */
+        std::vector<Particle<State>> getLatestParticleSet(){
+            return particleSets.back();
+        }
+
+        /**
+         * @brief getLatestControls
+         * @return const control object
+         */
+        const Control getLatestControls(){
+            return controls.back();
+        }
+
+        /**
+         * @brief getLatestObservation
+         * @return const observation object
+         */
+        const Observation getLatestObservation(){
+            return observations.back();
+        }
+
+
+        /**
+         * @brief getLatestTimestamp
+         * @return const Timestamp object
+         */
+        const Timestamp getLatestTimestamp(){
+            return timestamps.back();
+        }
+
+
+
+    };
+
+}
+
+#endif // FORWARDFILTERHISTORY_H

smc/smoothing/TwoFilterSmoothing.h

@@ -0,0 +1,187 @@
+/*
+ * CondensationBackwardFilter.h
+ *
+ *  Created on: Jun 23, 2015
+ *      Author: Toni Fetzer
+ */
+
+#ifndef TWOFILTERSMOOTHING_H_
+#define TWOFILTERSMOOTHING_H_
+
+
+#include <vector>
+#include <memory>
+
+#include "BackwardFilterTransition.h"
+#include "BackwardFilter.h"
+#include "ArtificialDistribution.h"
+
+#include "../Particle.h"
+
+#include "../filtering/resampling/ParticleFilterResampling.h"
+#include "../filtering/estimation/ParticleFilterEstimation.h"
+#include "../filtering/ParticleFilterEvaluation.h"
+#include "../filtering/ParticleFilterInitializer.h"
+
+#include "../../Assertions.h"
+
+namespace SMC {
+
+    /**
+     * Smoothing Forward and Backward Filter together.
+     * Call the Update Function.
+     * Algorithm taken from [Briers04] Smoothing Algorithms for State-Space Models
+     */
+    template <typename State>
+    class TwoFilterSmoothing {
+
+    private:
+
+        /** all smoothed particles 1 -> T*/
+        std::vector<std::vector<Particle<State>>> smoothedParticles;
+
+        /** the estimation function to use */
+        std::unique_ptr<ParticleFilterEstimation<State>> estimation;
+
+        /** the transition function to use */
+        std::unique_ptr<BackwardFilterTransition<State>> transition;
+
+        /** the resampler to use */
+        std::unique_ptr<ParticleFilterResampling<State>> resampler;
+
+        /** artificial distribuation */
+        std::unique_ptr<ArtificialDistribution<State>> artificialDistribution;
+
+        /** the percentage-of-efficient-particles-threshold for resampling */
+        double nEffThresholdPercent = 0.25;
+
+
+    public:
+
+        /** ctor */
+        TwoFilterSmoothing() {
+        }
+
+        /** dtor */
+        ~TwoFilterSmoothing() {
+            ;
+        }
+
+        /** access to all backward / smoothed particles */
+        const std::vector<std::vector<Particle<State>>>& getsmoothedParticles() {
+            return smoothedParticles;
+        }
+
+        /** set the estimation method to use */
+        void setEstimation(std::unique_ptr<ParticleFilterEstimation<State>> estimation) {
+            Assert::isNotNull(estimation, "setEstimation() MUST not be called with a nullptr!");
+            this->estimation = std::move(estimation);
+        }
+
+        /** set the transition method to use */
+        void setTransition(std::unique_ptr<BackwardFilterTransition<State>> transition) {
+            Assert::isNotNull(transition, "setTransition() MUST not be called with a nullptr!");
+            this->transition = std::move(transition);
+        }
+
+        /** set the resampling method to use */
+        void setResampling(std::unique_ptr<ParticleFilterResampling<State>> resampler) {
+            Assert::isNotNull(resampler, "setResampling() MUST not be called with a nullptr!");
+            this->resampler = std::move(resampler);
+        }
+
+        void setArtificialDistribution(std::unique_ptr<ArtificialDistribution<State>> artificialDistribution){
+            Assert::isNotNull(artificialDistribution, "setArtificialDistribution() MUST not be called with a nullptr!");
+            this->artificialDistribution = std::move(artificialDistribution);
+        }
+
+
+        /** set the resampling threshold as the percentage of efficient particles */
+        void setNEffThreshold(const double thresholdPercent) {
+                this->nEffThresholdPercent = thresholdPercent;
+        }
+
+        /** get the used transition method */
+        BackwardFilterTransition<State>* getTransition() {
+            return this->transition.get();
+        }
+
+        /**
+         * perform update: transition -> correction -> approximation
+         * particles from a forwards filter are used to re-weight those from a backwards filter
+         * so that they represent the target distribution.
+         * @param: forwardParticles at t-1
+         * @param: backwardparticles at t
+         */
+        State update(std::vector<Particle<State>> const& forwardParticles, std::vector<Particle<State>> const& backwardParticles) {
+
+            // sanity checks (if enabled)
+            Assert::isNotNull(transition, "transition MUST not be null! call setTransition() first!");
+            Assert::isNotNull(estimation, "estimation MUST not be null! call setEstimation() first!");
+
+            // perform the transition step p(backward_x_t|forward_x_t-1)
+            std::vector<std::vector<double>> predictionProbabilities = transition->transition(forwardParticles, backwardParticles);
+
+            //we are using the forwardparticles to re-weight the backward filter (other direction also possible?)
+            std::vector<Particle<State>> currentParticles = backwardParticles;
+
+            double weightSum = 0.0;
+
+            // calculate the correction factors
+            for(int j = 0; j < backwardParticles.size(); ++j){
+
+                double alpha = 0.0;
+                for(int i = 0; i < backwardParticles.size(); ++i){
+                    // alpha(j) = sum(forward_weight_t-1 * prediction)
+                    alpha += forwardParticles[i].weight * predictionProbabilities[j][i];
+
+                    if (alpha != alpha) {throw "detected NaN";}
+                }
+
+                double gamma = 1.0;
+                if(artificialDistribution){
+                    gamma = artificialDistribution->calculate(backwardParticles[j]);
+                }
+
+                //calc the weight
+                double weight = (currentParticles[j].weight / gamma) * alpha;
+
+
+                currentParticles[j].weight = weight;
+                weightSum += weight;
+            }
+
+            // normalize the particle weights and thereby calculate N_eff       
+            double sum = 0.0;
+            for (auto& p : currentParticles) {
+                p.weight /= weightSum;
+                sum += (p.weight * p.weight);
+            }
+
+            double neff = 1.0/sum;
+            if (neff != neff) {neff = 1.0;}
+
+            // estimate the current state
+            const State est = estimation->estimate(currentParticles);
+
+            if(resampler)
+            {
+                // if the number of efficient particles is too low, perform resampling
+                if (neff < currentParticles.size() * nEffThresholdPercent) { resampler->resample(currentParticles); }
+            }
+
+
+            // push_back the smoothedParticles
+            smoothedParticles.push_back(currentParticles);
+
+            // done
+            return est;
+
+        }
+
+    };
+
+}
+
+
+#endif /* TWOFILTERSMOOTHING_H_ */