diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp new file mode 100644 index 000000000..3fa28261e --- /dev/null +++ b/gtsam/navigation/AHRSFactor.cpp @@ -0,0 +1,159 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file AHRSFactor.cpp + * @author Krunal Chande + * @author Luca Carlone + * @date July 2014 + **/ + +#include + +/* GTSAM includes */ +#include +#include +#include +#include + +/* External or standard includes */ +#include + +namespace gtsam { + +//------------------------------------------------------------------------------ +// Inner class PreintegratedMeasurements +//------------------------------------------------------------------------------ +AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements( + const imuBias::ConstantBias& bias, const Matrix3& measuredOmegaCovariance) : + biasHat_(bias), deltaTij_(0.0) { + measurementCovariance_ << measuredOmegaCovariance; + delRdelBiasOmega_.setZero(); + PreintMeasCov_.setZero(); +} + +//------------------------------------------------------------------------------ +AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements() : + biasHat_(imuBias::ConstantBias()), deltaTij_(0.0) { + measurementCovariance_.setZero(); + delRdelBiasOmega_.setZero(); + delRdelBiasOmega_.setZero(); + PreintMeasCov_.setZero(); +} + +//------------------------------------------------------------------------------ +void AHRSFactor::PreintegratedMeasurements::print(const std::string& s) const { + std::cout << s << std::endl; + biasHat_.print(" biasHat"); + deltaRij_.print(" deltaRij "); + std::cout << " measurementCovariance [" << measurementCovariance_ << " ]" + << std::endl; + std::cout << " PreintMeasCov [ " << PreintMeasCov_ << " ]" << std::endl; +} + +//------------------------------------------------------------------------------ +bool AHRSFactor::PreintegratedMeasurements::equals( + const PreintegratedMeasurements& expected, double tol) const { + return biasHat_.equals(expected.biasHat_, tol) + && equal_with_abs_tol(measurementCovariance_, + expected.measurementCovariance_, tol) + && deltaRij_.equals(expected.deltaRij_, tol) + && std::fabs(deltaTij_ - expected.deltaTij_) < tol + && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol); +} + +//------------------------------------------------------------------------------ +void AHRSFactor::PreintegratedMeasurements::resetIntegration() { + deltaRij_ = Rot3(); + deltaTij_ = 0.0; + delRdelBiasOmega_.setZero(); + PreintMeasCov_.setZero(); +} + +//------------------------------------------------------------------------------ +void AHRSFactor::PreintegratedMeasurements::integrateMeasurement( + const Vector3& measuredOmega, double deltaT, + boost::optional body_P_sensor) { + + // NOTE: order is important here because each update uses old values. + // First we compensate the measurements for the bias + Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega); + + // Then compensate for sensor-body displacement: we express the quantities + // (originally in the IMU frame) into the body frame + if (body_P_sensor) { + Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); + // rotation rate vector in the body frame + correctedOmega = body_R_sensor * correctedOmega; + } + + // rotation vector describing rotation increment computed from the + // current rotation rate measurement + const Vector3 theta_incr = correctedOmega * deltaT; + + // rotation increment computed from the current rotation rate measurement + const Rot3 Rincr = Rot3::Expmap(theta_incr); + + // Right jacobian computed at theta_incr + const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); + + // Update Jacobians + // --------------------------------------------------------------------------- + delRdelBiasOmega_ = Rincr.transpose() * delRdelBiasOmega_ + - Jr_theta_incr * deltaT; + + // Update preintegrated measurements covariance + // --------------------------------------------------------------------------- + const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3) + const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3inverse(theta_i); + + Rot3 Rot_j = deltaRij_ * Rincr; + const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3) + const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j); + + // Update preintegrated measurements covariance: as in [2] we consider a first order propagation that + // can be seen as a prediction phase in an EKF framework + Matrix3 H_angles_angles = Jrinv_theta_j * Rincr.transpose() * Jr_theta_i; + // analytic expression corresponding to the following numerical derivative + // Matrix H_angles_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij); + + // overall Jacobian wrt preintegrated measurements (df/dx) + Matrix3 F; + F << H_angles_angles; + + // first order uncertainty propagation + // the deltaT allows to pass from continuous time noise to discrete time noise + PreintMeasCov_ = F * PreintMeasCov_ * F.transpose() + + measurementCovariance_ * deltaT; + + // Update preintegrated measurements + // --------------------------------------------------------------------------- + deltaRij_ = deltaRij_ * Rincr; + deltaTij_ += deltaT; +} + +//------------------------------------------------------------------------------ +Vector AHRSFactor::PreintegratedMeasurements::PreIntegrateIMUObservations_delta_angles( + const Vector& msr_gyro_t, const double msr_dt, + const Vector3& delta_angles) { + + // Note: all delta terms refer to an IMU\sensor system at t0 + + // Calculate the corrected measurements using the Bias object + Vector body_t_omega_body = msr_gyro_t; + + Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); + + R_t_to_t0 = R_t_to_t0 * Rot3::Expmap(body_t_omega_body * msr_dt); + return Rot3::Logmap(R_t_to_t0); +} + +} //namespace gtsam diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index 686beb204..e3c96b091 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -11,7 +11,9 @@ /** * @file AHRSFactor.h - * @author Krunal Chande, Luca Carlone + * @author Krunal Chande + * @author Luca Carlone + * @date July 2014 **/ #pragma once @@ -20,7 +22,6 @@ #include #include #include -#include #include /* External or standard includes */ @@ -37,138 +38,56 @@ public: /** CombinedPreintegratedMeasurements accumulates (integrates) the Gyroscope measurements (rotation rates) * and the corresponding covariance matrix. The measurements are then used to build the Preintegrated AHRS factor*/ class PreintegratedMeasurements { + friend class AHRSFactor; + protected: imuBias::ConstantBias biasHat_;///< Acceleration and angular rate bias values used during preintegration. Note that we won't be using the accelerometer - Matrix measurementCovariance_;///< (Raw measurements uncertainty) Covariance of the vector [measuredOmega] in R^(3X3) + Matrix3 measurementCovariance_;///< (Raw measurements uncertainty) Covariance of the vector [measuredOmega] in R^(3X3) Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) double deltaTij_; ///< Time interval from i to j Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias - Matrix PreintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*) - public: - /** Default constructor, initialize with no measurements */ - PreintegratedMeasurements( - const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases - const Matrix3& measuredOmegaCovariance ///< Covariance matrix of measured angular rate - ) : biasHat_(bias), measurementCovariance_(3,3), deltaTij_(0.0), - delRdelBiasOmega_(Matrix3::Zero()), PreintMeasCov_(3,3) { - measurementCovariance_ < body_P_sensor = boost::none ///< Sensor frame - ) { + ); - // NOTE: order is important here because each update uses old values. - // First we compensate the measurements for the bias - Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega); - - // Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame - if (body_P_sensor) { - Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); - correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame - // linear acceleration vector in the body frame - } - const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement - const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement - const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr - - // Update Jacobians - /* ----------------------------------------------------------------------------------------------------------------------- */ - delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - - Jr_theta_incr * deltaT; - - // Update preintegrated measurements covariance - /* ----------------------------------------------------------------------------------------------------------------------- */ - const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3) - const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3inverse(theta_i); - - Rot3 Rot_j = deltaRij_ * Rincr; - const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3) - const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse( - theta_j); - - // Update preintegrated measurements covariance: as in [2] we consider a first order propagation that - // can be seen as a prediction phase in an EKF framework - Matrix H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() - * Jr_theta_i; - // analytic expression corresponding to the following numerical derivative - // Matrix H_angles_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij); - - // overall Jacobian wrt preintegrated measurements (df/dx) - Matrix F(3, 3); - F << H_angles_angles; - - // first order uncertainty propagation - // the deltaT allows to pass from continuous time noise to discrete time noise - PreintMeasCov_ = F * PreintMeasCov_ * F.transpose() - + measurementCovariance_ * deltaT; - - // Update preintegrated measurements - /* ----------------------------------------------------------------------------------------------------------------------- */ - deltaRij_ = deltaRij_ * Rincr; - deltaTij_ += deltaT; - } - - /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) - static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt, - const Vector3& delta_angles){ - - // Note: all delta terms refer to an IMU\sensor system at t0 - - // Calculate the corrected measurements using the Bias object - Vector body_t_omega_body= msr_gyro_t; - - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); - - R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt ); - return Rot3::Logmap(R_t_to_t0); - } - /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ + static Vector PreIntegrateIMUObservations_delta_angles( + const Vector& msr_gyro_t, const double msr_dt, + const Vector3& delta_angles); private: /** Serialization function */