Using R To Calculate A Simple Speed Model For Rally Stage Routes

A few months ago, I started hacking together an online e-book on Visualising WRC Rally Stages With rayshader and R. One of the sections (Estimating speeds) described the construction of a simple speed model based around the curvature of the stage route.

As part of another sprint into some rally data tinkering, this time focusing on Visualising WRC Telemetry Data With R, I’ve extracted just the essential code for creating the speed model and split it into a more self-contained extract: Creating a Route Speed Model. The intention is that I can use this speed model to help improve interpolation within a sparse telemetry time series.

Also on the to do list is to see if I can validate – or not! – the speed model using actual telemetry.

The recipe for building the model builds up from the a boundary convexity tool (bct()) that can be found in the rLFT processing linear features R package. This tool provides a handy routine for modeling the curvature along each point of a route in the form, a process that also returns the co-ordinates of a center of curvature for each sement. A separate function inspired by the pracma::circlefit() function, then finds the radius.

Because I don’t know how to write vectorised functions properly, I use the base::Vectorize() function to do the lifting for me around a simpler, non-vectorised function.

library(devtools)

# The curvature function takes an arc defined over
# x and y coordinate lists

#circlefit, from pracma::
circlefit = function (xp, yp, fast = TRUE) 
{
    if (!is.vector(xp, mode = "numeric") || !is.vector(yp, mode = "numeric")) 
        stop("Arguments 'xp' and 'yp' must be numeric vectors.")
    if (length(xp) != length(yp)) 
        stop("Vectors 'xp' and 'yp' must be of the same length.")
    if (!fast) 
        warning("Option 'fast' is deprecated and will not be used!", 
            call. = FALSE, immediate. = TRUE)
    n <- length(xp)
    p <- qr.solve(cbind(xp, yp, 1), matrix(xp^2 + yp^2, ncol = 1))
    v <- c(p[1]/2, p[2]/2, sqrt((p[1]^2 + p[2]^2)/4 + p[3]))
    rms <- sqrt(sum((sqrt((xp - v[1])^2 + (yp - v[2])^2) - v[3])^2)/n)
    #cat("RMS error:", rms, "\n")
    return(v)
}

curvature = function(x,y){
  #729181.8, 729186.1, 729190.4
  #4957667 , 4957676, 4957685
  tryCatch({
      # circlefit gives an error if we pass a straight line
      # Also hide the print statement in circlefit
      # circlefit() returns the x and y coords of the circle center
      # as well as the radius of curvature
      # We could then also calculate the angle and arc length
      circlefit(x,y)[3]
    },
    error = function(err) { 
      # For a straight, return the first co-ord and Inf diameter
      # Alternatively, pass zero diameter?
      c(x[1], y[1], Inf)[3]})
}

curvature2 = function(x1, x2, x3, y1, y2, y3){
  curvature(c(x1, x2, x3), c(y1, y2, y3))
}

# The base::Vectorize function provides a lazy way of 
# vectorising a non-vectorised function
curvatures = Vectorize(curvature2)

# The Midpoint values are calculated by rLFT::bct()
route_convexity$radius = curvatures(lag(route_convexity$Midpoint_X), 
                                    route_convexity$Midpoint_X,
                                    lead(route_convexity$Midpoint_X),
                                    lag(route_convexity$Midpoint_Y),
                                    route_convexity$Midpoint_Y,
                                    lead(route_convexity$Midpoint_Y)

A corner speed model than bins each segment into a corner type. This is inspired by the To See The Invisible rally pacenotes tutorial series by David Nafría which uses a simple numerical value to categorise the severity of each corner as well as identifying a nominal target speed for each corner category.

corner_speed_model = function(route_convexity){
  invisible_bins = c(0, 10, 15, 20, 27.5, 35,
                    45, 60, 77.5, 100, 175, Inf)

  route_convexity$invisible_ci = cut(route_convexity$radius,
                                     breaks = invisible_bins,
                                     labels = 1:(length(invisible_bins)-1),
                                     ordered_result=TRUE)
  
  # Speeds in km/h
  invisible_speeds = c(10, 40, 50, 60, 70, 80,
                       95, 110, 120, 130, 145)
  
  
  route_convexity$invisible_sp = cut(route_convexity$radius,
                                     breaks = invisible_bins,
                                     labels = invisible_speeds,
                                     ordered_result=TRUE)
  
  # Cast speed as factor, via character, to integer
  route_convexity$invisible_sp = as.integer(as.character(route_convexity$invisible_sp))
  
  route_convexity
}

We can now build up the speed model for the route. At each step we accelerate towards a nominal sector target speed (the invisible_sp value). We can’t accelerate infinitely fast, so our actual target accumulated speed for the segment, acc_sp, is a simple function of the current speed and the notional target speed. We can then calculate the notional time to complete that segment, invisible_time.

acceleration_model = function(route_convexity, stepdist=10){
  # Acceleration model
  sp = route_convexity$invisible_sp
  # Nominal starting target speed
  # In we don't set this, we don't get started moving
  sp[1] = 30 

  # Crude acceleration / brake weights
  acc = 1
  dec = 1
  for (i in 2:(length(sp)-1)) {
    # Simple linear model - accumulated speed is based on
    # the current speed and the notional segment speed
    # Accelerate up
    if (sp[i-1]<=sp[i]) sp[i] = (sp[i-1] + acc * sp[i]) / (1+acc)

    # Decelerate down
    if (sp[i]>sp[i+1]) sp[i] = (dec * sp[i] + sp[i+1]) / (1+dec)
  }

  route_convexity$acc_sp = sp
  route_convexity$acc_sp[length(sp)] = route_convexity$invisible_sp[length(sp)]

  # New time model
  # Also get speed in m/s for time calculation
  meters = 1000
  seconds_per_hour = 3600 # 60 * 60
  kph_unit = meters / seconds_per_hour
  route_convexity = route_convexity %>% 
                      mutate(segment_sp = route_convexity$acc_sp * kph_unit,
                             invisible_time = dist/segment_sp,
                             acc_time = cumsum(invisible_time))


  # So now we need to generate kilometer marks
  route_convexity$kmsection = 1 + trunc(route_convexity$MidMeas/1000)
  # We can use this to help find the time over each km

  route_convexity
}

With the speed model, we can then generate a simple plot of the anticipated speed against distance into route:

We can also plot the accumulated time into the route:

Finally, a simple cumulative sum of the time taken to complete each segment gives us an estimate of the stage time:

anticipated_time = function(route_convexity) {
  anticipated_time = sum(route_convexity$invisible_time[1:nrow(route_convexity)-1])
  cat(paste0("Anticipated stage time: ", anticipated_time %/% 60,
           'm ', round(anticipated_time %% 60, 1), 's' ))
}

anticipated_time(route_convexity)

# Anticipated stage time: 8m 40.3s

Next on my to do list is to generate an “ideal” route from a collection of telemetry traces from different drivers on the same stage.

If we know the start and end of the route are nominally at the same location, we can normalise the route length of multiple routes, maps equidistant points onto each other, and then take the average. For example, this solution: https://stackoverflow.com/a/65341730/454773 seems to offer a sensible way forward. See also https://en.wikipedia.org/wiki/Dynamic_time_warping and https://dynamictimewarping.github.io/r/ .

I was rather surprised, though, not to find a related funciton in one of the ecology / animal tracking R packages that would, for example, pull out a “mean” route based on a collection of locations from a tracked animal or group of animals following the same (ish) path over a period of time. Or maybe I just didnlt spot it? (If you know of just such a function I can reuse, please let me know via the comments…)