From dc83c6f6185ade4bc0b56914c9d276662cbeb5c7 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 15 Nov 2022 19:22:33 -0500
Subject: [PATCH] update CombinedScenarioRunner to accept preintMeasCov

---
 gtsam/navigation/ScenarioRunner.cpp | 2 +-
 gtsam/navigation/ScenarioRunner.h   | 8 ++++++--
 2 files changed, 7 insertions(+), 3 deletions(-)

diff --git a/gtsam/navigation/ScenarioRunner.cpp b/gtsam/navigation/ScenarioRunner.cpp
index 9d3e258de..856f0e692 100644
--- a/gtsam/navigation/ScenarioRunner.cpp
+++ b/gtsam/navigation/ScenarioRunner.cpp
@@ -108,7 +108,7 @@ Matrix6 ScenarioRunner::estimateNoiseCovariance(size_t N) const {
 PreintegratedCombinedMeasurements CombinedScenarioRunner::integrate(
     double T, const Bias& estimatedBias, bool corrupted) const {
   gttic_(integrate);
-  PreintegratedCombinedMeasurements pim(p_, estimatedBias);
+  PreintegratedCombinedMeasurements pim(p_, estimatedBias, preintMeasCov_);
 
   const double dt = imuSampleTime();
   const size_t nrSteps = T / dt;
diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h
index cee5a54ab..7f1efa063 100644
--- a/gtsam/navigation/ScenarioRunner.h
+++ b/gtsam/navigation/ScenarioRunner.h
@@ -118,15 +118,19 @@ class GTSAM_EXPORT CombinedScenarioRunner : public ScenarioRunner {
  private:
   const SharedParams p_;
   const Bias estimatedBias_;
+  const Eigen::Matrix<double, 15, 15> preintMeasCov_;
 
  public:
   CombinedScenarioRunner(const Scenario& scenario, const SharedParams& p,
                          double imuSampleTime = 1.0 / 100.0,
-                         const Bias& bias = Bias())
+                         const Bias& bias = Bias(),
+                         const Eigen::Matrix<double, 15, 15>& preintMeasCov =
+                             Eigen::Matrix<double, 15, 15>::Zero())
       : ScenarioRunner(scenario, static_cast<ScenarioRunner::SharedParams>(p),
                        imuSampleTime, bias),
         p_(p),
-        estimatedBias_(bias) {}
+        estimatedBias_(bias),
+        preintMeasCov_(preintMeasCov) {}
 
   /// Integrate measurements for T seconds into a PIM
   PreintegratedCombinedMeasurements integrate(