calculateSLoWC.R (2696B)
1 #' Calculate the "severity loss of Well Clear" (SLoWC) metric, described in RTCA 2 #' SC-228 Closed-Loop Metrics White Paper. 3 #' 4 #' @param trajectory1 A \code{flighttrajectory} object corresponding to the 5 #' first aircraft. 6 #' @param trajectory2 A \code{flighttrajectory} object corresponding to the 7 #' second aircraft. 8 #' @return The numeric vector giving the SLoWC metric. Values lie in the range 9 #' [0, 100]. A SLoWC of 0 indicates Well Clear, while a value of 100 10 #' corresponds to "full penetration" (i.e., a collision). 11 #' 12 #' @details Note that the RTCA definition of Well Clear is undergoing revision. 13 #' This code is based on the SLoWC formulation by Ethan Pratt and Jacob Kay as 14 #' implemented in a MATLAB script by Ethan Pratt dated 2016-04-18. 15 #' 16 #' @export 17 calculateSLoWC <- function(trajectory1, trajectory2) { 18 checkTrajectories(trajectory1, trajectory2) 19 20 # DAA Well Clear thresholds 21 DMOD <- 4000 # ft 22 DH_thr <- 450 # ft 23 TauMod_thr <- 35 # s 24 25 if (!is.flattrajectory(trajectory1)) { 26 lon0 <- mean(c(trajectory1$longitude, trajectory2$longitude)) 27 lat0 <- mean(c(trajectory1$latitude, trajectory2$latitude)) 28 29 trajectory1 <- trajectoryToXYZ(trajectory1, c(lon0, lat0)) 30 trajectory2 <- trajectoryToXYZ(trajectory2, c(lon0, lat0)) 31 } 32 33 # Distance between aircraft 34 dXYZ <- trajectory2$position - trajectory1$position 35 dXYZ[, 3] <- abs(dXYZ[, 3]) 36 relativeVelocity <- trajectory2$velocity - trajectory1$velocity 37 38 # Calculate the range 39 R <- sqrt(apply(dXYZ[, 1:2, drop = FALSE]^2, 1, sum)) 40 41 # Time to the closest point of approach (s) 42 tCPA <- calculateTCPA(trajectory1, trajectory2) 43 44 # Horizontal missed distance 45 HMD <- sqrt((dXYZ[, 1] + relativeVelocity[, 1] * tCPA)^2 + 46 (dXYZ[, 2] + relativeVelocity[, 2] * tCPA)^2 ) 47 48 # Note: the code below here is very close to a direct translation of the 49 # MATLAB script to R (with a few modifications to permit parallelization). 50 # Additional optimizations are possible. 51 52 # Horizontal size of the Hazard Zone (TauMod_thr boundary) 53 Rdot <- apply(dXYZ[, 1:2, drop = FALSE] * relativeVelocity, 1, sum) / R 54 S <- pmax(DMOD, .5 * (sqrt( (Rdot * TauMod_thr)^2 + 4 * DMOD^2) - Rdot * TauMod_thr)) 55 # Safeguard against x/0 56 S[R < 1e-4] <- DMOD 57 58 # Three penetration components (Range, HMD, vertical separation) 59 RangePen <- pmin((R / S), 1) 60 HMDPen <- pmin((HMD / DMOD), 1) 61 DHPen <- pmin((dXYZ[, 3] / DH_thr), 1) 62 63 hpen <- FGnorm(RangePen, HMDPen) 64 vSLoWC <- 100 * (1 - FGnorm(hpen, DHPen)) 65 66 return(vSLoWC) 67 } 68 69 # The Fernandez-Gausti squircular operator. 70 FGnorm <- function(x, y) { 71 return(sqrt(x^2 + (1 - x^2) * y^2)) 72 }