An R package with a Particle Filter implemented in C to estimate the position and velocity of a moving object tracked by passive sensors.
We introduce the bearing-only tracking problem for a moving vehicle. For this R package, which comes with a real-life dataset, we impement in C a Particle Filter to estimate the position and velocity of a moving object tracked by passive sensors based on a non-linear state-space model with known parameters. The details of the model, the Sequential Importance Resampling algorithm, and an example are described in the vignette.
devtools::install_github("luisdamiano/TrackingParticles")
In Summer 2014, a tractor harvested an experimental agricultural site codenamed Interim, which is located at the Neal Smith National Wildlife Refuge. The vehicle was equipped with a yield monitor, a common device for precision agriculture that records several quantities of interests generated by sensors in real time. We assume the existence of two passive sensors situated at the coordinates 41°33'22.9"N 93°14'58.1"W and 41°33'27.6"N 93°14'51.1"W tracking solely this target. Every second, each sensor would record the horizontal angle in radians in the direction of the moving target and itself. The dataset contains a total of 11027 pairs of observations.
> head(vehicle)
a1 a2
1 -1.660934 -2.375897
2 -1.668618 -2.378755
3 -1.674765 -2.380928
4 -1.679765 -2.382801
5 -1.683469 -2.384178
6 -1.683469 -2.384178
library(TrackingParticles)
# Set up model constants and known values ---------------------------------
nParticles <- 100
# Time step in seconds
dt <- 1
# Location of sensors 1 and 2
s1 <- c(x = -93.2494663765932, y = 41.5563518606521)
s2 <- c(x = -93.2475338232000, y = 41.5576632356000)
# Measurement model parameters
sr <- 0.01
# State model parameters
q1 <- 0.0005
q2 <- 0.0005
# State model priors
statepriorMu <- c(-93.24952047, 41.55575337)
statepriorDiag <- c(5.0E-09, 3.5E-08, 5.0E-04, 5.0E-04)
# Importance distribution parameters
importanceDiag <- 0.0025 * c(5.00E-10, 1.75E-08, 5.00E-05, 5.00E-05)
# Run ---------------------------------------------------------------------
res <- particle_filter(
vehicle,
dt,
s1, s2,
sr,
q1, q2,
statepriorMu, statepriorDiag,
importanceDiag,
nParticles
)
print(str(res))
# Package comes with a basic visualization routine ------------------------
plot(res, pch = 16, col = "darkgray", cex = 0.3)See the vignette for an extended example.
Simo Sarkka. 2013. "Bayesian Filtering and Smoothing". Cambridge University Press. [http://users.aalto.fi/~ssarkka/pub/cup_book_online_20131111.pdf](Read online).