From 2040571ad32876fabf927abd8b91063810996168 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 2 Jan 2016 14:53:41 -0800 Subject: [PATCH] Predict covariance now calculated correctly --- gtsam/navigation/AggregateImuReadings.cpp | 105 +++++++++++++++------- gtsam/navigation/AggregateImuReadings.h | 15 ++-- gtsam/navigation/ScenarioRunner.cpp | 4 +- gtsam/navigation/ScenarioRunner.h | 2 +- 4 files changed, 83 insertions(+), 43 deletions(-) diff --git a/gtsam/navigation/AggregateImuReadings.cpp b/gtsam/navigation/AggregateImuReadings.cpp index 27f3f11d2..9e4158f6f 100644 --- a/gtsam/navigation/AggregateImuReadings.cpp +++ b/gtsam/navigation/AggregateImuReadings.cpp @@ -38,6 +38,22 @@ using symbol_shorthand::V; // for velocity static const Symbol kBiasKey('B', 0); +AggregateImuReadings::AggregateImuReadings(const boost::shared_ptr& p, + const Bias& estimatedBias) + : p_(p), + accelerometerNoiseModel_(Diagonal(p->accelerometerCovariance)), + gyroscopeNoiseModel_(Diagonal(p->gyroscopeCovariance)), + estimatedBias_(estimatedBias), + k_(0), + deltaTij_(0.0) { + // Initialize values with zeros + static const Vector3 kZero(Vector3::Zero()); + values.insert(T(0), kZero); + values.insert(P(0), kZero); + values.insert(V(0), kZero); + values.insert(kBiasKey, estimatedBias_); +} + SharedDiagonal AggregateImuReadings::discreteAccelerometerNoiseModel( double dt) const { return noiseModel::Diagonal::Sigmas(accelerometerNoiseModel_->sigmas() / @@ -50,6 +66,32 @@ SharedDiagonal AggregateImuReadings::discreteGyroscopeNoiseModel( std::sqrt(dt)); } +void AggregateImuReadings::updateEstimate(const Vector3& measuredAcc, + const Vector3& measuredOmega, + double dt) { + // Get current estimates + const Vector3 theta = values.at(T(k_)); + const Vector3 pos = values.at(P(k_)); + const Vector3 vel = values.at(V(k_)); + + // Correct measurements + const Vector3 correctedAcc = measuredAcc - estimatedBias_.accelerometer(); + const Vector3 correctedOmega = measuredOmega - estimatedBias_.gyroscope(); + + // Calculate exact mean propagation + Matrix3 H; + const Rot3 R = Rot3::Expmap(theta, H); + const Vector3 theta_plus = theta + H.inverse() * correctedOmega * dt; + const Vector3 vel_plus = vel + R.rotate(correctedAcc) * dt; + const Vector3 vel_avg = 0.5 * (vel + vel_plus); + const Vector3 pos_plus = pos + vel_avg * dt; + + // Add those values to estimate and linearize around them + values.insert(T(k_ + 1), theta_plus); + values.insert(P(k_ + 1), pos_plus); + values.insert(V(k_ + 1), vel_plus); +} + NonlinearFactorGraph AggregateImuReadings::createGraph( const Vector3_& theta_, const Vector3_& pos_, const Vector3_& vel_, const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) const { @@ -86,11 +128,7 @@ AggregateImuReadings::SharedBayesNet AggregateImuReadings::initPosterior( // We create a factor graph and then compute P(zeta|bias) auto graph = createGraph(zero_, zero_, zero_, measuredAcc, measuredOmega, dt); - // These values are exact the first time - values.insert(T(k_ + 1), measuredOmega * dt); - values.insert(P(k_ + 1), measuredAcc * (0.5 * dt * dt)); - values.insert(V(k_ + 1), measuredAcc * dt); - values.insert(kBiasKey, estimatedBias_); + // Linearize using updated values (updateEstimate must have been called) auto linear_graph = graph.linearize(values); // eliminate all but biases @@ -99,35 +137,17 @@ AggregateImuReadings::SharedBayesNet AggregateImuReadings::initPosterior( return linear_graph->eliminatePartialSequential(keys, EliminateQR).first; } -AggregateImuReadings::SharedBayesNet AggregateImuReadings::integrateCorrected( +AggregateImuReadings::SharedBayesNet AggregateImuReadings::updatePosterior( const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) { static const Vector3 kZero(Vector3::Zero()); static const auto constrModel = noiseModel::Constrained::All(3); // We create a factor graph and then compute P(zeta|bias) + // TODO(frank): Expmap and ExpmapDerivative are called again :-( auto graph = createGraph(Vector3_(T(k_)), Vector3_(P(k_)), Vector3_(V(k_)), measuredAcc, measuredOmega, dt); - // Get current estimates - const Vector3 theta = values.at(T(k_)); - const Vector3 pos = values.at(P(k_)); - const Vector3 vel = values.at(V(k_)); - - // Calculate exact solution: means we do not have to update values - // TODO(frank): Expmap and ExpmapDerivative are called again :-( - const Vector3 correctedAcc = measuredAcc - estimatedBias_.accelerometer(); - const Vector3 correctedOmega = measuredOmega - estimatedBias_.gyroscope(); - Matrix3 H; - const Rot3 R = Rot3::Expmap(theta, H); - const Vector3 theta_plus = theta + H.inverse() * correctedOmega * dt; - const Vector3 vel_plus = vel + R.rotate(correctedAcc) * dt; - const Vector3 vel_avg = 0.5 * (vel + vel_plus); - const Vector3 pos_plus = pos + vel_avg * dt; - - // Add those values to estimate and linearize around them - values.insert(T(k_ + 1), theta_plus); - values.insert(P(k_ + 1), pos_plus); - values.insert(V(k_ + 1), vel_plus); + // Linearize using updated values (updateEstimate must have been called) auto linear_graph = graph.linearize(values); // add previous posterior @@ -157,11 +177,15 @@ void AggregateImuReadings::integrateMeasurement(const Vector3& measuredAcc, double dt) { typedef map Terms; - // Handle first time differently + // Do exact mean propagation + updateEstimate(measuredAcc, measuredOmega, dt); + + // Use factor graph machinery to linearize aroud exact propagation and + // calculate posterior density on the prediction if (k_ == 0) posterior_k_ = initPosterior(measuredAcc, measuredOmega, dt); else - posterior_k_ = integrateCorrected(measuredAcc, measuredOmega, dt); + posterior_k_ = updatePosterior(measuredAcc, measuredOmega, dt); // increment counter and time k_ += 1; @@ -187,7 +211,7 @@ NavState AggregateImuReadings::predict(const NavState& state_i, Vector3 vel = values.at(V(k_)); // Correct for initial velocity and gravity -#if 0 +#if 1 Rot3 Ri = state_i.attitude(); Matrix3 Rit = Ri.transpose(); Vector3 gt = deltaTij_ * p_->n_gravity; @@ -202,13 +226,32 @@ NavState AggregateImuReadings::predict(const NavState& state_i, } SharedGaussian AggregateImuReadings::noiseModel() const { + // Get covariance on zeta from Bayes Net, which stores P(zeta|bias) as a + // quadratic |R*zeta + S*bias -d|^2 Matrix RS; Vector d; boost::tie(RS, d) = posterior_k_->matrix(); + // NOTEfrank): R'*R = inv(zetaCov) + const Matrix9 R = RS.block<9, 9>(0, 0); + Matrix9 zetaCov = (R.transpose() * R).inverse(); + + // Correct for application of retract, by calcuating the retract derivative H + // TODO(frank): yet another application of expmap and expmap derivative + Vector3 theta = values.at(T(k_)); + Matrix3 D_R_theta; + const Rot3 iRb = Rot3::Expmap(theta, D_R_theta); + Matrix9 H; + H << D_R_theta, Z_3x3, Z_3x3, // + Z_3x3, iRb.transpose(), Z_3x3, // + Z_3x3, Z_3x3, iRb.transpose(); + Matrix9 predictCov = H * zetaCov * H.transpose(); - // R'*R = A'*A = inv(Cov) // TODO(frank): think of a faster way - implement in noiseModel - return noiseModel::Gaussian::SqrtInformation(RS.block<9, 9>(0, 0), false); + return noiseModel::Gaussian::Covariance(predictCov, false); + + // TODO(frank): can we use SqrtInformation like below? + // Matrix3 predictSqrtInfo = H * R; + // return noiseModel::Gaussian::SqrtInformation(predictSqrtInfo, false); } Matrix9 AggregateImuReadings::preintMeasCov() const { diff --git a/gtsam/navigation/AggregateImuReadings.h b/gtsam/navigation/AggregateImuReadings.h index e3ab143d1..eb59c3b46 100644 --- a/gtsam/navigation/AggregateImuReadings.h +++ b/gtsam/navigation/AggregateImuReadings.h @@ -67,13 +67,7 @@ class AggregateImuReadings { public: AggregateImuReadings(const boost::shared_ptr& p, - const Bias& estimatedBias = Bias()) - : p_(p), - accelerometerNoiseModel_(Diagonal(p->accelerometerCovariance)), - gyroscopeNoiseModel_(Diagonal(p->gyroscopeCovariance)), - estimatedBias_(estimatedBias), - k_(0), - deltaTij_(0.0) {} + const Bias& estimatedBias = Bias()); // We obtain discrete-time noise models by dividing the continuous-time // covariances by dt: @@ -104,6 +98,9 @@ class AggregateImuReadings { Matrix9 preintMeasCov() const; private: + void updateEstimate(const Vector3& measuredAcc, const Vector3& measuredOmega, + double dt); + NonlinearFactorGraph createGraph(const Vector3_& theta_, const Vector3_& pose_, const Vector3_& vel_, const Vector3& measuredAcc, @@ -115,8 +112,8 @@ class AggregateImuReadings { const Vector3& measuredOmega, double dt); // integrate - SharedBayesNet integrateCorrected(const Vector3& measuredAcc, - const Vector3& measuredOmega, double dt); + SharedBayesNet updatePosterior(const Vector3& measuredAcc, + const Vector3& measuredOmega, double dt); }; } // namespace gtsam diff --git a/gtsam/navigation/ScenarioRunner.cpp b/gtsam/navigation/ScenarioRunner.cpp index d443024cf..a6fea095d 100644 --- a/gtsam/navigation/ScenarioRunner.cpp +++ b/gtsam/navigation/ScenarioRunner.cpp @@ -60,9 +60,9 @@ Matrix9 ScenarioRunner::estimateCovariance(double T, size_t N, Vector9 sum = Vector9::Zero(); for (size_t i = 0; i < N; i++) { auto pim = integrate(T, estimatedBias, true); -#if 0 +#if 1 NavState sampled = predict(pim); - Vector9 zeta = sampled.localCoordinates(prediction); + Vector9 xi = sampled.localCoordinates(prediction); #else Vector9 xi = pim.zeta(); #endif diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h index 02dfa5515..c8b5efc15 100644 --- a/gtsam/navigation/ScenarioRunner.h +++ b/gtsam/navigation/ScenarioRunner.h @@ -86,7 +86,7 @@ class ScenarioRunner { const Bias& estimatedBias = Bias()) const; /// Compute a Monte Carlo estimate of the predict covariance using N samples - Matrix9 estimateCovariance(double T, size_t N = 10000, + Matrix9 estimateCovariance(double T, size_t N = 1000, const Bias& estimatedBias = Bias()) const; /// Estimate covariance of sampled noise for sanity-check