gm_phd_intensity: Intensity function for the GM PHD filter

In ick003/vesselett: A suite of target tracking algorithm, with a specialization for marine radar

Description

Intensity function for the GM PHD filter

Usage

gm_phd_intensity(zeta, z)

Arguments

zeta	List: list of gaussian mixture, with at least three elements (weight, mean vector, covariance matrix)
z	vector: Location p to calculate the filter intensity at.

Value

The intensity value of the filter, at the given location z.

Examples

## Not run: 
library(tidyverse)
library(ggrepel)
data(maritime2)

obs_x <- maritime2

res = gm_phd_tracker(obs_x, wt = 1e-3, sigma2_m = 1e-2,sigma2_v = 1e-3, sigma2_p = 1e-2,
kappa = 1e-8, P_B = 0.1, tau = 1e-4, T_s = 60, return.intensity = TRUE)
zeta_test = res$intensity[[8]]$zeta_tracks
ztest = obs_x[obs_x$time == res$intensity[[8]]$time, c("north", "east", "v_north", "v_east")]
Dk = gm_phd_intensity(zeta = zeta_test, z = ztest)

rtest = obs_x[obs_x$time == res$intensity[[8]]$time, c("radar_north", "radar_east")][1,]
zstest = cbind(create.scan.grid(c(rtest$radar_north, rtest$radar_east),
range = 8e3, precision = 30),0,0)

Dsk <- cbind(zstest, gm_phd_intensity(zeta= zeta_test, z= zstest))
colnames(Dsk) <- c("north", "east", "v_north", "v_east", "D")

ggplot(Dsk, aes(x = east, y = north)) +
geom_tile(aes(fill = D)) +
geom_point(data = rtest, aes(y = radar_north, x = radar_east), col = "white", shape = 3) +
geom_hline(data = rtest,
aes(yintercept = radar_north), col = "darkgreen", size = 0.5, alpha = 0.75)+
geom_vline(data = rtest,
aes(xintercept = radar_east), col = "darkgreen", size = 0.5, alpha = 0.75)+
geom_point(data = ztest %>% dplyr::filter(north > -5055000),
 aes(y = north, x = east), col = "red", size = 0.2) +
scale_fill_gradient(trans = 'log', na.value = "black", low = 'black',high = 'white') +
theme(panel.border = element_blank(),
    panel.grid.major = element_blank(),panel.grid.minor = element_blank(),
    panel.background = element_rect(fill = "white", colour = NA),
    legend.position = 'none',
    axis.line = element_line(size = 0.5, linetype = "solid", colour = "black"))

data(player_tracking)
trackx <- player_tracking %>% dplyr::filter(between(time,1,20))
init_field = list(list(m = c(1.5,2.5,0,0), P = diag(.1, nrow = 4), w = 1, l = 1),
list(m = c(2.25,2.75,0,0), P = diag(.1, nrow = 4), w = 1, l = 2),
 list(m = c(3.25,2.5,0,0), P = diag(.1, nrow = 4), w = 1, l = 3),
                  list(m = c(1.5,1,0,0), P = diag(.1, nrow = 4), w = 1, l = 4),
                  list(m = c(1.25,1.75,0,0), P = diag(.1, nrow = 4), w = 1, l = 5),
                  list(m = c(2.75,2,0,0), P = diag(.1, nrow = 4), w = 1, l = 6))
res = gm_phd_tracker(trackx, type = "non-linear" ,T_s = 0.05, wt =0.1, kappa = 2e-1,
sigma2_m = 1e-2, sigma2_p = 1e-1, sigma2_v = 1e-4, P_B = 0, P_D = 0.7,P_S = 1, tau = 1e-2,
U = 0.1, init_field = init_field, return.intensity = TRUE)
zeta_test = res$intensity[[1]]$zeta_init
ztest = trackx[trackx$time == res$intensity[[1]]$time, c("north", "east", "v_north", "v_east")]
Dk = gm_phd_intensity(zeta = zeta_test, z = ztest)

rtest = trackx[trackx$time == res$intensity[[1]]$time, c("radar_north", "radar_east")][1,]
zstest = cbind(create.scan.grid(c(rtest$radar_north, rtest$radar_east),
               range = 2.5, precision = 0.01), 0,0)

Dsk <- cbind(zstest, gm_phd_intensity(zeta= zeta_test, z= zstest))
colnames(Dsk) <- c("north", "east", "v_north", "v_east", "D")

ggplot(Dsk %>% dplyr::filter(between(east,0,4) & between(north, 0.5,3.75)),
aes(x = east, y = north)) +
geom_tile(aes(fill = D)) +
geom_point(data = ztest, aes(y = north, x = east), col = "red", size = 2) +
scale_fill_gradient(na.value = "white", low = 'white',high = 'black') +
datavolley::ggcourt("lower", labels = "")+
theme(legend.position = 'none')


## End(Not run)

ick003/vesselett documentation built on July 20, 2020, 9:08 p.m.