# A particle filter for animal movement
# https://tidyecology.com/posts/particle-filter-for-movement/
#
# The R code of this Tidy Ecology tutorial, chunk by chunk, in the order of the post.
# Run it top to bottom in a fresh R session. Code the post shows but does not run
# is commented out. The prose, and the numbers it quotes, are in the post itself.
# Post last modified: 2026-10-15

## ---- setup ----
suppressMessages({library(ggplot2); library(dplyr); library(tidyr)})
ink<-"#16241d"; body<-"#2c3a31"; forest<-"#275139"; sage<-"#93a87f"; paper<-"#f5f4ee"
line<-"#dad9ca"; faint<-"#5d6b61"; gold<-"#cda23f"; red<-"#b5534e"
theme_te <- function(base=12){
  theme_minimal(base_size=base) +
  theme(plot.background=element_rect(fill=paper,colour=NA),
        panel.background=element_rect(fill=paper,colour=NA),
        panel.grid.minor=element_blank(),
        panel.grid.major=element_line(colour=line,linewidth=.3),
        axis.title=element_text(colour=body), axis.text=element_text(colour=faint),
        plot.title=element_text(colour=ink,face="bold",size=base+1),
        plot.subtitle=element_text(colour=faint,size=base-2),
        strip.text=element_text(colour=ink,face="bold"),
        legend.title=element_text(colour=body,size=base-2),
        legend.text=element_text(colour=faint,size=base-2))
}

## ---- simulate ----
set.seed(2207)
n<-300; gamma_t<-0.6; sigma_t<-5; tau_t<-5; p_out<-0.12; k_out<-7
vx<-vy<-numeric(n)
vx[1]<-rnorm(1,0,sigma_t/sqrt(1-gamma_t^2)); vy[1]<-rnorm(1,0,sigma_t/sqrt(1-gamma_t^2))
for(t in 2:n){ vx[t]<-gamma_t*vx[t-1]+rnorm(1,0,sigma_t); vy[t]<-gamma_t*vy[t-1]+rnorm(1,0,sigma_t) }
px<-cumsum(vx); py<-cumsum(vy)
out<-rbinom(n,1,p_out)                          # which fixes are gross outliers
sdv<-ifelse(out==1, k_out*tau_t, tau_t)
ox<-px+rnorm(n,0,sdv); oy<-py+rnorm(n,0,sdv)
n_out<-sum(out); worst<-max(sqrt((ox-px)^2+(oy-py)^2))
c(n=n, outliers=n_out, worst_error=round(worst))

## ---- kalman ----
kf_coord<-function(y,g,s2,h2){
  n<-length(y); Tm<-matrix(c(1,0,g,g),2,2); Q<-s2*matrix(c(1,1,1,1),2,2); Z<-matrix(c(1,0),1,2)
  a<-c(y[1],0); P<-matrix(c(h2,0,0,s2/(1-g^2)),2,2); ll<-0; af<-matrix(NA,n,2); af[1,]<-a
  for(t in 2:n){ ap<-as.vector(Tm%*%a); Pp<-Tm%*%P%*%t(Tm)+Q
    v<-y[t]-as.vector(Z%*%ap); Ft<-as.vector(Z%*%Pp%*%t(Z))+h2; K<-as.vector(Pp%*%t(Z))/Ft
    a<-ap+K*v; P<-Pp-(K%*%Z)%*%Pp; ll<-ll-0.5*(log(2*pi)+log(Ft)+v^2/Ft); af[t,]<-a }
  list(ll=ll, p=af[,1]) }
nll<-function(par){g<-plogis(par[1]);s2<-exp(par[2]);h2<-exp(par[3])
  -(kf_coord(ox,g,s2,h2)$ll+kf_coord(oy,g,s2,h2)$ll)}
fk<-optim(c(qlogis(.5),log(25),log(25)),nll,method="Nelder-Mead",control=list(reltol=1e-10,maxit=3000))
gk<-plogis(fk$par[1]); s2k<-exp(fk$par[2]); h2k<-exp(fk$par[3])
kfx<-kf_coord(ox,gk,s2k,h2k)$p; kfy<-kf_coord(oy,gk,s2k,h2k)$p
c(gamma=round(gk,3), sigma=round(sqrt(s2k),2), tau=round(sqrt(h2k),2))

## ---- particle-filter ----
systematic_resample<-function(w){
  N<-length(w); pos<-(runif(1)+0:(N-1))/N; cw<-cumsum(w); idx<-integer(N); j<-1
  for(i in 1:N){ while(cw[j]<pos[i]) j<-j+1; idx[i]<-j }; idx }

run_pf<-function(nu, resample=TRUE, N=5000, seed=99){
  set.seed(seed)
  g<-gamma_t; s<-sigma_t; sc<-tau_t                 # process at truth; obs model = t_nu(scale sc)
  Px<-matrix(0,N,4)                                 # columns: px, vx, py, vy
  Px[,2]<-rnorm(N,0,s/sqrt(1-g^2)); Px[,4]<-rnorm(N,0,s/sqrt(1-g^2))
  Px[,1]<-ox[1]+rnorm(N,0,sc); Px[,3]<-oy[1]+rnorm(N,0,sc)
  w<-rep(1/N,N); est<-matrix(NA,n,2); ess<-numeric(n)
  est[1,]<-c(weighted.mean(Px[,1],w), weighted.mean(Px[,3],w)); ess[1]<-1/sum(w^2)
  for(t in 2:n){
    Px[,2]<-g*Px[,2]+rnorm(N,0,s); Px[,1]<-Px[,1]+Px[,2]     # propagate
    Px[,4]<-g*Px[,4]+rnorm(N,0,s); Px[,3]<-Px[,3]+Px[,4]
    ld<-dt((ox[t]-Px[,1])/sc,df=nu,log=TRUE)-log(sc)+
        dt((oy[t]-Px[,3])/sc,df=nu,log=TRUE)-log(sc)         # heavy-tailed weights
    lw<-log(w)+ld; lw<-lw-max(lw); w<-exp(lw); w<-w/sum(w)
    est[t,]<-c(sum(w*Px[,1]), sum(w*Px[,3])); ess[t]<-1/sum(w^2)
    if(resample && ess[t]<N/2){ idx<-systematic_resample(w); Px<-Px[idx,]; w<-rep(1/N,N) }
  }
  list(px=est[,1], py=est[,2], ess=ess) }

pf      <- run_pf(nu=4, resample=TRUE)
pf_nores<- run_pf(nu=4, resample=FALSE)

## ---- compare ----
rmse<-function(ex,ey) sqrt(mean((ex-px)^2+(ey-py)^2))
rmse_obs<-rmse(ox,oy); rmse_kf<-rmse(kfx,kfy); rmse_pf<-rmse(pf$px,pf$py)
ess_med<-median(pf$ess); n_res<-sum(pf$ess<5000/2)
collapse<-which(pf_nores$ess<10)[1]
c(rmse_obs=round(rmse_obs,2), rmse_kf=round(rmse_kf,2), rmse_pf=round(rmse_pf,2))

## ---- fig-track ----
dist<-sqrt((ox-px)^2+(oy-py)^2); ic<-which.max(dist); w<-max(1,ic-18):min(n,ic+18)
trk<-bind_rows(
  data.frame(x=px[w],y=py[w],src="True path"),
  data.frame(x=kfx[w],y=kfy[w],src="Gaussian Kalman filter"),
  data.frame(x=pf$px[w],y=pf$py[w],src="Particle filter"))
trk$src<-factor(trk$src,levels=c("True path","Gaussian Kalman filter","Particle filter"))
obw<-data.frame(x=ox[w],y=oy[w],outlier=factor(ifelse(out[w]==1,"outlier","normal")))
ggplot()+
  geom_point(data=obw,aes(x,y,shape=outlier),colour=sage,size=1.8)+
  scale_shape_manual(values=c(normal=16,outlier=4),name="Fix")+
  geom_path(data=trk,aes(x,y,colour=src),linewidth=.85)+
  scale_colour_manual(values=c("True path"=forest,"Gaussian Kalman filter"=gold,"Particle filter"=red),name=NULL)+
  coord_equal()+
  labs(title="One bad fix, and least-squares filtering follows it",
       subtitle="The Gaussian filter lurches toward the two largest outliers (crosses);\nthe particle filter holds there, but the smaller ones pull it off course",
       x="Easting (m)", y="Northing (m)")+
  theme_te()+theme(legend.position="top",legend.box="vertical",plot.title.position="plot")

## ---- fig-ess ----
ess<-bind_rows(
  data.frame(step=1:n, ess=pf$ess,       scheme="With resampling (ESS < N/2)"),
  data.frame(step=1:n, ess=pf_nores$ess, scheme="No resampling"))
ggplot(ess,aes(step,ess,colour=scheme))+
  geom_line(linewidth=.8)+ scale_y_log10()+
  scale_colour_manual(values=c("With resampling (ESS < N/2)"=forest,"No resampling"=red),name=NULL)+
  labs(title="Why a particle filter needs resampling",
       subtitle="Without it, effective sample size collapses to one within a few steps",
       x="Time step", y="Effective sample size (log)")+
  theme_te()+theme(legend.position="top")
