## ----message=F, warning=FALSE-------------------------------------------------
library(navigation)

## -----------------------------------------------------------------------------
data("lemniscate_traj_ned") # trajectory in proper format as shown bellow
head(lemniscate_traj_ned)

## -----------------------------------------------------------------------------
traj <- make_trajectory(data = lemniscate_traj_ned, system = "ned")
class(traj)

## -----------------------------------------------------------------------------
timing <- make_timing(
  nav.start = 0, # time at which to begin filtering
  nav.end = 100,
  freq.imu = 100, # frequency of the IMU, can be slower wrt trajectory frequency
  freq.gps = 1, # gnss frequency
  freq.baro = 1, # barometer frequency (to disable, put it very low, e.g. 1e-5)
  gps.out.start = 60, # to simulate a GNSS outage, set a time before nav.end
  gps.out.end = 80
)

## -----------------------------------------------------------------------------
snsr.mdl <- list()
acc.mdl <- WN(sigma2 = 5.989778e-05) + AR1(phi = 9.982454e-01, sigma2 = 1.848297e-10) + AR1(phi = 9.999121e-01, sigma2 = 2.435414e-11) + AR1(phi = 9.999998e-01, sigma2 = 1.026718e-12)
gyr.mdl <- WN(sigma2 = 1.503793e-06) + AR1(phi = 9.968999e-01, sigma2 = 2.428980e-11) + AR1(phi = 9.999001e-01, sigma2 = 1.238142e-12)

snsr.mdl$imu <- make_sensor(name = "imu", frequency = timing$freq.imu, error_model1 = acc.mdl, error_model2 = gyr.mdl)

## -----------------------------------------------------------------------------
gps.mdl.pos.hor <- WN(sigma2 = 0.025^2)
gps.mdl.pos.ver <- WN(sigma2 = 0.05^2)
gps.mdl.vel.hor <- WN(sigma2 = 0.01^2)
gps.mdl.vel.ver <- WN(sigma2 = 0.02^2)
snsr.mdl$gps <- make_sensor(
  name = "gps", frequency = timing$freq.gps,
  error_model1 = gps.mdl.pos.hor,
  error_model2 = gps.mdl.pos.ver,
  error_model3 = gps.mdl.vel.hor,
  error_model4 = gps.mdl.vel.ver
)

## -----------------------------------------------------------------------------
baro.mdl <- WN(sigma2 = 0.5^2)
snsr.mdl$baro <- make_sensor(name = "baro", frequency = timing$freq.baro, error_model1 = baro.mdl)

## -----------------------------------------------------------------------------
KF.mdl <- list()

KF.mdl$imu <- make_sensor(name = "imu", frequency = timing$freq.imu, error_model1 = acc.mdl, error_model2 = gyr.mdl)
KF.mdl$gps <- snsr.mdl$gps
KF.mdl$baro <- snsr.mdl$baro

## -----------------------------------------------------------------------------
wrong_acc.mdl <- WN(sigma2 = 5.989778e-05)
wrong_gyr.mdl <- WN(sigma2 = 1.503793e-06)
wrong_KF.mdl <- list()
wrong_KF.mdl$imu <- make_sensor(name = "imu", frequency = timing$freq.imu, error_model1 = wrong_acc.mdl, error_model2 = wrong_gyr.mdl)
wrong_KF.mdl$gps <- snsr.mdl$gps
wrong_KF.mdl$baro <- snsr.mdl$baro

## ----message=FALSE, results='hide', eval=T------------------------------------
num.runs <- 10 # number of Monte-Carlo simulations
res <- navigation(
  traj.ref = traj,
  timing = timing,
  snsr.mdl = snsr.mdl,
  KF.mdl = KF.mdl,
  num.runs = num.runs,
  noProgressBar = TRUE,
  PhiQ_method = "1",
  parallel.ncores = 1,
  P_subsampling = timing$freq.imu,
  compute_PhiQ_each_n = 20
) # keep one covariance every second

## ----message=FALSE, results='hide', eval=T------------------------------------
wrong_res <- navigation(
  traj.ref = traj,
  timing = timing,
  snsr.mdl = snsr.mdl,
  KF.mdl = wrong_KF.mdl, # < here the model is the wrong one
  num.runs = num.runs,
  noProgressBar = TRUE,
  PhiQ_method = "1",
  parallel.ncores = 1,
  P_subsampling = timing$freq.imu,
  compute_PhiQ_each_n = 20
) # keep one covariance every second

## ----eval=T-------------------------------------------------------------------
pe_res <- compute_mean_position_err(res, step = 25)
pe_wrong_res <- compute_mean_position_err(wrong_res, step = 25)

oe_res <- compute_mean_orientation_err(res, step = 25)
oe_wrong_res <- compute_mean_orientation_err(wrong_res, step = 25)

## ----results='hide', fig.align='center', fig.width=7, fig.height=5, eval=T, fig.alt=c("NEES statistics for the correct sensor model.", "NEES statistics for the wrong sensor model.", "Coverage statistics for the correct sensor model.", "Coverage statistics for the wrong sensor model.")----
nees <- compute_nees(res, step = timing$freq.imu)
wrong_nees <- compute_nees(wrong_res, step = timing$freq.imu)

coverage <- compute_coverage(res, alpha = 0.7, step = timing$freq.imu)
wrong_coverage <- compute_coverage(wrong_res, alpha = 0.7, step = timing$freq.imu)

## ----fig.height=5, fig.align='center', fig.width=6, eval=T, fig.alt=c("Mean position error comparison between the correct and wrong sensor models.", "Mean orientation error comparison between the correct and wrong sensor models.", "NEES comparison between the correct and wrong sensor models.", "Coverage comparison between the correct and wrong sensor models.")----
plot(pe_res, pe_wrong_res, legend = c("correct model", "wrong model"))

plot(oe_res, oe_wrong_res, legend = c("correct model", "wrong model"))

plot(nees, wrong_nees, legend = c("correct model", "wrong model"))

plot(coverage, wrong_coverage, legend = c("correct model", "wrong model"))