nonlin_singleprop2.c

用MATLAC做的非线性时间序列预测和平滑,可以用来做非线性模型的预测和估计

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

/*strat filter for Nonlin model.

bimodal proposal

n observations - z
M particles
parameters - sn, ss and sp (sd for obserations, 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 NL_singleprop2(double *z,long *n,long *M,double *sn, double *ss, double *sp,double *df, long *SMOOTH,double *val,double *LE, double *est, double *ests, double *pr_est,long *OUTPUT)
{
   FILE *out; 
  double *particles,*particles_new,*weight,*weight_new,*w,*mu,*sd,*w_temp;
  long i,j,jj,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,ww;
  

#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*/
  
  U=runif()/(double) *M;
  /*simulate particles*/
  for(i=0;i< *M;i++){
    /* U=(runif()+(double)i)/(double)*M;*/
    particles[i]=qgen(qt,Nq,U)*sig;
    U+=1/(double)*M;
  }
  sum=0.0;
  /*weight particles*/
  for(i=0;i< *M;i++){
    x=particles[i];
    mu[0]=x*x/20.0;
    weight[i]=  exp( -0.5*(z[0]-mu[0])*(z[0]-mu[0])/(*sn* *sn)) / *sn; /*likelihood*/
    weight[i]*= exp( -0.5*(x*x)/(sig*sig))/(sig*S2PI);/*prior*/
      weight[i]/=dt((double) *df,(particles[i])/sig)/sig; /*prop*/
    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++){
    
    /*calculate approx - purely using likelihoos*/
    if(z[t]>0){
      Me=20.0*z[t]*exp(-0.5*log(20.0*z[t])); /*mean*/
       V=20.0* *sn*exp(-0.5*log(20.0*z[t]+20.0 * *sn));  /*sd*/
       /* V=2.23* *sn *exp(-0.5*log(z[t]+ *sn)); /*sqrt(5)*sig/(y+1)^1/2*/
    }else{
      Me=0.01;
      /*V=2.23* *sn *exp(-0.5*log(*sn));/**/
      V=20.0* *sn*exp(-0.5*log(20.0 * *sn)); //used in paper
     } 

/*fprintf(stdout,"Approx Mean =%lf SD = %lf\n",Me,V);
    /*sample *M new particles and weight*/
    U=runif()/(double)*M;
    a=pt((double)*df,Me/V);
    /*fprintf(stdout,"%lf \n",a);*/
    /*a=1.0;*/
    jj=-1;
     for(i=0;i< *M;i++){
       U=(runif()+(double)i)/(double)*M;
      if(U<0.5){
	particles_new[i]= qgen(qt,Nq,2.0*a*U);
	particles_new[i]=particles_new[i]*V-Me;
      }else{
	if(jj==-1) jj=i;
	particles_new[i]= qgen(qt,Nq,2.0*a*(U-0.5)+1.0-a);
	particles_new[i]=particles_new[i]*V+Me;
      } 	
      /*U+=1/(double)*M;*/
     }
     c=a;
     b=8.0*cos(1.2*t);
    for(i=0;i<*M;i++){
      weight_new[i]=0.0;
      if(i<jj)
	qinv=2*c*V/(dt((double) *df,(particles_new[i]+Me)/V));
      else
	qinv=2*c*V/(dt((double) *df,(particles_new[i]-Me)/V));
      x=particles_new[i];
      for(j=0;j<*M;j++){
	a=particles[j];
	m=0.5*a+25.0*a/(1.0+a*a)+b;
	weight_new[i]+= weight[j]*exp( -0.5*(x-m)*(x-m)/(sig*sig) )/ (S2PI*sig ) ; /*system*/
      }
      weight_new[i]*=qinv; /*divide by proposal*/
      x=particles_new[i]*particles_new[i]/20.0; /*expected observation*/
      weight_new[i]*=exp(-0.5*(z[t]-x)*(z[t]-x)/(*sn * *sn))/(*sn); /*likelihood*/
    }

    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(*OUTPUT){
    out=fopen("NLsp_PARTICLES.txt","w");
    for(i=0;i< *n;i++){
      for(j=0;j< *M;j++)
	fprintf(out,"%4.3le ",part_st[i][j]);
      fprintf(out,"\n");
    }
    fclose(out);
    out=fopen("NLsp_WEIGHTS.txt","w");
    for(i=0;i< *n;i++){
      for(j=0;j< *M;j++)
	fprintf(out,"%4.3le ",w_st[i][j]);
      fprintf(out,"\n");
    }
    fclose(out);
  }


  if(*SMOOTH){
  /*smoothing - marginal*/
    ests[*n-1]=est[*n-1];
    for(t= *n-2;t>=0;t--){
      b=8.0*cos(1.2*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++){
	  x=part_st[t][i];
	  m=0.5*x+25*x/(1.0+x*x)+b;
	  
	  w_temp[i]=w_st[t][i]*dnorm(  (part_st[t+1][j]-m)/sig)/sig;
	  sum+=w_temp[i];
	}
	if(sum>0){
	  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);
for(i=0;i<*n;i++){
    free(w_st[i]);
    free(part_st[i]);
  }
  free(w_st);
  free(part_st);



}
/*
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);
 

  }

}