diff --git a/gtsam/navigation/ScenarioRunner.cpp b/gtsam/navigation/ScenarioRunner.cpp index 4b546c5c5..708c27afc 100644 --- a/gtsam/navigation/ScenarioRunner.cpp +++ b/gtsam/navigation/ScenarioRunner.cpp @@ -54,29 +54,43 @@ Vector3 PreintegratedMeasurements2::currentTheta() const { return zetaValues.at(T(k_)); } +SharedDiagonal PreintegratedMeasurements2::discreteAccelerometerNoiseModel( + double dt) const { + return noiseModel::Diagonal::Sigmas(accelerometerNoiseModel_->sigmas() / + std::sqrt(dt)); +} + +SharedDiagonal PreintegratedMeasurements2::discreteGyroscopeNoiseModel( + double dt) const { + return noiseModel::Diagonal::Sigmas(gyroscopeNoiseModel_->sigmas() / + std::sqrt(dt)); +} + PreintegratedMeasurements2::SharedBayesNet PreintegratedMeasurements2::initPosterior(const Vector3& correctedAcc, const Vector3& correctedOmega, double dt) const { typedef map Terms; + // We create a factor graph and then compute P(zeta|bias) GaussianFactorGraph graph; // theta(1) = (correctedOmega - bias_delta) * dt // => theta(1) + bias_delta * dt = correctedOmega * dt graph.add({{T(k_ + 1), I_3x3}, {kBiasKey, omega_H_bias * dt}}, - correctedOmega * dt, gyroscopeNoiseModel_); + correctedOmega * dt, discreteGyroscopeNoiseModel(dt)); // pose(1) = (correctedAcc - bias_delta) * dt^2/2 // => pose(1) + bias_delta * dt^2/2 = correctedAcc * dt^2/2 double dt22 = 0.5 * dt * dt; + auto accModel = discreteAccelerometerNoiseModel(dt); graph.add({{P(k_ + 1), I_3x3}, {kBiasKey, acc_H_bias * dt22}}, - correctedAcc * dt22, accelerometerNoiseModel_); + correctedAcc * dt22, accModel); // vel(1) = (correctedAcc - bias_delta) * dt // => vel(1) + bias_delta * dt = correctedAcc * dt graph.add({{V(k_ + 1), I_3x3}, {kBiasKey, acc_H_bias * dt}}, - correctedAcc * dt, accelerometerNoiseModel_); + correctedAcc * dt, accModel); // eliminate all but biases // NOTE(frank): After this, posterior_k_ contains P(zeta(1)|bias) @@ -105,23 +119,24 @@ PreintegratedMeasurements2::integrateCorrected(const Vector3& correctedAcc, // => H*theta(k+1) - H*theta(k) + bias_delta dt = (measuredOmega - bias) dt Matrix3 H = Rot3::ExpmapDerivative(theta_k); graph.add({{T(k_ + 1), H}, {T(k_), -H}, {kBiasKey, omega_H_bias * dt}}, - correctedOmega * dt, gyroscopeNoiseModel_); + correctedOmega * dt, discreteGyroscopeNoiseModel(dt)); // pos(k+1) = pos(k) + vel(k) dt + Rk*(correctedAcc - bias_delta) dt^2/2 // => Rkt*pos(k+1) - Rkt*pos(k) - Rkt*vel(k) dt + bias_delta dt^2/2 // = correctedAcc dt^2/2 double dt22 = 0.5 * dt * dt; + auto accModel = discreteAccelerometerNoiseModel(dt); graph.add({{P(k_ + 1), Rkt}, {P(k_), -Rkt}, {V(k_), -Rkt * dt}, {kBiasKey, acc_H_bias * dt22}}, - correctedAcc * dt22, accelerometerNoiseModel_); + correctedAcc * dt22, accModel); // vel(k+1) = vel(k) + Rk*(correctedAcc - bias_delta) dt // => Rkt*vel(k+1) - Rkt*vel(k) + bias_delta dt = correctedAcc * dt graph.add( {{V(k_ + 1), Rkt}, {V(k_), -Rkt}, {kBiasKey, acc_H_bias * dt}}, - correctedAcc * dt, accelerometerNoiseModel_); + correctedAcc * dt, accModel); // eliminate all but biases // TODO(frank): does not seem to eliminate in order I want. What gives? @@ -182,11 +197,7 @@ NavState PreintegratedMeasurements2::predict( SharedGaussian PreintegratedMeasurements2::noiseModel() const { Matrix RS; Vector d; - GTSAM_PRINT(*posterior_k_); boost::tie(RS, d) = posterior_k_->matrix(); - cout << RS << endl - << endl; - cout << d.transpose() << endl; // R'*R = A'*A = inv(Cov) // TODO(frank): think of a faster way - implement in noiseModel diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h index 485a0a51f..8ef3d83e3 100644 --- a/gtsam/navigation/ScenarioRunner.h +++ b/gtsam/navigation/ScenarioRunner.h @@ -94,6 +94,12 @@ class PreintegratedMeasurements2 { // estimate theta given estimated biases Vector3 currentTheta() const; + // We obtain discrete-time noise models by dividing the continuous-time + // covariances by dt: + + SharedDiagonal discreteAccelerometerNoiseModel(double dt) const; + SharedDiagonal discreteGyroscopeNoiseModel(double dt) const; + // initialize posterior with first (corrected) IMU measurement SharedBayesNet initPosterior(const Vector3& correctedAcc, const Vector3& correctedOmega, double dt) const; @@ -102,7 +108,6 @@ class PreintegratedMeasurements2 { SharedBayesNet integrateCorrected(const Vector3& correctedAcc, const Vector3& correctedOmega, double dt) const; - }; /*