cf_filters.h

Go to the documentation of this file.

#ifndef CF_FILTERS_H
#define CF_FILTERS_H
 
 
#ifdef DROPPINGTHISINRPACKAGE
    #include <RcppEigen.h>
    // [[Rcpp::depends(RcppEigen)]]
#else
    #include <Eigen/Dense>
#endif
 
#include <math.h>       /* log */
 
#include "rv_eval.h"
#include "pf_base.h"
 
namespace pf {
 
namespace filters {
 
 
 
 
 
template<size_t dimstate, size_t dimobs, size_t diminput, typename float_t, bool debug = false>
class kalman : public bases::cf_filter<dimstate, dimobs, float_t> {
 
public:    
    
    using ssv = Eigen::Matrix<float_t,dimstate,1>;
    
    using osv = Eigen::Matrix<float_t,dimobs,1>;
    
    using isv = Eigen::Matrix<float_t,diminput,1>;
    
    using ssMat = Eigen::Matrix<float_t,dimstate,dimstate>;
    
    using osMat = Eigen::Matrix<float_t,dimobs,dimobs>;
    
    using siMat = Eigen::Matrix<float_t,dimstate,diminput>;
        
    using oiMat = Eigen::Matrix<float_t,dimobs,diminput>;
 
    using obsStateSizeMat = Eigen::Matrix<float_t,dimobs,dimstate>;    
 
    using stateObsSizeMat = Eigen::Matrix<float_t,dimstate,dimobs>;
 
 
 
    kalman();
 
 
 
    kalman(const ssv &initStateMean, const ssMat &initStateVar);
    
    
    virtual ~kalman();
    
 
    float_t getLogCondLike() const;
    
    
    ssv getFiltMean() const;
    
    
    ssMat getFiltVar() const;
    
 
    osv getPredYMean(const ssMat &stateTrans,
                     const obsStateSizeMat &obsMat, 
                     const siMat &stateInptAffector,
                     const oiMat &obsInptAffector, 
                     const isv &inputData) const;
 
 
    osMat getPredYVar(const ssMat &stateTrans,
                      const ssMat &cholStateVar,
                      const obsStateSizeMat &obsMat,
                      const osMat &cholObsVar) const;
 
 
 
    void update(const osv &yt, 
                const ssMat &stateTrans, 
                const ssMat &cholStateVar, 
                const siMat &stateInptAffector, 
                const isv &inputData,
                const obsStateSizeMat &obsMat,
                const oiMat &obsInptAffector, 
                const osMat &cholObsVar);
                
private: 
 
    ssv m_predMean;
    
    ssv m_filtMean;
    
    ssMat m_predVar;
    
    ssMat m_filtVar;
    
    float_t m_lastLogCondLike; 
    
    bool m_fresh;
    
    const float_t m_pi;
    
    void updatePrior(const ssMat &stateTransMat, 
                     const ssMat &cholStateVar, 
                     const siMat &stateInptAffector, 
                     const isv &inputData);
                     
    
    void updatePosterior(const osv &yt, 
                         const obsStateSizeMat &obsMat, 
                         const oiMat &obsInptAffector, 
                         const isv &inputData, 
                         const osMat &cholObsVar);
};
 
 
template<size_t dimstate, size_t dimobs, size_t diminput, typename float_t, bool debug>  
kalman<dimstate,dimobs,diminput,float_t,debug>::kalman() 
        : bases::cf_filter<dimstate,dimobs,float_t>()
        , m_predMean(ssv::Zero())
        , m_predVar(ssMat::Zero()) 
        , m_fresh(true)
        , m_pi(3.14159265358979)
{
}
    
 
template<size_t dimstate, size_t dimobs, size_t diminput, typename float_t, bool debug>  
kalman<dimstate,dimobs,diminput,float_t,debug>::kalman(const ssv &initStateMean, const ssMat &initStateVar) 
        : bases::cf_filter<dimstate,dimobs,float_t>()
        , m_predMean(initStateMean)
        , m_predVar(initStateVar) 
        , m_fresh(true)
        , m_pi(3.14159265358979)
{
}
 
 
template<size_t dimstate, size_t dimobs, size_t diminput, typename float_t, bool debug>
kalman<dimstate,dimobs,diminput,float_t,debug>::~kalman() {}
 
 
template<size_t dimstate, size_t dimobs, size_t diminput, typename float_t, bool debug>
void kalman<dimstate,dimobs,diminput,float_t,debug>::updatePrior(const ssMat &stateTransMat, 
                        const ssMat &cholStateVar, 
                        const siMat &stateInptAffector, 
                        const isv &inputData)
{
    ssMat Q = cholStateVar.transpose() * cholStateVar;
    m_predMean = stateTransMat * m_filtMean + stateInptAffector * inputData;
    m_predVar  = stateTransMat * m_filtVar * stateTransMat.transpose() + Q;
    
}
 
 
template<size_t dimstate, size_t dimobs, size_t diminput, typename float_t, bool debug>
void kalman<dimstate,dimobs,diminput,float_t,debug>::updatePosterior(const osv &yt, 
                             const obsStateSizeMat &obsMat, 
                             const oiMat &obsInptAffector, 
                             const isv &inputData, 
                             const osMat &cholObsVar)
{
    osMat R = cholObsVar.transpose() * cholObsVar; //obs
    osMat sigma = obsMat * m_predVar * obsMat.transpose() + R; // pred or APA' + R 
    osMat symSigma = (sigma.transpose() + sigma )/2.0; // ensure symmetric
    osMat siginv = symSigma.inverse();
    stateObsSizeMat K = m_predVar * obsMat.transpose() * siginv;
    osv obsPred = obsMat * m_predMean + obsInptAffector * inputData;
    osv innov = yt - obsPred;
    m_filtMean = m_predMean + K*innov;
    m_filtVar  = m_predVar - K * obsMat * m_predVar;
 
    // conditional likelihood stuff
    osMat quadForm = innov.transpose() * siginv * innov;
    osMat cholSig ( sigma.llt().matrixL() );
    float_t logDet = 2.0*cholSig.diagonal().array().log().sum();
    m_lastLogCondLike = -.5*innov.rows()*log(2*m_pi) - .5*logDet - .5*quadForm(0,0);
 
    #ifndef DROPPINGTHISINRPACKAGE
    if constexpr(debug)
        std::cout << "transposed innovation: " << innov.transpose() << ", quadratic formula: " << quadForm(0,0) << ", logDet: " << logDet << ", log cond like: " << m_lastLogCondLike << "\n";
    #endif
    
}
 
 
template<size_t dimstate, size_t dimobs, size_t diminput, typename float_t, bool debug>
float_t kalman<dimstate,dimobs,diminput,float_t,debug>::getLogCondLike() const
{
    return m_lastLogCondLike;
}
 
 
template<size_t dimstate, size_t dimobs, size_t diminput, typename float_t, bool debug>
auto kalman<dimstate,dimobs,diminput,float_t,debug>::getFiltMean() const -> ssv
{
    return m_filtMean;
}
 
 
template<size_t dimstate, size_t dimobs, size_t diminput, typename float_t, bool debug>
auto kalman<dimstate,dimobs,diminput,float_t,debug>::getFiltVar() const -> ssMat
{
    return m_filtVar;
}
 
 
template<size_t dimstate, size_t dimobs, size_t diminput, typename float_t, bool debug>
void kalman<dimstate,dimobs,diminput,float_t,debug>::update(const osv &yt, 
                                              const ssMat &stateTrans, 
                                              const ssMat &cholStateVar, 
                                              const siMat &stateInptAffector, 
                                              const isv &inData,
                                              const obsStateSizeMat &obsMat,
                                              const oiMat &obsInptAffector, 
                                              const osMat &cholObsVar)
{
    // this assumes that we have latent states x_{1:...} and y_{1:...} (NOT x_{0:...})
    // for that reason, we don't have to run updatePrior() on the first iteration
    if (m_fresh == true)
    {
        this->updatePosterior(yt, obsMat, obsInptAffector, inData, cholObsVar);
        m_fresh = false;
    }else 
    {
        this->updatePrior(stateTrans, cholStateVar, stateInptAffector, inData);
        this->updatePosterior(yt, obsMat, obsInptAffector, inData, cholObsVar);
    }
}
    
 
template<size_t dimstate, size_t dimobs, size_t diminput, typename float_t, bool debug>
auto kalman<dimstate,dimobs,diminput,float_t,debug>::getPredYMean(
        const ssMat &stateTrans,
        const obsStateSizeMat &obsMat, 
        const siMat &stateInptAffector,
        const oiMat &obsInptAffector, 
        const isv &futureInputData) const -> osv
{
    return obsMat * (stateTrans * m_filtMean + stateInptAffector * futureInputData) + obsInptAffector * futureInputData;
}
 
 
template<size_t dimstate, size_t dimobs, size_t diminput, typename float_t, bool debug>
auto kalman<dimstate,dimobs,diminput,float_t,debug>::getPredYVar(
        const ssMat &stateTrans,
        const ssMat &cholStateVar,
        const obsStateSizeMat &obsMat,
        const osMat &cholObsVar) const -> osMat
{
    return obsMat * (stateTrans * m_filtVar * stateTrans.transpose() + cholStateVar.transpose()*cholStateVar) * obsMat.transpose() + cholObsVar.transpose() * cholObsVar;
}
 
 
 
template<size_t dimstate, size_t dimobs, typename float_t, bool debug = false>
class hmm : public bases::cf_filter<dimstate,dimobs,float_t>
{
 
public:
 
    using ssv = Eigen::Matrix<float_t,dimstate,1>;
    
    using osv = Eigen::Matrix<float_t,dimobs,1>;
    
    using ssMat = Eigen::Matrix<float_t,dimstate,dimstate>;
 
 
 
    hmm();
 
 
 
    hmm(const ssv &initStateDistrLogProbs, const ssMat &transMatLogProbs);
    
    
    virtual ~hmm();
    
 
 
    float_t getLogCondLike() const;
    
    
 
    ssv getFilterVecLogProbs() const;
    
        
 
    void update(const ssv &logCondDensVec);
 
 
 
    float_t log_sum_exp(const ssv& logProbVec);
 
 
 
    ssv log_product(const ssMat& logTransMat, const ssv& logProbVec);
 
private:
 
    ssv m_filtVecLogProbs;
    
    ssMat m_transMatLogProbsTranspose;
    
    float_t m_lastLogCondLike; 
    
    bool m_fresh;
 
};
 
 
template<size_t dimstate, size_t dimobs, typename float_t, bool debug>
hmm<dimstate,dimobs,float_t,debug>::hmm() 
    : bases::cf_filter<dimstate,dimobs,float_t>::cf_filter()
    , m_lastLogCondLike(0.0)
    , m_fresh(true)
{
}
    
 
template<size_t dimstate, size_t dimobs, typename float_t, bool debug>
hmm<dimstate,dimobs,float_t,debug>::hmm(const ssv &initStateDistrLogProbs, const ssMat &transMatLogProbs) 
    : bases::cf_filter<dimstate,dimobs,float_t>()
    , m_filtVecLogProbs(initStateDistrLogProbs)
    , m_transMatLogProbsTranspose(transMatLogProbs.transpose())
    , m_lastLogCondLike(0.0)
    , m_fresh(true)
{
    // check initial probabilities
    if( std::abs(this->log_sum_exp(m_filtVecLogProbs)) > .001)
        throw std::invalid_argument("Initial probabilities must sum to 1.");
    if ( m_filtVecLogProbs.maxCoeff() > 0.0 ) 
        throw std::invalid_argument("Initial probabilities cannot be greater than 1.0."); 
 
    // check transition matrix (with log probs)
    if( m_transMatLogProbsTranspose.maxCoeff() > 0.0)
        throw std::invalid_argument("Initial transition probabilities cannot be greater than 1.");
    for(size_t icol = 0; icol < dimstate; ++icol){
        if ( std::abs(this->log_sum_exp(m_transMatLogProbsTranspose.col(icol))) > .001)
            throw std::invalid_argument("Initial transition probabilities must sum to 1.");
    }
}
 
 
template<size_t dimstate, size_t dimobs, typename float_t, bool debug>
hmm<dimstate,dimobs,float_t,debug>::~hmm() {}
 
 
template<size_t dimstate, size_t dimobs, typename float_t, bool debug>
auto hmm<dimstate,dimobs,float_t,debug>::getLogCondLike() const -> float_t
{
    return m_lastLogCondLike;
}
 
 
template<size_t dimstate, size_t dimobs, typename float_t, bool debug>
auto hmm<dimstate,dimobs,float_t,debug>::getFilterVecLogProbs() const -> ssv
{
    return m_filtVecLogProbs;
}
 
    
template<size_t dimstate, size_t dimobs, typename float_t, bool debug>
float_t hmm<dimstate,dimobs,float_t,debug>::log_sum_exp(const ssv& logProbVec)
{
 
    float_t m = logProbVec.maxCoeff();
    return std::log( (logProbVec.array() - m).exp().sum() ) + m;
}
 
 
template<size_t dimstate, size_t dimobs, typename float_t, bool debug>
auto hmm<dimstate,dimobs,float_t,debug>::log_product(const ssMat& logTransMat, const ssv& logProbVec) -> ssv
{
    ssv col_sum;
    float_t m = logTransMat.maxCoeff();
    col_sum.fill(m);
    for(size_t ic = 0; ic < dimstate; ++ic){
        col_sum = col_sum + (logTransMat.col(ic).array() + logProbVec(ic) - m).exp().matrix();
    }
    return col_sum;
}
 
 
template<size_t dimstate, size_t dimobs, typename float_t, bool debug>
void hmm<dimstate,dimobs,float_t,debug>::update(const ssv &logCondDensVec)
{
    if( !m_fresh) { // has seen data before
        m_filtVecLogProbs = this->log_product(m_transMatLogProbsTranspose, m_filtVecLogProbs); // now log p(x_t |y_{1:t-1})
        m_filtVecLogProbs = m_filtVecLogProbs + logCondDensVec ; // now log p(y_t,x_t|y_{1:t-1})
        m_lastLogCondLike = this->log_sum_exp(m_filtVecLogProbs);
 
        #ifndef DROPPINGTHISINRPACKAGE 
        if constexpr(debug) 
            std::cout << "logConDensVec sum " << logCondDensVec.sum() << ", p(y_t,x_t|y_{1:t-1}) sum: " << m_filtVecLogProbs.sum() << ", lastCondlike: " << m_lastLogCondLike << "\n";
        #endif
 
        m_filtVecLogProbs = (m_filtVecLogProbs.array() -  m_lastLogCondLike).matrix(); // now log p(x_t|y_{1:t})
    }
    else // hasn't seen data before and so filtVec is just time 1 state prior
    {
        m_filtVecLogProbs = m_filtVecLogProbs + logCondDensVec; // now it's log p(x_1, y_1)
        m_lastLogCondLike = this->log_sum_exp(m_filtVecLogProbs); // log p(y_1)
 
 
        #ifndef DROPPINGTHISINRPACKAGE
        if constexpr(debug) 
            std::cout << "logConDensVecSum " << logCondDensVec.sum() << ", log p(x1,y1) sum: " << m_filtVecLogProbs.sum() << ", lastLogCondlike: " << m_lastLogCondLike << "\n";
        #endif
 
        m_filtVecLogProbs = (m_filtVecLogProbs.array() - m_lastLogCondLike).matrix();
        m_fresh = false;
    }
}
    
 
 
 
 
 
 
 
 
template<size_t dim_pred, typename float_t>
class gamFilter : public bases::cf_filter<1,1,float_t>
{
 
public:
 
    using psv = Eigen::Matrix<float_t,dim_pred,1>;
    
    using tsv = Eigen::Matrix<float_t,2,1>;
 
 
 
    //gamFilter();
 
 
 
    gamFilter(const float_t &nOneTilde, const float_t &dOneTilde);
    
    
    virtual ~gamFilter();
    
 
 
    float_t getLogCondLike() const;
    
    
 
    tsv getFilterVec() const;
    
        
 
    void update(const float_t& yt, const psv &xt, const psv& beta, const float_t& sigmaSquared, const float_t& delta);
 
 
private:
 
    tsv m_filtVec;
    
    float_t m_lastLogCondLike; 
    
    bool m_fresh;
 
};
 
 
//template<typename dim_pred, typename float_t>
//gamFilter<dim_pred,float_t>::gamFilter() 
//    : cf_filter<1,1,float_t>::cf_filter()
//    , m_filtVec(tsv::Zero())
//    , m_lastCondLike(0.0)
//    , m_fresh(true)
//{
//}
    
 
template<size_t dim_pred, typename float_t>
gamFilter<dim_pred,float_t>::gamFilter(const float_t &nOneTilde, const float_t &dOneTilde)
    : bases::cf_filter<1,1,float_t>()
    , m_lastLogCondLike(0.0)
    , m_fresh(true)
{
    m_filtVec(0) = nOneTilde;
    m_filtVec(1) = dOneTilde;
}
 
 
template<size_t dim_pred, typename float_t>
gamFilter<dim_pred,float_t>::~gamFilter() {}
 
 
template<size_t dim_pred, typename float_t>
auto gamFilter<dim_pred,float_t>::getLogCondLike() const -> float_t
{
    return m_lastLogCondLike;
}
 
 
template<size_t dim_pred, typename float_t>
auto gamFilter<dim_pred,float_t>::getFilterVec() const -> tsv
{
    return m_filtVec;
}
 
 
 
template<size_t dim_pred, typename float_t>
void gamFilter<dim_pred,float_t>::update(const float_t& yt, const psv &xt, const psv& beta, const float_t& sigmaSquared, const float_t& delta)
{
 
    if(sigmaSquared <= 0 || delta <= 0)
        throw std::invalid_argument("ME: both sigma squared and delta have to be positive.\n");
 
    if (m_fresh)  // hasn't seen data before and so filtVec is just time 1 state prior
    {
        float_t tmpScale = std::sqrt(sigmaSquared*m_filtVec(1)/m_filtVec(0));
        m_lastLogCondLike = rveval::evalScaledT<float_t>(yt, xt.dot(beta), tmpScale, m_filtVec(0), true);
        m_filtVec(0) += 1;
        m_filtVec(1) += (yt - xt.dot(beta))*(yt - xt.dot(beta))/sigmaSquared;
        m_fresh = false;
        
    } else { // has seen data before
        
        m_filtVec(0) *= delta;
        m_filtVec(1) *= delta;
        float_t tmpScale = std::sqrt(sigmaSquared*m_filtVec(1)/m_filtVec(0));
        m_lastLogCondLike = rveval::evalScaledT<float_t>(yt, xt.dot(beta), tmpScale, m_filtVec(0), true);
        m_filtVec(0) += 1;
        m_filtVec(1) += (yt - xt.dot(beta))*(yt - xt.dot(beta))/sigmaSquared;
    }
}
 
 
// it's for a multivariate response.
template<size_t dim_obs, size_t dim_pred, typename float_t>
class multivGamFilter : public bases::cf_filter<1,dim_obs,float_t>
{
 
public:
 
    using psv = Eigen::Matrix<float_t,dim_pred,1>;
    
    using bsm = Eigen::Matrix<float_t,dim_obs, dim_pred>;
 
    using tsv = Eigen::Matrix<float_t,2,1>;
 
    using osv = Eigen::Matrix<float_t, dim_obs, 1>;
 
    using osm = Eigen::Matrix<float_t, dim_obs, dim_obs>;
 
 
 
    multivGamFilter(const float_t &nOneTilde, const float_t &dOneTilde);
    
    
    virtual ~multivGamFilter();
    
 
 
    float_t getLogCondLike() const;
    
    
 
    tsv getFilterVec() const;
    
        
 
    void update(const osv& yt, const psv &xt, const bsm& B, const osm& Sigma, const float_t& delta);
 
 
 
    osv getFcastMean(const psv &xtp1, const bsm& B, const osm& Sigma, const float_t& delta);
 
 
 
    osm getFcastCov(const psv &xtp1, const bsm& B, const osm& Sigma, const float_t& delta);
 
 
private:
 
    tsv m_filtVec;
    
    float_t m_lastLogCondLike; 
    
    bool m_fresh;
 
};
 
 
template<size_t dim_obs, size_t dim_pred, typename float_t>
multivGamFilter<dim_obs,dim_pred,float_t>::multivGamFilter(const float_t &nOneTilde, const float_t &dOneTilde)
    : bases::cf_filter<1,dim_obs,float_t>()
    , m_lastLogCondLike(0.0)
    , m_fresh(true)
{
    m_filtVec(0) = nOneTilde;
    m_filtVec(1) = dOneTilde;
}
 
 
template<size_t dim_obs, size_t dim_pred, typename float_t>
multivGamFilter<dim_obs,dim_pred,float_t>::~multivGamFilter() {}
 
 
template<size_t dim_obs, size_t dim_pred, typename float_t>
auto multivGamFilter<dim_obs,dim_pred,float_t>::getLogCondLike() const -> float_t
{
    return m_lastLogCondLike;
}
 
 
template<size_t dim_obs, size_t dim_pred, typename float_t>
auto multivGamFilter<dim_obs,dim_pred,float_t>::getFilterVec() const -> tsv
{
    return m_filtVec;
}
 
 
 
template<size_t dim_obs, size_t dim_pred, typename float_t>
void multivGamFilter<dim_obs,dim_pred,float_t>::update(const osv& yt, const psv &xt, const bsm& B, const osm& Sigma, const float_t& delta)
{
 
    // TODO: doesn't check that Sigma is positive definite or symmetric!
    if(delta <= 0)
        throw std::invalid_argument("ME: delta has to be positive (you're not even checking Sigma).\n");
 
    if (m_fresh)  // hasn't seen data before and so filtVec is just time 1 state prior
    {
        osm scaleMat = Sigma * m_filtVec(1)/m_filtVec(0);
        osv modeVec = B*xt;
        m_lastLogCondLike = rveval::evalMultivT<dim_obs,float_t>(yt, modeVec, scaleMat, m_filtVec(0), true);
        m_filtVec(0) += 1; 
        m_filtVec(1) += (Sigma.ldlt().solve(yt - modeVec)).squaredNorm();
        m_fresh = false;
        
    } else { // has seen data before
        
        m_filtVec(0) *= delta;
        m_filtVec(1) *= delta;
        
        osm scaleMat = Sigma * m_filtVec(1)/m_filtVec(0);
        osv modeVec = B*xt;
        m_lastLogCondLike = rveval::evalMultivT<dim_obs,float_t>(yt, modeVec, scaleMat, m_filtVec(0), true);
        
        m_filtVec(0) += 1;
        m_filtVec(1) += (Sigma.ldlt().solve(yt - modeVec)).squaredNorm();
    }
}
 
 
template<size_t dim_obs, size_t dim_pred, typename float_t>
auto multivGamFilter<dim_obs,dim_pred,float_t>::getFcastMean(const psv &xtp1, const bsm& B, const osm& Sigma, const float_t& delta) -> osv
{
    if(delta*m_filtVec(0) > 1.0)
        return B*xtp1;
}
 
 
template<size_t dim_obs, size_t dim_pred, typename float_t>
auto multivGamFilter<dim_obs,dim_pred,float_t>::getFcastCov(const psv &xtp1, const bsm& B, const osm& Sigma, const float_t& delta) -> osm
{
    if(delta * m_filtVec(0) > 2.0)
        return Sigma * delta * m_filtVec(1) / (delta * m_filtVec(0) - 2.0);
}
 
} // namespace filters
 
} //namespace pf
 
#endif //CF_FILTERS_H