sv_singleprop.c

是例子滤波方面的一个很好的算法模块

#include <math.h>
#include "rand.c"

/*strat filter for SV model.

n observations - z
M particles
parameters - beta, phi, ss and sp (sd for system and prior)

df= df of t proposal

output:
LE - log evidence - lose factor of (2pi)^-1/2 in likelihood
est - estimates (post mean)
pr_est[t] - prob < val[t]
*/
double qgen(double *qt,long Nq, double U);
void SV_ASIR(double *z,long *n,long *M,double *beta, double *phi, double *ss, double *sp,double *df, long *SMOOTH,double *val,double *LE, double *est, double *ests, double *pr_est)
{
  double *particles,*particles_new,*weight,*weight_new,*w,*mu,*sd,*w_temp;
  long i,j,t;
  double sig,a,b,c,sum,x,m,U,gam;
  double **w_st,**part_st;
  double *qt;
  double plast,xlast,pnew,xnew,qinv;
  long Nq;
  double Me,V;
  

#define S2PI 2.506628275

   FILE *seed_file;
 
  /*obtain seed*/
  seed_file = fopen(".seed","r");
  if (seed_file==NULL){
    (void) fprintf(stderr,"Can not open .seed\n Using default values.");
    *seed=1349761;
    *(seed+1)=2498265;
  }else  {
    seed_set(seed_file);
    (void)fclose(seed_file);
  }
     
  part_st=(double **)calloc(*n,sizeof(double *));
  w_st=(double **)calloc(*n,sizeof(double *));
  for(i=0;i< *n;i++){
    part_st[i]=(double *)calloc(*M,sizeof(double));
    w_st[i]=(double *)calloc(*M,sizeof(double));
  }
  particles=(double *)calloc(*M,sizeof(double));
  weight_new=(double *)calloc(*M,sizeof(double));
  particles_new=(double *)calloc(*M,sizeof(double));
  weight=(double *)calloc(*M,sizeof(double));
  w=(double *)calloc(*M,sizeof(double));
  w_temp=(double *)calloc(*M,sizeof(double));
  mu=(double *)calloc(*M,sizeof(double));
  sd=(double *)calloc(*M,sizeof(double));

  /*look-up table*/
  Nq=5* *M;
  if(Nq<1000) Nq=1000;
  qt=(double *)calloc(Nq,sizeof(double));

  /*calculate look-up table*/
  i=Nq-1;
  plast=0.5;
  xlast=0.0;
  pnew=0.5;
  xnew=0.0;
  while(i>=0){
    if(pnew< (i+0.5)/(2.0*Nq)){
      if(pnew==plast)
	qt[i]=xnew;
      else{
	qt[i]=xnew+ ((i+0.5)/(2.0*Nq)-pnew)*(xlast-xnew)/(plast-pnew);
      }
      i--;
    }else{
      plast=pnew;
      xlast=xnew;
      xnew=xlast-0.001;
      pnew=pt((double) *df,xnew);
    }
  }

  sig= *sp; /*sd is sd for prior*/
  
  gam= z[0]*z[0]/(*beta* *beta);
  /*coefficents in approx to p(z,x)*/
  a=1.0/(sig*sig)+0.5*gam; /*quadratic*/
  b=0.5-0.5*gam; /*0.5*linear*/
  c=gam; /*constant*/

  mu[0]=-b/a;
  sd[0]=exp(-0.5*log(a));

  U=runif()/(double) *M;
  /*simulate particles*/
  for(i=0;i< *M;i++){
    /* U=(runif()+(double)i)/(double)*M;*/
    particles[i]=qgen(qt,Nq,U)*sd[0]+mu[0];
    U+=1/(double)*M;
  }
  sum=0.0;
  /*weight particles*/
  for(i=0;i< *M;i++){
    x=particles[i];
    weight[i]=exp( -0.5*(x + x*x/(sig*sig) +  z[0]*z[0]*exp(-x)/(*beta* *beta)  ) )/(*beta *sig * S2PI) ;
    weight[i]/=dt((double) *df,(particles[i]-mu[0])/sd[0])/sd[0];
    sum+=weight[i];
  }

  *LE= log(sum/(double)*M);
  for(i=0;i< *M;i++)
    weight[i]/=sum;

  est[0]=0.0;
  pr_est[0]=0.0;

 sig= *ss;
  for(i=0;i< *M;i++){
    est[0]+=weight[i]*particles[i];
    if(particles[i]>val[0])
      pr_est[0]+=weight[i];
    w_st[0][i]=weight[i];
    part_st[0][i]=particles[i];

  }
  for(t=1;t< *n;t++){
    /*asir approx*/
    sum=0.0;
    for(i=0;i< *M;i++){
      m= *phi *particles[i];
      gam= z[t]*z[t]*exp(-m)/(*beta* *beta);
      /*coefficents in approx to p(z,x)*/
      a=1.0/(sig*sig)+0.5*gam; /*quadratic*/
      b=0.5-0.5*gam*(1+m)-m/(sig*sig); /*0.5*linear*/
      c=m*m/(sig*sig)+gam*(1.0+m+0.5*m*m); /*constant*/
      
      mu[i]=-b/a;
      sd[i]=exp(-0.5*log(a));
      
      w[i]=weight[i]*sd[i]*exp(-0.5*(c-b*b/a));
      sum+=w[i];
    }
    for(i=0;i< *M;i++)
      w[i]/=sum;

    /*calculate approx*/
    Me=0.0;
    V=0.0;
    for(i=0;i< *M;i++)
      Me+=w[i]*mu[i];
    for(i=0;i< *M;i++){
      V+= w[i]*(sd[i]*sd[i]+(mu[i]-Me)*(mu[i]-Me));
    }
    V=exp(0.5*log(V));
    /*fprintf(stdout,"Approx Mean =%lf SD = %lf\n",Me,V);
    /*sample *M new particles and weight*/
    U=runif()/(double)*M;
    for(i=0;i< *M;i++){
      /* U=(runif()+(double)i)/(double)*M;*/
      particles_new[i]= qgen(qt,Nq,U);
      particles_new[i]=particles_new[i]*V+Me;
      U+=1/(double)*M;
    }
    for(i=0;i<*M;i++){
      weight_new[i]=0.0;
      qinv= V/dt((double) *df,(particles_new[i]-Me)/V);
      for(j=0;j<*M;j++){
	x=particles_new[i];
	m= *phi*particles[j];	

	weight_new[i]+= weight[j]*exp( -0.5*(x + (x-m)*(x-m)/(sig*sig) +  z[t]*z[t]*exp(-x)/(*beta* *beta)  ) )/(*beta *sig * S2PI) ;;
      }
      weight_new[i]*=qinv;
    }

    sum=0.0;
    for(i=0;i<*M;i++)
      sum+=weight_new[i];
    
    
    for(i=0;i< *M;i++){
      weight[i]=weight_new[i]/sum;
      particles[i]=particles_new[i];
    }
    *LE+=log(sum/(double)*M);
    est[t]=0.0;
    pr_est[t]=0.0;
    
    for(i=0;i< *M;i++){
      est[t]+=weight[i]*particles[i];
      if(particles[i]>val[t])
	pr_est[t]+=weight[i];
      w_st[t][i]=weight[i];
      part_st[t][i]=particles[i];
    }
  

  }
  if(*SMOOTH){
  /*smoothing - marginal*/
    ests[*n-1]=est[*n-1];
    for(t= *n-2;t>=0;t--){
      for(i=0;i< *M;i++)
	weight_new[i]=0.0;

      for(j=0;j<*M;j++){
	sum=0.0;
	for(i=0;i< *M;i++){
	  w_temp[i]=w_st[t][i]*dnorm( (part_st[t+1][j]-*phi*part_st[t][i])/ sig)/sig;
	  sum+=w_temp[i];
	}
	for(i=0;i< *M;i++)
	  weight_new[i]+= w_temp[i]*weight[j]/sum;
      }
      ests[t]=0.0;
      for(i=0;i< *M;i++){
	ests[t]+= weight_new[i] * part_st[t][i];
	weight[i]=weight_new[i];
      }
    }

  }  
   
  /*change .seed file*/
  seed_file=fopen(".seed","w");
  seed_put(seed_file);
  (void)fclose(seed_file);
  

  free(sd);
  free(particles);
  free(weight_new);
  free(particles_new);
  free(mu);
  free(weight);
  free(w);
  free(w_temp);


}
/*
calculate the Uth quantile given

qt[i]=x then Pr(t<x)= (1+0.5)/(2*Nq)
*/
double qgen(double *qt,long Nq, double U)
{
  double p;
  long i;
  if(U<=0.5){
    if(U> 0.5- 0.25/(double)Nq){
      return( qt[Nq-1]*(0.5-U)*4*Nq );
    }
    if(U< 0.25/(double)(Nq)){
      p=0.25/(double)(Nq);
      return( qt[0]+(U-p)*(qt[1]-qt[0])/(2.0*p) );
    }
    i=(int)(2*Nq*U+0.5);
    p= (i-0.5)/(2.0*Nq);
    return( qt[i-1]+(U-p)*(qt[i]-qt[i-1])*2.0*Nq);

  }else{
    U=1-U;
    if(U> 0.5- 0.25/(double)Nq){
      return( -qt[Nq-1]*(0.5-U)*4*Nq );
    }
    if(U< 0.25/(double)(Nq)){
      p=0.25/(double)(Nq);
      return( -qt[0]-(U-p)*(qt[1]-qt[0])/(2.0*p) );
    }
    i=(int)(2*Nq*U+0.5);
    p= (i-0.5)/(2.0*Nq);
    return( -qt[i-1]-(U-p)*(qt[i]-qt[i-1])*2.0*Nq);
 

  }

}