# Regularising irregular animal tracks
# https://tidyecology.com/posts/regularising-animal-tracks/
#
# 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-08

## ---- 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))
}

## ---- kfs ----
kfs_irw <- function(times, y, sig2, tau2){
  n <- length(times)
  Amk <- function(dt) matrix(c(1,0,dt,1),2,2)
  Qmk <- function(dt) sig2*matrix(c(dt^3/3, dt^2/2, dt^2/2, dt),2,2)
  Z <- matrix(c(1,0),1,2)
  a <- c(if(!is.na(y[1])) y[1] else 0, 0)
  P <- matrix(c(1e6,0,0,1e2),2,2)                # vague prior
  ll <- 0
  af<-matrix(NA,n,2); Pf<-vector("list",n); ap<-matrix(NA,n,2); Pp<-vector("list",n); Al<-vector("list",n)
  for(t in 1:n){
    if(t==1){ A<-diag(2); Q<-matrix(0,2,2) } else { dt<-times[t]-times[t-1]; A<-Amk(dt); Q<-Qmk(dt) }
    ap_t<-as.vector(A%*%a); Pp_t<-A%*%P%*%t(A)+Q
    Al[[t]]<-A; ap[t,]<-ap_t; Pp[[t]]<-Pp_t
    if(!is.na(y[t])){
      v<-y[t]-as.vector(Z%*%ap_t); Ft<-as.vector(Z%*%Pp_t%*%t(Z))+tau2
      K<-as.vector(Pp_t%*%t(Z))/Ft
      a<-ap_t+K*v; P<-Pp_t-(K%*%Z)%*%Pp_t
      if(t>1) ll<-ll-0.5*(log(2*pi)+log(Ft)+v^2/Ft)
    } else { a<-ap_t; P<-Pp_t }                   # gap: predict only
    af[t,]<-a; Pf[[t]]<-P
  }
  as<-af; Ps<-Pf                                  # RTS smoother
  for(t in (n-1):1){
    A1<-Al[[t+1]]; Pp1<-Pp[[t+1]]; C<-Pf[[t]]%*%t(A1)%*%solve(Pp1)
    as[t,]<-af[t,]+as.vector(C%*%(as[t+1,]-ap[t+1,]))
    Ps[[t]]<-Pf[[t]]+C%*%(Ps[[t+1]]-Pp1)%*%t(C)
  }
  list(ll=ll, sm_p=as[,1], sm_psd=sapply(Ps, function(M) sqrt(max(M[1,1],0))))
}

## ---- simulate ----
set.seed(8143)
sig_t <- 1.0; tau_t <- 4.0
dt0 <- 0.25; Tmax <- 100; fg <- seq(0, Tmax, by=dt0); nf <- length(fg)
simcoord <- function(){
  p<-numeric(nf); v<-numeric(nf); p[1]<-0; v[1]<-rnorm(1,0,1)
  for(i in 2:nf){
    Q<-sig_t^2*matrix(c(dt0^3/3,dt0^2/2,dt0^2/2,dt0),2,2)
    e<-MASS::mvrnorm(1,c(0,0),Q); v[i]<-v[i-1]+e[2]; p[i]<-p[i-1]+dt0*v[i-1]+e[1]
  }
  p
}
px<-simcoord(); py<-simcoord()

set.seed(8144)
elig <- which(fg > 0 & fg < Tmax & !(fg >= 44 & fg <= 58))   # silent window
keep <- sort(sample(elig, 56)); keep <- c(1, keep, nf)
ot <- fg[keep]
ox <- px[keep] + rnorm(length(keep),0,tau_t)
oy <- py[keep] + rnorm(length(keep),0,tau_t)
n_obs <- length(ot); gap_med <- median(diff(ot)); gap_max <- max(diff(ot))
c(n_obs=n_obs, median_gap=round(gap_med,2), max_gap=round(gap_max,2))

## ---- fit ----
nll <- function(par){ s2<-exp(par[1]); h2<-exp(par[2])
  -(kfs_irw(ot,ox,s2,h2)$ll + kfs_irw(ot,oy,s2,h2)$ll) }
fit <- optim(c(log(1),log(16)), nll, method="Nelder-Mead", control=list(reltol=1e-10,maxit=3000))
sig_h<-sqrt(exp(fit$par[1])); tau_h<-sqrt(exp(fit$par[2]))
c(sigma=round(sig_h,3), tau=round(tau_h,3))

## ---- predict ----
grid <- seq(0, Tmax, by=1.0)
utimes <- sort(unique(c(ot, grid)))
yx <- rep(NA_real_, length(utimes)); yx[match(ot, utimes)] <- ox
yy <- rep(NA_real_, length(utimes)); yy[match(ot, utimes)] <- oy
smx <- kfs_irw(utimes, yx, sig_h^2, tau_h^2)
smy <- kfs_irw(utimes, yy, sig_h^2, tau_h^2)
gi <- match(grid, utimes)
gx<-smx$sm_p[gi]; gy<-smy$sm_p[gi]; gsd<-smx$sm_psd[gi]
nx <- approx(ot, ox, xout=grid)$y; ny <- approx(ot, oy, xout=grid)$y   # naive interpolation
tx <- px[match(grid, fg)]; ty <- py[match(grid, fg)]
rmse<-function(ex,ey) sqrt(mean((ex-tx)^2+(ey-ty)^2))
rmse_naive<-rmse(nx,ny); rmse_sm<-rmse(gx,gy)
c(rmse_naive=round(rmse_naive,2), rmse_smoother=round(rmse_sm,2),
  sd_min=round(min(gsd),2), sd_max=round(max(gsd),2))

## ---- fig-track ----
tr<-data.frame(x=px,y=py); sm<-data.frame(x=smx$sm_p, y=smy$sm_p); ob<-data.frame(x=ox,y=oy)
ggplot()+
  geom_path(data=tr,aes(x,y),colour=sage,linewidth=.5)+
  geom_path(data=sm,aes(x,y),colour=red,linewidth=.8)+
  geom_point(data=ob,aes(x,y),colour=forest,size=1.3)+
  coord_equal()+
  labs(title="From scattered, irregular fixes to a continuous track",
       subtitle="True path (sage), fixes (green), smoother reconstruction (red)",
       x="Easting (m)", y="Northing (m)")+theme_te()+
  theme(plot.title.position="plot")

## ---- fig-gap ----
gi_df<-data.frame(t=grid, e=gx, sd=gsd, ni=nx)
win<-dplyr::filter(gi_df, t>=30 & t<=72)
obw<-dplyr::filter(data.frame(t=ot,e=ox), t>=30 & t<=72)
truw<-dplyr::filter(data.frame(t=fg,e=px), t>=30 & t<=72)
ggplot()+
  geom_ribbon(data=win,aes(t,ymin=e-1.96*sd,ymax=e+1.96*sd),fill=red,alpha=.15)+
  geom_line(data=truw,aes(t,e),colour=sage,linewidth=.6)+
  geom_line(data=win,aes(t,ni),colour=gold,linewidth=.7,linetype="longdash")+
  geom_line(data=win,aes(t,e),colour=red,linewidth=.8)+
  geom_point(data=obw,aes(t,e),colour=forest,size=1.6)+
  labs(title="Uncertainty is honest across a reporting gap",
       subtitle="Smoother (red) with 95% band vs naive interpolation (gold dashed); truth in sage",
       x="Time", y="Easting (m)")+theme_te()

## ---- identifiability ----
taus <- seq(2.0, 6.5, by=0.25)
prof <- function(t_ot,t_ox,t_oy) sapply(taus, function(tt){
  f<-optimise(function(ls){ s2<-exp(ls)
    -(kfs_irw(t_ot,t_ox,s2,tt^2)$ll + kfs_irw(t_ot,t_oy,s2,tt^2)$ll) }, c(log(1e-3),log(30)))
  -f$objective })
pll <- prof(ot,ox,oy)
set.seed(8145)
thin <- sort(c(1, sample(2:(n_obs-1), 20), n_obs))
pll2 <- prof(ot[thin], ox[thin], oy[thin])
w_full <- range(taus[pll  >= max(pll)-2])
w_thin <- range(taus[pll2 >= max(pll2)-2])
rbind(full=round(w_full,2), thinned=round(w_thin,2))
