iapfonly_1d_separate_particles

#ess resampling
ESS <- function(t,w, is.log=FALSE){
  if(is.log) {
    mx <- max(w[t-1,])
    s <- sum(exp(w[t-1,]-mx))
    ess <- 1/sum((exp(w[t-1,]-mx)/s)^2)
  }else{
    s <- sum(w[t-1,])
    ess <- 1/sum((w[t-1,]/s)^2) 
  }
  return(ess)  
}

#trans prob
#N(; Ax, B)
f <- function(x){
  return (rnorm(d) + as.vector(A*x))   
}

#obs prob  
#N(; Cx, D)
g <- function(y, x){  
  return (det(diag(2*pi, nrow = d, ncol = d))^(-1/2)*exp((-1/2)*(y-x)^2)) 
}

#twisted mu
mu_aux <- function(psi_pa, l, N, t){
  return(rnorm(N[l], mean =  psi_pa[t,1]/(1+psi_pa[t,2]^2), sd = psi_pa[t,2]^2/(1+psi_pa[t,2]^2)))
}

#twisted g
g_aux <- function(y, x, t, psi_pa, n){  
  if(t == (n-L+1)){
    return(dnorm(x, y)*psi_tilda(x, psi_pa, t, n)*(2*pi*(psi_pa[t, 2]^2+1))^
             (-1/2)*exp((-1/2)*(-psi_pa[t, 1])^2/(psi_pa[t, 2]^2+1))/psi_t(x, psi_pa, t, n))  #initialisation of g = t=1 or t=L?
  }else{
    return(dnorm(x, y)*psi_tilda(x, psi_pa, t, n)/psi_t(x, psi_pa, t, n))  #g_2:T 
  }
}

#twisted f
f_aux <- function(x, psi_pa, t){
  return(rnorm(1, (psi_pa[t,2]^2*A*x +psi_pa[t,1])/(psi_pa[t,2]^2+1), 
               sqrt(psi_pa[t,2]^2 / (psi_pa[t,2]^2+1))))  #f_2:T 
}

#psi.tilda_t = f (xt, ψt+1); psi.tilda_n = 1
psi_tilda <- function(x, psi_pa, t, n){   
  if(t == n){
    psi_tilda <- 1
  }else{   #psi_pa_t = psi_t
    psi_tilda <- (2*pi*(psi_pa[t+1, 2]^2+1))^   
      (-1/2)*exp((-1/2)*(A*x-psi_pa[t+1, 1])^2/(psi_pa[t+1, 2]^2+1))  
  }
  return(psi_tilda)
}

#ψt(xt) = N (xt; m_t, Σ_t), m_t, Σ_t obtained in Psi function
psi_t <- function(x, psi_pa, t, n){  
  if(t == (n + 1)){
    psi_t <- 1
  }else{
    psi_t <- (2*pi*psi_pa[t, 2]^2)^(-1/2)*exp((-1/2)*(x-psi_pa[t, 1])^2/(psi_pa[t, 2]^2))  
  }
  return(psi_t)
}

#the distribution here and the distribution below should be double checked

#the initial distribution mu at time kL
# ∑W_kL(?)^i*f(X_{k−1}^i,(k−1)L, ·), i in 1:N
#I used the weights at time (k-1)L because these are the closest weights, 
#sample particles X_{kL-L+1} using the particles at time X_(k-1)L

change_mu <- function(w, X){
  sample <- vector()
  mx <- max(w[n-L,])
  w_ <- exp(w[n-L,]-mx)/sum(exp(w[n-L,] - mx))
  s <- sample(1:Num, size = Num, replace = TRUE, prob = w_) 
  mus <- X[n-L,]
  for(i in 1:Num){
    sample[i] <- rnorm(1, mean = A*mus[s[i]], B)
  }
  return(list(sample, w_))
}

#the initial distribution mu.tilda at time kL
#the particles and weights I use are exactly the same as above, and I use
#f.tilda to generate particles instead of f

change_mupsi <- function(w, X, psi_pa, t, N, l){
  sample <- vector()
 
  #here we need to adjust the weights
  w_adj <- w[n-L,]*exp((psi_pa[t,2]^2*A*X[n-L,] +psi_pa[t,1])^2/(2*(psi_pa[t,2]^2+1)*psi_pa[t,2]^2) -
                (psi_pa[t,2]^2*(A*X[n-L,])^2 + psi_pa[t,1]^2)/(2*psi_pa[t,2]^2))
  
  mx <- max(w_adj)
  w_ <- exp(w_adj - mx)/sum(exp(w_adj - mx))
  s <- sample(1:Num, size = Num, replace = TRUE, prob = w_) 
  mus <- X[n-L,]
  for(i in 1:N[l]){
    sample[i] <- f_aux(mus[s[i]], psi_pa, t)
    
  }
  return(list(sample, w_))
}

#Below is the iAPF algorithm
#First we do the initialization
####init_APF####
init_APF <- function(w, X){
  l = 1
  Z_apf[1] = 0
  X_init <- matrix(NA, Time, N[l])
  w_init <- matrix(NA, Time, N[l])
  X_init_s <- X_init
  w_init_s <- w_init
  
  #when n = L, we use the mu as the initialization distribution mu; 
  #when n = kL, we use the new distribution
  #I tried to modify the previous paths from 1 to n-L+1
  if(n == L){
    X_init[1:(n-L+1),] <- rnorm(N[l])  
    w_init[1:(n-L+1),] <- dnorm(X_init[n-L+1,], obs[n-L+1], log = TRUE)
  }else{
    X_init[1:(n-L+1),] <- change_mu(w, X)[[1]]
    w_ <- change_mu(w, X)[[2]]
    
    for (i in 1:Num){
      w_init[1:(n-L+1), i] <- log(sum(w_*dnorm(X_init[n-L+1,], A*X[n-L,i], B)))
    }
  }
  
  X_init_s <- X_init
  w_init_s <- w_init
  
  
  for(t in (n-L+2):n){
    
    if(ESS(t, w_init, is.log=TRUE) <= kappa*N[l]){
      mx <- max(w_init[t-1,])
      w_ <- exp(w_init[t-1,] - mx)/sum(exp(w_init[t-1,] - mx))
      Z_apf[1] = Z_apf[1] + log(mean(exp(w_init[t-1,] - mx))) + mx
      mix <- sample(1:N[l], N[l], replace = TRUE, prob = w_)
      
      X_init_s[1:(t-1),] <- X_init_s[1:(t-1), mix] 
      # at the initialization stage, we want filtering particles for psi
      
      X_init[t,1:N[l]] <- rnorm(N[l]) + A*X_init[t-1, mix]   
      w_init[t,1:N[l]] <- dnorm(X_init[t,mix], obs[t], log = TRUE) 
      
      #smoothing particles
      X_init_s[t,1:N[l]] <- rnorm(N[l]) + A*X_init_s[t-1, ]   
      w_init_s[t,1:N[l]] <- dnorm(X_init_s[t,], obs[t], log = TRUE) 
      
    }else{
      X_init[t,1:N[l]] <- rnorm(N[l]) + A*X_init[t-1,] 
      w_init[t,1:N[l]] <- w_init[t-1,] + dnorm(X_init[t,], obs[t], log = TRUE)
      
      #smoothing particles
      X_init_s[t,1:N[l]] <- rnorm(N[l]) + A*X_init_s[t-1,] 
      w_init_s[t,1:N[l]] <- w_init_s[t-1,] + dnorm(X_init_s[t,], obs[t], log = TRUE)
    }
  }
  
  mx <- max(w_init[n, 1:N[l]])
  Z_apf[1] <- Z_apf[1] + log(mean(exp(w_init[n, 1:N[l]]-mx))) + mx
  
  return(list(X_init = X_init, w_init = w_init, Z_apf = Z_apf, X_init_s, w_init_s))
}

#Within the iAPF scheme, when the number of iterations l >= 2, run APF
####APF####
APF <- function(w, X, psi_pa, l, Z_apf, N){
  #l >= 2
  X_apf <- matrix(NA, Time, N[l])
  w_apf <- matrix(NA, Time, N[l])
  X_apf_s <- X_apf
  w_apf_s <- w_apf
  
  Z_apf[l] <- 0
  
  #when n = kL, we use the new mu.tilda to initialize particles
  if(n == L){
    X_apf[1:(n-L+1),1:N[l]] <- mu_aux(psi_pa, l, N, n-L+1) 
    for(i in 1:N[l]){
      w_apf[1:(n-L+1),i] <- log(g_aux(obs[n-L+1], X_apf[n-L+1,i],n-L+1, psi_pa, n)) 
    }
  }else{
    X_apf[1:(n-L+1),1:N[l]] <- change_mupsi(w, X, psi_pa, n-L+1, N, l)[[1]]
    w_ <- change_mupsi(w, X, psi_pa, n-L+1, N, l)[[2]]
    for (i in 1:N[l]){
      w_apf[1:(n-L+1), i] <- log(sum(w_*dnorm(X_apf[n-L+1,], (psi_pa[n-L+1,2]^2*A*X[n-L,i] +psi_pa[n-L+1,1])/(psi_pa[n-L+1,2]^2+1),
                                              sqrt(psi_pa[n-L+1,2]^2 / (psi_pa[n-L+1,2]^2+1)))))
    }
  }
  
  X_apf_s <- X_apf
  w_apf_s <- w_apf
  
  for(t in (n-L+2):n){
    
    if(ESS(t,w_apf, is.log = TRUE) <= kappa*N[l]){
      
      mx <- max(w_apf[t-1,])
      w_ <- exp(w_apf[t-1,1:N[l]]-mx)/sum(exp(w_apf[t-1, 1:N[l]] - mx))
      Z_apf[l] = Z_apf[l] + log(mean(exp(w_apf[t-1,]-mx))) + mx
      mix <- sample(1:N[l],N[l], replace = TRUE, prob = w_)
      
      #record the smoothing particles using X_apf_s
      X_apf_s[1:(t-1),] <- X_apf_s[1:(t-1),mix]
      
      for(i in 1:N[l]){
        #filtering particles
        X_apf[t,i] <- f_aux(X_apf[t-1, mix[i]], psi_pa, t)
        w_apf[t,i] <- log(g_aux(obs[t], X_apf[t,mix[i]], t, psi_pa, n))  
        
        #smoothing particles
        X_apf_s[t,i] <- f_aux(X_apf_s[t-1, i], psi_pa, t)
        w_apf_s[t,i] <- log(g_aux(obs[t], X_apf_s[t,i], t, psi_pa, n))  
      }
    }else{
      
      for(i in 1:N[l]){
        #filtering particles
        X_apf[t,i] <- f_aux(X_apf[t-1,i], psi_pa, t) 
        w_apf[t,i] <- w_apf[t-1,i] + log(g_aux(obs[t], X_apf[t,i], t, psi_pa, n))
        
        #smoothing particles
        X_apf_s[t,i] <- f_aux(X_apf_s[t-1,i], psi_pa, t) 
        w_apf_s[t,i] <- w_apf_s[t-1,i] + log(g_aux(obs[t], X_apf_s[t,i], t, psi_pa, n)) 
      }
    }
    
  }
  
  mx <- max(w_apf[n, 1:N[l]])
  Z_apf[l] <- Z_apf[l] + log(mean(exp(w_apf[n, 1:N[l]]-mx))) + mx
  
  return(list(X_apf, w_apf, Z_apf, X_apf_s, w_apf_s))
}

####Psi####
Psi <- function(l, n, X_apf, N){
  psi <- matrix(NA, nrow = Time, ncol = N[l])
  psi_pa <- matrix(NA, nrow = Time, ncol = 2)
  
  #calculate psi
  for(t in n:(n-L+1)){
    if(t == n){
      psi[t,] <- dnorm(X_apf[t,1:N[l]], obs[t], D)   
      
    }else{
      psi[t,] <- dnorm(X_apf[t,], obs[t])*dnorm(A*X_apf[t,], psi_pa[t+1, 1], sqrt(psi_pa[t+1,2]^2+1))
    }
    
    #calculate psi_t
    fn <- function(x, X_apf, psi){
      lambda <-  2*sum(dnorm(X_apf[t,1:N[l]],mean=x[1],sd=x[2]) * psi[t,1:N[l]]) / sum(psi[t,]^2)
      return(sum((psi[t,1:N[l]] - (1/lambda)*dnorm(X_apf[t,1:N[l]],mean=x[1],sd=x[2]))^2))
    }
    
    #get the distribution of psi_t
    if(t == n){
      psi_pa[t,] <- optim(par = c(mean(X_apf[t,1:N[l]]),1),
                          fn = fn, X = X_apf, psi = psi, method='L-BFGS-B',lower=c(-Inf,0.1),upper=c(Inf,Inf))$par
    }else{
      
      psi_pa[t,] <- optim(par = c(X_apf[t,which.max(psi[t,1:N[l]])],1), 
                          fn = fn, X = X_apf, psi = psi, method='L-BFGS-B',lower=c(-Inf,0.1),upper=c(Inf,Inf))$par
    }
    
    #print(psi_pa[t, 1])
    #print(obs[t])
  }
  return(psi_pa)
}

#The main part of iAPF to iterate and terminate
####psi_APF####
psi_APF <- function(n, X_apf, Z_apf, w, X){
  l = 1
  
  while(TRUE){
    
    output <- list()
    
    if(l != 1){
      #generate filtering particles X_apf for psi the next iteration
      #APF outputs filtering X_apf for the next psi, and smoothing X_apf_s
      #for the final calculation
      
      output <- APF(w, X, psi_pa, l, Z_apf, N)
      X_apf <- output[[1]]
      w_apf <- output[[2]]
      Z_apf <- output[[3]]
      X_apf_s <- output[[4]]
      w_apf_s <- output[[5]]
    }
    
    #to speed up the algorithm, I just fix the number of iterations to be k.
    #Here k = 5
    
    if(l <= k ){
      
      #receive filtering particles X_apf for psi
      psi_pa <- Psi(l, n, X_apf, N) 
      
      if(l > k & N[max(l-k,1)] == N[l] & is.unsorted(Z_apf[max(l-k,1):l])){  
        N[l+1] <- 2*N[l]
        
      }else{
        N[l+1] <- N[l]
      }
      
      l <- l+1
    }else break
  }
  
  #output smoothing X_apf_s generated by the psi-APF
  return(list(psi_pa, X_apf_s, w_apf_s, Z_apf[l]))
}