R/phd.R
In ick003/vesselett: A suite of target tracking algorithm, with a specialization for marine radar
Defines functions
gm_phd_intensity
gm_phd_tracker
Documented in
gm_phd_intensity
gm_phd_tracker
#' Wrapper function to the Gaussian Mixture PHD tracker
#' @param x data.frame: requires time, utm position north and east (m), utm speed north and east (m/s).
#' @param varnames vector: vector of names. Default to "north", "east", "v_north", "v_east".
#' @param type string: random-walk, constant-velocity-model or coordinated-turn-model (Type of kinematic)
#' @param init_field list: List of starting parameters
#' @param birth_field list: List of birth parameters
#' @param T_s scalar: Time interval to start the filter
#' @param sigma2_process scalar, vector or matrix: Process variance of the kinematic movement
#' @param sigma2_measure scalar, vector or matrix: Measurement variance
#' @param P_S scalar: Probability of survival
#' @param kappa scalar: Intensity of clutter
#' @param P_D scalar: Probability of detection
#' @param tau scalar: Pruning threshold
#' @param U scalar: Merging threshold
#' @param wt scalar: Track confirmation threshold
#' @param w scalar:
#' @param P_Sp scalar: Probability of spawning
#' @param P_B scalar: Probabilty of birth
#' @param mergingOption string: How to merge the measures and create (or continue) the labeled tracks
#' @param associationOption string: How to associate the new measures with the old tracks
#' @param position_only logical: Position only observed (TRUE or FALSE)
#' @param Jmax numeric: Maximum number of Gaussian mixtures carried over to the following time step
#' @param return.intensity logical: Returns all the parameters required to calculate the PHD (TRUE or FALSE)
#' @details The GM PHD tracker that is implemented here proposes multiple options to choose from in
#' terms of kinematic type (linear vs non-linear), data association options (through merging option and association optiob, defaulted both to Panta2009),
#' and radar precision (position only vs position and velocity).
#' @references
#' \insertRef{Panta2009}{vesselett}
#' @return data.frame with an additional column indication the target id, unique for the entire
#' tracking history, and a few additional kinematic columns
#' @examples
#' \dontrun{
#' library(tidyverse)
#' library(ggrepel)
#' data(maritime2)
#' obs_x <- mutate(maritime2, time = time / 60, radar_north = (radar_north - min(north))/1000,
#' radar_east = (radar_east - min(east))/1000, radar_range = radar_range/1000,
#' north = (north - min(north))/1000, east = (east-min(east))/1000,
#' ) %>% filter(between(time, 150,250))
#' plot_obs(obs_x, varnames = c("north", "east"))
#' mI = unlist(obs_x[obs_x$time == min(obs_x$time),c("north", "east", "v_north", "v_east")])
#' init_field = list(list(m = mI, P = diag(1, nrow = 4), w = 1, l = "1"))
#' res = gm_phd_tracker(obs_x, wt = 5e-1, sigma2_measure = 1e-2,
#' sigma2_process = 1e-1, kappa = 1e-10, tau = 1e-2, T_s = 1, check = FALSE,
#' init_field = init_field, P_B = 0.1, P_D = 0.9, P_S = 0.9)
#' Tracks = res$Tracks
#' Tracks <- left_join(Tracks,
#' unique(obs_x %>% select(time, radar_east, radar_north, radar_range)),
#' by = "time") %>% arrange(time)
#' tracksP = Tracks %>%
#' select(north, east, time, target_id, radar_north, radar_east, radar_range) %>%
#' pivot_longer(c(north, east))
#' ggplot() +
#' geom_point(data = tracksP ,
#' aes(x = time, y = value, col = as.factor(target_id)), size = 1, shape = 3)+
#' geom_line(data = tracksP ,
#' aes(x = time, y = value, group = target_id),col = "black", size = 0.5) +
#' facet_wrap( ~ name, ncol = 1, strip.position="left", scales = "free") +
#' 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"))
#'
#' ggplot() + geom_point(data = obs_x %>% group_by(time, target_id) %>%
#' summarize(n = n()) %>%
#' ungroup() %>% group_by(time) %>% summarise(n =n()), aes(x = time, y = n)) +
#' geom_path(data = Tracks %>% group_by(time) %>%
#' summarize(n = n()), aes(x = time, y = n), col ="red", size = 0.5) +
#' geom_path(data = Tracks %>% group_by(time) %>%
#' summarize(n = round(sum(weight, na.rm=TRUE))), aes(x = time, y = n), col ="blue", size = 0.5)+
#' geom_point(data = Tracks %>% group_by(time) %>%
#' summarize(n = round(sum(weight, na.rm=TRUE))), aes(x = time, y = n), col ="blue", size=1)
#'}
#' @export
gm_phd_tracker <- function(x, varnames, type = "constant-velocity-model", init_field = NULL, birth_field = NULL,
mergingOption = "Panta2009", associationOption = "Panta2009",
position_only = FALSE, T_s, sigma2_process, sigma2_measure, P_S, Jmax = 20,
kappa, P_D, tau, U, wt, w, P_Sp, P_B, return.intensity = FALSE){
if(missing(T_s)){T_s = 1}
if(missing(varnames)){
varnames = c("north", "east", "v_north", "v_east")
}
dimObs = length(varnames)
if(position_only){
varnames = c(varnames, paste0("v_",varnames))
}
dimSS = length(varnames)
if(missing(sigma2_process)){sigma2_process = 1}
if(missing(sigma2_measure)){sigma2_measure = 1}
if(missing(P_S)){P_S = 0.99}
if(missing(P_D)){P_D = 0.99}
if(missing(tau)){tau = 1e-2}
if(missing(U)){U = 10}
if(missing(w)){w = 0.1}
if(missing(wt)){wt = 0.5}
if(missing(P_Sp)){P_Sp = 0}
if(missing(P_B)){P_B = 0.05}
assert_that(is.string(type))
type <- match.arg(tolower(type), c("random-walk", "constant-velocity-model", "coordinated-turn-model"))
assert_that(is.string(mergingOption))
mergingOption <- match.arg(mergingOption, c("Panta2009", "Vo2009"))
assert_that(is.string(associationOption))
associationOption <- match.arg(associationOption, c("Panta2009", "Munkres", "Pollard2011"))
assert_that(is.flag(position_only))
assert_that(is.numeric(T_s))
assert_that(is.numeric(P_S), between(P_S,0,1))
assert_that(is.numeric(Jmax))
assert_that(is.numeric(kappa))
assert_that(is.numeric(P_D), between(P_D,0,1))
assert_that(is.numeric(tau))
assert_that(is.numeric(U))
assert_that(is.numeric(wt))
assert_that(is.numeric(w))
assert_that(is.numeric(P_Sp), between(P_Sp,0,1))
assert_that(is.numeric(P_B), between(P_B,0,1))
assert_that(is.flag(return.intensity))
estimate_velocity = FALSE
scan_id = sort(unique(x$time))
# Initial scan
varepsilon_0 = x[x$time == min(scan_id),varnames[1:dimObs]]
radar_0 = unique(x[x$time == min(scan_id),c("radar_north", "radar_east", "radar_range")])
zeta_init = phd_mixture_initialisation(varnames, dimSS, dimObs, radar_0,varepsilon_0, init_field)$zeta
intensity = NULL
if(return.intensity){
intensity[[1]] = list(time = min(scan_id),
zeta_init = zeta_init)
}
list_target = unique(do.call(c, lapply(zeta_init, function(z) z$l)))
maxLabel = length(list_target)
#zeta_kt = zeta_init
zeta_k = zeta_init
Tracks = NULL
T_k = NULL
km = min(scan_id) - T_s
for(k in scan_id){
#zeta_km = zeta_tracks
#zeta_km = zeta_kt
#zeta_km = zeta_kk
zeta_km = zeta_k
radar_k = unique(x[x$time == k,c("radar_north", "radar_east", "radar_range")])
varepsilon_k = x[x$time == k,varnames[1:dimObs]]
# Prediction step
T_s = k - km
if(P_B>0){
zeta_birth = phd_mixture_birth(varnames, dimSS, dimObs, varepsilon_k, birth_field, P_B, zeta_km, position_only, maxLabel)$zeta
}else{zeta_birth = list(NULL)}
if(length(zeta_km) > 0){
zeta_kkm = phd_mixture_prediction(varnames, dimSS, dimObs, radar_k, zeta_km, zeta_birth, T_s, sigma2_process, P_S, w, P_Sp, type)$zeta
}else{zeta_kkm = NULL}
# Update step
#print("update")
if(missing(kappa)){
kappa = 1/max(radar_k$radar_range)^2
}
if(length(zeta_kkm) > 0){
zeta_kk = phd_mixture_update(varnames, dimSS, dimObs, varepsilon_k,zeta_kkm, kappa = kappa, sigma2_measure, P_D, position_only, estimate_velocity, T_s, zeta_h = zeta_km)$zeta
}else{zeta_kk = NULL}
#do.call(c, lapply(zeta_kk, function(z) z$w))
# Pruning step
#print("Pruning")
if(length(zeta_kk) > 0){
zeta_k = phd_component_pruning(varnames, dimSS, dimObs, zeta_kk, tau = tau, Jmax = Jmax)$zeta
}else{zeta_k = NULL}
#if(length(zeta_k) > 0){maxLabel = max(maxLabel, max(do.call(c, lapply(zeta_k, function(z) z$l)), na.rm=TRUE))}
# Merging Gaussian components
# print("Merging")
if(length(zeta_k)> 0){
zeta_kt = phd_component_merging(varnames, dimSS, dimObs, zeta_k, U, mergingOption, Tracks)
}
#do.call(c, lapply(zeta_k, function(z) z$l))
# Track Gaussian
#print("Tracks")
# Tracks
if(length(zeta_k)> 0){
zeta_tracks = phd_mtt_state_number_estimation(varnames, dimSS, dimObs, zeta_kt, wt, associationOption = associationOption, maxLab = maxLabel, T_km = T_k)$zeta
list_target = unique(c(list_target, unique(do.call(c, lapply(zeta_tracks, function(z) z$l)))))
maxLabel = length(list_target)
#maxLabel = max(do.call(c, lapply(zeta_tracks, function(z) z$l)), na.rm=TRUE)
}else{zeta_tracks = NULL}
# if(check){browser()}
if(length(zeta_tracks)>0){
idxM = do.call(c, lapply(zeta_tracks, function(z) z$measure))
idxM = idxM[idxM != 0]
T_k = as.data.frame(t(do.call(cbind, lapply(zeta_tracks, function(z) z$m))))
T_k$weight = do.call(c, lapply(zeta_tracks, function(z) z$w))
T_k$target_id = do.call(c, lapply(zeta_tracks, function(z) z$l))
T_k$status = do.call(c, lapply(zeta_tracks, function(z) z$status))
T_k$time = k
colnames(T_k)[1:dimSS] <- varnames
T_k$observed = do.call(c, lapply(zeta_tracks, function(z) z$o))
T_k[paste0("o_",varnames[1:dimObs])] <- with(T_k, NA)
if(length(idxM)>0){
T_k[T_k$status != "not detected",paste0("o_",varnames[1:dimObs])] = varepsilon_k[idxM,varnames[1:dimObs]]
}
if(length(idxM)<nrow(varepsilon_k)){
new_row = data.frame(time = rep(k,nrow(varepsilon_k)-length(idxM)),
observed = TRUE,
target_id = NA,
status = "clutter")
new_row[paste0("o_",varnames[1:dimObs])] <- with(new_row, NA)
new_row[,paste0("o_",varnames[1:dimObs])] <- varepsilon_k[setdiff(1:nrow(varepsilon_k), idxM),varnames[1:dimObs]]
T_k = dplyr::bind_rows(T_k, new_row)
}
Tracks = dplyr::bind_rows(Tracks, T_k)
} else{
T_k = data.frame(time = rep(k,nrow(varepsilon_k)), observed = TRUE, target_id = NA, status = "clutter")
T_k[paste0("o_",varnames[1:dimObs])] <- with(T_k, NA)
T_k[,paste0("o_",varnames[1:dimObs])] = varepsilon_k[,varnames[1:dimObs]]
Tracks = dplyr::bind_rows(Tracks, T_k)
#zeta_tracks = zeta_k
}
km = k
if(return.intensity){
intensity[[length(intensity) + 1]] = list(time = k,
zeta_tracks = zeta_tracks)
}
}
RET = list(Tracks = Tracks)
if(return.intensity){
RET = list(Tracks = Tracks, intensity = intensity)
}
class(RET) <- "phd"
attr(RET, "type") <- type
attr(RET, "varnames") <- varnames
return(RET)
}
#' Intensity function for the GM PHD filter
#' @param zeta List: list of gaussian mixture, with at least three elements (weight, mean vector, covariance matrix)
#' @param z vector: Location p to calculate the filter intensity at.
#' @return The intensity value of the filter, at the given location z.
#' @examples
#' \dontrun{
#' 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')
#'
#' }
#' @export
gm_phd_intensity <- function(zeta, z){
J_i = length(zeta)
D_i = rep(0, nrow(z))
for(ji in 1:J_i){
D_i_tmp = zeta[[ji]]$w * mvtnorm::dmvnorm(x = z, mean = zeta[[ji]]$m, sigma = zeta[[ji]]$P)
D_i = D_i + D_i_tmp
}
return(D = D_i)
}
ick003/vesselett documentation built on July 20, 2020, 9:08 p.m.
R Package Documentation
Browse R Packages
We want your feedback!
Note that we can't provide technical support on individual packages. You should contact the package authors for that.
Defines functions gm_phd_intensity gm_phd_tracker
Documented in gm_phd_intensity gm_phd_tracker
#' Wrapper function to the Gaussian Mixture PHD tracker
#' @param x data.frame: requires time, utm position north and east (m), utm speed north and east (m/s).
#' @param varnames vector: vector of names. Default to "north", "east", "v_north", "v_east".
#' @param type string: random-walk, constant-velocity-model or coordinated-turn-model (Type of kinematic)
#' @param init_field list: List of starting parameters
#' @param birth_field list: List of birth parameters
#' @param T_s scalar: Time interval to start the filter
#' @param sigma2_process scalar, vector or matrix: Process variance of the kinematic movement
#' @param sigma2_measure scalar, vector or matrix: Measurement variance
#' @param P_S scalar: Probability of survival
#' @param kappa scalar: Intensity of clutter
#' @param P_D scalar: Probability of detection
#' @param tau scalar: Pruning threshold
#' @param U scalar: Merging threshold
#' @param wt scalar: Track confirmation threshold
#' @param w scalar:
#' @param P_Sp scalar: Probability of spawning
#' @param P_B scalar: Probabilty of birth
#' @param mergingOption string: How to merge the measures and create (or continue) the labeled tracks
#' @param associationOption string: How to associate the new measures with the old tracks
#' @param position_only logical: Position only observed (TRUE or FALSE)
#' @param Jmax numeric: Maximum number of Gaussian mixtures carried over to the following time step
#' @param return.intensity logical: Returns all the parameters required to calculate the PHD (TRUE or FALSE)
#' @details The GM PHD tracker that is implemented here proposes multiple options to choose from in
#' terms of kinematic type (linear vs non-linear), data association options (through merging option and association optiob, defaulted both to Panta2009),
#' and radar precision (position only vs position and velocity).
#' @references
#' \insertRef{Panta2009}{vesselett}
#' @return data.frame with an additional column indication the target id, unique for the entire
#' tracking history, and a few additional kinematic columns
#' @examples
#' \dontrun{
#' library(tidyverse)
#' library(ggrepel)
#' data(maritime2)
#' obs_x <- mutate(maritime2, time = time / 60, radar_north = (radar_north - min(north))/1000,
#' radar_east = (radar_east - min(east))/1000, radar_range = radar_range/1000,
#' north = (north - min(north))/1000, east = (east-min(east))/1000,
#' ) %>% filter(between(time, 150,250))
#' plot_obs(obs_x, varnames = c("north", "east"))
#' mI = unlist(obs_x[obs_x$time == min(obs_x$time),c("north", "east", "v_north", "v_east")])
#' init_field = list(list(m = mI, P = diag(1, nrow = 4), w = 1, l = "1"))
#' res = gm_phd_tracker(obs_x, wt = 5e-1, sigma2_measure = 1e-2,
#' sigma2_process = 1e-1, kappa = 1e-10, tau = 1e-2, T_s = 1, check = FALSE,
#' init_field = init_field, P_B = 0.1, P_D = 0.9, P_S = 0.9)
#' Tracks = res$Tracks
#' Tracks <- left_join(Tracks,
#' unique(obs_x %>% select(time, radar_east, radar_north, radar_range)),
#' by = "time") %>% arrange(time)
#' tracksP = Tracks %>%
#' select(north, east, time, target_id, radar_north, radar_east, radar_range) %>%
#' pivot_longer(c(north, east))
#' ggplot() +
#' geom_point(data = tracksP ,
#' aes(x = time, y = value, col = as.factor(target_id)), size = 1, shape = 3)+
#' geom_line(data = tracksP ,
#' aes(x = time, y = value, group = target_id),col = "black", size = 0.5) +
#' facet_wrap( ~ name, ncol = 1, strip.position="left", scales = "free") +
#' 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"))
#'
#' ggplot() + geom_point(data = obs_x %>% group_by(time, target_id) %>%
#' summarize(n = n()) %>%
#' ungroup() %>% group_by(time) %>% summarise(n =n()), aes(x = time, y = n)) +
#' geom_path(data = Tracks %>% group_by(time) %>%
#' summarize(n = n()), aes(x = time, y = n), col ="red", size = 0.5) +
#' geom_path(data = Tracks %>% group_by(time) %>%
#' summarize(n = round(sum(weight, na.rm=TRUE))), aes(x = time, y = n), col ="blue", size = 0.5)+
#' geom_point(data = Tracks %>% group_by(time) %>%
#' summarize(n = round(sum(weight, na.rm=TRUE))), aes(x = time, y = n), col ="blue", size=1)
#'}
#' @export
gm_phd_tracker <- function(x, varnames, type = "constant-velocity-model", init_field = NULL, birth_field = NULL,
mergingOption = "Panta2009", associationOption = "Panta2009",
position_only = FALSE, T_s, sigma2_process, sigma2_measure, P_S, Jmax = 20,
kappa, P_D, tau, U, wt, w, P_Sp, P_B, return.intensity = FALSE){
if(missing(T_s)){T_s = 1}
if(missing(varnames)){
varnames = c("north", "east", "v_north", "v_east")
}
dimObs = length(varnames)
if(position_only){
varnames = c(varnames, paste0("v_",varnames))
}
dimSS = length(varnames)
if(missing(sigma2_process)){sigma2_process = 1}
if(missing(sigma2_measure)){sigma2_measure = 1}
if(missing(P_S)){P_S = 0.99}
if(missing(P_D)){P_D = 0.99}
if(missing(tau)){tau = 1e-2}
if(missing(U)){U = 10}
if(missing(w)){w = 0.1}
if(missing(wt)){wt = 0.5}
if(missing(P_Sp)){P_Sp = 0}
if(missing(P_B)){P_B = 0.05}
assert_that(is.string(type))
type <- match.arg(tolower(type), c("random-walk", "constant-velocity-model", "coordinated-turn-model"))
assert_that(is.string(mergingOption))
mergingOption <- match.arg(mergingOption, c("Panta2009", "Vo2009"))
assert_that(is.string(associationOption))
associationOption <- match.arg(associationOption, c("Panta2009", "Munkres", "Pollard2011"))
assert_that(is.flag(position_only))
assert_that(is.numeric(T_s))
assert_that(is.numeric(P_S), between(P_S,0,1))
assert_that(is.numeric(Jmax))
assert_that(is.numeric(kappa))
assert_that(is.numeric(P_D), between(P_D,0,1))
assert_that(is.numeric(tau))
assert_that(is.numeric(U))
assert_that(is.numeric(wt))
assert_that(is.numeric(w))
assert_that(is.numeric(P_Sp), between(P_Sp,0,1))
assert_that(is.numeric(P_B), between(P_B,0,1))
assert_that(is.flag(return.intensity))
estimate_velocity = FALSE
scan_id = sort(unique(x$time))
# Initial scan
varepsilon_0 = x[x$time == min(scan_id),varnames[1:dimObs]]
radar_0 = unique(x[x$time == min(scan_id),c("radar_north", "radar_east", "radar_range")])
zeta_init = phd_mixture_initialisation(varnames, dimSS, dimObs, radar_0,varepsilon_0, init_field)$zeta
intensity = NULL
if(return.intensity){
intensity[[1]] = list(time = min(scan_id),
zeta_init = zeta_init)
}
list_target = unique(do.call(c, lapply(zeta_init, function(z) z$l)))
maxLabel = length(list_target)
#zeta_kt = zeta_init
zeta_k = zeta_init
Tracks = NULL
T_k = NULL
km = min(scan_id) - T_s
for(k in scan_id){
#zeta_km = zeta_tracks
#zeta_km = zeta_kt
#zeta_km = zeta_kk
zeta_km = zeta_k
radar_k = unique(x[x$time == k,c("radar_north", "radar_east", "radar_range")])
varepsilon_k = x[x$time == k,varnames[1:dimObs]]
# Prediction step
T_s = k - km
if(P_B>0){
zeta_birth = phd_mixture_birth(varnames, dimSS, dimObs, varepsilon_k, birth_field, P_B, zeta_km, position_only, maxLabel)$zeta
}else{zeta_birth = list(NULL)}
if(length(zeta_km) > 0){
zeta_kkm = phd_mixture_prediction(varnames, dimSS, dimObs, radar_k, zeta_km, zeta_birth, T_s, sigma2_process, P_S, w, P_Sp, type)$zeta
}else{zeta_kkm = NULL}
# Update step
#print("update")
if(missing(kappa)){
kappa = 1/max(radar_k$radar_range)^2
}
if(length(zeta_kkm) > 0){
zeta_kk = phd_mixture_update(varnames, dimSS, dimObs, varepsilon_k,zeta_kkm, kappa = kappa, sigma2_measure, P_D, position_only, estimate_velocity, T_s, zeta_h = zeta_km)$zeta
}else{zeta_kk = NULL}
#do.call(c, lapply(zeta_kk, function(z) z$w))
# Pruning step
#print("Pruning")
if(length(zeta_kk) > 0){
zeta_k = phd_component_pruning(varnames, dimSS, dimObs, zeta_kk, tau = tau, Jmax = Jmax)$zeta
}else{zeta_k = NULL}
#if(length(zeta_k) > 0){maxLabel = max(maxLabel, max(do.call(c, lapply(zeta_k, function(z) z$l)), na.rm=TRUE))}
# Merging Gaussian components
# print("Merging")
if(length(zeta_k)> 0){
zeta_kt = phd_component_merging(varnames, dimSS, dimObs, zeta_k, U, mergingOption, Tracks)
}
#do.call(c, lapply(zeta_k, function(z) z$l))
# Track Gaussian
#print("Tracks")
# Tracks
if(length(zeta_k)> 0){
zeta_tracks = phd_mtt_state_number_estimation(varnames, dimSS, dimObs, zeta_kt, wt, associationOption = associationOption, maxLab = maxLabel, T_km = T_k)$zeta
list_target = unique(c(list_target, unique(do.call(c, lapply(zeta_tracks, function(z) z$l)))))
maxLabel = length(list_target)
#maxLabel = max(do.call(c, lapply(zeta_tracks, function(z) z$l)), na.rm=TRUE)
}else{zeta_tracks = NULL}
# if(check){browser()}
if(length(zeta_tracks)>0){
idxM = do.call(c, lapply(zeta_tracks, function(z) z$measure))
idxM = idxM[idxM != 0]
T_k = as.data.frame(t(do.call(cbind, lapply(zeta_tracks, function(z) z$m))))
T_k$weight = do.call(c, lapply(zeta_tracks, function(z) z$w))
T_k$target_id = do.call(c, lapply(zeta_tracks, function(z) z$l))
T_k$status = do.call(c, lapply(zeta_tracks, function(z) z$status))
T_k$time = k
colnames(T_k)[1:dimSS] <- varnames
T_k$observed = do.call(c, lapply(zeta_tracks, function(z) z$o))
T_k[paste0("o_",varnames[1:dimObs])] <- with(T_k, NA)
if(length(idxM)>0){
T_k[T_k$status != "not detected",paste0("o_",varnames[1:dimObs])] = varepsilon_k[idxM,varnames[1:dimObs]]
}
if(length(idxM)<nrow(varepsilon_k)){
new_row = data.frame(time = rep(k,nrow(varepsilon_k)-length(idxM)),
observed = TRUE,
target_id = NA,
status = "clutter")
new_row[paste0("o_",varnames[1:dimObs])] <- with(new_row, NA)
new_row[,paste0("o_",varnames[1:dimObs])] <- varepsilon_k[setdiff(1:nrow(varepsilon_k), idxM),varnames[1:dimObs]]
T_k = dplyr::bind_rows(T_k, new_row)
}
Tracks = dplyr::bind_rows(Tracks, T_k)
} else{
T_k = data.frame(time = rep(k,nrow(varepsilon_k)), observed = TRUE, target_id = NA, status = "clutter")
T_k[paste0("o_",varnames[1:dimObs])] <- with(T_k, NA)
T_k[,paste0("o_",varnames[1:dimObs])] = varepsilon_k[,varnames[1:dimObs]]
Tracks = dplyr::bind_rows(Tracks, T_k)
#zeta_tracks = zeta_k
}
km = k
if(return.intensity){
intensity[[length(intensity) + 1]] = list(time = k,
zeta_tracks = zeta_tracks)
}
}
RET = list(Tracks = Tracks)
if(return.intensity){
RET = list(Tracks = Tracks, intensity = intensity)
}
class(RET) <- "phd"
attr(RET, "type") <- type
attr(RET, "varnames") <- varnames
return(RET)
}
#' Intensity function for the GM PHD filter
#' @param zeta List: list of gaussian mixture, with at least three elements (weight, mean vector, covariance matrix)
#' @param z vector: Location p to calculate the filter intensity at.
#' @return The intensity value of the filter, at the given location z.
#' @examples
#' \dontrun{
#' 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')
#'
#' }
#' @export
gm_phd_intensity <- function(zeta, z){
J_i = length(zeta)
D_i = rep(0, nrow(z))
for(ji in 1:J_i){
D_i_tmp = zeta[[ji]]$w * mvtnorm::dmvnorm(x = z, mean = zeta[[ji]]$m, sigma = zeta[[ji]]$P)
D_i = D_i + D_i_tmp
}
return(D = D_i)
}
R Package Documentation
Browse R Packages
We want your feedback!
Note that we can't provide technical support on individual packages. You should contact the package authors for that.
Embedding an R snippet on your website
Add the following code to your website.
For more information on customizing the embed code, read Embedding Snippets.