R/create_tracking_history.R
In ick003/vesselett: A suite of target tracking algorithm, with a specialization for marine radar
Defines functions
create_tracking_history
Documented in
create_tracking_history
#' Wrapper function to strt the JPDAF algorithm
#' @param n integer: number of targets
#' @param nscans integer: number of scans by the radar
#' @param delta_time scalar: time between two scans
#' @param starting_position vector: location for the initial position. If multiple targets, locations are randomly chosen around the vector
#' @param starting_speed vector: speed for the target birth.
#' @param measure_rate scalar: Average number of measure generated by one target for a given scan.
#' @param target_type string: at the moment, only point or extended.
#' @param with_radar logical: create a trajectory for the radar
#' @param prob_missed numeric: Probability of target not being detected, between 0 and 1
#' @param prob_clutter numeric: Probability of false target being detected, between 0 and 1
#' @param prob_ext numeric: Probability of target vanishing
#' @param clutter_intensity integer: Number of potential false targets
#' @param scan_range scalar: Extent of the radar scan, at each time.
#' @param turning_rate scale:
#' @param kinematic_parameters list:
#' @return If type is point, data.frame with time, utm north and east (in meters) and unique target identifier per scan. If extended, returns values for speed, turning rate, size and shape.
#' @examples
#' \dontrun{
#' library(ggplot2)
#' library(ggforce)
#' library(tidyverse)
#'
#' ## Example 1
#' x <- create_tracking_history()
#' ggplot(x$obs_target, aes(x = east, y = north, col = target_id)) +
#' geom_point(size = 0.5) +
#' geom_path(data = x$obs_target %>% dplyr::filter(.data$target_id != "0"))
#'
#' ## Example 2
#' x <- create_tracking_history(n = 1, target_type = "extended", measure_rate = 150)$obs_target
#' ggplot(data = x) +
#' geom_point(aes(x = east, y = north, col = time), size = 1) +
#' theme_bw()
#'
#' }
#'
#' @export
create_tracking_history <- function(n = 2,
nscans = 30,
delta_time = 1,
starting_position = c(0,0),
starting_speed = c(0,0),
measure_rate = 1,
target_type = "point",
with_radar = TRUE,
prob_missed = 0.1,
prob_clutter = 0.1,
clutter_intensity = 0,
scan_range = 200,
turning_rate = 0.1,
prob_ext = 0.9,
kinematic_parameters){
T_s = delta_time
if(missing(kinematic_parameters)){
w = turning_rate
kinematic_parameters = list(
Fmat = matrix(c(1,0,sin(T_s*w)/w, (-1+cos(T_s*w))/w,0,
0,1,(1-cos(T_s*w))/w, sin(T_s*w)/w,0,
0,0,cos(T_s*w), -sin(T_s*w),0,
0,0,sin(T_s*w), cos(T_s*w), 0,
0,0,0,0,1), ncol = 5, byrow = TRUE),
Gmat = matrix(c(T_s^2/2,0,0,
0,T_s^2/2,0,
T_s,0,0,
0,T_s,0,
0,0,T_s),ncol = 3, byrow = TRUE),
sigma2_a = 10e-2,
sigma2_w = (0.1*pi/180*T_s)^2
)
}
Fmat = kinematic_parameters$Fmat
Gmat = kinematic_parameters$Gmat
sigma2_a = kinematic_parameters$sigma2_a
sigma2_w = kinematic_parameters$sigma2_w
obs_radar = NULL
if(with_radar){
Qmat = Gmat %*% diag(c(sigma2_a, sigma2_a, sigma2_w)) %*% t(Gmat)
radar_nscans = seq(1*T_s, nscans*T_s, by = T_s)
scan_id = NULL
for(k in radar_nscans){
if(k == min(radar_nscans)){
x_h = matrix(c(starting_position, starting_speed, 0.01), nrow = 1)}
nMeasures = 1#rpois(1,measure_rate)
scan_id = c(scan_id, rep(k, nMeasures))
if(k == min(radar_nscans)){next}
# Position, Speed, and Turning rate:
w_k = x_h[nrow(x_h),5]
if(nMeasures == 0){next}
x_k = mvtnorm::rmvnorm(nMeasures,Fmat %*% x_h[nrow(x_h),], sigma = Qmat)
x_h = rbind(x_h, x_k)
}
obs_radar = as.data.frame(cbind(x_h, scan_id))
colnames(obs_radar) <- c("radar_north", "radar_east", "radar_v_north", "radar_v_east","turn_rate","time")
}
if(target_type == "point"){
Qmat = Gmat %*% diag(c(sigma2_a, sigma2_a, sigma2_w)) %*% t(Gmat)
obs_target = NULL
target_id = NULL
for(nt in 1:n){
w_0 = 0.1
transition_u = 100
gamma_h = measure_rate
target_id = NULL
scan_id = NULL
start =sample(1:nscans,1, replace = F)
end = start + rbinom(1, size = nscans - start, prob = prob_ext)
target_nscans = seq(start*T_s, end*T_s, by = T_s)
for(k in target_nscans){
if(k == min(target_nscans)){
x_h = matrix(c(runif(1,starting_position[1]- scan_range, starting_position[1] + scan_range),
runif(1,starting_position[2]- scan_range, starting_position[2] + scan_range),
starting_speed, 0.01), nrow = 1)}
nMeasures = 1#rpois(1,measure_rate)
target_id = c(target_id, rep(nt, nMeasures))
scan_id = c(scan_id, rep(k, nMeasures))
if(k == min(target_nscans)){next}
# Position, Speed, and Turning rate:
w_k = x_h[nrow(x_h),5]
if(nMeasures == 0){next}
x_k = mvtnorm::rmvnorm(nMeasures,Fmat %*% x_h[nrow(x_h),], sigma = Qmat)
x_h = rbind(x_h, x_k)
# Measurement rate
gamma_k = rep(gamma_h[length(gamma_h)], nMeasures)
gamma_h = c(gamma_h, gamma_k)
}
obs_target = rbind(obs_target, as.data.frame(cbind(x_h, gamma_h, scan_id, target_id)))
}
colnames(obs_target) <- c("north", "east", "v_north", "v_east", "turn_rate", "generation_rate", "time", "target_id")
clutters = rbinom(nscans,clutter_intensity,prob_clutter)
clutter_coords = NULL
for(k in which(clutters > 0)){
for(kk in 1:clutters[k]){
selectedRow = obs_target[obs_target$time == ((1:nscans)*T_s)[k],]
if(nrow(selectedRow) == 0) {next}
selectedRow = selectedRow[sample(1:nrow(selectedRow), size = 1),]
clutter_coords = rbind(clutter_coords,
cbind(runif(1, selectedRow$north - scan_range,selectedRow$north +scan_range),
runif(1, selectedRow$east - scan_range,selectedRow$east + scan_range),
sample(obs_target$v_north, size = 1),
sample(obs_target$v_east, size = 1),
k, 0)
)
}
}
if(!is.null(clutter_coords)){
clutter_coords <- as.data.frame(clutter_coords)
colnames(clutter_coords) = c("north", "east","v_north", "v_east", "time", "target_id")
clutter_coords$target_id <- as.character(clutter_coords$target_id)
obs_target$target_id <- as.character(obs_target$target_id)
obs_target = dplyr::bind_rows(obs_target, clutter_coords)
} else{
obs_target$target_id <- as.character(obs_target$target_id)
obs_target$target_id <- as.factor(obs_target$target_id)
}
}
if(target_type == "extended"){
Qmat = Gmat %*% diag(c(sigma2_a, sigma2_a, sigma2_w)) %*% t(Gmat)
obs_target = NULL
target_id = NULL
for(nt in 1:n){
w_0 = 0.1
X_h = matrix(c(1,0.5,0.5,1), nrow = 1, byrow= TRUE)
transition_u = 100
gamma_h = measure_rate
target_id = NULL
scan_id = NULL
start =sample(1:nscans,1, replace = F)
end = start + rbinom(1, size = nscans - start, prob = prob_ext)
target_nscans = seq(start*T_s, end*T_s, by = T_s)
for(k in target_nscans){
if(k == min(target_nscans)){
x_h = matrix(c(runif(1,starting_position[1]- scan_range, starting_position[1] + scan_range),
runif(1,starting_position[2]- scan_range, starting_position[2] + scan_range),
starting_speed, 0.01), nrow = 1)
target_id = c(target_id, rep(nt, 1))
scan_id = c(scan_id, rep(k, 1))
}
if(k == min(target_nscans)){next}
nMeasures = rpois(1,measure_rate)
target_id = c(target_id, rep(nt, nMeasures))
scan_id = c(scan_id, rep(k, nMeasures))
# Position, Speed, and Turning rate:
w_k = x_h[nrow(x_h),5]
if(nMeasures == 0){next}
# Extension state
Mx_k = matrix(c(cos(w_k*T_s), -sin(w_k*T_s), sin(w_k*T_s), cos(w_k)*T_s),ncol = 2, byrow = TRUE)
X_h_mat = matrix(X_h[nrow(X_h),], ncol =2, byrow=TRUE)
X_k = rWishart(1, transition_u, 1/transition_u * Mx_k %*% X_h_mat %*% t(Mx_k))
X_k_v = matrix(X_k, ncol = 4, nrow= nMeasures, byrow = TRUE)
X_h = rbind(X_h, X_k_v)
#Multiple measures:
mk = Fmat %*% x_h[nrow(x_h),]
if(mk[3] != 0){
theta = atan(mk[4]/mk[3])
}else{
theta = 0
}
Rm = matrix(c(cos(theta),-sin(theta),sin(theta),cos(theta)), ncol = 2, byrow = TRUE)
Sm = diag(c(X_k_v[1,c(1,4)]))
QmatN = rbind(cbind(Rm %*% Sm %*% Sm %*% solve(Rm), Qmat[1:2,3:5]), Qmat[3:5,])
x_k = mvtnorm::rmvnorm(nMeasures,Fmat %*% x_h[nrow(x_h),], sigma = QmatN)
x_h = rbind(x_h, x_k)
# Measurement rate
gamma_k = rep(gamma_h[length(gamma_h)], nMeasures)
gamma_h = c(gamma_h, gamma_k)
}
obs_target = rbind(obs_target, as.data.frame(cbind(x_h, X_h, gamma_h, scan_id, target_id)))
}
colnames(obs_target) <- c("north", "east", "v_north", "v_east", "turn_rate", "size","ss_1", "shape","ss_2", "generation_rate", "time", "target_id")
clutters = rbinom(nscans,clutter_intensity,prob_clutter)
clutter_coords = NULL
for(k in which(clutters > 0)){
for(kk in 1:clutters[k]){
selectedRow = obs_target[obs_target$time == ((1:nscans)*T_s)[k],]
if(nrow(selectedRow) == 0) {next}
selectedRow = selectedRow[sample(1:nrow(selectedRow), size = 1),]
clutter_coords = rbind(clutter_coords,
cbind(runif(1, selectedRow$north - scan_range,selectedRow$north +scan_range),
runif(1, selectedRow$east - scan_range,selectedRow$east + scan_range),
sample(obs_target$v_north, size = 1),
sample(obs_target$v_east, size = 1),
k, 0)
)
}
}
if(!is.null(clutter_coords)){
clutter_coords <- as.data.frame(clutter_coords)
colnames(clutter_coords) = c("north", "east","v_north", "v_east", "time", "target_id")
clutter_coords$target_id <- as.character(clutter_coords$target_id)
obs_target$target_id <- as.character(obs_target$target_id)
obs_target = dplyr::bind_rows(obs_target, clutter_coords)
} else{
obs_target$target_id <- as.character(obs_target$target_id)
obs_target$target_id <- as.factor(obs_target$target_id)
}
}
return(list(obs_target = obs_target, obs_radar = obs_radar))
}
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 create_tracking_history
Documented in create_tracking_history
#' Wrapper function to strt the JPDAF algorithm
#' @param n integer: number of targets
#' @param nscans integer: number of scans by the radar
#' @param delta_time scalar: time between two scans
#' @param starting_position vector: location for the initial position. If multiple targets, locations are randomly chosen around the vector
#' @param starting_speed vector: speed for the target birth.
#' @param measure_rate scalar: Average number of measure generated by one target for a given scan.
#' @param target_type string: at the moment, only point or extended.
#' @param with_radar logical: create a trajectory for the radar
#' @param prob_missed numeric: Probability of target not being detected, between 0 and 1
#' @param prob_clutter numeric: Probability of false target being detected, between 0 and 1
#' @param prob_ext numeric: Probability of target vanishing
#' @param clutter_intensity integer: Number of potential false targets
#' @param scan_range scalar: Extent of the radar scan, at each time.
#' @param turning_rate scale:
#' @param kinematic_parameters list:
#' @return If type is point, data.frame with time, utm north and east (in meters) and unique target identifier per scan. If extended, returns values for speed, turning rate, size and shape.
#' @examples
#' \dontrun{
#' library(ggplot2)
#' library(ggforce)
#' library(tidyverse)
#'
#' ## Example 1
#' x <- create_tracking_history()
#' ggplot(x$obs_target, aes(x = east, y = north, col = target_id)) +
#' geom_point(size = 0.5) +
#' geom_path(data = x$obs_target %>% dplyr::filter(.data$target_id != "0"))
#'
#' ## Example 2
#' x <- create_tracking_history(n = 1, target_type = "extended", measure_rate = 150)$obs_target
#' ggplot(data = x) +
#' geom_point(aes(x = east, y = north, col = time), size = 1) +
#' theme_bw()
#'
#' }
#'
#' @export
create_tracking_history <- function(n = 2,
nscans = 30,
delta_time = 1,
starting_position = c(0,0),
starting_speed = c(0,0),
measure_rate = 1,
target_type = "point",
with_radar = TRUE,
prob_missed = 0.1,
prob_clutter = 0.1,
clutter_intensity = 0,
scan_range = 200,
turning_rate = 0.1,
prob_ext = 0.9,
kinematic_parameters){
T_s = delta_time
if(missing(kinematic_parameters)){
w = turning_rate
kinematic_parameters = list(
Fmat = matrix(c(1,0,sin(T_s*w)/w, (-1+cos(T_s*w))/w,0,
0,1,(1-cos(T_s*w))/w, sin(T_s*w)/w,0,
0,0,cos(T_s*w), -sin(T_s*w),0,
0,0,sin(T_s*w), cos(T_s*w), 0,
0,0,0,0,1), ncol = 5, byrow = TRUE),
Gmat = matrix(c(T_s^2/2,0,0,
0,T_s^2/2,0,
T_s,0,0,
0,T_s,0,
0,0,T_s),ncol = 3, byrow = TRUE),
sigma2_a = 10e-2,
sigma2_w = (0.1*pi/180*T_s)^2
)
}
Fmat = kinematic_parameters$Fmat
Gmat = kinematic_parameters$Gmat
sigma2_a = kinematic_parameters$sigma2_a
sigma2_w = kinematic_parameters$sigma2_w
obs_radar = NULL
if(with_radar){
Qmat = Gmat %*% diag(c(sigma2_a, sigma2_a, sigma2_w)) %*% t(Gmat)
radar_nscans = seq(1*T_s, nscans*T_s, by = T_s)
scan_id = NULL
for(k in radar_nscans){
if(k == min(radar_nscans)){
x_h = matrix(c(starting_position, starting_speed, 0.01), nrow = 1)}
nMeasures = 1#rpois(1,measure_rate)
scan_id = c(scan_id, rep(k, nMeasures))
if(k == min(radar_nscans)){next}
# Position, Speed, and Turning rate:
w_k = x_h[nrow(x_h),5]
if(nMeasures == 0){next}
x_k = mvtnorm::rmvnorm(nMeasures,Fmat %*% x_h[nrow(x_h),], sigma = Qmat)
x_h = rbind(x_h, x_k)
}
obs_radar = as.data.frame(cbind(x_h, scan_id))
colnames(obs_radar) <- c("radar_north", "radar_east", "radar_v_north", "radar_v_east","turn_rate","time")
}
if(target_type == "point"){
Qmat = Gmat %*% diag(c(sigma2_a, sigma2_a, sigma2_w)) %*% t(Gmat)
obs_target = NULL
target_id = NULL
for(nt in 1:n){
w_0 = 0.1
transition_u = 100
gamma_h = measure_rate
target_id = NULL
scan_id = NULL
start =sample(1:nscans,1, replace = F)
end = start + rbinom(1, size = nscans - start, prob = prob_ext)
target_nscans = seq(start*T_s, end*T_s, by = T_s)
for(k in target_nscans){
if(k == min(target_nscans)){
x_h = matrix(c(runif(1,starting_position[1]- scan_range, starting_position[1] + scan_range),
runif(1,starting_position[2]- scan_range, starting_position[2] + scan_range),
starting_speed, 0.01), nrow = 1)}
nMeasures = 1#rpois(1,measure_rate)
target_id = c(target_id, rep(nt, nMeasures))
scan_id = c(scan_id, rep(k, nMeasures))
if(k == min(target_nscans)){next}
# Position, Speed, and Turning rate:
w_k = x_h[nrow(x_h),5]
if(nMeasures == 0){next}
x_k = mvtnorm::rmvnorm(nMeasures,Fmat %*% x_h[nrow(x_h),], sigma = Qmat)
x_h = rbind(x_h, x_k)
# Measurement rate
gamma_k = rep(gamma_h[length(gamma_h)], nMeasures)
gamma_h = c(gamma_h, gamma_k)
}
obs_target = rbind(obs_target, as.data.frame(cbind(x_h, gamma_h, scan_id, target_id)))
}
colnames(obs_target) <- c("north", "east", "v_north", "v_east", "turn_rate", "generation_rate", "time", "target_id")
clutters = rbinom(nscans,clutter_intensity,prob_clutter)
clutter_coords = NULL
for(k in which(clutters > 0)){
for(kk in 1:clutters[k]){
selectedRow = obs_target[obs_target$time == ((1:nscans)*T_s)[k],]
if(nrow(selectedRow) == 0) {next}
selectedRow = selectedRow[sample(1:nrow(selectedRow), size = 1),]
clutter_coords = rbind(clutter_coords,
cbind(runif(1, selectedRow$north - scan_range,selectedRow$north +scan_range),
runif(1, selectedRow$east - scan_range,selectedRow$east + scan_range),
sample(obs_target$v_north, size = 1),
sample(obs_target$v_east, size = 1),
k, 0)
)
}
}
if(!is.null(clutter_coords)){
clutter_coords <- as.data.frame(clutter_coords)
colnames(clutter_coords) = c("north", "east","v_north", "v_east", "time", "target_id")
clutter_coords$target_id <- as.character(clutter_coords$target_id)
obs_target$target_id <- as.character(obs_target$target_id)
obs_target = dplyr::bind_rows(obs_target, clutter_coords)
} else{
obs_target$target_id <- as.character(obs_target$target_id)
obs_target$target_id <- as.factor(obs_target$target_id)
}
}
if(target_type == "extended"){
Qmat = Gmat %*% diag(c(sigma2_a, sigma2_a, sigma2_w)) %*% t(Gmat)
obs_target = NULL
target_id = NULL
for(nt in 1:n){
w_0 = 0.1
X_h = matrix(c(1,0.5,0.5,1), nrow = 1, byrow= TRUE)
transition_u = 100
gamma_h = measure_rate
target_id = NULL
scan_id = NULL
start =sample(1:nscans,1, replace = F)
end = start + rbinom(1, size = nscans - start, prob = prob_ext)
target_nscans = seq(start*T_s, end*T_s, by = T_s)
for(k in target_nscans){
if(k == min(target_nscans)){
x_h = matrix(c(runif(1,starting_position[1]- scan_range, starting_position[1] + scan_range),
runif(1,starting_position[2]- scan_range, starting_position[2] + scan_range),
starting_speed, 0.01), nrow = 1)
target_id = c(target_id, rep(nt, 1))
scan_id = c(scan_id, rep(k, 1))
}
if(k == min(target_nscans)){next}
nMeasures = rpois(1,measure_rate)
target_id = c(target_id, rep(nt, nMeasures))
scan_id = c(scan_id, rep(k, nMeasures))
# Position, Speed, and Turning rate:
w_k = x_h[nrow(x_h),5]
if(nMeasures == 0){next}
# Extension state
Mx_k = matrix(c(cos(w_k*T_s), -sin(w_k*T_s), sin(w_k*T_s), cos(w_k)*T_s),ncol = 2, byrow = TRUE)
X_h_mat = matrix(X_h[nrow(X_h),], ncol =2, byrow=TRUE)
X_k = rWishart(1, transition_u, 1/transition_u * Mx_k %*% X_h_mat %*% t(Mx_k))
X_k_v = matrix(X_k, ncol = 4, nrow= nMeasures, byrow = TRUE)
X_h = rbind(X_h, X_k_v)
#Multiple measures:
mk = Fmat %*% x_h[nrow(x_h),]
if(mk[3] != 0){
theta = atan(mk[4]/mk[3])
}else{
theta = 0
}
Rm = matrix(c(cos(theta),-sin(theta),sin(theta),cos(theta)), ncol = 2, byrow = TRUE)
Sm = diag(c(X_k_v[1,c(1,4)]))
QmatN = rbind(cbind(Rm %*% Sm %*% Sm %*% solve(Rm), Qmat[1:2,3:5]), Qmat[3:5,])
x_k = mvtnorm::rmvnorm(nMeasures,Fmat %*% x_h[nrow(x_h),], sigma = QmatN)
x_h = rbind(x_h, x_k)
# Measurement rate
gamma_k = rep(gamma_h[length(gamma_h)], nMeasures)
gamma_h = c(gamma_h, gamma_k)
}
obs_target = rbind(obs_target, as.data.frame(cbind(x_h, X_h, gamma_h, scan_id, target_id)))
}
colnames(obs_target) <- c("north", "east", "v_north", "v_east", "turn_rate", "size","ss_1", "shape","ss_2", "generation_rate", "time", "target_id")
clutters = rbinom(nscans,clutter_intensity,prob_clutter)
clutter_coords = NULL
for(k in which(clutters > 0)){
for(kk in 1:clutters[k]){
selectedRow = obs_target[obs_target$time == ((1:nscans)*T_s)[k],]
if(nrow(selectedRow) == 0) {next}
selectedRow = selectedRow[sample(1:nrow(selectedRow), size = 1),]
clutter_coords = rbind(clutter_coords,
cbind(runif(1, selectedRow$north - scan_range,selectedRow$north +scan_range),
runif(1, selectedRow$east - scan_range,selectedRow$east + scan_range),
sample(obs_target$v_north, size = 1),
sample(obs_target$v_east, size = 1),
k, 0)
)
}
}
if(!is.null(clutter_coords)){
clutter_coords <- as.data.frame(clutter_coords)
colnames(clutter_coords) = c("north", "east","v_north", "v_east", "time", "target_id")
clutter_coords$target_id <- as.character(clutter_coords$target_id)
obs_target$target_id <- as.character(obs_target$target_id)
obs_target = dplyr::bind_rows(obs_target, clutter_coords)
} else{
obs_target$target_id <- as.character(obs_target$target_id)
obs_target$target_id <- as.factor(obs_target$target_id)
}
}
return(list(obs_target = obs_target, obs_radar = obs_radar))
}
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.