pfilter:

#--LIBRARY FOR ALL PARTICLE FILTERS--

#OUTPUT(Not all filters have all these outputs)
#return(list(x.out=x,y.out=y, x.pf.out=x.pf, m.out=m,MSE.k.out=MSE.k,
#            conf.out=conf,N.eff.out=N.eff,w.out=w,res.point.out=res.point,w.mat.out=w.mat,var.s.out=var.s))
#x.out- vector of true states
#y.out- vector measurements
#m.out- vector filtered means
# MSE.k.out- vector of (x[t]-m[t])^2
#conf.out - matrix of unweighted 95% credible interval
#w.out- vector of  weight at time t(final time)
#res.point- logical vector of resampling points
#w.mat.out- matrix of weights for each time
#var.s.out- vector of weighted sample variance


#--Load libraries--
source("generate_mod.R")
source("resample_mod.R")
source("diagnostic_mod.R")


#--Generic particle filter Random Walk(prior proposal)--
# set N.thr=1, if want to resample every time
# resampling is relatively slow when it is hard-coded(may resort to base resample() function)
# this function is for 1D random walk example
# N- No of particles
# x- vector of states
# y- vector of measurements
# x0- starting value
# sigma- standard deviation of process
# sigma.meas- standard deviation of measurment
# resample.type - type of resampling (multinomial,stratified,systematic,residual,standard)
particle.filter<-function(N,x,y,x0,sigma=1,sigma.meas=1,resample.type="multinomial",N.thr.per=0.5){

  #threshold N
  N.thr<-as.integer(N.thr.per*N)

  #set up filter
  x0.pf<-rnorm(N,mean=x0,sd=sigma) #suppose this is the correct sigma
  x.pf <- matrix(NA,ncol=N,nrow=tau)
  w.mat<-matrix(NA,ncol=N,nrow=tau)

  #set up diagnostics
  #- adds variables for diagnoistics
  diagnostic.init(tau)

  #local function to choose resampling method
  FUN.resample<-function(N,x,w){
    switch(resample.type,
           multinomial = mult.resample(N=N,x=x,w=w) ,
           stratified = strat.resample(N=N,x=x,w=w) ,
           systematic = sys.resample(N=N,x=x,w=w),
           residual= res.resample(N=N,x=x,w=w),
           standard=sample(x=x,size=N,replace=TRUE,prob=w)
           )
  }

  # Initialize
  x.pf[1, ] <- x0.pf+rnorm(N,sd=sigma) #project particles from x0 to the first time step
  w.tilde<- dnorm(y[1],mean=x.pf[1,],sd=sigma.meas) #weights of x0 is equal
  w<-w.tilde/sum(w.tilde)
  w.mat[1,]<-w.tilde/sum(w.tilde)



  #compute diagnostics
  #- mean, effective sample size, confidence intervals
  m[1]<-sum(w*x.pf[1,])
  MSE.k[1]<-(x[1]-m[1])^2
  N.eff[1]<-1/sum(w^2)
  var.s[1]<-sum(w*(x.pf[1,]-m[1])^2)
  conf[1,]<-quantile(x.pf[1,],probs=c(0.025,0.975))


  for (t in 2:(tau)) {


    #project  state forward
    x.pf[t, ] <-x.pf[t-1,]+rnorm(N,sd=sigma)

    #compute weights
    w.tilde <- dnorm(y[t], mean=x.pf[t, ],sd=sigma.meas)*w
    w <- w.tilde/sum(w.tilde)
    w.mat[t,]<-w.tilde/sum(w.tilde)


    #compute diagnostics
    N.eff[t]<-1/sum(w^2)
    m[t]<-sum(w*x.pf[t,])
    MSE.k[t]<-(x[t]-m[t])^2
    var.s[t]<-sum(w*(x.pf[t,]-m[t])^2)
    conf[t,]<-quantile(x.pf[t,],probs=c(0.025,0.975))


    #resample (bootstrap means resample everytime)
    if(N.eff[t]<N.thr){
      x.pf[t,]<-FUN.resample(N=N,x=x.pf[t,],w=w)
      w<-rep(1/N,N)
      res.point[t]<-1 #add "yes" for resampling point
    }



  }

  #return output
  return(list(x.out=x,y.out=y, x.pf.out=x.pf, m.out=m,MSE.k.out=MSE.k,
               conf.out=conf,N.eff.out=N.eff,w.out=w,res.point.out=res.point,w.mat.out=w.mat,var.s.out=var.s))

}



#--Generic particle filter Random Walk(prior proposal) Smoothed--
# Resample paths, not particles
# N- No of particles
# x- vector of states
# y- vector of measurements
# x0- starting value
# sigma- standard deviation of process
# sigma.meas- standard deviation of measurment
# resample.type - type of resampling (multinomial,stratified,systematic,residual,standard)
particle.filter.path<-function(N,x,y,x0,sigma=1,sigma.meas=1,resample.type="multinomial",N.thr.per=0.5){

  #compute tau
  tau<-length(x)

  #threshold N
  N.thr<-as.integer(N.thr.per*N)

  #set up filter
  x0.pf<-rnorm(N,mean=x0,sd=sigma)
  x.pf <- matrix(NA,ncol=N,nrow=tau)

  #set up diagnostics
  diagnostic.init(tau)

  #local function to choose resampling method
  FUN.resample<-function(N,x,w){
    switch(resample.type,
           multinomial = mult.resample(N=N,x=x,w=w) ,
           stratified = strat.resample(N=N,x=x,w=w) ,
           systematic = sys.resample(N=N,x=x,w=w),
           residual= res.resample(N=N,x=x,w=w),
           standard=sample(x=x,size=N,replace=TRUE,prob=w)
    )
  }

  # Initialize
  x.pf[1, ] <- x0.pf+rnorm(N,sd=sigma) #project particles from x0 to the first time step
  w.tilde<- dnorm(y[1],mean=x.pf[1,],sd=sigma.meas) #weights of x0 is equal
  w<-w.tilde/sum(w.tilde)

  #compute diagnostics
  #- mean, effective sample size, confidence intervals
  m[1]<-sum(w*x.pf[1,])
  MSE.k[1]<-(x[1]-m[1])^2
  N.eff[1]<-1/sum(w^2)
  conf[1,]<-quantile(x.pf[1,],probs=c(0.025,0.975))


  for (t in 2:(tau)) {


    #project  state forward
    x.pf[t, ] <-x.pf[t-1,]+rnorm(N,sd=sigma)

    #compute weights
    w.tilde <- dnorm(y[t], mean=x.pf[t, ],sd=sigma.meas)*w
    w <- w.tilde/sum(w.tilde)

    #compute diagnostics
    N.eff[t]<-1/sum(w^2)
    m[t]<-sum(w*x.pf[t,])
    MSE.k[t]<-(x[t]-m[t])^2
    conf[t,]<-quantile(x.pf[t,],probs=c(0.025,0.975))

    #resample (bootstrap means resample everytime)
    if(N.eff[t]<N.thr){
      s<-FUN.resample(N=N,x=1:N,w=w)
      x.pf<-x.pf[,s]
      w<-rep(1/N,N)
      res.point[t]<-1 #add "yes" for resampling point
    }


  }

  #return output
  return(list(x.out=x,y.out=y, x.pf.out=x.pf, m.out=m,MSE.k.out=MSE.k,
              conf.out=conf,N.eff.out=N.eff,w.out=w,res.point.out=res.point))

}


#--Generic particle filter Random Walk(fixed proposal)--
# fixed filter chooses normal centred at x0
# N- No of particles
# x- vector of states
# y- vector of measurements
# x0- starting value
# sigma- standard deviation of process
# sigma.meas- standard deviation of measurment
# resample.type - type of resampling (multinomial,stratified,systematic,residual,standard)

fixed.filter<-function(N,x,y,x0,sigma=1,sigma.meas=1,resample.type="multinomial",N.thr.per=0.5){

  #threshold N
  N.thr<-as.integer(N.thr.per*N)

  #set up filter
  x0.pf<-rnorm(N,mean=x0,sd=sigma)
  x.pf <- matrix(NA,ncol=N,nrow=tau)

  #set up diagnostics
  #-adds variables for diagnoistics
  diagnostic.init(tau)

  #local function to choose resampling method
  FUN.resample<-function(N,x,w){
    switch(resample.type,
           multinomial = mult.resample(N=N,x=x,w=w) ,
           stratified = strat.resample(N=N,x=x,w=w) ,
           systematic = sys.resample(N=N,x=x,w=w),
           residual= res.resample(N=N,x=x,w=w),
           standard=sample(x=x,size=N,replace=TRUE,prob=w)
    )
  }

  # Initialize
  x.pf[1, ] <-rnorm(N,x0) #project particles from x0 to the first time step
  w.tilde<- dnorm(y[1],mean=x.pf[1,],sd=sigma.meas)*dnorm(x.pf[1,],x0.pf,sd=sigma)/dnorm(x.pf[1,],x0.pf) #weights of x0 is equal
  w<-w.tilde/sum(w.tilde)



  #compute diagnostics
  #- mean, effective sample size, confidence intervals
  m[1]<-sum(w*x.pf[1,])
  MSE.k[1]<-(x[1]-m[1])^2
  N.eff[1]<-1/sum(w^2)
  conf[1,]<-quantile(x.pf[1,],probs=c(0.025,0.975))


  for (t in 2:(tau)) {

    #project  state forward
    x.pf[t, ] <-rnorm(N,x0,sd=sigma)

    #compute weights
    w.tilde <- (dnorm(y[t], mean=x.pf[t, ],sd=sigma.meas)*dnorm(x.pf[t,],x.pf[t-1,],sd=sigma)*w )/dnorm(x.pf[t,],x0.pf)
    w <- w.tilde/sum(w.tilde)

    #compute diagnostics
    N.eff[t]<-1/sum(w^2)
    m[t]<-sum(w*x.pf[t,])
    MSE.k[t]<-(x[t]-m[t])^2
    conf[t,]<-quantile(x.pf[t,],probs=c(0.025,0.975))

    #resample (bootstrap means resample everytime)
    if(N.eff[t]<N.thr){
      x.pf[t,]<-FUN.resample(N=N,x=x.pf[t,],w=w)
      w<-rep(1/N,N)
      res.point[t]<-1 #add "yes" for resampling point
    }


  }

  #return output
  return(list(x.out=x,y.out=y, x.pf.out=x.pf, m.out=m,MSE.k.out=MSE.k,
              conf.out=conf,N.eff.out=N.eff,w.out=w,res.point.out=res.point))

}


#--Generic Paricle Filter Random Walk (Optimum proposal)--
# N- No of particles
# x- vector of states
# y- vector of measurements
# x0- starting value
# sigma- standard deviation of process
# sigma.meas- standard deviation of measurment
# resample.type - type of resampling

optimum.filter<-function(N,x,y,x0,sigma=1,sigma.meas=1,resample.type="multinomial",N.thr.per=0.5){

  #threshold N
  N.thr<-as.integer(N.thr.per*N)

  #set up filter
  x0.pf<-rnorm(N,mean=x0,sd=sigma)
  x.pf <-matrix(NA,ncol=N,nrow=tau)

  #set up diagnostics
  #-adds variables for diagnoistics
  diagnostic.init(tau)


  #local function to choose resampling method
  FUN.resample<-function(N,x,w){
    switch(resample.type,
           multinomial = mult.resample(N=N,x=x,w=w) ,
           stratified = strat.resample(N=N,x=x,w=w) ,
           systematic = sys.resample(N=N,x=x,w=w),
           residual= res.resample(N=N,x=x,w=w),
           standard=sample(x=x,size=N,replace=TRUE,prob=w)
           )
  }


  #initialise
  sigma.prop<-1/sqrt(1/sigma^2+1/sigma.meas^2)
  mu.prop<-sigma.prop^2*(x0.pf/sigma^2+y[1]/sigma.meas^2)
  x.pf[1, ] <- mu.prop+rnorm(N,sd=sigma.prop)
  w.tilde<- dnorm(y[1],mean=x0.pf,sd=sqrt(sigma^2+sigma.meas^2))
  w<-w.tilde/sum(w.tilde)


  #compute diagnostics
  #- mean, effective sample size, confidence intervals
  m[1]<-sum(w*x.pf[1,])
  MSE.k[1]<-(x[1]-m[1])^2
  N.eff[1]<-1/sum(w^2)
  conf[1,]<-quantile(x.pf[1,],probs=c(0.025,0.975))

  for (t in 2:(tau)) {

    #initialise
    sigma.prop<-1/sqrt(1/sigma^2+1/sigma.meas^2)
    mu.prop<-sigma.prop^2*(x.pf[t-1,]/sigma^2+y[t]/sigma.meas^2)

    #project state forward
    x.pf[t, ] <- mu.prop+rnorm(N,sd=sigma.prop)

    #compute weights
    w.tilde<- dnorm(y[t],mean=x.pf[t-1,],sd=sqrt(sigma^2+sigma.meas^2))*w
    w<-w.tilde/sum(w.tilde)


    #compute diagnostics
    m[t]<-sum(w*x.pf[t,])
    MSE.k[t]<-(x[t]-m[t])^2
    conf[t,]<-quantile(x.pf[t,],probs=c(0.025,0.975))
    N.eff[t]<-1/sum(w^2)

    #resample (bootstrap means resample everytime)
    if(N.eff[t]<N.thr){
      x.pf[t,]<-FUN.resample(N=N,x=x.pf[t,],w=w)
      w<-rep(1/N,N)
      res.point[t]<-1 #add "yes" for resampling point

    }



  }

  #return output
  return(list(x.out=x,y.out=y, x.pf.out=x.pf, m.out=m,MSE.k.out=MSE.k,
              conf.out=conf, N.eff.out=N.eff,w.out=w,res.point.out=res.point))

}

#--Exact Paricle Filter Random Walk--
# did not resample on the first step
# N- No of particles
# x- vector of states
# y- vector of measurements
# x0- starting value
# sigma- standard deviation of process
# sigma.meas- standard deviation of measurment
# resample.type - type of resampling

exact.filter<-function(N,x,y,x0,sigma=1,sigma.meas=1,resample.type="multinomial"){


  #set up filter
  x0.pf<-rnorm(N,mean=x0,sd=sigma)
  x.pf <-matrix(NA,ncol=N,nrow=tau)

  #set up diagnostics
  #-adds variables for diagnoistics
  diagnostic.init(tau)

  #local function to choose resampling method
  FUN.resample<-function(N,x,w){
    switch(resample.type,
           multinomial = mult.resample(N=N,x=x,w=w) ,
           stratified = strat.resample(N=N,x=x,w=w) ,
           systematic = sys.resample(N=N,x=x,w=w),
           residual= res.resample(N=N,x=x,w=w),
           standard=sample(x=x,size=N,replace=TRUE,prob=w)
           )
  }

  #initialise
  w.tilde<- dnorm(y[1],mean=x0.pf,sd=sqrt(sigma^2+sigma.meas^2))
  w<-w.tilde/sum(w.tilde)
  x0.pf<-FUN.resample(N=N,x=x0.pf,w=w) #changed t to t-1
  sigma.prop<-1/sqrt(1/sigma^2+1/sigma.meas^2) #1/sqrt(1/sigma^2+1/sigma.meas^2)
  mu.prop<-sigma.prop^2*(x0.pf/sigma^2+y[1]/sigma.meas^2)#sigma.prop^2*(x0.pf/sigma+y[1]/sigma.meas)
  x.pf[1, ] <- mu.prop+rnorm(N,sd=sigma.prop)




  #compute diagnostics
  #- mean, effective sample size, confidence intervals
  m[1]<-sum(w*x.pf[1,]) #compute mean
  MSE.k[1]<-(x[1]-m[1])^2
  N.eff[1]<-1/sum(w^2) #compute effective sample size
  conf[1,]<-quantile(x.pf[1,],probs=c(0.025,0.975))

  for (t in 2:(tau)) {


    #compute weights
    w.tilde<- dnorm(y[t],mean=x.pf[t-1,],sd=sqrt(sigma^2+sigma.meas^2))#*w
    w<-w.tilde/sum(w.tilde)

    #resample
    x.pf[t-1,]<-FUN.resample(N=N,x=x.pf[t-1,],w=w) #changed t to t-1
    #w<-rep(1/N,N)

    #initialise
    sigma.prop<-1/sqrt(1/sigma^2+1/sigma.meas^2)#1/sqrt(1/sigma^2+1/sigma.meas^2)
    mu.prop<-sigma.prop^2*(x.pf[t-1,]/sigma^2+y[t]/sigma.meas^2) #sigma.prop^2*(x.pf[t-1,]/sigma^2+y[t]/sigma.meas^2)

    #project state forward
    x.pf[t, ] <- mu.prop+rnorm(N,sd=sigma.prop)

    #compute diagnostics
    N.eff[t]<-1/sum(w^2)
    m[t]<-sum(w*x.pf[t,])
    MSE.k[t]<-(x[t]-m[t])^2
    conf[t,]<-quantile(x.pf[t,],probs=c(0.025,0.975))

  }

  #return output
  return(list(x.out=x,y.out=y, x.pf.out=x.pf, m.out=m,MSE.k.out=MSE.k, conf.out=conf,
              N.eff.out=N.eff,w.out=w,res.point.out=res.point))

}


#--Generic particle filter Non-linear(prior proposal) --
# N- No of particles
# x- vector of states
# y- vector of measurements
# x0- starting value
# sigma- standard deviation of process
# sigma.meas- standard deviation of measurment
# resample.type - type of resampling

particle.filter.pop<-function(N,x,y,x0,sigma=1,sigma.meas=1,resample.type="multinomial",N.thr.per=0.5){

  #threshold N
  N.thr<-as.integer(N.thr.per*N)

  #set up filter
  x0.pf<-rnorm(N,mean=x0,sd=sigma)
  x.pf <- matrix(NA,ncol=N,nrow=tau)

  #set up diagnostics
  #-adds variables for diagnoistics
  diagnostic.init(tau)


  #local function to choose resampling method
  FUN.resample<-function(N,x,w){
    switch(resample.type,
           multinomial = mult.resample(N=N,x=x,w=w) ,
           stratified = strat.resample(N=N,x=x,w=w) ,
           systematic = sys.resample(N=N,x=x,w=w),
           residual= res.resample(N=N,x=x,w=w),
           standard=sample(x=x,size=N,replace=TRUE,prob=w)
           )

  }

  # Initialize
  x.pf[1, ] <- f(x0.pf,1)+rnorm(N,sd=sigma) #project particles from x0 to the first time step
  w.tilde<- dnorm(y[1],mean=g(x.pf[1,]),sd=sigma.meas) #the previous weight would be equal weights 1/N depending on how x0 was chosen
  w<-w.tilde/sum(w.tilde) #normalise weights



  #compute diagnostics
  #- mean, effective sample size, confidence intervals
  m[1]<-sum(w*x.pf[1,])
  MSE.k[1]<-(x[1]-m[1])^2
  N.eff[1]<-1/sum(w^2)
  conf[1,]<-quantile(x.pf[1,],probs=c(0.025,0.975))

  for (t in 2:(tau)) {

    #project  state forward
    x.pf[t, ] <-f(x.pf[t-1,],t)+rnorm(N,sd=sigma)

    #compute weights
    w.tilde <- dnorm(y[t], mean=g(x.pf[t, ]),sd=sigma.meas)*w #when add w resampling does not help
    w <- w.tilde/sum(w.tilde) #normalise weights


    #compute diagnostics
    m[t]<-sum(w*x.pf[t,])
    MSE.k[t]<-(x[t]-m[t])^2
    conf[t,]<-quantile(x.pf[t,],probs=c(0.025,0.975))
    N.eff[t]<-1/sum(w^2)

    #resample (bootstrap means resample everytime)
    if(N.eff[t]<N.thr){
      x.pf[t,]<-FUN.resample(N=N,x=x.pf[t,],w=w)
      w<-rep(1/N,N)
      res.point[t]<-1 #add "yes" for resampling point
    }



  }

  #return output
  return(list(x.out=x,y.out=y, x.pf.out=x.pf, m.out=m,MSE.k.out=MSE.k, conf.out=conf,
              N.eff.out=N.eff,w.out=w,res.point.out=res.point))

}


#--Likelihood particle filter Random Walk --
# N- No of particles
# x- vector of states
# y- vector of measurements
# x0- starting value
# sigma- standard deviation of process
# sigma.meas- standard deviation of measurment
# resample.type - type of resampling

likelihood.filter<-function(N,x,y,x0,sigma=1,sigma.meas=1,resample.type="multinomial",N.thr.per=0.5){

  #threshold N
  N.thr<-as.integer(N.thr.per*N)

  #set up filter
  x0.pf<-rnorm(N,mean=x0,sd=sigma) #suppose this is the correct sigma
  x.pf <- matrix(NA,ncol=N,nrow=tau)

  #set up diagnostics
  #-adds variables for diagnoistics
  diagnostic.init(tau)

  #local function to choose resampling method
  FUN.resample<-function(N,x,w){
    switch(resample.type,
           multinomial = mult.resample(N=N,x=x,w=w) ,
           stratified = strat.resample(N=N,x=x,w=w) ,
           systematic = sys.resample(N=N,x=x,w=w),
           residual= res.resample(N=N,x=x,w=w),
           standard=sample(x=x,size=N,replace=TRUE,prob=w)
    )
  }

  # Initialize
  x.pf[1,]<-y[1]-rnorm(N,sd=sigma.meas)
  w.tilde<-dnorm(x.pf[1,],mean=x0,sd=sigma)
  w<-w.tilde/sum(w.tilde)


  #compute diagnostics
  #- mean, effective sample size, confidence intervals
  m[1]<-sum(w*x.pf[1,])
  MSE.k[1]<-(x[1]-m[1])^2
  N.eff[1]<-1/sum(w^2)
  conf[1,]<-quantile(x.pf[1,],probs=c(0.025,0.975))


  for (t in 2:(tau)) {


    #project  state forward
    x.pf[t, ] <-y[t]-rnorm(N,sd=sigma.meas)

    #compute weights
    w.tilde <- dnorm(x.pf[t,], mean=x.pf[t-1,],sd=sigma)*w
    w <- w.tilde/sum(w.tilde)

    #compute diagnostics
    m[t]<-sum(w*x.pf[t,])
    MSE.k[t]<-(x[t]-m[t])^2
    conf[t,]<-quantile(x.pf[t,],probs=c(0.025,0.975))
    N.eff[t]<-1/sum(w^2)

    #resample (bootstrap means resample everytime)
    if(N.eff[t]<N.thr){
      x.pf[t,]<-FUN.resample(N=N,x=x.pf[t,],w=w)
      w<-rep(1/N,N)
      res.point[t]<-1 #add "yes" for resampling point
    }



  }

  #return output
  return(list(x.out=x,y.out=y, x.pf.out=x.pf, m.out=m,MSE.k.out=MSE.k,
              conf.out=conf,N.eff.out=N.eff,w.out=w,res.point.out=res.point))

}

#--Auxiliary particle filter Random Walk(Not Used in thesis)--
# N- No of particles
# x- vector of states
# y- vector of measurements
# x0- starting value
# sigma- standard deviation of process
# sigma.meas- standard deviation of measurment
# resample.type - type of resampling (multinomial,stratified,systematic,residual,standard)

auxiliary.filter<-function(N,x,y,x0,sigma=1,sigma.meas=1,resample.type="multinomial"){

  #set up filter
  x0.pf<-rnorm(N,mean=x0,sd=sigma) #suppose this is the correct sigma
  x.pf <- matrix(NA,ncol=N,nrow=tau)

  #set up diagnostics
  #-adds variables for diagnoistics
  diagnostic.init(tau)

  #local function to choose resampling method
  FUN.resample<-function(N,x,w){
    switch(resample.type,
           multinomial = mult.resample(N=N,x=x,w=w) ,
           stratified = strat.resample(N=N,x=x,w=w) ,
           systematic = sys.resample(N=N,x=x,w=w),
           residual= res.resample(N=N,x=x,w=w),
           standard=sample(x=x,size=N,replace=TRUE,prob=w)
    )
  }

  # Initialize
  mu<-x0.pf #if use true mean x0 must be replicated N times
  #compute parent weights
  v<-dnorm(y[1],mu,sd=sigma.meas)
  #normalise parent weights
  v<-v/ sum(v)
  #sample parents
  samp<-sample(1:N,size=N,replace=TRUE,prob=v)
  #sample from prior
  x.pf[1,]<-rnorm(N,x0.pf[samp],sd=sigma)
  #set weights
  w.tilde<-dnorm(y[1],x.pf[1,],sd=sigma.meas)/dnorm(y[1],x0.pf[samp],sd=sigma.meas)
  #normalise weights
  w<-w.tilde/sum(w.tilde)


  #compute diagnostics
  #- mean, effective sample size, confidence intervals
  m[1]<-sum(w*x.pf[1,])
  MSE.k[1]<-(x[1]-m[1])^2
  N.eff[1]<-1/sum(w^2)
  conf[1,]<-quantile(x.pf[1,],probs=c(0.025,0.975))


  for (t in 2:(tau)) {


    # calculate mean of previous statistics
    mu<-x.pf[t-1,]
    v<-w*dnorm(y[t],mu,sd=sigma.meas)
    #normalise parent weights
    v<-v/ sum(v)
    #sample parent trajectories
    samp<-sample(1:N,size=N,replace=TRUE,prob=v)
    #sample from prior
    x.pf[t,]<-rnorm(N,x.pf[t-1,samp],sd=sigma)
    #set weights
    w.tilde<-dnorm(y[t],x.pf[t,],sd=sigma.meas)/dnorm(y[t],x.pf[t-1,samp],sd=sigma.meas)
    #normalise weights
    w<-w.tilde/sum(w.tilde)

    #compute diagnostics
    m[t]<-sum(w*x.pf[t,])
    N.eff[t]<-1/sum(w^2)
    MSE.k[t]<-(x[t]-m[t])^2
    conf[t,]<-quantile(x.pf[t,],probs=c(0.025,0.975))

  }

  #return output
  return(list(x.out=x,y.out=y, x.pf.out=x.pf, m.out=m,MSE.k.out=MSE.k,
              conf.out=conf,N.eff.out=N.eff,w.out=w,res.point.out=res.point))

}


#--Auxiliary particle filter Non-linear --
# N- No of particles
# x- vector of states
# y- vector of measurements
# x0- starting value
# sigma- standard deviation of process
# sigma.meas- standard deviation of measurment
# resample.type - type of resampling (multinomial,stratified,systematic,residual,standard)

auxiliary.filter.pop<-function(N,x,y,x0,sigma=1,sigma.meas=1,resample.type="multinomial"){

  #set up filter
  x0.pf<-rnorm(N,mean=x0,sd=sigma) #suppose this is the correct sigma
  x.pf <- matrix(NA,ncol=N,nrow=tau)

  #set up diagnostics
  #-adds variables for diagnoistics
  diagnostic.init(tau)

  #local function to choose resampling method
  FUN.resample<-function(N,x,w){
    switch(resample.type,
           multinomial = mult.resample(N=N,x=x,w=w) ,
           stratified = strat.resample(N=N,x=x,w=w) ,
           systematic = sys.resample(N=N,x=x,w=w),
           residual= res.resample(N=N,x=x,w=w),
           standard=sample(x=x,size=N,replace=TRUE,prob=w)
    )
  }

  # Initialize
  mu<-f(x0.pf,1) #rep(mean(f(x0.pf,1)),N) #if use true mean x0 must be replicated N times
  #compute parent weights
  v<-dnorm(y[1],g(mu),sd=sigma.meas)
  #normalise parent weights
  v<-v/ sum(v)
  #sample parents
  samp<-sample(1:N,size=N,replace=TRUE,prob=v)
  #sample from prior
  x.pf[1,]<-rnorm(N,f(x0.pf[samp],1),sd=sigma)
  #set weights
  w.tilde<-dnorm(y[1],g(x.pf[1,]),sd=sigma.meas)/dnorm(y[1],g(x0.pf[samp]),sd=sigma.meas)
  #normalise weights
  w<-w.tilde/sum(w.tilde)

  #compute diagnostics
  #- mean, effective sample size, confidence intervals
  m[1]<-sum(w*x.pf[1,])
  MSE.k[1]<-(x[1]-m[1])^2
  N.eff[1]<-1/sum(w^2)
  conf[1,]<-quantile(x.pf[1,],probs=c(0.025,0.975))


  for (t in 2:(tau)) {


    # calculate mean of previous statistics
    mu<-f(x.pf[t-1,],t) #rep(mean(f(x.pf[t-1,],t)),N)
    v<-w*dnorm(y[t],g(mu),sd=sigma.meas)
    #normalise parent weights
    v<-v/ sum(v)
    #sample parent trajectories
    samp<-sample(1:N,size=N,replace=TRUE,prob=v)
    #sample from prior
    x.pf[t,]<-rnorm(N,f(x.pf[t-1,samp],t),sd=sigma)
    #set weights
    w.tilde<-dnorm(y[t],g(x.pf[t,]),sd=sigma.meas)/dnorm(y[t],g(x.pf[t-1,samp]),sd=sigma.meas)
    #normalise weights
    w<-w.tilde/sum(w.tilde)

    #compute diagnostics
    m[t]<-sum(w*x.pf[t,])
    N.eff[t]<-1/sum(w^2)
    MSE.k[t]<-(x[t]-m[t])^2
    conf[t,]<-quantile(x.pf[t,],probs=c(0.025,0.975))

  }

  #return output
  return(list(x.out=x,y.out=y, x.pf.out=x.pf, m.out=m,MSE.k.out=MSE.k,
              conf.out=conf,N.eff.out=N.eff,w.out=w,res.point.out=res.point))

}


#--Regularized particle filter Non-linear(prior proposal)--
# we use optimal bin width, given Epanechnikov kernel, assuming filtered distribution is gaussian
# N- No of particles
# x- vector of states
# y- vector of measurements
# x0- starting value
# sigma- standard deviation of process
# sigma.meas- standard deviation of measurment
# resample.type - type of resampling (multinomial,stratified,systematic,residual,standard)

regularized.filter.pop<-function(N,x,y,x0,sigma=1,sigma.meas=1,resample.type="multinomial",N.thr.per=0.5){

  #threshold N
  N.thr<-as.integer(N.thr.per*N)

  #set up filter
  x0.pf<-rnorm(N,mean=x0,sd=sigma) #suppose this is the correct sigma
  x.pf <- matrix(NA,ncol=N,nrow=tau)

  #set up diagnostics
  #-adds variables for diagnoistics
  diagnostic.init(tau)

  # Compute constants
  dim<-1 #dimesnion of state
  c<-2  #volume of a unit sphere in [dim] dimensions
  A<-((8/c)*(dim+4)*2*sqrt(pi))^(1/(dim+4))
  h<-A*N^(1/(dim+4)) #compute optimal bandwidth

  #local function to choose resampling method
  FUN.resample<-function(N,x,w){
    switch(resample.type,
           multinomial = mult.resample(N=N,x=x,w=w) ,
           stratified = strat.resample(N=N,x=x,w=w) ,
           systematic = sys.resample(N=N,x=x,w=w),
           residual= res.resample(N=N,x=x,w=w),
           standard=sample(x=x,size=N,replace=TRUE,prob=w)
    )
  }

  # Initialize
  x.pf[1, ] <- f(x0.pf,1)+rnorm(N,sd=sigma) #project particles from x0 to the first time step
  w.tilde<- dnorm(y[1],mean=g(x.pf[1,]),sd=sigma.meas) #weights of x0 is equal
  w<-w.tilde/sum(w.tilde)


  #compute diagnostics
  #- mean, effective sample size, confidence intervals
  m[1]<-sum(w*x.pf[1,])
  MSE.k[1]<-(x[1]-m[1])^2
  N.eff[1]<-1/sum(w^2)
  conf[1,]<-quantile(x.pf[1,],probs=c(0.025,0.975))


  for (t in 2:(tau)) {


    #project  state forward
    x.pf[t, ] <-f(x.pf[t-1,],t)+rnorm(N,sd=sigma)

    #compute weights
    w.tilde <- dnorm(y[t], mean=g(x.pf[t, ]),sd=sigma.meas)*w
    w <- w.tilde/sum(w.tilde)

    #compute diagnostics
    m[t]<-sum(w*x.pf[t,])
    MSE.k[t]<-(x[t]-m[t])^2
    conf[t,]<-quantile(x.pf[t,],probs=c(0.025,0.975))
    N.eff[t]<-1/sum(w^2)

    #compute empirical covariance
    d<-sqrt(sum(w*x.pf[t,]^2))


    #resample (bootstrap means resample everytime)
    if(N.eff[t]<N.thr){
      x.pf[t,]<-FUN.resample(N=N,x=x.pf[t,],w=w)
      #samp<-FUN.resample(N=N,x=1:N,w=w)
      #x.pf[t,]<-x.pf[t,samp]
      w<-rep(1/N,N)
      res.point[t]<-1 #add "yes" for resampling point

      #fit
      fit<-density(x.pf[t,])
      x.pf[t,]<- rnorm(N, sample(x.pf[t,], size = N, replace = TRUE), fit$bw)

    }


  }

  #return output
  return(list(x.out=x,y.out=y, x.pf.out=x.pf, m.out=m,MSE.k.out=MSE.k,
              conf.out=conf,N.eff.out=N.eff,w.out=w,res.point.out=res.point))

}


#--Rejection particle filter Non-linear(prior proposal)--

# N- No of particles
# x- vector of states
# y- vector of measurements
# x0- starting value
# sigma- standard deviation of process
# sigma.meas- standard deviation of measurment
# Mnorm= supremmum of numerator

rejection.filter.pop<-function(N,x,y,x0,sigma=1,sigma.meas=1,Mnorm=1){

  #set up filter
  x0.pf<-rnorm(N,mean=x0,sd=sigma) #suppose this is the correct sigma
  x.pf <- matrix(NA,ncol=N,nrow=tau)

  #set up diagnostics
  #-adds variables for diagnoistics
  diagnostic.init(tau)

  # Initialize

  #project particles from x0 to the first time step
  x.pf[1, ] <- f(x0.pf,1)+rnorm(N,sd=sigma)
  #compute c
  noise.j<-rnorm(N,sd=sigma)
  grid<-expand.grid(noise.j, f(x0.pf,1)) #j-noise, i- coord (t-1 ) particles   #each row is section is first x0.pf with every single noise
  grid.sum<-rowSums(grid)
  c<-(1/N^2)*sum(dnorm(y[1],g(grid.sum)))
  #compute ci
  index<-rep(1:N, each=N)
  ci<-sapply(1:N, function(s) mean(dnorm(y[1],g(grid.sum[index==s]))) )
  #compute weights and resample
  lambda<-ci/(N*c)
  x.old<-sample(x0.pf,  N,replace=TRUE,prob=lambda)

  u<-1
  for(i in 1:N){
    a<-0
    while(u>a){
      #make proposal
      z<-f(x.old[i],1)+rnorm(1,0,sd=sigma)
      #compute acceptance probability
      a<-( dnorm(y[1],g(z),sd=sigma.meas)*dnorm(z,f(x.old[i],1), sd=sigma)/dnorm(z,f(x.old[i],1),sd=sigma) )/Mnorm
      #print to see how long algo runs
      print(paste("u",u,"x.old",x.old[i],"i",i,"a",a))
      u<-runif(1,0,1)
      if(a>1){
        print("alert. increase M")
      }
    }
    # accept proposal
    x.pf[1,i]<-z
  }



  #compute diagnostics
  #- mean, effective sample size, confidence intervals
  m[1]<-mean(x.pf[1,])
  MSE.k[1]<-(x[1]-m[1])^2
  conf[1,]<-quantile(x.pf[1,],probs=c(0.025,0.975))


  for (t in 2:(tau)) {

    #project particles forward
    x.pf[t, ] <- f(x.pf[t-1,],t)+rnorm(N,sd=sigma)
    #compute c
    noise.j<-rnorm(N,sd=sigma)
    grid<-expand.grid(noise.j, f(x.pf[t-1,],t) )#j-noise, i- coord (t-1 ) particles
    grid.sum<-rowSums(grid)
    c<-(1/N^2)*sum(dnorm(y[t],g(grid.sum)))
    #compute ci
    index<-rep(1:N, each=N)
    ci<-sapply(1:N, function(s) mean(dnorm(y[t],g(grid.sum[index==s]))))
    #compute weights and resample
    lambda<-ci/(N*c)
    x.old<-sample(x.pf[t-1,],N,replace=TRUE,prob=lambda)

    u<-1
    for(i in 1:N){
      a<-0
      while(u>a){
        #make proposal
        z<-f(x.old[i],t)+rnorm(1,0,sd=sigma)
        #compute acceptance probability
        a<- (dnorm(y[t],g(z),sd=sigma.meas)*dnorm(z,f(x.old[i],t), sd=sigma)  /dnorm(z,f(x.old[i],t),sd=sigma) )/Mnorm #x.pf[t-1,i]
        #print to see how long algo runs
        print(paste("t",t,"u",u,"x.old",x.old[i],"i",i,"a",a))
        if(a>1){
          print("alert. increase M")
        }
        u<-runif(1,0,1)
      }
      # accept proposal
      x.pf[t,i]<-z
    }


    #compute diagnostics
    m[t]<-mean(x.pf[t,])
    MSE.k[t]<-(x[t]-m[t])^2
    conf[t,]<-quantile(x.pf[t,],probs=c(0.025,0.975))


  }

  #return output
  return(list(x.out=x,y.out=y, x.pf.out=x.pf, m.out=m,MSE.k.out=MSE.k,
              conf.out=conf))

}


#--Extended filter Non-linear(Not Used inside thesis)--
#uses optimal proposal
# N- No of particles
# x- vector of states
# y- vector of measurements
# x0- starting value
# sigma- standard deviation of process
# sigma.meas- standard deviation of measurment
# resample.type - type of resampling (multinomial,stratified,systematic,residual,standard)

extended.filter<-function(N,x,y,x0,sigma=1,sigma.meas=1,resample.type="multinomial",N.thr.per=0.5){

  #threshold N
  N.thr<-as.integer(N.thr.per*N)

  #set up filter
  x0.pf<-rnorm(N,mean=x0,sd=sigma)
  x.pf <-matrix(NA,ncol=N,nrow=tau)

  #set up diagnostics
  #-adds variables for diagnoistics
  diagnostic.init(tau)


  #local function to choose resampling method
  FUN.resample<-function(N,x,w){
    switch(resample.type,
           multinomial = mult.resample(N=N,x=x,w=w) ,
           stratified = strat.resample(N=N,x=x,w=w) ,
           systematic = sys.resample(N=N,x=x,w=w),
           residual= res.resample(N=N,x=x,w=w),
           standard=sample(x=x,size=N,replace=TRUE,prob=w)
    )
  }


  #initialise
  sigma.prop<-1/sqrt( 1/sigma^2+diff.g(f(x0.pf,1))^2/sigma.meas^2 )
  mu.prop<-sigma.prop^2*(f(x0.pf,1)/sigma^2+diff.g(f(x0.pf,1))/sigma.meas^2)*(y[1]-g(f(x0.pf,1))+diff.g(f(x0.pf,1))*f(x0.pf,1))
  x.pf[1, ] <- f(mu.prop,1)+rnorm(N,sd=sigma.prop)
  w.tilde<- dnorm(y[1],mean=g.tay(x.pf[1,],x0.pf,1),sd=sqrt(sigma^2+diff.g(f(x0.pf,1))^2*sigma.meas^2)) #does not use linearise
  w<-w.tilde/sum(w.tilde)


  #compute diagnostics
  #- mean, effective sample size, confidence intervals
  m[1]<-sum(w*x.pf[1,])
  MSE.k[1]<-(x[1]-m[1])^2
  N.eff[1]<-1/sum(w^2)
  conf[1,]<-quantile(x.pf[1,],probs=c(0.025,0.975))

  #second step--

  for (t in 2:(tau)) {

    #initialise
    sigma.prop<-1/sqrt( 1/sigma^2+diff.g(f(x.pf[t-1,],t))^2/sigma.meas^2 )
    mu.prop<-sigma.prop^2*(f(x.pf[t-1,],t)/sigma^2+diff.g(f(x.pf[t-1,],t))/sigma.meas^2)*(y[t]-g(f(x.pf[t-1,],t))+diff.g(f(x.pf[t-1,],t))*f(x.pf[t-1,],t))

    #project state forward
    x.pf[t, ] <- f(mu.prop,t)+rnorm(N,sd=sigma.prop)
    x.pf[1, ] <- f(mu.prop,1)+rnorm(N,sd=sigma.prop)

    #compute weights
    #w.tilde<- dnorm(y[t],mean=g.tay(x.pf[t-1,],x.pf[t-2],t),sd=sqrt(sigma^2+sigma.meas^2))*w
    w.tilde<- dnorm(y[t],mean=g.tay(x.pf[t,],x.pf[t-1,],t),sd=sqrt(sigma^2+diff.g(f(x.pf[t-1,],1))^2*sigma.meas^2)) #does not use linearise

    w<-w.tilde/sum(w.tilde)


    #effective sample size
    N.eff[t]<-1/sum(w^2)

    #resample (bootstrap means resample everytime)
    if(N.eff[t]<N.thr){
      x.pf[t,]<-FUN.resample(N=N,x=x.pf[t,],w=w)
      w<-rep(1/N,N)
      res.point[t]<-1 #add "yes" for resampling point

    }

    #compute diagnostics
    m[t]<-sum(w*x.pf[t,])
    MSE.k[t]<-(x[t]-m[t])^2
    conf[t,]<-quantile(x.pf[t,],probs=c(0.025,0.975))

  }

  #return output
  return(list(x.out=x,y.out=y, x.pf.out=x.pf, m.out=m,MSE.k.out=MSE.k,
              conf.out=conf, N.eff.out=N.eff,w.out=w,res.point.out=res.point))

}