12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

update CombinedScenarioRunner to accept preintMeasCov

release/4.3a0
Varun Agrawal 2022-11-15 19:22:33 -05:00
parent 6eef703019
commit dc83c6f618
2 changed files with 7 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/navigation/ScenarioRunner.cpp
View File

@ -108,7 +108,7 @@ Matrix6 ScenarioRunner::estimateNoiseCovariance(size_t N) const {
PreintegratedCombinedMeasurements CombinedScenarioRunner::integrate( PreintegratedCombinedMeasurements CombinedScenarioRunner::integrate(
double T, const Bias& estimatedBias, bool corrupted) const { double T, const Bias& estimatedBias, bool corrupted) const {
gttic_(integrate); gttic_(integrate);
PreintegratedCombinedMeasurements pim(p_, estimatedBias); PreintegratedCombinedMeasurements pim(p_, estimatedBias, preintMeasCov_);
const double dt = imuSampleTime(); const double dt = imuSampleTime();
const size_t nrSteps = T / dt; const size_t nrSteps = T / dt;

8
gtsam/navigation/ScenarioRunner.h
View File

@ -118,15 +118,19 @@ class GTSAM_EXPORT CombinedScenarioRunner : public ScenarioRunner {
private: private:
const SharedParams p_; const SharedParams p_;
const Bias estimatedBias_; const Bias estimatedBias_;
const Eigen::Matrix<double, 15, 15> preintMeasCov_;
public: public:
CombinedScenarioRunner(const Scenario& scenario, const SharedParams& p, CombinedScenarioRunner(const Scenario& scenario, const SharedParams& p,
double imuSampleTime = 1.0 / 100.0, 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), : ScenarioRunner(scenario, static_cast<ScenarioRunner::SharedParams>(p),
imuSampleTime, bias), imuSampleTime, bias),
p_(p), p_(p),
estimatedBias_(bias) {} estimatedBias_(bias),
preintMeasCov_(preintMeasCov) {}
/// Integrate measurements for T seconds into a PIM /// Integrate measurements for T seconds into a PIM
PreintegratedCombinedMeasurements integrate( PreintegratedCombinedMeasurements integrate(