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 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& preintMeasCov = + Eigen::Matrix::Zero()) : ScenarioRunner(scenario, static_cast(p), imuSampleTime, bias), p_(p), - estimatedBias_(bias) {} + estimatedBias_(bias), + preintMeasCov_(preintMeasCov) {} /// Integrate measurements for T seconds into a PIM PreintegratedCombinedMeasurements integrate(