R/VaccaroZaki.R

In Yuming-Zhang/synimu: Scale-wise Variance Optimization

# computes the partial inverse of the matrix A ignoring n singular values
partial_inv <- function(A, n) {
  ret = matrix(0, nrow = dim(A)[1], ncol = dim(A)[2])

  ev = eigen(A, only.values = T)$values

  dec = svd(A)

  # for (i in which(ev > 0)) {
  for (i in 1:(length(dec$d)-n)) {
    ret = ret + dec$d[i]^-1 * dec$u[,i] %*% t(dec$v[,i])
  }

  t(ret)
}

#' @title Estimate Optimal Coefficients based on Reduced-Drift Virtual Gyro
#' @description This function computes the optimal coefficients for a linear combination of \eqn{p} gyroscopes according to
#' Equation (43) in Richard J. Vaccaro and Ahmed S. Zaki, "Reduced-Drift Virtual Gyro from an Array of Low-Cost Gyros".
#' @export
#' @param Q A \code{matrix} of size \eqn{p} by \eqn{p} specifying the covariance of the random walk innovation.
#' @return c A \code{vector} of coefficients.
#' @author Davide Antonio Cucci

find_optimal_coefs_vaccaro = function(Q) {
  o = matrix(1, nrow=dim(Q)[1], ncol=1)

  c = ( solve(Q) %*% o ) / (t(o) %*% solve(Q) %*% o)[1]

  as.vector(c)
}

#' @title Estimate Optimal Coefficients based on Reduced-Drift Virtual Gyro
#' @description This function computes the optimal coefficients for a linear combination of \eqn{p} gyroscopes according to
#' Equation (43) in Richard J. Vaccaro and Ahmed S. Zaki, "Reduced-Drift Virtual Gyro from an Array of Low-Cost Gyros", where
#' \eqn{Q^{-1}} is replaced by the pseudo-inverse of \eqn{Q}, ignoring \eqn{n} singular values, as suggested in Equation (54).
#' @export
#' @param Q A \code{matrix} of size \eqn{p} by \eqn{p} specifying the covariance of the random walk innovation.
#' @param n An \code{integer} indicating the number of singular values to ignore in the calculation of the pseudo-inverse of \eqn{Q}.
#' @return c A \code{vector} of coefficients.
#' @author Davide Antonio Cucci

find_optimal_coefs_vaccaro_pinv = function(Q, n) {
  o = matrix(1, nrow=dim(Q)[1], ncol=1)

  pinv = partial_inv(Q, n)

  c = ( pinv %*% o ) / (t(o) %*% pinv %*% o)[1]

  as.vector(c)
}


#' @title Empirical Allan variance
#' @description This function provides an empirical estimate of the Allan variance given multiple processes.
#' The detailed definition can be found in Equation (3) in Richard J. Vaccaro and Ahmed S. Zaki, "Reduced-Drift Virtual Gyro from an Array of Low-Cost Gyros".
#' @export
#' @param Xt A \code{matrix} of dimension T by p, where T is the length of the time series and p is the number of processes.
#' @return A \code{list} with the following structure:
#' \itemize{
#'  \item J: An \code{integer} indicating the maximum level.
#'  \item scales: A \code{vector} of the employed levels (\eqn{N} in [1]).
#'  \item variance: A \code{matrix} of the estimated Allan variance.
#' }
#' @author Davide Antonio Cucci

av = function(Xt) {
  J = floor(log2(length(Xt))) - 3
  scales = 2^(1:J)

  variance = rep(0, length(scales))

  for (is in 1:length(scales)) {
    segs = seq(from=1, to=length(Xt), by=scales[is])

    segs_means = sapply( 1:(length(segs)-1),
                         function (iseg) {
                           mean(Xt[segs[iseg]:(segs[iseg+1]-1)])
                         })

    ds = diff(segs_means)^2

    variance[is] = mean(ds)/2
  }

  return(
    list(
      J = J,
      scales = scales,
      variance = variance
    )
  )
}

#' @title Empirical Allan covariance
#' @description This function provides an empirical estimate of the Allan covariance given multiple processes.
#' The detailed definition can be found in Equation (23) in Richard J. Vaccaro and Ahmed S. Zaki, "Reduced-Drift Virtual Gyro from an Array of Low-Cost Gyros".
#' @export
#' @param Xt A \code{matrix} of dimension T by p, where T is the length of the time series and p is the number of processes.
#' @return A \code{list} with the following structure:
#' \itemize{
#'  \item J: An \code{integer} indicating the maximum level.
#'  \item scales: A \code{vector} of the employed levels (\eqn{m} in [1]).
#'  \item covariance: A \code{list} of \code{matrix} for each level \eqn{m} of the estimated Allan covariance.
#' }
#' @author Davide Antonio Cucci

acov = function(X) {
  n_ts = dim(X)[2]
  N = dim(X)[1]

  J = floor(log2(N)) - 3
  scales = 2^(1:J)

  covariance = list()

  for (is in 1:length(scales)) {
    covariance[[is]] = matrix(NA, nrow=n_ts, ncol=n_ts)

    segs_means = list()
    for (ix in 1:n_ts) {
      segs = seq(from=1, to=N, by=scales[is])

      segs_means[[ix]] = sapply( 1:(length(segs)-1),
                           function (iseg) {
                             mean(X[segs[iseg]:(segs[iseg+1]-1), ix])
                           })
    }

    diffs = lapply(segs_means, diff)

    for (ixa in 1:n_ts) {
      for (ixb in ixa:n_ts) {
        covariance[[is]][ixa,ixb] = mean(diffs[[ixa]]*diffs[[ixb]])/2
        covariance[[is]][ixb,ixa] = covariance[[is]][ixa,ixb]
      }
    }
  }

  return(
    list(
      J = J,
      scales = scales,
      covariance = covariance
    )
  )
}

# as in algorithm 1, point 6 in Richard J. Vaccaro and Ahmed S. Zaki, "Reduced-Drift Virtual Gyro from an Array of Low-Cost Gyros".
CR = function(R, N, scales) {
  cr = matrix(NA, nrow = length(scales), ncol = length(scales))
  for (i_m1 in 1:length(scales)) {
    for (i_m2 in i_m1:length(scales)) {
      M1 = N/scales[i_m1]
      M2 = N/scales[i_m2]

      p = scales[i_m2]/scales[i_m1]

      cr[i_m1, i_m2] = (3*M2-4)/((M1-1)*(M2-1)*p^2)*R^2/scales[i_m1]^2
      cr[i_m2, i_m1] = cr[i_m1, i_m2]
    }
  }

  cr
}

# as in algorithm 1, point 6 in Richard J. Vaccaro and Ahmed S. Zaki, "Reduced-Drift Virtual Gyro from an Array of Low-Cost Gyros".
CQ = function(Q, N, scales) {
  cq = matrix(NA, nrow = length(scales), ncol = length(scales))
  for (i_m1 in 1:length(scales)) {
    for (i_m2 in i_m1:length(scales)) {
      M1 = N/scales[i_m1]
      M2 = N/scales[i_m2]

      p = scales[i_m2]/scales[i_m1]

      cq[i_m1, i_m2] = (((12*p^3-6*p+3)*M2-2*(6*p^3-3*p+2))*Q^2*(scales[i_m1])^2)/(36*(M1-1)*(M2-1)*p^2)
      cq[i_m2, i_m1] = cq[i_m1, i_m2]
    }
  }

  cq
}

# as in algorithm 2, point 5 in Richard J. Vaccaro and Ahmed S. Zaki, "Reduced-Drift Virtual Gyro from an Array of Low-Cost Gyros".
CCR = function(R1, R2, N, scales) {
  ccr = matrix(NA, nrow = length(scales), ncol = length(scales))
  for (i_m1 in 1:length(scales)) {
    for (i_m2 in i_m1:length(scales)) {
      M1 = N/scales[i_m1]
      M2 = N/scales[i_m2]

      p = scales[i_m2]/scales[i_m1]

      ccr[i_m1, i_m2] = (3*M2-4)/(2*(M1-1)*(M2-1)*p^2)*R1*R2/scales[i_m1]^2
      ccr[i_m2, i_m1] = ccr[i_m1, i_m2]
    }
  }

  ccr
}

# as in algorithm 2, point 5 in Richard J. Vaccaro and Ahmed S. Zaki, "Reduced-Drift Virtual Gyro from an Array of Low-Cost Gyros".
CCQ = function(Q1, Q2, Q12, N, scales) {
  ccq = matrix(NA, nrow = length(scales), ncol = length(scales))
  for (i_m1 in 1:length(scales)) {
    for (i_m2 in i_m1:length(scales)) {
      M1 = N/scales[i_m1]
      M2 = N/scales[i_m2]

      p = scales[i_m2]/scales[i_m1]

      ccq[i_m1, i_m2] = (((12*p^3-6*p+3)*M2-2*(6*p^3-3*p+2))*(Q1*Q2+Q12^2)*(scales[i_m1])^2)/(72*(M1-1)*(M2-1)*p^2)
      ccq[i_m2, i_m1] = ccq[i_m1, i_m2]
    }
  }

  ccq
}

#' @title Estimation of White Noise and Random Walk Innovation Variances \eqn{R} and \eqn{Q}
#' @description This function provides an estimate based on the Allan variance for the white noise and the random walk innovation variances \eqn{R} and \eqn{Q}.
#' The implementation reflects Algorithm 1 in Richard J. Vaccaro and Ahmed S. Zaki, "Reduced-Drift Virtual Gyro from an Array of Low-Cost Gyros".
#' @export
#' @param X A \code{matrix} of dimension T by p, where T is the length of the time series and p is the number of processes.
#' @param remove_last_scale wether the last scale of the Allan variance should be removed
#' @return A \code{list} with the following structure:
#' \itemize{
#'  \item R: A \code{vector} of size p of the estimated white noise variances.
#'  \item Q: A \code{vector} of size p of the estimated random walk innovation variances.
#' }
#' @author Davide Antonio Cucci

algorithm_1 = function(X, remove_last_scale = FALSE) {
  allan = av(X)

  if (remove_last_scale == TRUE) {
    allan$J = allan$J-1
    allan$scales = allan$scales[-length(allan$scales)]
    allan$variance = allan$variance[-length(allan$variance)]
  }

  m0 = allan$scales[which.min(allan$variance)]

  i1 = which(allan$scales < m0/8)
  m1 = allan$scales[i1]
  N1 = tail(i1,1)

  Omega_Cm1 = solve(CR(1, length(X), m1))
  HR = matrix(1/2^(1:N1), nrow=N1, ncol=1)

  R0 = solve(t(HR) %*% Omega_Cm1 %*% HR) %*% t(HR) %*% Omega_Cm1 %*% allan$variance[i1]
  Q0 = 3*R0/m0^2

  Omega_C = solve(CR(R0, length(X), allan$scales) + CQ(Q0, length(X), allan$scales))

  HR = 2^(1:(allan$J))/3  # in the paper they say up to J-2 (non-comformable ...)
  HQ = 1/(2^(1:(allan$J)))
  H = cbind(HR, HQ)

  sol = solve(t(H) %*% Omega_C %*% H) %*% t(H) %*% Omega_C %*% allan$variance

  return(list
    (
      Q = sol[1],
      R = sol[2]
    ))
}


algorithm_1_ms = function(X, remove_last_scale) {
  n_ts = dim(X)[2]

  R = rep(NA, n_ts)
  Q = rep(NA, n_ts)

  for (i in 1:n_ts) {
    ret = algorithm_1(X[,i], remove_last_scale)
    R[i] = ret$R
    Q[i] = ret$Q
  }

  return(list(
    R = R,
    Q = Q
  ))

}

#' @title Estimation of White Noise and Random Walk Innovation Covariances \eqn{R} and \eqn{Q}
#' @description This function provides an estimate based on the Allan variance for the white noise and the random walk innovation covariances \eqn{R} and \eqn{Q}.
#' The implementation reflects Algorithm 2 in Richard J. Vaccaro and Ahmed S. Zaki, "Reduced-Drift Virtual Gyro from an Array of Low-Cost Gyros".
#' @export
#' @param X A \code{matrix} of dimension T by p, where T is the length of the time series and p is the number of processes.
#' @param remove_last_scale wether the last scale of the Allan variance should be removed
#' @return A \code{list} with the following structure:
#' \itemize{
#'  \item R: A \code{vector} of size p of the estimated white noise variances.
#'  \item Q: A \code{matrix} of size p by p of the estimated random walk innovation covariance.
#' }
#' @author Davide Antonio Cucci

algorithm_2 = function(X, remove_last_scale = FALSE) {
  if (log2(dim(X)[1]) %% 1 != 0) {
    X = X[1:(2^floor(log2(dim(X)[1]))),]
    warning(paste("truncating X to the first 2^", log2(dim(X)[1]),"samples"))
  }

  ret = algorithm_1_ms(X, remove_last_scale)

  R = ret$R
  Q = diag(ret$Q)

  allancov = acov(X)

  if (remove_last_scale == TRUE) {
    n = allancov$J

    allancov$J = allancov$J-1
    allancov$scales = allancov$scales[-n]

    allancov$covariance = allancov$covariance[-n]
  }

  H = matrix(allancov$scales/3, nrow=length(allancov$scales), ncol=1)

  n_ts = dim(X)[2]
  N = dim(X)[1]

  for (ia in 1:n_ts) {
    for (ib in ia:n_ts) {
      if (ia == ib) {
        next
      }

      a = sapply(allancov$covariance, function(x) {x[ia,ib]})

      Omega_C = solve(CCR(R[ia], R[ib], N, allancov$scales) + CCQ(Q[ia,ia], Q[ib, ib], 0, N, allancov$scales))

      Q[ia, ib] = solve(t(H) %*% Omega_C %*% H) %*% t(H) %*% Omega_C %*% a
      Q[ib, ia] = Q[ia, ib]
    }
  }

  return(list(
    R = R,
    Q = Q
  ))
}