In this example, we consider the tracking of an object in 2D, observed by a radar.

Reference: B. Ristic. Beyond the Kalman filter: Particle filters for Tracking applications. Artech House, 2004.

Statistical model

The statistical model is defined as follows.

Let \(X_t\) be a 4-D vector containing the position and velocity of an object in 2D. We obtain distance-angular measurements \(Y_t\) from a radar.

The model is defined as follows. For \(t=1,\ldots,t_{\max}\)

\[ X_t = F X_{t-1} + G V_t,~~ V_t\sim\mathcal N(0,Q)\]

\[ Y_{t} = g(X_t) + W_t,~~ W_t \sim\mathcal N(0,R)\]

\(F\) and \(G\) are known matrices, \(g(X_t)\) is the known nonlinear measurement function and \(Q\) and \(R\) are known covariance matrices.

Statistical model in BUGS language

model_file = 'hmm_4d_nonlin_tracking.bug' # BUGS model filename
cat(readLines(model_file), sep = "\n")
var v_true[2,t_max-1], x_true[4,t_max], x_radar_true[2,t_max],
v[2,t_max-1], x[4,t_max], x_radar[2,t_max], y[2,t_max]

data
{
  x_true[,1] ~ dmnorm(mean_x_init, prec_x_init)
  x_radar_true[,1] <- x_true[1:2,1] - x_pos 
  mu_y_true[1,1] <- sqrt(x_radar_true[1,1]^2+x_radar_true[2,1]^2)
  mu_y_true[2,1] <- arctan(x_radar_true[2,1]/x_radar_true[1,1])
  y[,1] ~ dmnorm(mu_y_true[,1], prec_y)

  for (t in 2:t_max)
  {
    v_true[,t-1] ~ dmnorm(mean_v, prec_v)
    x_true[,t] <- F %*% x_true[,t-1] + G %*% v_true[,t-1]
    x_radar_true[,t] <- x_true[1:2,t] - x_pos
    mu_y_true[1,t] <- sqrt(x_radar_true[1,t]^2+x_radar_true[2,t]^2)
    mu_y_true[2,t] <- arctan(x_radar_true[2,t]/x_radar_true[1,t])
    y[,t] ~ dmnorm(mu_y_true[,t], prec_y)
  }
}

model
{
  x[,1] ~ dmnorm(mean_x_init, prec_x_init)
  x_radar[,1] <- x[1:2,1] - x_pos
  mu_y[1,1] <- sqrt(x_radar[1,1]^2+x_radar[2,1]^2)
  mu_y[2,1] <- arctan(x_radar[2,1]/x_radar[1,1])
  y[,1] ~ dmnorm(mu_y[,1], prec_y)

  for (t in 2:t_max)
  {
    v[,t-1] ~ dmnorm(mean_v, prec_v)
    x[,t] <- F %*% x[,t-1] + G %*% v[,t-1]
    x_radar[,t] <- x[1:2,t] - x_pos
    mu_y[1,t] <- sqrt(x_radar[1,t]^2+x_radar[2,t]^2)
    mu_y[2,t] <- arctan(x_radar[2,t]/x_radar[1,t])
    y[,t] ~ dmnorm(mu_y[,t], prec_y)
  }
}
  1. Load rbiips package
require(rbiips)

General settings

par(bty='l')
light_blue = rgb(.7, .7, 1)
light_red = rgb(1, .7, .7)

Set the random numbers generator seed for reproducibility

set.seed(5)

Load model and data

Model parameters

t_max = 20
mean_x_init = c(0, 0, 1, 0)
prec_x_init = diag(1000, nrow = 4)
x_pos = c(60, 0)
mean_v = c(0, 0)
prec_v = diag(1, 2)
prec_y = diag(c(100, 500))
delta_t = 1
F = matrix(c(1, 0, delta_t, 0,
             0, 1, 0, delta_t,
             0, 0, 1, 0,
             0, 0, 0, 1), nrow=4, byrow=TRUE)
G = matrix(c(delta_t^2/2, 0,
    0, delta_t^2/2,
    delta_t, 0,
    0, delta_t), nrow=4, byrow=TRUE)

data = list(t_max=t_max, mean_x_init=mean_x_init, prec_x_init=prec_x_init,
            x_pos=x_pos, mean_v=mean_v, prec_v=prec_v, prec_y=prec_y,
            F=F, G=G)

Compile BUGS model and sample data

sample_data = TRUE # Boolean
model = biips_model(model_file, data, sample_data=sample_data) # Create Biips model and sample data
* Parsing model in: hmm_4d_nonlin_tracking.bug
* Compiling data graph
  Declaring variables
  Resolving undeclared variables
  Allocating nodes
  Graph size: 327
  Sampling data
  Reading data back into data table
* Compiling model graph
  Declaring variables
  Resolving undeclared variables
  Allocating nodes
  Graph size: 331
data = model$data()
x_pos_true = data$x_true[1:2,]

Biips Particle filter

Parameters of the algorithm

n_part = 100000 # Number of particles
variables = c('x') # Variables to be monitored

Run SMC

out_smc = biips_smc_samples(model, variables, n_part)
* Assigning node samplers
* Running SMC forward sampler with 100000 particles
  |--------------------------------------------------| 100%
  |**************************************************| 20 iterations in 48.86 s

Diagnostics of the algorithm

diag_smc = biips_diagnosis(out_smc)
* Diagnosis of variable: x[1:4,1:20] 
  Filtering: GOOD
  Smoothing: GOOD

Plot particles

matplot(out_smc$x$f$values[1,,], out_smc$x$f$values[2,,],
        type='p', col='red', pch=16, cex=.2,
        xlab='Position X', ylab='Position Y')
lines(x_pos_true[1,], x_pos_true[2,], type='l', col='green', lwd=2, lty=2)
points(x_pos[1], x_pos[2], col='black', pch=0, lwd=2)
legend('bottomleft', leg=c('Particles (filtering)', 'True trajectory',
                           'Position of the radar'),
       col=c('red', 'green', 'black'),
       lwd=2, pch=c(16,NA,0), lty=c(NA,2,NA),
       bty='n')
SMC: Particles (filtering)

SMC: Particles (filtering)

Summary statistics

summ_smc = biips_summary(out_smc, probs=c(.025, .975))

Plot estimates

x_f_mean = summ_smc$x$f$mean
x_s_mean = summ_smc$x$s$mean

plot(x_f_mean[1,], x_f_mean[2,], type='l', col='blue', lwd=2,
     xlab='Position X', ylab='Position Y')
lines(x_s_mean[1,], x_s_mean[2,], type='l', col='red', lwd=2, lty=4)
lines(x_pos_true[1,], x_pos_true[2,], type='l', col='green', lwd=2, lty=2)
points(x_pos[1], x_pos[2], col='black', pch=0, lwd=2)
legend('bottomleft', leg=c('Filtering estimate', 'Smoothing estimate',
                        'True trajectory', 'Position of the radar'),
       col=c('blue', 'red', 'green', 'black'),
       lwd=2, pch=c(NA,NA,NA,0), lty=c(1,4,2,NA),
       bty='n')

title_fig = c('Position X', 'Position Y', 'Velocity X', 'Velocity Y')
SMC: Filtering and smoothing estimates

SMC: Filtering and smoothing estimates

Plot Filtering estimates

x_f_quant = summ_smc$x$f$quant

for (k in 1:4) {
  xx = c(1:t_max, t_max:1)
  yy = c(x_f_quant[[1]][k,], rev(x_f_quant[[2]][k,]))
  plot(xx, yy, type='n', xlab='Time', ylab=title_fig[k],
       main=title_fig[k])

  polygon(xx, yy, col=light_blue, border=NA)
  lines(1:t_max, x_f_mean[k,], col='blue', lwd=3)
  lines(1:t_max, data$x_true[k,], col='green', lwd=2)
  legend('topleft', leg=c('95% credible interval', 'Filtering mean estimate', 'True value'),
         col=c(light_blue,'blue','green'), lwd=c(NA,3,2), pch=c(15,NA,NA), pt.cex=c(2,1,1), bty='n')
}
SMC: Filtering estimatesSMC: Filtering estimatesSMC: Filtering estimatesSMC: Filtering estimates

SMC: Filtering estimates

Plot Smoothing estimates

x_f_quant = summ_smc$x$f$quant
x_s_mean = summ_smc$x$s$mean
x_s_quant = summ_smc$x$s$quant

for (k in 1:4) {
  xx = c(1:t_max, t_max:1)
  yy = c(x_s_quant[[1]][k,], rev(x_s_quant[[2]][k,]))
  plot(xx, yy, type='n', xlab='Time', ylab=title_fig[k],
       main=title_fig[k])

  polygon(xx, yy, col=light_red, border=NA)
  lines(1:t_max, x_s_mean[k,], col='red', lwd=3)
  lines(1:t_max, data$x_true[k,], col='green', lwd=2)
  legend('topleft', leg=c('95% credible interval', 'Smoothing mean estimate', 'True value'),
         col=c(light_red,'red','green'), lwd=c(NA,3,2), pch=c(15,NA,NA), pt.cex=c(2,1,1), bty='n')
}
SMC: Smoothing estimatesSMC: Smoothing estimatesSMC: Smoothing estimatesSMC: Smoothing estimates

SMC: Smoothing estimates