# State-space models for animal movement
# https://tidyecology.com/posts/state-space-model-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-09-30

## ---- 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 ----
sim_track <- function(n, gamma, sigma, tau, seed){
  set.seed(seed)
  vx <- numeric(n); vy <- numeric(n)
  vx[1] <- rnorm(1, 0, sigma/sqrt(1-gamma^2))   # stationary start
  vy[1] <- rnorm(1, 0, sigma/sqrt(1-gamma^2))
  for(t in 2:n){
    vx[t] <- gamma*vx[t-1] + rnorm(1,0,sigma)
    vy[t] <- gamma*vy[t-1] + rnorm(1,0,sigma)
  }
  px <- cumsum(vx); py <- cumsum(vy)
  ox <- px + rnorm(n,0,tau); oy <- py + rnorm(n,0,tau)
  list(px=px, py=py, ox=ox, oy=oy)
}
n <- 400; gamma_t <- 0.7; sigma_t <- 4; tau_t <- 6
tr <- sim_track(n, gamma_t, sigma_t, tau_t, seed=6041)

## ---- kalman ----
kf_coord <- function(y, g, s2, h2){
  n <- length(y)
  Tm <- matrix(c(1,0, g,g), 2,2)          # rows: (1, g), (0, g)
  Q  <- s2*matrix(c(1,1,1,1),2,2)
  Z  <- matrix(c(1,0),1,2)
  a  <- c(y[1], 0)                         # condition on first fix
  P  <- matrix(c(h2,0,0, s2/(1-g^2)),2,2)
  ll <- 0
  af <- matrix(NA,n,2); Pf <- vector("list",n)
  ap <- matrix(NA,n,2); Pp <- vector("list",n)
  af[1,]<-a; Pf[[1]]<-P; ap[1,]<-a; Pp[[1]]<-P
  for(t in 2:n){
    ap_t <- as.vector(Tm %*% a)
    Pp_t <- Tm %*% P %*% t(Tm) + Q
    v  <- y[t] - as.vector(Z %*% ap_t)     # prediction error (innovation)
    Ft <- as.vector(Z %*% Pp_t %*% t(Z)) + h2
    K  <- as.vector(Pp_t %*% t(Z)) / Ft    # Kalman gain
    a  <- ap_t + K*v
    P  <- Pp_t - (K %*% Z) %*% Pp_t
    ll <- ll - 0.5*(log(2*pi)+log(Ft)+v^2/Ft)
    af[t,]<-a; Pf[[t]]<-P; ap[t,]<-ap_t; Pp[[t]]<-Pp_t
  }
  list(ll=ll, af=af, Pf=Pf, ap=ap, Pp=Pp, Tm=Tm)
}

rts <- function(fk){                        # Rauch-Tung-Striebel smoother
  n <- nrow(fk$af); Tm <- fk$Tm
  as <- fk$af; Ps <- fk$Pf
  for(t in (n-1):1){
    Pp1 <- fk$Pp[[t+1]]
    C <- fk$Pf[[t]] %*% t(Tm) %*% solve(Pp1)
    as[t,] <- fk$af[t,] + as.vector(C %*% (as[t+1,]-fk$ap[t+1,]))
    Ps[[t]] <- fk$Pf[[t]] + C %*% (Ps[[t+1]]-Pp1) %*% t(C)
  }
  list(as=as, Ps=Ps)
}

## ---- fit ----
nll <- function(par, ox, oy){
  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)
}
start <- c(qlogis(0.5), log(var(diff(tr$ox))/2), log(var(diff(tr$ox))/2))
fit <- optim(start, nll, ox=tr$ox, oy=tr$oy, method="Nelder-Mead",
             control=list(maxit=2000, reltol=1e-10))
g_hat   <- plogis(fit$par[1])
sig_hat <- sqrt(exp(fit$par[2]))
tau_hat <- sqrt(exp(fit$par[3]))
c(gamma=round(g_hat,3), sigma=round(sig_hat,3), tau=round(tau_hat,3))

## ---- smooth ----
fkx <- kf_coord(tr$ox, g_hat, sig_hat^2, tau_hat^2); smx <- rts(fkx)
fky <- kf_coord(tr$oy, g_hat, sig_hat^2, tau_hat^2); smy <- rts(fky)
sx <- smx$as[,1]; sy <- smy$as[,1]

## ---- fig-track ----
w <- 120:220
obs1 <- data.frame(x=tr$ox[w], y=tr$oy[w])
lin1 <- bind_rows(data.frame(x=tr$px[w],y=tr$py[w],src="True path"),
                  data.frame(x=sx[w], y=sy[w], src="Smoothed estimate"))
lin1$src <- factor(lin1$src, levels=c("True path","Smoothed estimate"))
ggplot() +
  geom_point(data=obs1, aes(x,y), colour=sage, size=1.1, alpha=.85) +
  geom_path(data=lin1, aes(x,y,colour=src), linewidth=.85) +
  scale_colour_manual(values=c("True path"=forest,"Smoothed estimate"=red), name=NULL) +
  coord_equal() +
  labs(title="A tracking dataset is not the animal's true path",
       subtitle="Fixes scatter around the latent track; the smoother pulls them back",
       x="Easting (m)", y="Northing (m)") +
  theme_te() + theme(legend.position="top")

## ---- metrics ----
plen <- function(x,y) sum(sqrt(diff(x)^2+diff(y)^2))
turn_Rbar <- function(x,y){
  h <- atan2(diff(y),diff(x))
  ta <- diff(h); ta <- atan2(sin(ta),cos(ta))
  abs(mean(exp(1i*ta)))
}
pl_true <- plen(tr$px,tr$py); pl_obs <- plen(tr$ox,tr$oy); pl_sm <- plen(sx,sy)
Rb_true <- turn_Rbar(tr$px,tr$py); Rb_obs <- turn_Rbar(tr$ox,tr$oy); Rb_sm <- turn_Rbar(sx,sy)
rmse <- function(ex,ey) sqrt(mean((ex-tr$px)^2+(ey-tr$py)^2))
rmse_obs <- rmse(tr$ox,tr$oy); rmse_sm <- rmse(sx,sy)

## ---- fig-metrics ----
steps <- function(x,y) sqrt(diff(x)^2+diff(y)^2)
turns <- function(x,y){h<-atan2(diff(y),diff(x)); ta<-diff(h); atan2(sin(ta),cos(ta))}
f2 <- bind_rows(
  data.frame(value=steps(tr$px,tr$py), src="True",     metric="Step length (m)"),
  data.frame(value=steps(tr$ox,tr$oy), src="Observed", metric="Step length (m)"),
  data.frame(value=steps(sx,sy),       src="Smoothed", metric="Step length (m)"),
  data.frame(value=turns(tr$px,tr$py), src="True",     metric="Turning angle (rad)"),
  data.frame(value=turns(tr$ox,tr$oy), src="Observed", metric="Turning angle (rad)"),
  data.frame(value=turns(sx,sy),       src="Smoothed", metric="Turning angle (rad)"))
f2$src <- factor(f2$src, levels=c("True","Observed","Smoothed"))
ggplot(f2, aes(value, colour=src)) +
  geom_density(linewidth=.8) +
  facet_wrap(~metric, scales="free") +
  scale_colour_manual(values=c(True=forest, Observed=red, Smoothed=gold), name=NULL) +
  labs(title="Why raw movement metrics mislead",
       subtitle="Error inflates apparent steps and randomises apparent turns",
       x=NULL, y="Density") +
  theme_te() + theme(legend.position="top")
