R/cphd.R
In ick003/vesselett: A suite of target tracking algorithm, with a specialization for marine radar
Defines functions
gm_cphd_tracker
Documented in
gm_cphd_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 display_tracking logical: Plot timestep tracking estimates (TRUE or FALSE)
#' @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 nMax numeric: Maximum number of potential targets at any given time.
#' @param cutDist numeric: distance for which an association is deemed impossible
#' @param return.intensity logical: Returns all the parameters required to calculate the PHD (TRUE or FALSE)
#' @details The GM CPHD 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{Pollard2011}{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_cphd_tracker(obs_x, varnames = c("north","east"), T_s = 1, wt =0.5, kappa = 1e-2,
#' position_only = TRUE, sigma2_measure = 1e-2, sigma2_process = 1e-1, P_B = 0.1, P_D = 0.9,
#' P_S = .7, tau = 1e-2, cutDist = 0.5, nMax= 6, init_field = init_field,check = FALSE,
#' associationOption = "Pollard2011")
#' plot(res)
#'
#' 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(aes(x = unique(Tracks$time), y = as.matrix(res$Cardinality) %*% t(t(c(0:6)))),
#' col = "blue")
#'}
#' @export
gm_cphd_tracker <- function(x,
varnames,
type = "constant-velocity-model", init_field = NULL, birth_field = NULL,
mergingOption = "Panta2009", associationOption = "Panta2009",nMax = 20,display_tracking = FALSE,
position_only = FALSE, T_s, sigma2_process, sigma2_measure, P_S, Jmax = 20,
kappa, P_D, tau, U, wt, w, P_Sp, P_B, cutDist, 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}
if(missing(cutDist)){cutDist = 1}
estimate_velocity = FALSE
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.numeric(nMax))
assert_that(is.flag(display_tracking))
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.numeric(cutDist))
assert_that(is.flag(return.intensity))
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
nu_k = rep(1/(nMax+1), nMax + 1)
T_k = as.data.frame(t(do.call(cbind, lapply(zeta_init, function(z) z$m))))
T_k$weight = do.call(c, lapply(zeta_init, function(z) z$w))
T_k$target_id = do.call(c, lapply(zeta_init, function(z) z$l))
T_k$score = 0
T_k$time = 0
T_k$status = "init"
colnames(T_k)[1:dimSS] <- varnames
T_k$observed = 0
Tracks = NULL
Cards = NULL
km = min(scan_id) - T_s
for(k in scan_id){
#if(check){browser()}
#zeta_km = zeta_tracks
#zeta_km = zeta_kt
#zeta_km = zeta_kk
zeta_km = zeta_k
nu_km = nu_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 | length(zeta_birth)>0){
res_kkm = cphd_mixture_prediction(varnames, dimSS, dimObs, radar_k, zeta_km, nu_km, zeta_birth, T_s, sigma2_process, P_S, w, P_Sp, P_B, type, nMax)
zeta_kkm = res_kkm$zeta
nu_kkm = res_kkm$nu
}else{zeta_kkm = NULL}
# Update step
#print("update")
if(missing(kappa)){
kappa = 1/max(radar_k$radar_range)^2
}
if(length(zeta_kkm) > 0){
res_kk = cphd_mixture_update(varnames, dimSS, dimObs, varepsilon_k,zeta_kkm, nu_kkm, kappa = kappa, sigma2_measure, P_D, position_only, estimate_velocity, T_s, zeta_h = zeta_km, radar_range = radar_k$radar_range, nMax = nMax, cutDist)
zeta_kk = res_kk$zeta
nu_kk = res_kk$nu
}else{zeta_kk = NULL}
#do.call(c, lapply(zeta_kk, function(z) z$w))
# Pruning step
#print("Pruning")
if(length(zeta_kk) > 0){
res_k = cphd_component_pruning_merging(varnames, dimSS, dimObs, zeta_kk, nu_kk, tau = tau, U = U, Jmax = Jmax, mergingOption = mergingOption)
zeta_k = res_k$zeta
nu_k = res_k$nu
}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))}
# Track Gaussian
#print("Tracks")
# Tracks
if(length(zeta_k)> 0){
zeta_tracks = cphd_mtt_state_number_estimation(varnames, dimSS, dimObs, zeta_k, nu_k, wt, associationOption = associationOption, maxLab = maxLabel, T_km = T_k, P_D, kappa, P_B, cutDist)$zeta
list_target = unique(c(list_target, unique(do.call(c, lapply(zeta_tracks, function(z) z$l)))))
maxLabel = length(list_target)
}else{zeta_tracks = NULL}
if(display_tracking){
if(k == min(scan_id)){
df_init = mutate(plyr::ldply(zeta_init, data.frame), variable = rep(varnames, length(zeta_init)))
df_init_m = mutate(pivot_wider(select(df_init, .data$m, .data$variable, .data$l, .data$w), names_from = .data$variable, values_from = .data$m),
track_init = as.factor(.data$l))
df_init_ell = do.call(rbind.data.frame, lapply(zeta_init, function(z) data.frame(mixtools::ellipse(mu = z$m[1:2], sigma = z$P[1:2, 1:2], draw = FALSE, npoints = 50), l = as.factor(z$l))))
df_zeta_k = mutate(plyr::ldply(zeta_k, data.frame), variable = rep(varnames, length(zeta_k)))
df_zeta_k_m = pivot_wider(select(df_zeta_k, .data$m, .data$variable, .data$l, .data$w), names_from = .data$variable, values_from = .data$m)
df_zeta_k_ell = mutate(do.call(rbind.data.frame, lapply(zeta_k, function(z) data.frame(mixtools::ellipse(mu = z$m[1:2], sigma = z$P[1:2, 1:2], draw = FALSE, npoints = 50)))), l = as.factor(rep(1:length(zeta_k), each = 50)))
df_zeta_tracks = mutate(plyr::ldply(zeta_tracks, data.frame), variable = rep(varnames, length(zeta_tracks)))
df_zeta_tracks_m = pivot_wider(select(df_zeta_tracks, .data$m, .data$variable, .data$l, .data$w), names_from = .data$variable, values_from = .data$m)
p_progress <-ggplot() +
geom_point(data = df_init_m, aes_(x=as.name(varnames[1]), y = as.name(varnames[2]), col = "track_init")) +
geom_point(data = df_zeta_k_m, aes_(x=as.name(varnames[1]), y = as.name(varnames[2])), shape = 15) +
geom_polygon(data = df_zeta_k_ell, aes_string(x = "X1", y ="X2", group = "l"), alpha = 0.1) +
geom_point(data = varepsilon_k, aes_(x=as.name(varnames[1]), y = as.name(varnames[2])), col = "orange", shape = 17, size=3)+
geom_point(data = df_zeta_tracks_m, aes_(x=as.name(varnames[1]), y = as.name(varnames[2]), col = "l"), shape = 7) +
theme_bw()
} else{
df_zeta_k = mutate(plyr::ldply(zeta_k, data.frame), variable = rep(varnames, length(zeta_k)))
df_zeta_k_m = pivot_wider(select(df_zeta_k, .data$m, .data$variable, .data$l, .data$w), names_from = .data$variable, values_from = .data$m)
df_zeta_k_ell = mutate(do.call(rbind.data.frame, lapply(zeta_k, function(z) data.frame(mixtools::ellipse(mu = z$m[1:2], sigma = z$P[1:2, 1:2], draw = FALSE, npoints = 50)))), l = as.factor(rep(1:length(zeta_k), each = 50)))
df_zeta_tracks = mutate(plyr::ldply(zeta_tracks, data.frame), variable = rep(varnames, length(zeta_tracks)))
df_zeta_tracks_m = pivot_wider(select(df_zeta_tracks, .data$m, .data$variable, .data$l, .data$w), names_from = .data$variable, values_from = .data$m)
p_progress <- ggplot() +
geom_path(data = drop_na(Tracks, .data$target_id), aes_(x=as.name(varnames[1]), y = as.name(varnames[2])))+
geom_point(data = drop_na(Tracks, .data$target_id), aes_(x=as.name(varnames[1]), y = as.name(varnames[2])), size = 0.2)+ # estimated
geom_point(data = df_zeta_k_m, aes_(x=as.name(varnames[1]), y = as.name(varnames[2])), shape = 4) + # gaussian center
geom_polygon(data = df_zeta_k_ell, aes_string(x = "X1", y ="X2", group = "l"), alpha=0.1) + # gaussian ellipse
geom_point(data = varepsilon_k, aes_(x=as.name(varnames[1]), y = as.name(varnames[2])), col = "orange", shape = 17, size=3)+ # observation
geom_point(data = df_zeta_tracks_m, aes_(x=as.name(varnames[1]), y = as.name(varnames[2])), shape = 15) + # predicted chosen
theme_bw()
}
print(p_progress)
readline(prompt = "Pause. Press <Enter> to continue...")
}
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$score = do.call(c, lapply(zeta_tracks, function(z) z$score))
T_k$time = k
colnames(T_k)[1:length(varnames)] <- 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){
idxM = do.call(c, lapply(zeta_tracks, function(z) z$measure))
T_k[idxM > 0,paste0("o_",varnames[1:dimObs])] = varepsilon_k[idxM,varnames[1:dimObs]]
idxM = idxM[idxM != 0]
}
if(length(idxM)<nrow(varepsilon_k)){
new_row_t = data.frame(time = rep(k,nrow(varepsilon_k)-length(idxM)),
observed = TRUE,
target_id = NA,
status = "clutter")
new_row_t[paste0("o_",varnames[1:dimObs])] <- with(new_row_t, NA)
new_row_t[,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_t)
}
Tracks = dplyr::bind_rows(Tracks, T_k)
Cards = dplyr::bind_rows(Cards, as.data.frame(t(nu_k)))
} else{
T_k = data.frame(time = rep(k,nrow(varepsilon_k)), observed = TRUE, target_id = NA, status = "clutter")
T_k[paste0("o_",varnames)] <- with(T_k, NA)
T_k[,paste0("o_",varnames)] = varepsilon_k[,varnames]
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, Cardinality = Cards)
if(return.intensity){
RET = list(Tracks = Tracks, Cardinality = Cards, intensity = intensity)
}
class(RET) <- "cphd"
attr(RET, "type") <- type
attr(RET, "varnames") <- varnames
return(RET)
}
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_cphd_tracker
Documented in gm_cphd_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 display_tracking logical: Plot timestep tracking estimates (TRUE or FALSE)
#' @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 nMax numeric: Maximum number of potential targets at any given time.
#' @param cutDist numeric: distance for which an association is deemed impossible
#' @param return.intensity logical: Returns all the parameters required to calculate the PHD (TRUE or FALSE)
#' @details The GM CPHD 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{Pollard2011}{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_cphd_tracker(obs_x, varnames = c("north","east"), T_s = 1, wt =0.5, kappa = 1e-2,
#' position_only = TRUE, sigma2_measure = 1e-2, sigma2_process = 1e-1, P_B = 0.1, P_D = 0.9,
#' P_S = .7, tau = 1e-2, cutDist = 0.5, nMax= 6, init_field = init_field,check = FALSE,
#' associationOption = "Pollard2011")
#' plot(res)
#'
#' 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(aes(x = unique(Tracks$time), y = as.matrix(res$Cardinality) %*% t(t(c(0:6)))),
#' col = "blue")
#'}
#' @export
gm_cphd_tracker <- function(x,
varnames,
type = "constant-velocity-model", init_field = NULL, birth_field = NULL,
mergingOption = "Panta2009", associationOption = "Panta2009",nMax = 20,display_tracking = FALSE,
position_only = FALSE, T_s, sigma2_process, sigma2_measure, P_S, Jmax = 20,
kappa, P_D, tau, U, wt, w, P_Sp, P_B, cutDist, 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}
if(missing(cutDist)){cutDist = 1}
estimate_velocity = FALSE
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.numeric(nMax))
assert_that(is.flag(display_tracking))
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.numeric(cutDist))
assert_that(is.flag(return.intensity))
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
nu_k = rep(1/(nMax+1), nMax + 1)
T_k = as.data.frame(t(do.call(cbind, lapply(zeta_init, function(z) z$m))))
T_k$weight = do.call(c, lapply(zeta_init, function(z) z$w))
T_k$target_id = do.call(c, lapply(zeta_init, function(z) z$l))
T_k$score = 0
T_k$time = 0
T_k$status = "init"
colnames(T_k)[1:dimSS] <- varnames
T_k$observed = 0
Tracks = NULL
Cards = NULL
km = min(scan_id) - T_s
for(k in scan_id){
#if(check){browser()}
#zeta_km = zeta_tracks
#zeta_km = zeta_kt
#zeta_km = zeta_kk
zeta_km = zeta_k
nu_km = nu_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 | length(zeta_birth)>0){
res_kkm = cphd_mixture_prediction(varnames, dimSS, dimObs, radar_k, zeta_km, nu_km, zeta_birth, T_s, sigma2_process, P_S, w, P_Sp, P_B, type, nMax)
zeta_kkm = res_kkm$zeta
nu_kkm = res_kkm$nu
}else{zeta_kkm = NULL}
# Update step
#print("update")
if(missing(kappa)){
kappa = 1/max(radar_k$radar_range)^2
}
if(length(zeta_kkm) > 0){
res_kk = cphd_mixture_update(varnames, dimSS, dimObs, varepsilon_k,zeta_kkm, nu_kkm, kappa = kappa, sigma2_measure, P_D, position_only, estimate_velocity, T_s, zeta_h = zeta_km, radar_range = radar_k$radar_range, nMax = nMax, cutDist)
zeta_kk = res_kk$zeta
nu_kk = res_kk$nu
}else{zeta_kk = NULL}
#do.call(c, lapply(zeta_kk, function(z) z$w))
# Pruning step
#print("Pruning")
if(length(zeta_kk) > 0){
res_k = cphd_component_pruning_merging(varnames, dimSS, dimObs, zeta_kk, nu_kk, tau = tau, U = U, Jmax = Jmax, mergingOption = mergingOption)
zeta_k = res_k$zeta
nu_k = res_k$nu
}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))}
# Track Gaussian
#print("Tracks")
# Tracks
if(length(zeta_k)> 0){
zeta_tracks = cphd_mtt_state_number_estimation(varnames, dimSS, dimObs, zeta_k, nu_k, wt, associationOption = associationOption, maxLab = maxLabel, T_km = T_k, P_D, kappa, P_B, cutDist)$zeta
list_target = unique(c(list_target, unique(do.call(c, lapply(zeta_tracks, function(z) z$l)))))
maxLabel = length(list_target)
}else{zeta_tracks = NULL}
if(display_tracking){
if(k == min(scan_id)){
df_init = mutate(plyr::ldply(zeta_init, data.frame), variable = rep(varnames, length(zeta_init)))
df_init_m = mutate(pivot_wider(select(df_init, .data$m, .data$variable, .data$l, .data$w), names_from = .data$variable, values_from = .data$m),
track_init = as.factor(.data$l))
df_init_ell = do.call(rbind.data.frame, lapply(zeta_init, function(z) data.frame(mixtools::ellipse(mu = z$m[1:2], sigma = z$P[1:2, 1:2], draw = FALSE, npoints = 50), l = as.factor(z$l))))
df_zeta_k = mutate(plyr::ldply(zeta_k, data.frame), variable = rep(varnames, length(zeta_k)))
df_zeta_k_m = pivot_wider(select(df_zeta_k, .data$m, .data$variable, .data$l, .data$w), names_from = .data$variable, values_from = .data$m)
df_zeta_k_ell = mutate(do.call(rbind.data.frame, lapply(zeta_k, function(z) data.frame(mixtools::ellipse(mu = z$m[1:2], sigma = z$P[1:2, 1:2], draw = FALSE, npoints = 50)))), l = as.factor(rep(1:length(zeta_k), each = 50)))
df_zeta_tracks = mutate(plyr::ldply(zeta_tracks, data.frame), variable = rep(varnames, length(zeta_tracks)))
df_zeta_tracks_m = pivot_wider(select(df_zeta_tracks, .data$m, .data$variable, .data$l, .data$w), names_from = .data$variable, values_from = .data$m)
p_progress <-ggplot() +
geom_point(data = df_init_m, aes_(x=as.name(varnames[1]), y = as.name(varnames[2]), col = "track_init")) +
geom_point(data = df_zeta_k_m, aes_(x=as.name(varnames[1]), y = as.name(varnames[2])), shape = 15) +
geom_polygon(data = df_zeta_k_ell, aes_string(x = "X1", y ="X2", group = "l"), alpha = 0.1) +
geom_point(data = varepsilon_k, aes_(x=as.name(varnames[1]), y = as.name(varnames[2])), col = "orange", shape = 17, size=3)+
geom_point(data = df_zeta_tracks_m, aes_(x=as.name(varnames[1]), y = as.name(varnames[2]), col = "l"), shape = 7) +
theme_bw()
} else{
df_zeta_k = mutate(plyr::ldply(zeta_k, data.frame), variable = rep(varnames, length(zeta_k)))
df_zeta_k_m = pivot_wider(select(df_zeta_k, .data$m, .data$variable, .data$l, .data$w), names_from = .data$variable, values_from = .data$m)
df_zeta_k_ell = mutate(do.call(rbind.data.frame, lapply(zeta_k, function(z) data.frame(mixtools::ellipse(mu = z$m[1:2], sigma = z$P[1:2, 1:2], draw = FALSE, npoints = 50)))), l = as.factor(rep(1:length(zeta_k), each = 50)))
df_zeta_tracks = mutate(plyr::ldply(zeta_tracks, data.frame), variable = rep(varnames, length(zeta_tracks)))
df_zeta_tracks_m = pivot_wider(select(df_zeta_tracks, .data$m, .data$variable, .data$l, .data$w), names_from = .data$variable, values_from = .data$m)
p_progress <- ggplot() +
geom_path(data = drop_na(Tracks, .data$target_id), aes_(x=as.name(varnames[1]), y = as.name(varnames[2])))+
geom_point(data = drop_na(Tracks, .data$target_id), aes_(x=as.name(varnames[1]), y = as.name(varnames[2])), size = 0.2)+ # estimated
geom_point(data = df_zeta_k_m, aes_(x=as.name(varnames[1]), y = as.name(varnames[2])), shape = 4) + # gaussian center
geom_polygon(data = df_zeta_k_ell, aes_string(x = "X1", y ="X2", group = "l"), alpha=0.1) + # gaussian ellipse
geom_point(data = varepsilon_k, aes_(x=as.name(varnames[1]), y = as.name(varnames[2])), col = "orange", shape = 17, size=3)+ # observation
geom_point(data = df_zeta_tracks_m, aes_(x=as.name(varnames[1]), y = as.name(varnames[2])), shape = 15) + # predicted chosen
theme_bw()
}
print(p_progress)
readline(prompt = "Pause. Press <Enter> to continue...")
}
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$score = do.call(c, lapply(zeta_tracks, function(z) z$score))
T_k$time = k
colnames(T_k)[1:length(varnames)] <- 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){
idxM = do.call(c, lapply(zeta_tracks, function(z) z$measure))
T_k[idxM > 0,paste0("o_",varnames[1:dimObs])] = varepsilon_k[idxM,varnames[1:dimObs]]
idxM = idxM[idxM != 0]
}
if(length(idxM)<nrow(varepsilon_k)){
new_row_t = data.frame(time = rep(k,nrow(varepsilon_k)-length(idxM)),
observed = TRUE,
target_id = NA,
status = "clutter")
new_row_t[paste0("o_",varnames[1:dimObs])] <- with(new_row_t, NA)
new_row_t[,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_t)
}
Tracks = dplyr::bind_rows(Tracks, T_k)
Cards = dplyr::bind_rows(Cards, as.data.frame(t(nu_k)))
} else{
T_k = data.frame(time = rep(k,nrow(varepsilon_k)), observed = TRUE, target_id = NA, status = "clutter")
T_k[paste0("o_",varnames)] <- with(T_k, NA)
T_k[,paste0("o_",varnames)] = varepsilon_k[,varnames]
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, Cardinality = Cards)
if(return.intensity){
RET = list(Tracks = Tracks, Cardinality = Cards, intensity = intensity)
}
class(RET) <- "cphd"
attr(RET, "type") <- type
attr(RET, "varnames") <- varnames
return(RET)
}
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.