root/bdm/stat/libEF.cpp @ 330

#include <itpp/base/bessel.h>
#include "libEF.h"
#include <math.h>

namespace bdm{

Uniform_RNG UniRNG;
Normal_RNG NorRNG;
Gamma_RNG GamRNG;

using std::cout;

void BMEF::bayes ( const vec &dt ) {this->bayes ( dt,1.0 );};

vec egiw::sample() const {
        it_warning ( "Function not implemented" );
        return vec_1 ( 0.0 );
}

double egiw::evallog_nn ( const vec &val ) const {
        int vend = val.length()-1;

        if ( dimx==1 ) { //same as the following, just quicker.
                double r = val ( vend ); //last entry!
                vec Psi ( nPsi+dimx );
                Psi ( 0 ) = -1.0;
                Psi.set_subvector ( 1,val ( 0,vend-1 ) ); // fill the rest

                double Vq=V.qform ( Psi );
                return -0.5* ( nu*log ( r ) + Vq /r );
        }
        else {
                mat Th= reshape ( val ( 0,nPsi*dimx-1 ),nPsi,dimx );
                fsqmat R ( reshape ( val ( nPsi*dimx,vend ),dimx,dimx ) );
                mat Tmp=concat_vertical ( -eye ( dimx ),Th );
                fsqmat iR ( dimx );
                R.inv ( iR );

                return -0.5* ( nu*R.logdet() + trace ( iR.to_mat() *Tmp.T() *V.to_mat() *Tmp ) );
        }
}

double egiw::lognc() const {
        const vec& D = V._D();

        double m = nu - nPsi -dimx-1;
#define log2  0.693147180559945286226763983
#define logpi 1.144729885849400163877476189
#define log2pi 1.83787706640935
#define Inf std::numeric_limits<double>::infinity()

        double nkG = 0.5* dimx* ( -nPsi *log2pi + sum ( log ( D ( dimx,D.length()-1 ) ) ) );
        // temporary for lgamma in Wishart
        double lg=0;
        for ( int i =0; i<dimx;i++ ) {lg+=lgamma ( 0.5* ( m-i ) );}

        double nkW = 0.5* ( m*sum ( log ( D ( 0,dimx-1 ) ) ) ) \
                     - 0.5*dimx* ( m*log2 + 0.5* ( dimx-1 ) *log2pi )  - lg;

        it_assert_debug ( ( ( -nkG-nkW ) >-Inf ) && ( ( -nkG-nkW ) <Inf ), "ARX improper" );
        return -nkG-nkW;
}

vec egiw::est_theta() const {
        if ( dimx==1 ) {
                const mat &L = V._L();
                int end = L.rows() - 1;

                mat iLsub = ltuinv ( L ( dimx,end,dimx,end ) );

                vec L0 = L.get_col(0);

                return iLsub * L0(1,end);
        }
        else {
                it_error("ERROR: est_theta() not implemented for dimx>1");
                return 0;
        }
}

ldmat egiw::est_theta_cov() const {
        if ( dimx == 1 ) {
                const mat &L = V._L();
                const vec &D = V._D();
                int end = D.length() - 1;

                mat Lsub = L ( 1, end, 1, end);
                mat Dsub = diag(D(1, end));

                return inv( transpose(Lsub) * Dsub * Lsub );

        }
        else {
                it_error("ERROR: est_theta_cov() not implemented for dimx>1");
                return 0;
        }

}

vec egiw::mean() const {

        if ( dimx==1 ) {
                const vec &D= V._D();
                int end = D.length()-1;

                vec m ( dim );
                m.set_subvector ( 0, est_theta() );
                m ( end ) = D ( 0 ) / ( nu -nPsi -2*dimx -2 );
                return m;
        }
        else {
                mat M;
                mat R;
                mean_mat ( M,R );
                return cvectorize ( concat_vertical ( M,R ) );
        }

}

vec egiw::variance() const {

        if ( dimx==1 ) {
                int l=V.rows();
                const ldmat tmp(V,linspace(1,l-1));
                ldmat itmp(l);
                tmp.inv(itmp);
                double cove = V._D() ( 0 ) / ( nu -nPsi -2*dimx -2 );
                
                vec var(l);
                var.set_subvector(0,diag(itmp.to_mat())*cove);
                var(l-1)=cove*cove/( nu -nPsi -2*dimx -2 );
                return var;
        }
        else {it_error("not implemented"); return vec(0);}
}

void egiw::mean_mat ( mat &M, mat&R ) const {
        const mat &L= V._L();
        const vec &D= V._D();
        int end = L.rows()-1;

        ldmat ldR ( L ( 0,dimx-1,0,dimx-1 ), D ( 0,dimx-1 ) / ( nu -nPsi -2*dimx -2 ) ); //exp val of R
        mat iLsub=ltuinv ( L ( dimx,end,dimx,end ) );

        // set mean value
        mat Lpsi = L ( dimx,end,0,dimx-1 );
        M= iLsub*Lpsi;
        R= ldR.to_mat()  ;
}

vec egamma::sample() const {
        vec smp ( dim);
        int i;

        for ( i=0; i<dim; i++ ) {
                if ( beta ( i ) >std::numeric_limits<double>::epsilon() ) {
                        GamRNG.setup ( alpha ( i ),beta ( i ) );
                }
                else {
                        GamRNG.setup ( alpha ( i ),std::numeric_limits<double>::epsilon() );
                }
#pragma omp critical
                smp ( i ) = GamRNG();
        }

        return smp;
}

// mat egamma::sample ( int N ) const {
//      mat Smp ( rv.count(),N );
//      int i,j;
//
//      for ( i=0; i<rv.count(); i++ ) {
//              GamRNG.setup ( alpha ( i ),beta ( i ) );
//
//              for ( j=0; j<N; j++ ) {
//                      Smp ( i,j ) = GamRNG();
//              }
//      }
//
//      return Smp;
// }

double egamma::evallog ( const vec &val ) const {
        double res = 0.0; //the rest will be added
        int i;

        for ( i=0; i<dim; i++ ) {
                res += ( alpha ( i ) - 1 ) *std::log ( val ( i ) ) - beta ( i ) *val ( i );
        }
        double tmp=res-lognc();;
        it_assert_debug ( std::isfinite ( tmp ),"Infinite value" );
        return tmp;
}

double egamma::lognc() const {
        double res = 0.0; //will be added
        int i;

        for ( i=0; i<dim; i++ ) {
                res += lgamma ( alpha ( i ) ) - alpha ( i ) *std::log ( beta ( i ) ) ;
        }

        return res;
}

//MGamma
void mgamma::set_parameters ( double k0, const vec &beta0 ) {
        k=k0;
        ep = &epdf;
        epdf.set_parameters ( k*ones ( beta0.length()),beta0 );
        dimc = ep->dimension();
};

ivec eEmp::resample (RESAMPLING_METHOD method) {
        ivec ind=zeros_i ( n );
        ivec N_babies = zeros_i ( n );
        vec cumDist = cumsum ( w );
        vec u ( n );
        int i,j,parent;
        double u0;

        switch ( method ) {
                case MULTINOMIAL:
                        u ( n - 1 ) = pow ( UniRNG.sample(), 1.0 / n );

                        for ( i = n - 2;i >= 0;i-- ) {
                                u ( i ) = u ( i + 1 ) * pow ( UniRNG.sample(), 1.0 / ( i + 1 ) );
                        }

                        break;

                case STRATIFIED:

                        for ( i = 0;i < n;i++ ) {
                                u ( i ) = ( i + UniRNG.sample() ) / n;
                        }

                        break;

                case SYSTEMATIC:
                        u0 = UniRNG.sample();

                        for ( i = 0;i < n;i++ ) {
                                u ( i ) = ( i + u0 ) / n;
                        }

                        break;

                default:
                        it_error ( "PF::resample(): Unknown resampling method" );
        }

        // U is now full
        j = 0;

        for ( i = 0;i < n;i++ ) {
                while ( u ( i ) > cumDist ( j ) ) j++;

                N_babies ( j ) ++;
        }
        // We have assigned new babies for each Particle
        // Now, we fill the resulting index such that:
        // * particles with at least one baby should not move *
        // This assures that reassignment can be done inplace;

        // find the first parent;
        parent=0; while ( N_babies ( parent ) ==0 ) parent++;

        // Build index
        for ( i = 0;i < n;i++ ) {
                if ( N_babies ( i ) > 0 ) {
                        ind ( i ) = i;
                        N_babies ( i ) --; //this index was now replicated;
                }
                else {
                        // test if the parent has been fully replicated
                        // if yes, find the next one
                        while ( ( N_babies ( parent ) ==0 ) || ( N_babies ( parent ) ==1 && parent>i ) ) parent++;

                        // Replicate parent
                        ind ( i ) = parent;

                        N_babies ( parent ) --; //this index was now replicated;
                }

        }

        // copy the internals according to ind
        for ( i=0;i<n;i++ ) {
                if ( ind ( i ) !=i ) {
                        samples ( i ) =samples ( ind ( i ) );
                }
                w ( i ) = 1.0/n;
        }

        return ind;
}

void eEmp::set_statistics ( const vec &w0, const epdf* epdf0 ) {
        //it_assert_debug(rv==epdf0->rv(),"Wrong epdf0");
        dim = epdf0->dimension();
        w=w0;
        w/=sum ( w0 );//renormalize
        n=w.length();
        samples.set_size ( n );
        dim = epdf0->dimension();

        for ( int i=0;i<n;i++ ) {samples ( i ) =epdf0->sample();}
}

void eEmp::set_samples ( const epdf* epdf0 ) {
        //it_assert_debug(rv==epdf0->rv(),"Wrong epdf0");
        w=1;
        w/=sum ( w );//renormalize

        for ( int i=0;i<n;i++ ) {samples ( i ) =epdf0->sample();}
}

};