rbpf.h

Go to the documentation of this file.

#ifndef RBPF_H
#define RBPF_H
 
#include <functional>
#include <vector>
#include <array>
 
#ifdef DROPPINGTHISINRPACKAGE
    #include <RcppEigen.h>
    // [[Rcpp::depends(RcppEigen)]]
#else
    #include <Eigen/Dense>
#endif
 
#include <algorithm> // std::fill
 
#include "pf_base.h"
#include "cf_filters.h" // for closed form filter objects
 
 
namespace pf {
 
namespace filters {
 
 
 
template<size_t nparts, size_t dimnss,size_t dimss,size_t dimy,typename resamp_t, typename float_t, bool debug=false>
class rbpf_hmm : public bases::rbpf_base<float_t, dimss, dimnss, dimy >
{
public:
 
    using sssv = Eigen::Matrix<float_t,dimss,1>;
    using nsssv = Eigen::Matrix<float_t,dimnss,1>;
    using osv = Eigen::Matrix<float_t,dimy,1>;
    using nsssMat = Eigen::Matrix<float_t,dimnss,dimnss>;
    using Mat = Eigen::Matrix<float_t,Eigen::Dynamic,Eigen::Dynamic>;
    using arrayVec = std::array<sssv,nparts>;
    using arrayfloat_t = std::array<float_t,nparts>;
    using cfModType = hmm<dimnss,dimy,float_t,debug>;
    using arrayMod = std::array<cfModType,nparts>;
 
 
    rbpf_hmm(const unsigned int &resamp_sched=1);
 
 
    virtual ~rbpf_hmm();
 
 
 
    void filter(const osv &data,
                const std::vector<std::function<const Mat(const nsssv &x1tLogProbs, const sssv &x2t)> >& fs 
                    = std::vector<std::function<const Mat(const nsssv&, const sssv&)> >());//, const std::vector<std::function<const Mat(const Vec&)> >& fs);
 
 
 
    float_t getLogCondLike() const;
    
 
    std::vector<Mat> getExpectations() const;
 
 
    virtual float_t logMuEv(const sssv &x21) = 0;
    
    
 
    virtual sssv q1Samp(const osv &y1) = 0;
    
    
 
    virtual nsssv initHMMLogProbVec(const sssv &x21) = 0;
    
    
 
    virtual nsssMat initHMMLogTransMat(const sssv &x21) = 0;
 
 
    virtual sssv qSamp(const sssv &x2tm1, const osv &yt) = 0;
    
    
 
    virtual float_t logQ1Ev(const sssv &x21, const osv &y1) = 0;
    
    
 
    virtual float_t logFEv(const sssv &x2t, const sssv &x2tm1) = 0;
    
    
 
    virtual float_t logQEv(const sssv &x2t, const sssv &x2tm1, const osv &yt ) = 0;
    
    
 
    virtual void updateHMM(cfModType &aModel, const osv &yt, const sssv &x2t) = 0;
 
private:
    
    unsigned int m_now;
    float_t m_lastLogCondLike;
    unsigned int m_rs;
    arrayMod m_p_innerMods;
    arrayVec m_p_samps;
    arrayfloat_t m_logUnNormWeights;
    resamp_t m_resampler;
    std::vector<Mat> m_expectations; 
 
};
 
 
template<size_t nparts, size_t dimnss, size_t dimss, size_t dimy, typename resamp_t, typename float_t, bool debug>
rbpf_hmm<nparts,dimnss,dimss,dimy,resamp_t,float_t,debug>::rbpf_hmm(const unsigned int &resamp_sched)
    : m_now(0)
    , m_lastLogCondLike(0.0)
    , m_rs(resamp_sched)
{
    std::fill(m_logUnNormWeights.begin(), m_logUnNormWeights.end(), 0.0);
}
 
 
template<size_t nparts, size_t dimnss, size_t dimss, size_t dimy, typename resamp_t, typename float_t,bool debug>
rbpf_hmm<nparts,dimnss,dimss,dimy,resamp_t,float_t,debug>::~rbpf_hmm() {}
 
 
template<size_t nparts, size_t dimnss, size_t dimss, size_t dimy, typename resamp_t, typename float_t,bool debug>
void rbpf_hmm<nparts,dimnss,dimss,dimy,resamp_t,float_t,debug>::filter(const osv &data, const std::vector<std::function<const Mat(const nsssv &x1tLogProbs, const sssv &x2t)> >& fs)
{
 
    if(m_now > 0)
    { //m_now > 0
        
        // update
        sssv newX2Samp;
        float_t sumexpdenom(0.0);
        float_t m1(-std::numeric_limits<float_t>::infinity()); // for revised log weights
        float_t m2 = *std::max_element(m_logUnNormWeights.begin(), m_logUnNormWeights.end());
        for(size_t ii = 0; ii < nparts; ++ii){
            
            newX2Samp = qSamp(m_p_samps[ii], data);
            updateHMM(m_p_innerMods[ii], data, newX2Samp);
            sumexpdenom += std::exp(m_logUnNormWeights[ii] - m2);
            
            m_logUnNormWeights[ii] += m_p_innerMods[ii].getLogCondLike()
                                    + logFEv(newX2Samp, m_p_samps[ii]) 
                                    - logQEv(newX2Samp, m_p_samps[ii], data);
            
            // update a max
            if(m_logUnNormWeights[ii] > m1)
                m1 = m_logUnNormWeights[ii];
 
            // print stuff if debug mode is on
            #ifndef DROPPINGTHISINRPACKAGE
            if constexpr(debug)
                std::cout << "time: " << m_now << ", transposed x2 sample: " << newX2Samp.transpose() << ", log unnorm weight: " << m_logUnNormWeights[ii] << "\n";
            #endif
 
            m_p_samps[ii] = newX2Samp;
        }
        
        // calculate log p(y_t | y_{1:t-1})
        float_t sumexpnumer(0.0);
        for(size_t p = 0; p < nparts; ++p)
            sumexpnumer += std::exp(m_logUnNormWeights[p] - m1);
        m_lastLogCondLike = m1 + std::log(sumexpnumer) - m2 - std::log(sumexpdenom);
        
        // calculate expectations before you resample
        unsigned int fId(0);
        //float_t m = *std::max_element(m_logUnNormWeights.begin(), m_logUnNormWeights.end());
        for(auto & h : fs){
            
            Mat testOutput = h(m_p_innerMods[0].getFilterVecLogProbs(), m_p_samps[0]);
            unsigned int rows = testOutput.rows();
            unsigned int cols = testOutput.cols();
            Mat numer = Mat::Zero(rows,cols);
            float_t denom(0.0);
            for(size_t prtcl = 0; prtcl < nparts; ++prtcl){ 
                numer += h(m_p_innerMods[prtcl].getFilterVecLogProbs(), m_p_samps[prtcl]) * std::exp(m_logUnNormWeights[prtcl] - m1);
                denom += std::exp( m_logUnNormWeights[prtcl] - m1 );
            }
            m_expectations[fId] = numer/denom;
 
            // print stuff if debug mode is on
            #ifndef DROPPINGTHISINRPACKAGE
            if constexpr(debug)
                std::cout << "transposed expec " << fId << ": " << m_expectations[fId].transpose() << "\n";
            #endif
 
            fId++;
        }
 
        // resample (unnormalized weights ok)
        if( (m_now+1) % m_rs == 0)
            m_resampler.resampLogWts(m_p_innerMods, m_p_samps, m_logUnNormWeights);
        
        // update time step
        m_now ++;
    }
    else //( m_now == 0)
    { // first data point coming
    
        // initialize and update the closed-form mods        
        nsssv tmpLogProbs;
        nsssMat tmpLogTransMat;
        float_t m1(-std::numeric_limits<float_t>::infinity());
        for(size_t ii = 0; ii < nparts; ++ii){
            
            m_p_samps[ii] = q1Samp(data); 
            tmpLogProbs = initHMMLogProbVec(m_p_samps[ii]);
            tmpLogTransMat = initHMMLogTransMat(m_p_samps[ii]);
            m_p_innerMods[ii] = cfModType(tmpLogProbs, tmpLogTransMat);
            this->updateHMM(m_p_innerMods[ii], data, m_p_samps[ii]);
            m_logUnNormWeights[ii] = m_p_innerMods[ii].getLogCondLike() + logMuEv(m_p_samps[ii]) - logQ1Ev(m_p_samps[ii], data);
 
            // print stuff if debug mode is on
            #ifndef DROPPINGTHISINRPACKAGE
            if constexpr(debug)
                std::cout << "time: " << m_now << ", transposed x2 sample: " << m_p_samps[ii].transpose() << ", log unnorm weight: " << m_logUnNormWeights[ii] << "\n";
            #endif
 
            // maximum to be used in likelihood calc
            if(m_logUnNormWeights[ii] > m1)
                m1 = m_logUnNormWeights[ii];
        }
 
        // calc log p(y1)
        float_t sumexp(0.0);
        for(size_t p = 0; p < nparts; ++p)
            sumexp += std::exp(m_logUnNormWeights[p] - m1);
        m_lastLogCondLike = m1 + std::log(sumexp) - std::log(static_cast<float_t>(nparts));
 
        // calculate expectations before you resample
        m_expectations.resize(fs.size());
        unsigned int fId(0);
        //float_t m = *std::max_element(m_logUnNormWeights.begin(), m_logUnNormWeights.end());
        for(auto & h : fs){
 
            Mat testOutput = h(m_p_innerMods[0].getFilterVecLogProbs(), m_p_samps[0]);
            unsigned int rows = testOutput.rows();
            unsigned int cols = testOutput.cols();
            Mat numer = Mat::Zero(rows,cols);
            float_t denom(0.0);
            for(size_t prtcl = 0; prtcl < nparts; ++prtcl){ 
                numer += h(m_p_innerMods[prtcl].getFilterVecLogProbs(), m_p_samps[prtcl]) * std::exp(m_logUnNormWeights[prtcl] - m1);
                denom += std::exp(m_logUnNormWeights[prtcl] - m1);
            }
            m_expectations[fId] = numer/denom;
 
            // print stuff if debug mode is on
            #ifndef DROPPINGTHISINRPACKAGE
            if constexpr(debug)
                std::cout << "transposed expec " << fId << ": " << m_expectations[fId].transpose() << "\n";
            #endif
 
            fId++;
        }
        
        // resample (unnormalized weights ok)
        if( (m_now+1) % m_rs == 0)
            m_resampler.resampLogWts(m_p_innerMods, m_p_samps, m_logUnNormWeights);
 
        // advance time step
        m_now ++;
    }
   
}
 
 
template<size_t nparts, size_t dimnss, size_t dimss, size_t dimy, typename resamp_t, typename float_t, bool debug>
float_t rbpf_hmm<nparts,dimnss,dimss,dimy,resamp_t,float_t,debug>::getLogCondLike() const
{
    return m_lastLogCondLike;
}
 
 
template<size_t nparts, size_t dimnss, size_t dimss, size_t dimy, typename resamp_t, typename float_t, bool debug>
auto rbpf_hmm<nparts,dimnss,dimss,dimy,resamp_t,float_t,debug>::getExpectations() const -> std::vector<Mat>
{
    return m_expectations;
}
 
 
 
template<size_t nparts, size_t dimnss, size_t dimss, size_t dimy, typename resamp_t, typename float_t, bool debug = false>
class rbpf_hmm_bs : public bases::rbpf_base<float_t,dimss,dimnss,dimy>
{
public:
 
    using sssv = Eigen::Matrix<float_t,dimss,1>;
    using nsssv = Eigen::Matrix<float_t,dimnss,1>;
    using osv = Eigen::Matrix<float_t,dimy,1>;
    using nsssMat = Eigen::Matrix<float_t,dimnss,dimnss>;
    using Mat = Eigen::Matrix<float_t,Eigen::Dynamic,Eigen::Dynamic>;
    using cfModType = hmm<dimnss,dimy,float_t,debug>;
    using arrayMod = std::array<cfModType,nparts>;
    using arrayVec = std::array<sssv,nparts>;
    using arrayfloat_t = std::array<float_t,nparts>;
 
 
 
    rbpf_hmm_bs(const unsigned int &resamp_sched=1);
    
    
    virtual ~rbpf_hmm_bs();
    
 
 
    void filter(const osv &data,
                const std::vector<std::function<const Mat(const nsssv &x1tLogProbs, const sssv &x2t)> >& fs 
                    = std::vector<std::function<const Mat(const nsssv&, const sssv&)> >());//, const std::vector<std::function<const Mat(const Vec&)> >& fs);
 
 
 
    float_t getLogCondLike() const;
    
 
    std::vector<Mat> getExpectations() const;
    
    
 
    virtual sssv muSamp() = 0;
    
    
 
    virtual nsssv initHMMLogProbVec(const sssv &x21) = 0;
    
    
 
    virtual nsssMat initHMMLogTransMat(const sssv &x21) = 0;
 
 
    virtual sssv fSamp(const sssv &x2tm1) = 0;
    
    
 
    virtual void updateHMM(cfModType &aModel, const osv &yt, const sssv &x2t) = 0;
 
private:
    
    unsigned int m_now;
    float_t m_lastLogCondLike;
    unsigned int m_rs;
    arrayMod m_p_innerMods;
    arrayVec m_p_samps;
    arrayfloat_t m_logUnNormWeights;
    resamp_t m_resampler;
    std::vector<Mat> m_expectations; 
 
};
 
 
template<size_t nparts, size_t dimnss, size_t dimss, size_t dimy, typename resamp_t, typename float_t, bool debug>
rbpf_hmm_bs<nparts,dimnss,dimss,dimy,resamp_t,float_t,debug>::rbpf_hmm_bs(const unsigned int &resamp_sched)
    : m_now(0)
    , m_lastLogCondLike(0.0)
    , m_rs(resamp_sched)
{
    std::fill(m_logUnNormWeights.begin(), m_logUnNormWeights.end(), 0.0);
}
 
 
template<size_t nparts, size_t dimnss, size_t dimss, size_t dimy, typename resamp_t, typename float_t, bool debug>
rbpf_hmm_bs<nparts,dimnss,dimss,dimy,resamp_t,float_t,debug>::~rbpf_hmm_bs() {}
 
 
template<size_t nparts, size_t dimnss, size_t dimss, size_t dimy, typename resamp_t, typename float_t, bool debug>
void rbpf_hmm_bs<nparts,dimnss,dimss,dimy,resamp_t,float_t,debug>::filter(const osv &data, const std::vector<std::function<const Mat(const nsssv &x1tLogProbs, const sssv &x2t)> >& fs)
{
 
    if(m_now > 0)
    {     
        // update
        sssv newX2Samp;
        float_t sumexpdenom(0.0);
        float_t m1(-std::numeric_limits<float_t>::infinity()); // for revised log weights
        float_t m2 = *std::max_element(m_logUnNormWeights.begin(), m_logUnNormWeights.end());
        for(size_t ii = 0; ii < nparts; ++ii){
            
            newX2Samp = fSamp(m_p_samps[ii]);
            updateHMM(m_p_innerMods[ii], data, newX2Samp);
            sumexpdenom += std::exp(m_logUnNormWeights[ii] - m2);
            
            m_logUnNormWeights[ii] += m_p_innerMods[ii].getLogCondLike();
            
            // print stuff if debug mode is on
            #ifndef DROPPINGTHISINRPACKAGE
            if constexpr(debug)
                std::cout << "time: " << m_now << ", transposed x2 sample: " << newX2Samp.transpose() << ", log unnorm weight: " << m_logUnNormWeights[ii] << "\n";
            #endif
 
            // update a max
            if(m_logUnNormWeights[ii] > m1)
                m1 = m_logUnNormWeights[ii];
            
            m_p_samps[ii] = newX2Samp;
        }
        
        // calculate log p(y_t | y_{1:t-1})
        float_t sumexpnumer(0.0);
        for(size_t p = 0; p < nparts; ++p)
            sumexpnumer += std::exp(m_logUnNormWeights[p] - m1);
        m_lastLogCondLike = m1 + std::log(sumexpnumer) - m2 - std::log(sumexpdenom);
        
        // calculate expectations before you resample
        unsigned int fId(0);
        //float_t m = *std::max_element(m_logUnNormWeights.begin(), m_logUnNormWeights.end());
        for(auto & h : fs){
            
            Mat testOutput = h(m_p_innerMods[0].getFilterVecLogProbs(), m_p_samps[0]);
            unsigned int rows = testOutput.rows();
            unsigned int cols = testOutput.cols();
            Mat numer = Mat::Zero(rows,cols);
            float_t denom(0.0);
            for(size_t prtcl = 0; prtcl < nparts; ++prtcl){ 
                numer += h(m_p_innerMods[prtcl].getFilterVecLogProbs(), m_p_samps[prtcl])*std::exp(m_logUnNormWeights[prtcl] - m1);
                denom += std::exp( m_logUnNormWeights[prtcl] - m1 );
            }
            m_expectations[fId] = numer/denom;
                        
            // print stuff if debug mode is on
            #ifndef DROPPINGTHISINRPACKAGE
            if constexpr(debug)
                std::cout << "transposed expec " << fId << ": " << m_expectations[fId].transpose() << "\n";
            #endif
 
            fId++;
        }
 
        // resample (unnormalized weights ok)
        if( (m_now+1) % m_rs == 0)
            m_resampler.resampLogWts(m_p_innerMods, m_p_samps, m_logUnNormWeights);
        
        // update time step
        m_now ++;
    }
    else// ( m_now == 0) // first data point coming
    {
        // initialize and update the closed-form mods        
        nsssv tmpLogProbs;
        nsssMat tmpLogTransMat;
        float_t m1(-std::numeric_limits<float_t>::infinity());
        for(size_t ii = 0; ii < nparts; ++ii){
            
            m_p_samps[ii] = muSamp(); 
            tmpLogProbs = initHMMLogProbVec(m_p_samps[ii]);
            tmpLogTransMat = initHMMLogTransMat(m_p_samps[ii]);
            m_p_innerMods[ii] = cfModType(tmpLogProbs, tmpLogTransMat);
            this->updateHMM(m_p_innerMods[ii], data, m_p_samps[ii]);
            m_logUnNormWeights[ii] = m_p_innerMods[ii].getLogCondLike();
                     
            // print stuff if debug mode is on
            #ifndef DROPPINGTHISINRPACKAGE
            if constexpr(debug)
                std::cout << "time: " << m_now << ", transposed x2 sample: " << m_p_samps[ii].transpose() << ", log unnorm weight: " << m_logUnNormWeights[ii] << "\n";
            #endif
 
 
            // maximum to be used in likelihood calc
            if(m_logUnNormWeights[ii] > m1)
                m1 = m_logUnNormWeights[ii];
        }
 
        // calc log p(y1)
        float_t sumexp(0.0);
        for(size_t p = 0; p < nparts; ++p)
            sumexp += std::exp(m_logUnNormWeights[p] - m1);
        m_lastLogCondLike = m1 + std::log(sumexp) - std::log(static_cast<float_t>(nparts));
 
        // calculate expectations before you resample
        m_expectations.resize(fs.size());
        unsigned int fId(0);
        //float_t m = *std::max_element(m_logUnNormWeights.begin(), m_logUnNormWeights.end()); /// TODO: can we just use m1?
        for(auto & h : fs){
 
            Mat testOutput = h(m_p_innerMods[0].getFilterVecLogProbs(), m_p_samps[0]);
            unsigned int rows = testOutput.rows();
            unsigned int cols = testOutput.cols();
            Mat numer = Mat::Zero(rows,cols);
            float_t denom(0.0);
            for(size_t prtcl = 0; prtcl < nparts; ++prtcl){ 
                numer += h(m_p_innerMods[prtcl].getFilterVecLogProbs(), m_p_samps[prtcl]) * std::exp(m_logUnNormWeights[prtcl] - m1);
                denom += std::exp( m_logUnNormWeights[prtcl] - m1 );
            }
            m_expectations[fId] = numer/denom;
                                    
            // print stuff if debug mode is on
            #ifndef DROPPINGTHISINRPACKAGE
            if constexpr(debug)
                std::cout << "transposed expec " << fId << ": " << m_expectations[fId].transpose() << "\n";
            #endif
 
            fId++;
        }
        
        // resample (unnormalized weights ok)
        if( (m_now+1) % m_rs == 0)
            m_resampler.resampLogWts(m_p_innerMods, m_p_samps, m_logUnNormWeights);
 
        // advance time step
        m_now ++;
    }
   
}
 
 
template<size_t nparts, size_t dimnss, size_t dimss, size_t dimy, typename resamp_t, typename float_t, bool debug>
float_t rbpf_hmm_bs<nparts,dimnss,dimss,dimy,resamp_t,float_t,debug>::getLogCondLike() const
{
    return m_lastLogCondLike;
}
 
 
template<size_t nparts, size_t dimnss, size_t dimss, size_t dimy, typename resamp_t, typename float_t, bool debug>
auto rbpf_hmm_bs<nparts,dimnss,dimss,dimy,resamp_t,float_t,debug>::getExpectations() const -> std::vector<Mat>
{
    return m_expectations;
}
 
 
 
template<size_t nparts, size_t dimnss, size_t dimss, size_t dimy, typename resamp_t, typename float_t, bool debug = false>
class rbpf_kalman : public bases::rbpf_base<float_t,dimss,dimnss,dimy>
{
 
public:
 
    using sssv = Eigen::Matrix<float_t,dimss,1>;
    using nsssv = Eigen::Matrix<float_t,dimnss,1>;
    using osv = Eigen::Matrix<float_t,dimy,1>;
    using Mat = Eigen::Matrix<float_t,Eigen::Dynamic,Eigen::Dynamic>;
    using nsssMat = Eigen::Matrix<float_t,dimnss,dimnss>;
    using cfModType = kalman<dimnss,dimy,0,float_t,debug>;
    using arrayMod = std::array<cfModType,nparts>;
    using arrayVec = std::array<sssv,nparts>;
    using arrayfloat_t = std::array<float_t,nparts>;
 
 
    rbpf_kalman(const unsigned int &resamp_sched=1);
    
    
    virtual ~rbpf_kalman();
    
    
 
    void filter(const osv &data, const std::vector<std::function<const Mat(const nsssv &x1t, const sssv &x2t)> >& fs
                                     = std::vector<std::function<const Mat(const nsssv &x1t, const sssv &x2t)> >() );
 
 
    float_t getLogCondLike() const; 
    
    
 
    std::vector<Mat> getExpectations() const;
    
    
 
    virtual float_t logMuEv(const sssv &x21) = 0;
    
    
 
    virtual sssv q1Samp(const osv &y1) = 0;
    
    
 
    virtual nsssv initKalmanMean(const sssv &x21) = 0;
    
    
 
    virtual nsssMat initKalmanVar(const sssv &x21) = 0;
    
    
 
    virtual sssv qSamp(const sssv &x2tm1, const osv &yt) = 0;
    
    
 
    virtual float_t logQ1Ev(const sssv &x21, const osv &y1) = 0;
    
    
 
    virtual float_t logFEv(const sssv &x2t, const sssv &x2tm1) = 0;
    
    
 
    virtual float_t logQEv(const sssv &x2t, const sssv &x2tm1, const osv &yt) = 0;
    
    
 
    virtual void updateKalman(cfModType &kMod, const osv &yt, const sssv &x2t) = 0;
    
private:
 
    unsigned int m_rs;
    arrayMod m_p_innerMods;
    arrayVec m_p_samps;
    arrayfloat_t m_logUnNormWeights;
    unsigned int m_now;
    float_t m_lastLogCondLike; 
    resamp_t m_resampler;
    std::vector<Mat> m_expectations;
};
 
 
template<size_t nparts, size_t dimnss, size_t dimss, size_t dimy, typename resamp_t, typename float_t, bool debug>
rbpf_kalman<nparts,dimnss,dimss,dimy,resamp_t,float_t,debug>::rbpf_kalman(const unsigned int &resamp_sched)
    : m_now(0)
    , m_lastLogCondLike(0.0)
    , m_rs(resamp_sched)
{
    std::fill(m_logUnNormWeights.begin(), m_logUnNormWeights.end(), 0.0);
}
 
 
template<size_t nparts, size_t dimnss, size_t dimss, size_t dimy, typename resamp_t, typename float_t, bool debug>
rbpf_kalman<nparts,dimnss,dimss,dimy,resamp_t,float_t,debug>::~rbpf_kalman() {}
 
 
template<size_t nparts, size_t dimnss, size_t dimss, size_t dimy, typename resamp_t, typename float_t, bool debug>
void rbpf_kalman<nparts,dimnss,dimss,dimy,resamp_t,float_t,debug>::filter(const osv &data, const std::vector<std::function<const Mat(const nsssv &x1t, const sssv &x2t)> >& fs)
{
    
    if(m_now > 0)
    {
        
        // update
        sssv newX2Samp;
        float_t m1(-std::numeric_limits<float_t>::infinity()); // for updated weights
        float_t m2 = *std::max_element(m_logUnNormWeights.begin(), m_logUnNormWeights.end());
        float_t sumexpdenom(0.0);
        for(size_t ii = 0; ii < nparts; ++ii){
            newX2Samp = qSamp(m_p_samps[ii], data);
            this->updateKalman(m_p_innerMods[ii], data, newX2Samp);
 
            // before you update the weights
            sumexpdenom += std::exp(m_logUnNormWeights[ii] - m2);
            
            // update the weights
            m_logUnNormWeights[ii] += m_p_innerMods[ii].getLogCondLike() + logFEv(newX2Samp, m_p_samps[ii]) - logQEv(newX2Samp, m_p_samps[ii], data);
                        
            // print stuff if debug mode is on
            #ifndef DROPPINGTHISINRPACKAGE
            if constexpr(debug)
                std::cout << "time: " << m_now << ", transposed x2 sample: " << newX2Samp.transpose() << ", log unnorm weight: " << m_logUnNormWeights[ii] << "\n";
            #endif
 
            // update a max
            if(m_logUnNormWeights[ii] > m1)
                m1 = m_logUnNormWeights[ii];
                
            m_p_samps[ii] = newX2Samp;
        }
        
        // calc log p(y_t | y_{1:t-1})
        float_t sumexpnumer(0.0);
        for(size_t p = 0; p < nparts; ++p)
            sumexpnumer += std::exp(m_logUnNormWeights[p] - m1);
        m_lastLogCondLike = m1 + std::log(sumexpnumer) - m2 - std::log(sumexpdenom);
        
        // calculate expectations before you resample
        unsigned int fId(0);
        //float_t m = *std::max_element(m_logUnNormWeights.begin(), m_logUnNormWeights.end());
        for(auto & h : fs){
 
            Mat testOutput = h(m_p_innerMods[0].getFilterVec(), m_p_samps[0]);
            unsigned int rows = testOutput.rows();
            unsigned int cols = testOutput.cols();
            Mat numer = Mat::Zero(rows,cols);
            float_t denom(0.0);
            for(size_t prtcl = 0; prtcl < nparts; ++prtcl){ 
                numer += h(m_p_innerMods[prtcl].getFilterVec(), m_p_samps[prtcl]) * std::exp(m_logUnNormWeights[prtcl] - m1);
                denom += std::exp( m_logUnNormWeights[prtcl] - m1 );
            }
            m_expectations[fId] = numer/denom;
                                                
            // print stuff if debug mode is on
            #ifndef DROPPINGTHISINRPACKAGE
            if constexpr(debug)
                std::cout << "transposed expec " << fId << ": " << m_expectations[fId].transpose() << "\n";
            #endif
 
            fId++;
        }
 
        // resample (unnormalized weights ok)
        if( (m_now+1)%m_rs == 0)
            m_resampler.resampLogWts(m_p_innerMods, m_p_samps, m_logUnNormWeights);
        
        // update time step
        m_now ++;
    }
    else //( m_now == 0) // first data point coming
    {
        // initialize and update the closed-form mods      
        nsssv tmpMean;
        nsssMat tmpVar;
        float_t m1(-std::numeric_limits<float_t>::infinity());
        for(size_t ii = 0; ii < nparts; ++ii){
            m_p_samps[ii] = q1Samp(data); 
            tmpMean = initKalmanMean(m_p_samps[ii]);
            tmpVar  = initKalmanVar(m_p_samps[ii]);
            m_p_innerMods[ii] = cfModType(tmpMean, tmpVar);   // TODO: allow for input or check to make sure this doesn't break anything else
            this->updateKalman(m_p_innerMods[ii], data, m_p_samps[ii]);
 
            m_logUnNormWeights[ii] = m_p_innerMods[ii].getLogCondLike() + logMuEv(m_p_samps[ii]) - logQ1Ev(m_p_samps[ii], data);
                        
            // print stuff if debug mode is on
            #ifndef DROPPINGTHISINRPACKAGE
            if constexpr(debug)
                std::cout << "time: " << m_now << ", transposed x2 sample: " << m_p_samps[ii].transpose() << ", log unnorm weight: " << m_logUnNormWeights[ii] << "\n";
            #endif 
 
            // update a max
            if(m_logUnNormWeights[ii] > m1)
                m1 = m_logUnNormWeights[ii];
        }
 
        // calculate log p(y1)
        float_t sumexp(0.0);
        for(size_t p = 0; p < nparts; ++p)
            sumexp += std::exp(m_logUnNormWeights[p] - m1);
        m_lastLogCondLike = m1 + std::log(sumexp) - std::log(static_cast<float_t>(nparts));  
 
        // calculate expectations before you resample
        m_expectations.resize(fs.size());
        unsigned int fId(0);
        //float_t m = *std::max_element(m_logUnNormWeights.begin(), m_logUnNormWeights.end());
        for(auto & h : fs){
 
            Mat testOutput = h(m_p_innerMods[0].getFilterVec(), m_p_samps[0]);
            unsigned int rows = testOutput.rows();
            unsigned int cols = testOutput.cols();
            Mat numer = Mat::Zero(rows,cols);
            float_t denom(0.0);
            for(size_t prtcl = 0; prtcl < nparts; ++prtcl){ 
                numer += h(m_p_innerMods[prtcl].getFilterVec(), m_p_samps[prtcl]) * std::exp(m_logUnNormWeights[prtcl] - m1);
                denom += std::exp( m_logUnNormWeights[prtcl] - m1 );
            }
            m_expectations[fId] = numer/denom;
                                                            
            // print stuff if debug mode is on
            #ifndef DROPPINGTHISINRPACKAGE
            if constexpr(debug)
                std::cout << "transposed expec " << fId << ": " << m_expectations[fId].transpose() << "\n";
            #endif
 
            fId++;
        }
        
        // resample (unnormalized weights ok)
        if( (m_now+1)%m_rs == 0)
            m_resampler.resampLogWts(m_p_innerMods, m_p_samps, m_logUnNormWeights);
            
        // advance time step
        m_now ++;
    }
 
}
 
 
template<size_t nparts, size_t dimnss, size_t dimss, size_t dimy, typename resamp_t, typename float_t, bool debug>
float_t rbpf_kalman<nparts,dimnss,dimss,dimy,resamp_t,float_t,debug>::getLogCondLike() const
{
    return m_lastLogCondLike;
}
 
 
template<size_t nparts, size_t dimnss, size_t dimss, size_t dimy, typename resamp_t, typename float_t, bool debug>
auto rbpf_kalman<nparts,dimnss,dimss,dimy,resamp_t,float_t,debug>::getExpectations() const -> std::vector<Mat>
{
    return m_expectations;
}
 
 
 
 
template<size_t nparts, size_t dimnss, size_t dimss, size_t dimy, typename resamp_t, typename float_t, bool debug = false>
class rbpf_kalman_bs : public bases::rbpf_base<float_t,dimss,dimnss,dimy>
{
 
public:
 
    using sssv = Eigen::Matrix<float_t,dimss,1>;
    using nsssv = Eigen::Matrix<float_t,dimnss,1>;
    using osv = Eigen::Matrix<float_t,dimy,1>;
    using Mat = Eigen::Matrix<float_t,Eigen::Dynamic,Eigen::Dynamic>;
    using nsssMat = Eigen::Matrix<float_t,dimnss,dimnss>;
    using cfModType = kalman<dimnss,dimy,0,float_t,debug>;
    using arrayMod = std::array<cfModType,nparts>;
    using arrayVec = std::array<sssv,nparts>;
    using arrayfloat_t = std::array<float_t,nparts>;
 
 
    rbpf_kalman_bs(const unsigned int &resamp_sched=1);
    
    
    virtual ~rbpf_kalman_bs();
    
    
 
    void filter(const osv &data, const std::vector<std::function<const Mat(const nsssv &x1t, const sssv &x2t)> >& fs
                                     = std::vector<std::function<const Mat(const nsssv &x1t, const sssv &x2t)> >() );
 
 
    float_t getLogCondLike() const; 
    
    
 
    std::vector<Mat> getExpectations() const;
    
    
 
    virtual sssv muSamp() = 0;
    
    
 
    virtual nsssv initKalmanMean(const sssv &x21) = 0;
    
    
 
    virtual nsssMat initKalmanVar(const sssv &x21) = 0;
    
    
 
    virtual sssv fSamp(const sssv &x2tm1) = 0;
    
    
 
    virtual void updateKalman(cfModType &kMod, const osv &yt, const sssv &x2t) = 0;
    
private:
 
    unsigned int m_rs;
    arrayMod m_p_innerMods;
    arrayVec m_p_samps;
    arrayfloat_t m_logUnNormWeights;
    unsigned int m_now;
    float_t m_lastLogCondLike; 
    resamp_t m_resampler;
    std::vector<Mat> m_expectations;
};
 
 
template<size_t nparts, size_t dimnss, size_t dimss, size_t dimy, typename resamp_t, typename float_t, bool debug>
rbpf_kalman_bs<nparts,dimnss,dimss,dimy,resamp_t,float_t,debug>::rbpf_kalman_bs(const unsigned int &resamp_sched)
    : m_now(0)
    , m_lastLogCondLike(0.0)
    , m_rs(resamp_sched)
{
    std::fill(m_logUnNormWeights.begin(), m_logUnNormWeights.end(), 0.0);
}
 
 
template<size_t nparts, size_t dimnss, size_t dimss, size_t dimy, typename resamp_t, typename float_t, bool debug>
rbpf_kalman_bs<nparts,dimnss,dimss,dimy,resamp_t,float_t,debug>::~rbpf_kalman_bs() {}
 
 
template<size_t nparts, size_t dimnss, size_t dimss, size_t dimy, typename resamp_t, typename float_t, bool debug>
void rbpf_kalman_bs<nparts,dimnss,dimss,dimy,resamp_t,float_t,debug>::filter(const osv &data, const std::vector<std::function<const Mat(const nsssv &x1t, const sssv &x2t)> >& fs)
{
    
    if(m_now > 0)
    {
        
        // update
        sssv newX2Samp;
        float_t m1(-std::numeric_limits<float_t>::infinity()); // for updated weights
        float_t m2 = *std::max_element(m_logUnNormWeights.begin(), m_logUnNormWeights.end());
        float_t sumexpdenom(0.0);
        for(size_t ii = 0; ii < nparts; ++ii){
            
            newX2Samp = fSamp(m_p_samps[ii], data);
            this->updateKalman(m_p_innerMods[ii], data, newX2Samp);
 
            // before you update the weights
            sumexpdenom += std::exp(m_logUnNormWeights[ii] - m2);
            
            // update the weights
            m_logUnNormWeights[ii] += m_p_innerMods[ii].getLogCondLike();
                                    
            // print stuff if debug mode is on
            #ifndef DROPPINGTHISINRPACKAGE
            if constexpr(debug)
                std::cout << "time: " << m_now << ", transposed x2 sample: " << newX2Samp.transpose() << ", log unnorm weight: " << m_logUnNormWeights[ii] << "\n";
            #endif 
 
            // update a max
            if(m_logUnNormWeights[ii] > m1)
                m1 = m_logUnNormWeights[ii];
                
            m_p_samps[ii] = newX2Samp;
        }
        
        // calc log p(y_t | y_{1:t-1})
        float_t sumexpnumer(0.0);
        for(size_t p = 0; p < nparts; ++p)
            sumexpnumer += std::exp(m_logUnNormWeights[p] - m1);
        m_lastLogCondLike = m1 + std::log(sumexpnumer) - m2 - std::log(sumexpdenom);
        
        // calculate expectations before you resample
        unsigned int fId(0);
        //float_t m = *std::max_element(m_logUnNormWeights.begin(), m_logUnNormWeights.end());
        for(auto & h : fs){
 
            Mat testOutput = h(m_p_innerMods[0].getFilterVec(), m_p_samps[0]);
            unsigned int rows = testOutput.rows();
            unsigned int cols = testOutput.cols();
            Mat numer = Mat::Zero(rows,cols);
            float_t denom(0.0);
            for(size_t prtcl = 0; prtcl < nparts; ++prtcl){ 
                numer += h(m_p_innerMods[prtcl].getFilterVec(), m_p_samps[prtcl]) * std::exp(m_logUnNormWeights[prtcl] - m1);
                denom += std::exp( m_logUnNormWeights[prtcl] - m1 );
            }
            m_expectations[fId] = numer/denom;
                                                                        
            // print stuff if debug mode is on
            #ifndef DROPPINGTHISINRPACKAGE
            if constexpr(debug)
                std::cout << "transposed expec " << fId << ": " << m_expectations[fId].transpose() << "\n";
            #endif
 
            fId++;
        }
 
        // resample (unnormalized weights ok)
        if( (m_now+1)%m_rs == 0)
            m_resampler.resampLogWts(m_p_innerMods, m_p_samps, m_logUnNormWeights);
        
        // update time step
        m_now ++;
    }
    else // ( m_now == 0) // first data point coming
    {
        // initialize and update the closed-form mods      
        nsssv tmpMean;
        nsssMat tmpVar;
        float_t m1(-std::numeric_limits<float_t>::infinity());
        for(size_t ii = 0; ii < nparts; ++ii){
            m_p_samps[ii] = muSamp(data); 
            tmpMean = initKalmanMean(m_p_samps[ii]);
            tmpVar  = initKalmanVar(m_p_samps[ii]);
            m_p_innerMods[ii] = cfModType(tmpMean, tmpVar);   // TODO: allow for input or check to make sure this doesn't break anything else
            this->updateKalman(m_p_innerMods[ii], data, m_p_samps[ii]);
 
            m_logUnNormWeights[ii] = m_p_innerMods[ii].getLogCondLike();
                                                
            // print stuff if debug mode is on
            #ifndef DROPPINGTHISINRPACKAGE
            if constexpr(debug)
                std::cout << "time: " << m_now << ", transposed x2 sample: " << m_p_samps[ii].transpose() << ", log unnorm weight: " << m_logUnNormWeights[ii] << "\n";
            #endif 
 
            // update a max
            if(m_logUnNormWeights[ii] > m1)
                m1 = m_logUnNormWeights[ii];
        }
 
        // calculate log p(y1)
        float_t sumexp(0.0);
        for(size_t p = 0; p < nparts; ++p)
            sumexp += std::exp(m_logUnNormWeights[p] - m1);
        m_lastLogCondLike = m1 + std::log(sumexp) - std::log(static_cast<float_t>(nparts));  
 
        // calculate expectations before you resample
        m_expectations.resize(fs.size());
        unsigned int fId(0);
        //float_t m = *std::max_element(m_logUnNormWeights.begin(), m_logUnNormWeights.end());
        for(auto & h : fs){
 
            Mat testOutput = h(m_p_innerMods[0].getFilterVec(), m_p_samps[0]);
            unsigned int rows = testOutput.rows();
            unsigned int cols = testOutput.cols();
            Mat numer = Mat::Zero(rows,cols);
            float_t denom(0.0);
            for(size_t prtcl = 0; prtcl < nparts; ++prtcl){ 
                numer += h(m_p_innerMods[prtcl].getFilterVec(), m_p_samps[prtcl])*std::exp(m_logUnNormWeights[prtcl] - m1);
                denom += std::exp( m_logUnNormWeights[prtcl] - m1 );
            }
            m_expectations[fId] = numer/denom;
                                                                                                
            // print stuff if debug mode is on
            #ifndef DROPPINGTHISINRPACKAGE
            if constexpr(debug)
                std::cout << "transposed expec " << fId << ": " << m_expectations[fId].transpose() << "\n";
            #endif
 
            fId++;
        }
        
        // resample (unnormalized weights ok)
        if( (m_now+1)%m_rs == 0)
            m_resampler.resampLogWts(m_p_innerMods, m_p_samps, m_logUnNormWeights);
            
        // advance time step
        m_now ++;
    }
 
}
 
 
template<size_t nparts, size_t dimnss, size_t dimss, size_t dimy, typename resamp_t, typename float_t, bool debug>
float_t rbpf_kalman_bs<nparts,dimnss,dimss,dimy,resamp_t,float_t,debug>::getLogCondLike() const
{
    return m_lastLogCondLike;
}
 
 
template<size_t nparts, size_t dimnss, size_t dimss, size_t dimy, typename resamp_t, typename float_t, bool debug>
auto rbpf_kalman_bs<nparts,dimnss,dimss,dimy,resamp_t,float_t,debug>::getExpectations() const -> std::vector<Mat>
{
    return m_expectations;
}
 
} //namespace filters
} //namespace pf
 
#endif //RBPF_H