diff --git a/gtsam/navigation/AggregateImuReadings.cpp b/gtsam/navigation/AggregateImuReadings.cpp index ef0419bed..bd7297b38 100644 --- a/gtsam/navigation/AggregateImuReadings.cpp +++ b/gtsam/navigation/AggregateImuReadings.cpp @@ -25,8 +25,10 @@ namespace gtsam { AggregateImuReadings::AggregateImuReadings(const boost::shared_ptr& p, const Bias& estimatedBias) : p_(p), - accelerometerNoiseModel_(Diagonal(p->accelerometerCovariance)), - gyroscopeNoiseModel_(Diagonal(p->gyroscopeCovariance)), + accelerometerNoiseModel_( + noiseModel::Gaussian::Covariance(p->accelerometerCovariance, true)), + gyroscopeNoiseModel_( + noiseModel::Gaussian::Covariance(p->gyroscopeCovariance, true)), estimatedBias_(estimatedBias), k_(0), deltaTij_(0.0) { @@ -34,28 +36,19 @@ AggregateImuReadings::AggregateImuReadings(const boost::shared_ptr& p, cov_.setZero(); } -SharedDiagonal AggregateImuReadings::discreteAccelerometerNoiseModel( - double dt) const { - return noiseModel::Diagonal::Sigmas(accelerometerNoiseModel_->sigmas() / - std::sqrt(dt)); -} - -SharedDiagonal AggregateImuReadings::discreteGyroscopeNoiseModel( - double dt) const { - return noiseModel::Diagonal::Sigmas(gyroscopeNoiseModel_->sigmas() / - std::sqrt(dt)); -} - // Tangent space sugar. namespace sugar { -typedef const Vector9 constV9; + static Eigen::Block dR(Vector9& v) { return v.segment<3>(0); } static Eigen::Block dP(Vector9& v) { return v.segment<3>(3); } static Eigen::Block dV(Vector9& v) { return v.segment<3>(6); } + +typedef const Vector9 constV9; static Eigen::Block dR(constV9& v) { return v.segment<3>(0); } static Eigen::Block dP(constV9& v) { return v.segment<3>(3); } static Eigen::Block dV(constV9& v) { return v.segment<3>(6); } -} + +} // namespace sugar Vector9 AggregateImuReadings::UpdateEstimate( const Vector9& zeta, const Vector3& correctedAcc, @@ -63,24 +56,38 @@ Vector9 AggregateImuReadings::UpdateEstimate( OptionalJacobian<9, 3> Ba, OptionalJacobian<9, 3> Bw) { using namespace sugar; + const Vector3 a_dt = correctedAcc * dt; + const Vector3 w_dt = correctedOmega * dt; + // Calculate exact mean propagation - Matrix3 dexp; - const Rot3 R = Rot3::Expmap(dR(zeta), dexp); - const Matrix3 F = dexp.inverse() * dt, H = R.matrix() * dt; + Matrix3 D_R_theta; + const Rot3 R = Rot3::Expmap(dR(zeta), D_R_theta).matrix(); + const Matrix3 invH = D_R_theta.inverse(); + + Matrix3 D_Radt_R, D_Radt_adt; + const Vector3 Radt = R.rotate(a_dt, A ? &D_Radt_R : 0, A ? &D_Radt_adt : 0); Vector9 zeta_plus; - dR(zeta_plus) = dR(zeta) + F * correctedOmega; - dP(zeta_plus) = dP(zeta) + dV(zeta) * dt + H * (0.5 * dt) * correctedAcc; - dV(zeta_plus) = dV(zeta) + H * correctedAcc; + const double dt2 = 0.5 * dt; + dR(zeta_plus) = dR(zeta) + invH * w_dt; + dP(zeta_plus) = dP(zeta) + dV(zeta) * dt + Radt * dt2; + dV(zeta_plus) = dV(zeta) + Radt; if (A) { - const Matrix3 Avt = skewSymmetric(-correctedAcc * dt); + // Exact derivative of R*a*dt with respect to theta: + const Matrix3 D_Radt_theta = D_Radt_R * D_R_theta; + + // First order (small angle) approximation of derivative of invH*w*dt: + const Matrix3 D_invHwdt_theta = skewSymmetric(-0.5 * w_dt); + A->setIdentity(); - A->block<3, 3>(6, 0) = Avt; + A->block<3, 3>(0, 0) += D_invHwdt_theta; + A->block<3, 3>(3, 0) = D_Radt_theta * dt2; A->block<3, 3>(3, 6) = I_3x3 * dt; + A->block<3, 3>(6, 0) = D_Radt_theta; } - if (Ba) *Ba << Z_3x3, H*(dt * 0.5), H; - if (Bw) *Bw << F, Z_3x3, Z_3x3; + if (Ba) *Ba << Z_3x3, D_Radt_adt* dt* dt2, D_Radt_adt* dt; + if (Bw) *Bw << invH* dt, Z_3x3, Z_3x3; return zeta_plus; } diff --git a/gtsam/navigation/AggregateImuReadings.h b/gtsam/navigation/AggregateImuReadings.h index f53c80629..219dfeb49 100644 --- a/gtsam/navigation/AggregateImuReadings.h +++ b/gtsam/navigation/AggregateImuReadings.h @@ -22,16 +22,6 @@ namespace gtsam { -// Convert covariance to diagonal noise model, if possible, otherwise throw -static noiseModel::Diagonal::shared_ptr Diagonal(const Matrix& covariance) { - bool smart = true; - auto model = noiseModel::Gaussian::Covariance(covariance, smart); - auto diagonal = boost::dynamic_pointer_cast(model); - if (!diagonal) - throw std::invalid_argument("ScenarioRunner::Diagonal: not a diagonal"); - return diagonal; -} - /** * Class that integrates state estimate on the manifold. * We integrate zeta = [theta, position, velocity] @@ -44,7 +34,7 @@ class AggregateImuReadings { private: const boost::shared_ptr p_; - const SharedDiagonal accelerometerNoiseModel_, gyroscopeNoiseModel_; + const SharedGaussian accelerometerNoiseModel_, gyroscopeNoiseModel_; const Bias estimatedBias_; size_t k_; ///< index/count of measurements integrated @@ -61,12 +51,6 @@ class AggregateImuReadings { const Vector9& zeta() const { return zeta_; } const Matrix9& zetaCov() const { return cov_; } - // We obtain discrete-time noise models by dividing the continuous-time - // covariances by dt: - - SharedDiagonal discreteAccelerometerNoiseModel(double dt) const; - SharedDiagonal discreteGyroscopeNoiseModel(double dt) const; - /** * Add a single IMU measurement to the preintegration. * @param measuredAcc Measured acceleration (in body frame) @@ -91,9 +75,9 @@ class AggregateImuReadings { static Vector9 UpdateEstimate(const Vector9& zeta, const Vector3& correctedAcc, const Vector3& correctedOmega, double dt, - OptionalJacobian<9, 9> A, - OptionalJacobian<9, 3> Ba, - OptionalJacobian<9, 3> Bw); + OptionalJacobian<9, 9> A = boost::none, + OptionalJacobian<9, 3> Ba = boost::none, + OptionalJacobian<9, 3> Bw = boost::none); }; } // namespace gtsam diff --git a/gtsam/navigation/ScenarioRunner.cpp b/gtsam/navigation/ScenarioRunner.cpp index e71e13a23..6ff263510 100644 --- a/gtsam/navigation/ScenarioRunner.cpp +++ b/gtsam/navigation/ScenarioRunner.cpp @@ -65,7 +65,7 @@ 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 1 +#if 0 NavState sampled = predict(pim); Vector9 xi = sampled.localCoordinates(prediction); #else diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h index 00575d414..c94b6a00b 100644 --- a/gtsam/navigation/ScenarioRunner.h +++ b/gtsam/navigation/ScenarioRunner.h @@ -22,6 +22,16 @@ namespace gtsam { +// Convert covariance to diagonal noise model, if possible, otherwise throw +static noiseModel::Diagonal::shared_ptr Diagonal(const Matrix& covariance) { + bool smart = true; + auto model = noiseModel::Gaussian::Covariance(covariance, smart); + auto diagonal = boost::dynamic_pointer_cast(model); + if (!diagonal) + throw std::invalid_argument("ScenarioRunner::Diagonal: not a diagonal"); + return diagonal; +} + /* * Simple class to test navigation scenarios. * Takes a trajectory scenario as input, and can generate IMU measurements diff --git a/gtsam/navigation/tests/testAggregateImuReadings.cpp b/gtsam/navigation/tests/testAggregateImuReadings.cpp index 25b3efd7c..9454a929d 100644 --- a/gtsam/navigation/tests/testAggregateImuReadings.cpp +++ b/gtsam/navigation/tests/testAggregateImuReadings.cpp @@ -178,12 +178,12 @@ TEST(AggregateImuReadings, UpdateEstimate) { boost::bind(&AggregateImuReadings::UpdateEstimate, _1, _2, _3, kDt, boost::none, boost::none, boost::none); Vector9 zeta; - zeta << 0.1, 0.2, 0.3, 0.1, 0.2, 0.3, 0.1, 0.2, 0.3; - const Vector3 acc(0.1, 0.2, 0.3), omega(0.1, 0.2, 0.3); + zeta << 0.01, 0.02, 0.03, 100, 200, 300, 10, 5, 3; + const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3); pim.UpdateEstimate(zeta, acc, omega, kDt, aH1, aH2, aH3); - EXPECT(assert_equal(numericalDerivative31(f, zeta, acc, omega), aH1)); - EXPECT(assert_equal(numericalDerivative32(f, zeta, acc, omega), aH2)); - EXPECT(assert_equal(numericalDerivative33(f, zeta, acc, omega), aH3)); + EXPECT(assert_equal(numericalDerivative31(f, zeta, acc, omega), aH1, 1e-3)); + EXPECT(assert_equal(numericalDerivative32(f, zeta, acc, omega), aH2, 1e-5)); + EXPECT(assert_equal(numericalDerivative33(f, zeta, acc, omega), aH3, 1e-5)); } /* ************************************************************************* */ diff --git a/gtsam/navigation/tests/testScenarioRunner.cpp b/gtsam/navigation/tests/testScenarioRunner.cpp index c84c7f2f5..591a7d3d2 100644 --- a/gtsam/navigation/tests/testScenarioRunner.cpp +++ b/gtsam/navigation/tests/testScenarioRunner.cpp @@ -56,12 +56,6 @@ TEST(ScenarioRunner, Spin) { auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); - // Check noise model agreement - EXPECT(assert_equal(p->accelerometerCovariance / kDt, - pim.discreteAccelerometerNoiseModel(kDt)->covariance())); - EXPECT(assert_equal(p->gyroscopeCovariance / kDt, - pim.discreteGyroscopeNoiseModel(kDt)->covariance())); - #if 0 // Check sampled noise is kosher Matrix6 expected;