R/phd_ett.R

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

#' Wrapper function to strt the phd_ett algorithm
#' @param x data.frame: requires time, utm position north and east (m), utm speed north and east (m/s), heading, and size of the target.
#' @param type string: linear or non-linear filter
#' @param param list: List of starting parameters
#' @param T_s scalar:
#' @param sigma2_p scalar:
#' @param sigma2_v scalar:
#' @param sigma2_m scalar:
#' @param P_S scalar:
#' @param kappa scalar:
#' @param P_D scalar:
#' @param tau scalar:
#' @param U scalar:
#' @param wt scalar:
#' @param w scalar:
#' @param P_Sp scalar:
#' @param P_B scalar:
#' @param Nbirth scalar:
#' @param birthRegion string:
#' @param tauD scalar:
#' @references
#' \insertRef{Fowdur2019}{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{
#' x <- create_tracking_history(target_type = "extended")
#' phd_ett(x)
#' }
#'
#' @export
gm_phd_ett_tracker <- function(x, type = "linear", birthRegion = "detected",param, T_s, sigma2_p, sigma2_v, sigma2_m, P_S, Nbirth, kappa, P_D, tau, U,  wt, w, P_Sp, P_B, tauD){

    if(missing(param)){
        param =  list(N0 = 150, m0 = matrix(0, ncol = 1, nrow = 4), P0 = 10*diag(c(1,1,0.1,.1)), w0 = 1e-2)
    }

    if(missing(T_s)){T_s = 1}
    if(missing(sigma2_p)){sigma2_p = 10}
    if(missing(sigma2_v)){sigma2_v = 1}
    if(missing(sigma2_m)){sigma2_m = .1}
    if(missing(P_S)){P_S = 0.99}

    if(missing(P_D)){P_D = 0.99}
    if(missing(tau)){tau = 1e-7}
    if(missing(U)){U = 15}
    if(missing(Nbirth)){Nbirth = 9}

    if(missing(w)){w = 0.1}
    if(missing(wt)){wt = 0.5}
    if(missing(P_Sp)){P_Sp = 0.01}
    if(missing(P_B)){P_B = 0.05}

    scan_id = sort(unique(x$time))

    # Initial scan
    varepsilon_0 = x[x$time == min(scan_id),c("generation_rate", "north", "east", "v_north", "v_east")]
    radar_0 =  unique(x[x$time == min(scan_id),c("radar_north", "radar_east", "radar_range")])

    if(birthRegion == "fixed"){
        zeta = list(NULL)
        for(i in 1:param$N0){
            zeta[[i]] = list(alpha = param$alpha0, beta = param$beta0,
                             m = t(mvtnorm::rmvnorm(1, matrix(c(radar_0$radar_north, radar_0$radar_east,0,0), ncol = 1),sigma = param$P0)), P = param$P0, w = param$w0)
        }
    }
    if(birthRegion == "detected"){
        Nbirth = nrow(varepsilon_0)
        zeta = list(NULL)
        for(i in 1:Nbirth){
            zeta[[i]] = list(m =  unlist(varepsilon_0[i,c("north", "east", "v_north", "v_east")]),
                             P = param$P0,
                             w = P_B)
        }
    }

    zeta_tracks = phd_ett_mixture_initialisation(radar_0,zeta = zeta)$zeta

    maxLabel = max(do.call(c,lapply(zeta_tracks, function(z) z$l)))
    Tracks = NULL
    km = min(scan_id) - T_s
    for(k in scan_id){
        #print(k)

        zeta_km = zeta_tracks


        radar_k = unique(x[x$time == k,c("radar_north", "radar_east", "radar_range")])

        varepsilon_k = x[x$time == k,c("generation_rate", "north", "east", "v_north", "v_east")]

        # Prediction step

        T_s = k - km

        zeta_birth = phd_ett_mixture_birth(varepsilon_k, radar_k, birthRegion, param, P_B, U, zeta_km)$zeta

        zeta_kkm = phd_ett_mixture_prediction(radar_k, zeta_km, zeta_birth, T_s, sigma2_p, sigma2_v, P_S, w, P_Sp, maxLabel, type)$zeta


        # Update step
        #print("update")
        if(missing(kappa)){
            kappa = 1/max(radar_k$radar_range)^2
        }
        zeta_kk = phd_ett_mixture_update(varepsilon_k,zeta_kkm, kappa = kappa, sigma2_m, P_D)$zeta

        # Pruning step
        #print("Pruning")

        zeta_k = phd_ett_component_pruning(zeta_kk, tau = tau)$zeta

        if(length(zeta_k) == 0){ next }

        # Merging Gaussian components

        # print("Merging")

        zeta_k = phd_ett_component_merging(zeta_k, U)$zeta

        # Track Gaussian
        #print("Tracks")

        # Tracks
        zeta_tracks = phd_ett_mtt_state_number_estimation(zeta_k, wt)$zeta

        if(length(zeta_tracks)>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$time = k
            colnames(T_k)[1:4] <- c("north", "east", "v_north", "v_east")
            Tracks = rbind(Tracks, T_k)
        } else{
            zeta_tracks = zeta_k
        }
        maxLabel = max(maxLabel, do.call(c,lapply(zeta_tracks, function(z) z$l)))

        km = k

    }

    return(list(Tracks = Tracks))
}