R/ptrack.R

In tagtools: Work with Data from High-Resolution Biologging Tags

#' Estimate simple dead-reckoned track
#'
#' This function is used to estimate the simple dead-reckoned track (pseudo-track) based on speed and bodypointing angle.
#'
#' @param A An nx3 acceleration matrix with columns [ax ay az] or acceleration sensor list. Acceleration can be in any consistent unit, e.g., g or m/s^2.
#' @param M The magnetometer signal matrix, M = [mx,my,mz] in any consistent unit (e.g., in uT or Gauss)  or magnetometer sensor list. A and M must have the same size (and so are both measured at the same sampling rate).
#' @param s The forward speed of the animal in m/s. s can be a single number meaning that the animal is assumed to travel at a constant speed. s can also be a vector with the same number of rows as A and M, e.g., generated by speed_from_depth().
#' @param sampling_rate The sampling rate of the sensor data in Hz (samples per second). This input will be ignored if A and/or M are sensor lists, in which case the sampling rate will be extracted from them.
#' @param fc (optional) The cut-off frequency of a low-pass filter to apply to A and M before computing bodypointing angle. The filter cut-off frequency is in Hz. The filter length is 4*sampling_rate/fc. Filtering adds no group delay. If fc is empty or not given, the default value of 0.2 Hz (i.e., a 5 second time constant) is used.
#' @param return_pe Logical. If return_pe is TRUE, the estimated depth or altitude predicted will be returned with the estimated track. Default is FALSE.
#' @return The estimated track in a local level frame. The track is defined as meters of northward and eastward movement (variables 'northing' and 'easting' in the output data frame) relative to the animal's position at the start of the measurements  (which is defined as [0,0]). The track sampling rate is the same as for the input data and so each row of track object defines the track coordinates at times 0,1/sampling_rate,2/sampling_rate,... relative to the start time of the measurements. OR, if return_pe = TRUE, this function returns the above value and the estimated depth or altitude predicted from the speed and pitch angle. This can be compared against the measured depth/altitude to assess errors in the dead-reckoned track. Note that even if pe matches the observed depth, this does not guarantee that the track is accurate.
#' @note Frame: This function assumes a [north,east,up] navigation frame and a [forward,right,up] local frame. Both A and M must be rotated if needed to match the animal's cardinal axes otherwise the track will not be meaningful.
#' @note CAUTION: dead-reckoned tracks are usually very inaccurate. They are useful to get an idea of HOW animals move rather than WHERE they go. Few animals probably travel in exactly the direction of their longitudinal axis and anyway measuring the precise orientation of the longitudinal axis of a non-rigid animal is fraught with error. Moreover, if there is net flow in the medium, the animal will be affected by the flow in addition to its autonomous movement. For swimming animals this can lead to substantial errors. The forward speed is assumed to be with respect to the medium so the track derived here is NOT the 'track-made-good', i.e., the geographic movement of the animal. It estimates the movement of the animal with respect to the medium. There are numerous other sources of error so use at your own risk!
#' @seealso \code{\link[tagtools]{htrack}}, \code{\link[tagtools]{fit_tracks}}, \code{\link{track3D}}
#' @export
#' @examples
#' BW <- beaked_whale
#' list <- ptrack(A = BW$A$data, M = BW$M$data, s = 3, 
#' sampling_rate = BW$A$sampling_rate, fc = NULL, 
#' return_pe = TRUE)
#' plot(list$track$easting, list$track$northing, xlab = "Easting, m", ylab = "Northing, m")
#'
ptrack <- function(A, M, s, sampling_rate = NULL, fc = 0.2, return_pe = FALSE) {
  # input checks----------------------------------------------------------
  if (is.list(M) & is.list(A)) {
    if (A$sampling_rate != M$sampling_rate) {
      stop("A and M must be at the same sampling rate")
    }
    sampling_rate <- M$sampling_rate
    M <- M$data
    A <- A$data
  } else {
    if (missing(sampling_rate)) {
      stop("Inputs for A, M, s, and sampling_rate are all required if A and M are matrices")
    }
  }

  if (length(s) > 1) {
    if (length(s) != nrow(A)) {
      stop("ptrack: length of speed vector must equal column length of A and M\n")
    }
    s <- matrix(s / sampling_rate, nrow = nrow(A), ncol = 3)
  } else {
    s <- s / sampling_rate
  }

  kg <- stats::complete.cases(cbind(A, M))
  track <- matrix(nrow = nrow(A), ncol = 3)
  if (length(s) > 1) {
    s <- s[kg]
  }
  W <- body_axes(A[kg, ], M[kg, ], sampling_rate, fc)
  track[kg, ] <- apply((s * W$x), 1, cumsum)
  track <- data.frame(track)
  # track <- track[,c(1:2)]
  names(track) <- c("northing", "easting", "dunno")

  if (return_pe == TRUE) {
    pe <- matrix(nrow = nrow(A), ncol = 1)
    pitch <- a2pr(A[kg, ], sampling_rate, fc)$p
    pe[kg] <- -cumsum((s / sampling_rate) * sin(pitch))
    return(list(track = track, pe = pe))
  }
  return(track)
}