diff --git a/gtsam.h b/gtsam.h index 838c48efc..0c00047b1 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2299,7 +2299,8 @@ virtual class ConstantBias : gtsam::Value { #include class ImuFactorPreintegratedMeasurements { // Standard Constructor - ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance, Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance); + ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance, bool use2ndOrderIntegration); + ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance); ImuFactorPreintegratedMeasurements(const gtsam::ImuFactorPreintegratedMeasurements& rhs); // Testable @@ -2337,6 +2338,15 @@ class CombinedImuFactorPreintegratedMeasurements { Matrix biasAccCovariance, Matrix biasOmegaCovariance, Matrix biasAccOmegaInit); + CombinedImuFactorPreintegratedMeasurements( + const gtsam::imuBias::ConstantBias& bias, + Matrix measuredAccCovariance, + Matrix measuredOmegaCovariance, + Matrix integrationErrorCovariance, + Matrix biasAccCovariance, + Matrix biasOmegaCovariance, + Matrix biasAccOmegaInit, + bool use2ndOrderIntegration); CombinedImuFactorPreintegratedMeasurements(const gtsam::CombinedImuFactorPreintegratedMeasurements& rhs); // Testable @@ -2350,8 +2360,7 @@ class CombinedImuFactorPreintegratedMeasurements { virtual class CombinedImuFactor : gtsam::NonlinearFactor { CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias_i, size_t bias_j, - const gtsam::CombinedImuFactorPreintegratedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis, - const gtsam::noiseModel::Base* model); + const gtsam::CombinedImuFactorPreintegratedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis); // Standard Interface gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const; diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 06e79324d..725274acc 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -11,7 +11,7 @@ /** * @file CombinedImuFactor.h - * @author Luca Carlone, Stephen Williams, Richard Roberts, Vadim Indelman + * @author Luca Carlone, Stephen Williams, Richard Roberts, Vadim Indelman, David Jensen **/ #pragma once @@ -71,8 +71,9 @@ namespace gtsam { Matrix3 delVdelBiasAcc; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias Matrix3 delVdelBiasOmega; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias 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*) + bool use2ndOrderIntegration_; ///< Controls the order of integration + ///< In the combined factor is also includes the biases and keeps the correlation between the preintegrated measurements and the biases ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega] /** Default constructor, initialize with no IMU measurements */ @@ -83,12 +84,14 @@ namespace gtsam { const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc const Matrix3& biasAccCovariance, ///< Covariance matrix of biasAcc (random walk describing BIAS evolution) const Matrix3& biasOmegaCovariance, ///< Covariance matrix of biasOmega (random walk describing BIAS evolution) - const Matrix& biasAccOmegaInit ///< Covariance of biasAcc & biasOmega when preintegrating measurements + const Matrix& biasAccOmegaInit, ///< Covariance of biasAcc & biasOmega when preintegrating measurements + const bool use2ndOrderIntegration = false ///< Controls the order of integration ///< (this allows to consider the uncertainty of the BIAS choice when integrating the measurements) ) : biasHat(bias), measurementCovariance(21,21), deltaPij(Vector3::Zero()), deltaVij(Vector3::Zero()), deltaTij(0.0), delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()), delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()), - delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(Matrix::Zero(15,15)) + delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(Matrix::Zero(15,15)), + use2ndOrderIntegration_(use2ndOrderIntegration) { // COVARIANCE OF: [Integration AccMeasurement OmegaMeasurement BiasAccRandomWalk BiasOmegaRandomWalk (BiasAccInit BiasOmegaInit)] SIZE (21x21) measurementCovariance << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), @@ -136,6 +139,19 @@ namespace gtsam { && equal_with_abs_tol(delRdelBiasOmega, expected.delRdelBiasOmega, tol); } + void resetIntegration(){ + deltaPij = Vector3::Zero(); + deltaVij = Vector3::Zero(); + deltaRij = Rot3(); + deltaTij = 0.0; + delPdelBiasAcc = Matrix3::Zero(); + delPdelBiasOmega = Matrix3::Zero(); + delVdelBiasAcc = Matrix3::Zero(); + delVdelBiasOmega = Matrix3::Zero(); + delRdelBiasOmega = Matrix3::Zero(); + PreintMeasCov = Matrix::Zero(15,15); + } + /** Add a single IMU measurement to the preintegration. */ void integrateMeasurement( const Vector3& measuredAcc, ///< Measured linear acceleration (in body frame) @@ -163,11 +179,14 @@ namespace gtsam { // Update Jacobians /* ----------------------------------------------------------------------------------------------------------------------- */ - // delPdelBiasAcc += delVdelBiasAcc * deltaT; - // delPdelBiasOmega += delVdelBiasOmega * deltaT; - delPdelBiasAcc += delVdelBiasAcc * deltaT - 0.5 * deltaRij.matrix() * deltaT*deltaT; - delPdelBiasOmega += delVdelBiasOmega * deltaT - 0.5 * deltaRij.matrix() - * skewSymmetric(biasHat.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega; + if(!use2ndOrderIntegration_){ + delPdelBiasAcc += delVdelBiasAcc * deltaT; + delPdelBiasOmega += delVdelBiasOmega * deltaT; + }else{ + delPdelBiasAcc += delVdelBiasAcc * deltaT - 0.5 * deltaRij.matrix() * deltaT*deltaT; + delPdelBiasOmega += delVdelBiasOmega * deltaT - 0.5 * deltaRij.matrix() + * skewSymmetric(biasHat.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega; + } delVdelBiasAcc += -deltaRij.matrix() * deltaT; delVdelBiasOmega += -deltaRij.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega; @@ -222,32 +241,31 @@ namespace gtsam { G_measCov_Gt.block(0,0,3,3) = deltaT * measurementCovariance.block(0,0,3,3); G_measCov_Gt.block(3,3,3,3) = (1/deltaT) * (H_vel_biasacc) * - (measurementCovariance.block(3,3,3,3) + measurementCovariance.block(9,9,3,3) + measurementCovariance.block(15,15,3,3) ) * - (H_vel_biasacc.transpose()); + (measurementCovariance.block(3,3,3,3) + measurementCovariance.block(15,15,3,3) ) * + (H_vel_biasacc.transpose()); G_measCov_Gt.block(6,6,3,3) = (1/deltaT) * (H_angles_biasomega) * - (measurementCovariance.block(6,6,3,3) + measurementCovariance.block(12,12,3,3) + measurementCovariance.block(18,18,3,3) ) * - (H_angles_biasomega.transpose()); + (measurementCovariance.block(6,6,3,3) + measurementCovariance.block(18,18,3,3) ) * + (H_angles_biasomega.transpose()); G_measCov_Gt.block(9,9,3,3) = deltaT * measurementCovariance.block(9,9,3,3); G_measCov_Gt.block(12,12,3,3) = deltaT * measurementCovariance.block(12,12,3,3); - // OFF BLOCK DIAGONAL TERMS - Matrix3 block24 = H_vel_biasacc * measurementCovariance.block(9,9,3,3); - G_measCov_Gt.block(3,9,3,3) = block24; - G_measCov_Gt.block(9,3,3,3) = block24.transpose(); - - Matrix3 block35 = H_angles_biasomega * measurementCovariance.block(12,12,3,3); - G_measCov_Gt.block(6,12,3,3) = block35; - G_measCov_Gt.block(12,6,3,3) = block35.transpose(); + // NEW OFF BLOCK DIAGONAL TERMS + Matrix3 block23 = H_vel_biasacc * measurementCovariance.block(18,15,3,3) * H_angles_biasomega.transpose(); + G_measCov_Gt.block(3,6,3,3) = block23; + G_measCov_Gt.block(6,3,3,3) = block23.transpose(); PreintMeasCov = F * PreintMeasCov * F.transpose() + G_measCov_Gt; // Update preintegrated measurements /* ----------------------------------------------------------------------------------------------------------------------- */ - // deltaPij += deltaVij * deltaT; - deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT; + if(!use2ndOrderIntegration_){ + deltaPij += deltaVij * deltaT; + }else{ + deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT; + } deltaVij += deltaRij.matrix() * correctedAcc * deltaT; deltaRij = deltaRij * Rincr; deltaTij += deltaT; @@ -311,23 +329,40 @@ namespace gtsam { Vector3 omegaCoriolis_; boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect + public: /** Shorthand for a smart pointer to a factor */ - typedef boost::shared_ptr shared_ptr; +#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5 + typedef typename boost::shared_ptr shared_ptr; +#else + typedef boost::shared_ptr shared_ptr; +#endif /** Default constructor - only use for serialization */ CombinedImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6)) {} /** Constructor */ - CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, - const CombinedPreintegratedMeasurements& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis, - const SharedNoiseModel& model, boost::optional body_P_sensor = boost::none) : - Base(model, pose_i, vel_i, pose_j, vel_j, bias_i, bias_j), + CombinedImuFactor( + Key pose_i, ///< previous pose key + Key vel_i, ///< previous velocity key + Key pose_j, ///< current pose key + Key vel_j, ///< current velocity key + Key bias_i, ///< previous bias key + Key bias_j, ///< current bias key + const CombinedPreintegratedMeasurements& preintegratedMeasurements, ///< Preintegrated IMU measurements + const Vector3& gravity, ///< gravity vector + const Vector3& omegaCoriolis, ///< rotation rate of inertial frame + boost::optional body_P_sensor = boost::none, ///< The Pose of the sensor frame in the body frame + const bool use2ndOrderCoriolis = false ///< When true, the second-order term is used in the calculation of the Coriolis Effect + ) : + Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov), pose_i, vel_i, pose_j, vel_j, bias_i, bias_j), preintegratedMeasurements_(preintegratedMeasurements), gravity_(gravity), omegaCoriolis_(omegaCoriolis), - body_P_sensor_(body_P_sensor) { + body_P_sensor_(body_P_sensor), + use2ndOrderCoriolis_(use2ndOrderCoriolis){ } virtual ~CombinedImuFactor() {} @@ -360,7 +395,7 @@ namespace gtsam { virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); return e != NULL && Base::equals(*e, tol) - && preintegratedMeasurements_.equals(e->preintegratedMeasurements_) + && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol) && equal_with_abs_tol(gravity_, e->gravity_, tol) && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol) && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); @@ -370,6 +405,10 @@ namespace gtsam { const CombinedPreintegratedMeasurements& preintegratedMeasurements() const { return preintegratedMeasurements_; } + const Vector3& gravity() const { return gravity_; } + + const Vector3& omegaCoriolis() const { return omegaCoriolis_; } + /** implement functions needed to derive from Factor */ /** vector of errors */ @@ -414,19 +453,18 @@ namespace gtsam { const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); - if(H1) { - H1->resize(15,6); + /* (*H1) << // dfP/dRi Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr), // dfP/dPi - - Rot_i.matrix(), + - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij, // dfV/dRi Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr), // dfV/dPi - Matrix3::Zero(), + skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij, // dfR/dRi Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), // dfR/dPi @@ -435,6 +473,40 @@ namespace gtsam { Matrix3::Zero(), Matrix3::Zero(), //dBiasOmega/dPi Matrix3::Zero(), Matrix3::Zero(); + */ + if(H1) { + H1->resize(15,6); + + Matrix3 dfPdPi; + Matrix3 dfVdPi; + if(use2ndOrderCoriolis_){ + dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij; + dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij; + } + else{ + dfPdPi = - Rot_i.matrix(); + dfVdPi = Matrix3::Zero(); + } + + (*H1) << + // dfP/dRi + Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij + + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr), + // dfP/dPi + dfPdPi, + // dfV/dRi + Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij + + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr), + // dfV/dPi + dfVdPi, + // dfR/dRi + Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), + // dfR/dPi + Matrix3::Zero(), + //dBiasAcc/dPi + Matrix3::Zero(), Matrix3::Zero(), + //dBiasOmega/dPi + Matrix3::Zero(), Matrix3::Zero(); } if(H2) { @@ -445,14 +517,13 @@ namespace gtsam { + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper // dfV/dVi - Matrix3::Identity() - + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term - // dfR/dVi - Matrix3::Zero(), - //dBiasAcc/dVi - Matrix3::Zero(), - //dBiasOmega/dVi - Matrix3::Zero(); - + + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term + // dfR/dVi + Matrix3::Zero(), + //dBiasAcc/dVi + Matrix3::Zero(), + //dBiasOmega/dVi + Matrix3::Zero(); } if(H3) { @@ -524,7 +595,6 @@ namespace gtsam { Matrix3::Zero(), Matrix3::Identity(); } - // Evaluate residual error, according to [3] /* ---------------------------------------------------------------------------------------------------- */ const Vector3 fp = @@ -559,7 +629,8 @@ namespace gtsam { static void Predict(const Pose3& pose_i, const LieVector& vel_i, Pose3& pose_j, LieVector& vel_j, const imuBias::ConstantBias& bias_i, imuBias::ConstantBias& bias_j, const CombinedPreintegratedMeasurements& preintegratedMeasurements, - const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional body_P_sensor = boost::none) + const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional body_P_sensor = boost::none, + const bool use2ndOrderCoriolis = false) { const double& deltaTij = preintegratedMeasurements.deltaTij; @@ -571,18 +642,23 @@ namespace gtsam { // Predict state at time j /* ---------------------------------------------------------------------------------------------------- */ - const Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij - + preintegratedMeasurements.delPdelBiasAcc * biasAccIncr - + preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr) - + vel_i * deltaTij - - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - + 0.5 * gravity * deltaTij*deltaTij; + Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij + + preintegratedMeasurements.delPdelBiasAcc * biasAccIncr + + preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr) + + vel_i * deltaTij + - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper + + 0.5 * gravity * deltaTij*deltaTij; - vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij - + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr - + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr) - - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term - + gravity * deltaTij); + vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij + + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr + + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr) + - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term + + gravity * deltaTij); + + if(use2ndOrderCoriolis){ + pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position + vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity + } const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij.retract(preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP); // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index f5b5293f8..d68af4f8b 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -11,7 +11,7 @@ /** * @file ImuFactor.h - * @author Luca Carlone, Stephen Williams, Richard Roberts, Vadim Indelman + * @author Luca Carlone, Stephen Williams, Richard Roberts, Vadim Indelman, David Jensen **/ #pragma once @@ -60,7 +60,7 @@ namespace gtsam { imuBias::ConstantBias biasHat; ///< Acceleration and angular rate bias values used during preintegration Matrix measurementCovariance; ///< (Raw measurements uncertainty) Covariance of the vector [integrationError measuredAcc measuredOmega] in R^(9X9) - Vector3 deltaPij; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, , in [2]) (in frame i) + Vector3 deltaPij; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i) Vector3 deltaVij; ///< Preintegrated relative velocity (in global frame) Rot3 deltaRij; ///< Preintegrated relative orientation (in frame i) double deltaTij; ///< Time interval from i to j @@ -70,19 +70,20 @@ namespace gtsam { Matrix3 delVdelBiasAcc; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias Matrix3 delVdelBiasOmega; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias 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*) + bool use2ndOrderIntegration_; ///< Controls the order of integration /** Default constructor, initialize with no IMU measurements */ PreintegratedMeasurements( const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc - const Matrix3& integrationErrorCovariance ///< Covariance matrix of measuredAcc + const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc + const bool use2ndOrderIntegration = false ///< Controls the order of integration ) : biasHat(bias), measurementCovariance(9,9), deltaPij(Vector3::Zero()), deltaVij(Vector3::Zero()), deltaTij(0.0), delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()), delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()), - delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(9,9) + delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(9,9), use2ndOrderIntegration_(use2ndOrderIntegration) { measurementCovariance << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), measuredAccCovariance, Matrix3::Zero(), @@ -127,6 +128,19 @@ namespace gtsam { && equal_with_abs_tol(delRdelBiasOmega, expected.delRdelBiasOmega, tol); } + void resetIntegration(){ + deltaPij = Vector3::Zero(); + deltaVij = Vector3::Zero(); + deltaRij = Rot3(); + deltaTij = 0.0; + delPdelBiasAcc = Matrix3::Zero(); + delPdelBiasOmega = Matrix3::Zero(); + delVdelBiasAcc = Matrix3::Zero(); + delVdelBiasOmega = Matrix3::Zero(); + delRdelBiasOmega = Matrix3::Zero(); + PreintMeasCov = Matrix::Zero(9,9); + } + /** Add a single IMU measurement to the preintegration. */ void integrateMeasurement( const Vector3& measuredAcc, ///< Measured linear acceleration (in body frame) @@ -143,11 +157,8 @@ namespace gtsam { // 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 - Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); - correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); // linear acceleration vector in the body frame } @@ -159,16 +170,19 @@ namespace gtsam { // Update Jacobians /* ----------------------------------------------------------------------------------------------------------------------- */ - // delPdelBiasAcc += delVdelBiasAcc * deltaT; - // delPdelBiasOmega += delVdelBiasOmega * deltaT; - delPdelBiasAcc += delVdelBiasAcc * deltaT - 0.5 * deltaRij.matrix() * deltaT*deltaT; - delPdelBiasOmega += delVdelBiasOmega * deltaT - 0.5 * deltaRij.matrix() - * skewSymmetric(biasHat.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega; + if(!use2ndOrderIntegration_){ + delPdelBiasAcc += delVdelBiasAcc * deltaT; + delPdelBiasOmega += delVdelBiasOmega * deltaT; + }else{ + delPdelBiasAcc += delVdelBiasAcc * deltaT - 0.5 * deltaRij.matrix() * deltaT*deltaT; + delPdelBiasOmega += delVdelBiasOmega * deltaT - 0.5 * deltaRij.matrix() + * skewSymmetric(biasHat.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega; + } delVdelBiasAcc += -deltaRij.matrix() * deltaT; delVdelBiasOmega += -deltaRij.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega; delRdelBiasOmega = Rincr.inverse().matrix() * delRdelBiasOmega - Jr_theta_incr * deltaT; - // Update preintegrated mesurements covariance + // Update preintegrated measurements covariance /* ----------------------------------------------------------------------------------------------------------------------- */ Matrix3 Z_3x3 = Matrix3::Zero(); Matrix3 I_3x3 = Matrix3::Identity(); @@ -210,7 +224,11 @@ namespace gtsam { // Update preintegrated measurements /* ----------------------------------------------------------------------------------------------------------------------- */ - deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT; + if(!use2ndOrderIntegration_){ + deltaPij += deltaVij * deltaT; + }else{ + deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT; + } deltaVij += deltaRij.matrix() * correctedAcc * deltaT; deltaRij = deltaRij * Rincr; deltaTij += deltaT; @@ -274,24 +292,39 @@ namespace gtsam { Vector3 omegaCoriolis_; boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect + public: /** Shorthand for a smart pointer to a factor */ +#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5 + typedef typename boost::shared_ptr shared_ptr; +#else typedef boost::shared_ptr shared_ptr; - +#endif /** Default constructor - only use for serialization */ ImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()) {} /** Constructor */ - ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, - const PreintegratedMeasurements& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis, - boost::optional body_P_sensor = boost::none) : + ImuFactor( + Key pose_i, ///< previous pose key + Key vel_i, ///< previous velocity key + Key pose_j, ///< current pose key + Key vel_j, ///< current velocity key + Key bias, ///< previous bias key + const PreintegratedMeasurements& preintegratedMeasurements, ///< preintegrated IMU measurements + const Vector3& gravity, ///< gravity vector + const Vector3& omegaCoriolis, ///< rotation rate of the inertial frame + boost::optional body_P_sensor = boost::none, ///< The Pose of the sensor frame in the body frame + const bool use2ndOrderCoriolis = false ///< When true, the second-order term is used in the calculation of the Coriolis Effect + ) : Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov), pose_i, vel_i, pose_j, vel_j, bias), preintegratedMeasurements_(preintegratedMeasurements), gravity_(gravity), omegaCoriolis_(omegaCoriolis), - body_P_sensor_(body_P_sensor) { + body_P_sensor_(body_P_sensor), + use2ndOrderCoriolis_(use2ndOrderCoriolis){ } virtual ~ImuFactor() {} @@ -323,7 +356,7 @@ namespace gtsam { virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); return e != NULL && Base::equals(*e, tol) - && preintegratedMeasurements_.equals(e->preintegratedMeasurements_) + && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol) && equal_with_abs_tol(gravity_, e->gravity_, tol) && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol) && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); @@ -333,6 +366,10 @@ namespace gtsam { const PreintegratedMeasurements& preintegratedMeasurements() const { return preintegratedMeasurements_; } + const Vector3& gravity() const { return gravity_; } + + const Vector3& omegaCoriolis() const { return omegaCoriolis_; } + /** implement functions needed to derive from Factor */ /** vector of errors */ @@ -378,22 +415,35 @@ namespace gtsam { if(H1) { H1->resize(9,6); - (*H1) << - // dfP/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij - + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr), - // dfP/dPi - - Rot_i.matrix(), - // dfV/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij - + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr), - // dfV/dPi - Matrix3::Zero(), - // dfR/dRi - Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), - // dfR/dPi - Matrix3::Zero(); + + Matrix3 dfPdPi; + Matrix3 dfVdPi; + if(use2ndOrderCoriolis_){ + dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij; + dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij; + } + else{ + dfPdPi = - Rot_i.matrix(); + dfVdPi = Matrix3::Zero(); + } + + (*H1) << + // dfP/dRi + Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij + + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr), + // dfP/dPi + dfPdPi, + // dfV/dRi + Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij + + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr), + // dfV/dPi + dfVdPi, + // dfR/dRi + Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), + // dfR/dPi + Matrix3::Zero(); } + if(H2) { H2->resize(9,3); (*H2) << @@ -402,11 +452,11 @@ namespace gtsam { + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper // dfV/dVi - Matrix3::Identity() - + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term - // dfR/dVi - Matrix3::Zero(); - + + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term + // dfR/dVi + Matrix3::Zero(); } + if(H3) { H3->resize(9,6); @@ -418,6 +468,7 @@ namespace gtsam { // dfR/dPosej Jrinv_fRhat * ( Matrix3::Identity() ), Matrix3::Zero(); } + if(H4) { H4->resize(9,3); (*H4) << @@ -428,6 +479,7 @@ namespace gtsam { // dfR/dVj Matrix3::Zero(); } + if(H5) { const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); @@ -475,7 +527,8 @@ namespace gtsam { /** predicted states from IMU */ static void Predict(const Pose3& pose_i, const LieVector& vel_i, Pose3& pose_j, LieVector& vel_j, const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements, - const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional body_P_sensor = boost::none) + const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional body_P_sensor = boost::none, + const bool use2ndOrderCoriolis = false) { const double& deltaTij = preintegratedMeasurements.deltaTij; @@ -487,18 +540,23 @@ namespace gtsam { // Predict state at time j /* ---------------------------------------------------------------------------------------------------- */ - const Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij - + preintegratedMeasurements.delPdelBiasAcc * biasAccIncr - + preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr) - + vel_i * deltaTij - - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - + 0.5 * gravity * deltaTij*deltaTij; + Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij + + preintegratedMeasurements.delPdelBiasAcc * biasAccIncr + + preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr) + + vel_i * deltaTij + - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper + + 0.5 * gravity * deltaTij*deltaTij; - vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij - + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr - + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr) - - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term - + gravity * deltaTij); + vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij + + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr + + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr) + - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term + + gravity * deltaTij); + + if(use2ndOrderCoriolis){ + pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position + vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity + } const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij.retract(preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP); // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 10d582287..1a8b6160d 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -183,7 +183,7 @@ TEST( CombinedImuFactor, ErrorWithBiases ) ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.PreintMeasCov); - CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis, Combinedmodel); + CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis); Vector errorExpected = factor.evaluateError(x1, v1, x2, v2, bias); @@ -260,7 +260,7 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc)); EXPECT(assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega)); EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3,3))); - EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega)); + EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations } #include diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 6cd286f68..a6894898b 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -136,8 +136,9 @@ TEST( ImuFactor, PreintegratedMeasurements ) Rot3 expectedDeltaR1 = Rot3::RzRyRx(0.5 * M_PI/100.0, 0.0, 0.0); double expectedDeltaT1(0.5); + bool use2ndOrderIntegration = true; // Actual preintegrated values - ImuFactor::PreintegratedMeasurements actual1(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); + ImuFactor::PreintegratedMeasurements actual1(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration); actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); EXPECT(assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij), 1e-6)); @@ -177,7 +178,8 @@ TEST( ImuFactor, Error ) Vector3 measuredOmega; measuredOmega << M_PI/100, 0, 0; Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector(); double deltaT = 1.0; - ImuFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); + bool use2ndOrderIntegration = true; + ImuFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration); pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor @@ -217,7 +219,7 @@ TEST( ImuFactor, Error ) Matrix H1atop6 = H1a.topRows(6); EXPECT(assert_equal(H1etop6, H1atop6)); // rotations - EXPECT(assert_equal(RH1e, H1a.bottomRows(3))); // evaluate only the rotation residue + EXPECT(assert_equal(RH1e, H1a.bottomRows(3), 1e-5)); // 1e-5 needs to be added only when using quaternions for rotations EXPECT(assert_equal(H2e, H2a)); @@ -226,7 +228,7 @@ TEST( ImuFactor, Error ) Matrix H3atop6 = H3a.topRows(6); EXPECT(assert_equal(H3etop6, H3atop6)); // rotations - EXPECT(assert_equal(RH3e, H3a.bottomRows(3))); // evaluate only the rotation residue + EXPECT(assert_equal(RH3e, H3a.bottomRows(3), 1e-5)); // 1e-5 needs to be added only when using quaternions for rotations EXPECT(assert_equal(H4e, H4a)); // EXPECT(assert_equal(H5e, H5a)); @@ -324,7 +326,7 @@ TEST( ImuFactor, PartialDerivativeExpmap ) Matrix3 actualdelRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign // Compare Jacobians - EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega)); + EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations } @@ -436,7 +438,7 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements ) EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc)); EXPECT(assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega)); EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3,3))); - EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega)); + EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations } //#include diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 1aa840825..256f313ac 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -23,6 +23,7 @@ virtual class gtsam::NoiseModelFactor4; virtual class gtsam::GaussianFactor; virtual class gtsam::HessianFactor; virtual class gtsam::JacobianFactor; +virtual class gtsam::Cal3_S2; class gtsam::GaussianFactorGraph; class gtsam::NonlinearFactorGraph; class gtsam::Ordering; @@ -379,6 +380,24 @@ virtual class SmartRangeFactor : gtsam::NoiseModelFactor { }; +#include +template +virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor { + + SmartProjectionPoseFactor(double rankTol, double linThreshold, + bool manageDegeneracy, bool enableEPI, const POSE& body_P_sensor); + + SmartProjectionPoseFactor(double rankTol); + SmartProjectionPoseFactor(); + + void add(const gtsam::Point2& measured_i, size_t poseKey_i, const gtsam::noiseModel::Base* noise_i, + const CALIBRATION* K_i); + + // enabling serialization functionality + //void serialize() const; +}; + +typedef gtsam::SmartProjectionPoseFactor SmartProjectionPose3Factor; #include template diff --git a/matlab/unstable_examples/+imuSimulator/+lib/antisim.m b/matlab/unstable_examples/+imuSimulator/+lib/antisim.m new file mode 100644 index 000000000..6e9194567 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/+lib/antisim.m @@ -0,0 +1,7 @@ +function S = antisim(v) + +% costruisce la matrice antisimmetrica S (3x3) a partire dal vettore v +% Uso: S = antisim(v) + +S=[0 -v(3) v(2); v(3) 0 -v(1); -v(2) v(1) 0]; + diff --git a/matlab/unstable_examples/+imuSimulator/+lib/arrow3d.m b/matlab/unstable_examples/+imuSimulator/+lib/arrow3d.m new file mode 100644 index 000000000..aef3a5f10 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/+lib/arrow3d.m @@ -0,0 +1,140 @@ +function arrowHandle = arrow3D(pos, deltaValues, colorCode, stemRatio, rhoRatio) + +% arrowHandle = arrow3D(pos, deltaValues, colorCode, stemRatio) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% +% Used to plot a single 3D arrow with a cylindrical stem and cone arrowhead +% pos = [X,Y,Z] - spatial location of the starting point of the arrow (end of stem) +% deltaValues = [QX,QY,QZ] - delta parameters denoting the magnitude of the arrow along the x,y,z-axes (relative to 'pos') +% colorCode - Color parameters as per the 'surf' command. For example, 'r', 'red', [1 0 0] are all examples of a red-colored arrow +% stemRatio - The ratio of the length of the stem in proportion to the arrowhead. For example, a call of: +% arrow3D([0,0,0], [100,0,0] , 'r', 0.82) will produce a red arrow of magnitude 100, with the arrowstem spanning a distance +% of 82 (note 0.82 ratio of length 100) while the arrowhead (cone) spans 18. +% rhoRatio - The ratio of the cylinder radius (0.05 is the default) +% value) +% +% Example: +% arrow3D([0,0,0], [4,3,7]); %---- arrow with default parameters +% axis equal; +% +% Author: Shawn Arseneau +% Created: September 14, 2006 +% Updated: September 18, 2006 +% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + if nargin<2 || nargin>5 + error('Incorrect number of inputs to arrow3D'); + end + if numel(pos)~=3 || numel(deltaValues)~=3 + error('pos and/or deltaValues is incorrect dimensions (should be three)'); + end + if nargin<3 + colorCode = 'interp'; + end + if nargin<4 + stemRatio = 0.75; + end + if nargin<5 + rhoRatio = 0.05; + end + + X = pos(1); %---- with this notation, there is no need to transpose if the user has chosen a row vs col vector + Y = pos(2); + Z = pos(3); + + [sphi, stheta, srho] = cart2sph(deltaValues(1), deltaValues(2), deltaValues(3)); + + %******************************************* CYLINDER == STEM ********************************************* + cylinderRadius = srho*rhoRatio; + cylinderLength = srho*stemRatio; + [CX,CY,CZ] = cylinder(cylinderRadius); + CZ = CZ.*cylinderLength; %---- lengthen + + %----- ROTATE CYLINDER + [row, col] = size(CX); %---- initial rotation to coincide with X-axis + + newEll = rotatePoints([0 0 -1], [CX(:), CY(:), CZ(:)]); + CX = reshape(newEll(:,1), row, col); + CY = reshape(newEll(:,2), row, col); + CZ = reshape(newEll(:,3), row, col); + + [row, col] = size(CX); + newEll = rotatePoints(deltaValues, [CX(:), CY(:), CZ(:)]); + stemX = reshape(newEll(:,1), row, col); + stemY = reshape(newEll(:,2), row, col); + stemZ = reshape(newEll(:,3), row, col); + + %----- TRANSLATE CYLINDER + stemX = stemX + X; + stemY = stemY + Y; + stemZ = stemZ + Z; + + %******************************************* CONE == ARROWHEAD ********************************************* + coneLength = srho*(1-stemRatio); + coneRadius = cylinderRadius*1.5; + incr = 4; %---- Steps of cone increments + coneincr = coneRadius/incr; + [coneX, coneY, coneZ] = cylinder(cylinderRadius*2:-coneincr:0); %---------- CONE + coneZ = coneZ.*coneLength; + + %----- ROTATE CONE + [row, col] = size(coneX); + newEll = rotatePoints([0 0 -1], [coneX(:), coneY(:), coneZ(:)]); + coneX = reshape(newEll(:,1), row, col); + coneY = reshape(newEll(:,2), row, col); + coneZ = reshape(newEll(:,3), row, col); + + newEll = rotatePoints(deltaValues, [coneX(:), coneY(:), coneZ(:)]); + headX = reshape(newEll(:,1), row, col); + headY = reshape(newEll(:,2), row, col); + headZ = reshape(newEll(:,3), row, col); + + %---- TRANSLATE CONE + V = [0, 0, srho*stemRatio]; %---- centerline for cylinder: the multiplier is to set the cone 'on the rim' of the cylinder + Vp = rotatePoints([0 0 -1], V); + Vp = rotatePoints(deltaValues, Vp); + headX = headX + Vp(1) + X; + headY = headY + Vp(2) + Y; + headZ = headZ + Vp(3) + Z; + %************************************************************************************************************ + hStem = surf(stemX, stemY, stemZ, 'FaceColor', colorCode, 'EdgeColor', 'none'); + hold on; + hHead = surf(headX, headY, headZ, 'FaceColor', colorCode, 'EdgeColor', 'none'); + + if nargout==1 + arrowHandle = [hStem, hHead]; + end + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/matlab/unstable_examples/+imuSimulator/+lib/getxyz.m b/matlab/unstable_examples/+imuSimulator/+lib/getxyz.m new file mode 100644 index 000000000..d2c70db3a --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/+lib/getxyz.m @@ -0,0 +1,14 @@ +function [c] = getxyz(poses, j) +% The function extract the Cartesian variables from pose (pose.p = positions, +% pose.R = rotations). In particular, if there are T poses, +% - getxyz(pose, 1) estracts the vector x \in R^T, +% - getxyz(pose, 2) estracts the vector y \in R^T, +% - getxyz(pose, 3) estracts the vector z \in R^T. + +L = length(poses); +c = []; +for i=1:L % for each pose + c = [c poses(i).p(j)]; +end + +c = c(:); % column vector \ No newline at end of file diff --git a/matlab/unstable_examples/+imuSimulator/+lib/plot_trajectory.m b/matlab/unstable_examples/+imuSimulator/+lib/plot_trajectory.m new file mode 100644 index 000000000..1bababd33 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/+lib/plot_trajectory.m @@ -0,0 +1,19 @@ +function [] = plot_trajectory(pose, step, color, plot_name,a,b,c) +% Plot a collection of poses: positions are connected by a solid line +% of color "color" and it is shown a ref frame for each pose. +% Plot_name defines the name of the created figure. + +x = getxyz(pose,1); +y = getxyz(pose,2); +z = getxyz(pose,3); +plot3(x,y,z,color) +n = length(x)-1; +hold on +for i=1:step:n+1 + ref_frame_plot(pose(i).R,pose(i).p,a,b,c) +end +title(plot_name); +xlabel('x') +ylabel('y') +zlabel('z') +view(3) \ No newline at end of file diff --git a/matlab/unstable_examples/+imuSimulator/+lib/ref_frame_plot.m b/matlab/unstable_examples/+imuSimulator/+lib/ref_frame_plot.m new file mode 100644 index 000000000..9c73d1c69 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/+lib/ref_frame_plot.m @@ -0,0 +1,54 @@ +function ref_frame_plot(Ref,OriginRef,rhoRatio,stemRatio,axsize) +% ref_frame plot a 3D representation of a reference frame +% given by three unit vectors +% +% ref_frame(Ref,DimSpace,OriginRef) +% Ref is a 3 x 3 orthogonal matrix representing the unit vectors +% of the reference frame to be drawn +% DimSpace is a 3 x 2 matrix with min an max dimensions of the space +% [xmin xmax; ymin ymax; zmin zmax] +% default value = [-1,5 +1.5] for all dimensions +% OriginRef is the reference frame origin point +% default value = [0 0 0]' + +% Copright (C) Basilio Bona 2007 + +n=nargin; +if n == 1 + OriginRef=[0 0 0]'; + colorcode=['r','g','b']; + rhoRatio=0.05; + stemRatio=0.75; + axsize=1; +end +if n == 2 + colorcode=['r','g','b']; + rhoRatio=0.05; + stemRatio=0.75; + axsize=1; +end +if n == 3 + colorcode=['r','g','b']; + stemRatio=0.75; + axsize=1; +end + +if n == 4 + colorcode=['r','g','b']; + axsize=1; +end + +if n == 5 + colorcode=['r','g','b']; +end + + +% xproj=DimSpace(1,2); yproj=DimSpace(2,2); zproj=DimSpace(3,1); + +%hold off; +arrow3d(OriginRef, axsize*Ref(:,1), colorcode(1), stemRatio, rhoRatio); +arrow3d(OriginRef, axsize*Ref(:,2), colorcode(2), stemRatio, rhoRatio); +arrow3d(OriginRef, axsize*Ref(:,3), colorcode(3), stemRatio, rhoRatio); +axis equal; xlabel('X'); ylabel('Y'); zlabel('Z'); +% lighting phong; +% camlight left; diff --git a/matlab/unstable_examples/+imuSimulator/+lib/rotatePoints.m b/matlab/unstable_examples/+imuSimulator/+lib/rotatePoints.m new file mode 100644 index 000000000..4380d9d5d --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/+lib/rotatePoints.m @@ -0,0 +1,82 @@ +function rotatedData = rotatePoints(alignmentVector, originalData) + +% rotatedData = rotatePoints(alignmentVector, originalData) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% +% Rotate the 'originalData' in the form of Nx2 or Nx3 about the origin by aligning the x-axis with the alignment vector +% +% Rdata = rotatePoints([1,2,-1], [Xpts(:), Ypts(:), Zpts(:)]) - rotate the (X,Y,Z)pts in 3D with respect to the vector [1,2,-1] +% +% Rotating using spherical components can be done by first converting using [dX,dY,dZ] = cart2sph(theta, phi, rho); alignmentVector = [dX,dY,dZ]; +% +% Example: +% %% Rotate the point [3,4,-7] with respect to the following: +% %%%% Original associated vector is always [1,0,0] +% %%%% Calculate the appropriate rotation requested with respect to the x-axis. For example, if only a rotation about the z-axis is +% %%%% sought, alignmentVector = [2,1,0] %% Note that the z-component is zero +% rotData = rotatePoints(alignmentVector, [3,4,-7]); +% +% Author: Shawn Arseneau +% Created: Feb.2, 2006 +% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + alignmentDim = numel(alignmentVector); + DOF = size(originalData,2); %---- DOF = Degrees of Freedom (i.e. 2 for two dimensional and 3 for three dimensional data) + + if alignmentDim~=DOF + error('Alignment vector does not agree with originalData dimensions'); + end + if DOF<2 || DOF>3 + error('rotatePoints only does rotation in two or three dimensions'); + end + + + if DOF==2 % 2D rotation... + [rad_theta, rho] = cart2pol(alignmentVector(1), alignmentVector(2)); + deg_theta = -1 * rad_theta * (180/pi); + ctheta = cosd(deg_theta); stheta = sind(deg_theta); + + Rmatrix = [ctheta, -1.*stheta;... + stheta, ctheta]; + rotatedData = originalData*Rmatrix; + + else % 3D rotation... + [rad_theta, rad_phi, rho] = cart2sph(alignmentVector(1), alignmentVector(2), alignmentVector(3)); + rad_theta = rad_theta * -1; + deg_theta = rad_theta * (180/pi); + deg_phi = rad_phi * (180/pi); + ctheta = cosd(deg_theta); stheta = sind(deg_theta); + Rz = [ctheta, -1.*stheta, 0;... + stheta, ctheta, 0;... + 0, 0, 1]; %% First rotate as per theta around the Z axis + rotatedData = originalData*Rz; + + [rotX, rotY, rotZ] = sph2cart(-1* (rad_theta+(pi/2)), 0, 1); %% Second rotation corresponding to phi + rotationAxis = [rotX, rotY, rotZ]; + u = rotationAxis(:)/norm(rotationAxis); %% Code extract from rotate.m from MATLAB + cosPhi = cosd(deg_phi); + sinPhi = sind(deg_phi); + invCosPhi = 1 - cosPhi; + x = u(1); + y = u(2); + z = u(3); + Rmatrix = [cosPhi+x^2*invCosPhi x*y*invCosPhi-z*sinPhi x*z*invCosPhi+y*sinPhi; ... + x*y*invCosPhi+z*sinPhi cosPhi+y^2*invCosPhi y*z*invCosPhi-x*sinPhi; ... + x*z*invCosPhi-y*sinPhi y*z*invCosPhi+x*sinPhi cosPhi+z^2*invCosPhi]'; + + rotatedData = rotatedData*Rmatrix; + end + + + + + + + + + + + + + + diff --git a/matlab/unstable_examples/+imuSimulator/+lib/uth2rot.m b/matlab/unstable_examples/+imuSimulator/+lib/uth2rot.m new file mode 100644 index 000000000..6735ad6e5 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/+lib/uth2rot.m @@ -0,0 +1,14 @@ +function R = uth2rot(u,teta) + +% Uso: R = uth2rot(u,teta) +% +% calcola la matrice R +% a partire da asse u ed angolo di rotazione teta (in rad) + +S=antisim(u); +t=teta; + +n=norm(u); + +R = eye(3) + sin(t)/n*S + (1-cos(t))/n^2*S^2; + diff --git a/matlab/unstable_examples/+imuSimulator/IMUComparison.m b/matlab/unstable_examples/+imuSimulator/IMUComparison.m new file mode 100644 index 000000000..68e20bb25 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/IMUComparison.m @@ -0,0 +1,146 @@ +import gtsam.*; + +deltaT = 0.001; +summarizedDeltaT = 0.1; +timeElapsed = 1; +times = 0:deltaT:timeElapsed; + +omega = [0;0;2*pi]; +velocity = [1;0;0]; + +summaryTemplate = gtsam.ImuFactorPreintegratedMeasurements( ... + gtsam.imuBias.ConstantBias([0;0;0], [0;0;0]), ... + 1e-3 * eye(3), 1e-3 * eye(3), 1e-3 * eye(3)); + +%% Set initial conditions for the true trajectory and for the estimates +% (one estimate is obtained by integrating in the body frame, the other +% by integrating in the navigation frame) +% Initial state (body) +currentPoseGlobal = Pose3; +currentVelocityGlobal = velocity; +% Initial state estimate (integrating in navigation frame) +currentPoseGlobalIMUnav = currentPoseGlobal; +currentVelocityGlobalIMUnav = currentVelocityGlobal; +% Initial state estimate (integrating in the body frame) +currentPoseGlobalIMUbody = currentPoseGlobal; +currentVelocityGlobalIMUbody = currentVelocityGlobal; + +%% Prepare data structures for actual trajectory and estimates +% Actual trajectory +positions = zeros(3, length(times)+1); +positions(:,1) = currentPoseGlobal.translation.vector; +poses(1).p = positions(:,1); +poses(1).R = currentPoseGlobal.rotation.matrix; + +% Trajectory estimate (integrated in the navigation frame) +positionsIMUnav = zeros(3, length(times)+1); +positionsIMUnav(:,1) = currentPoseGlobalIMUbody.translation.vector; +posesIMUnav(1).p = positionsIMUnav(:,1); +posesIMUnav(1).R = poses(1).R; + +% Trajectory estimate (integrated in the body frame) +positionsIMUbody = zeros(3, length(times)+1); +positionsIMUbody(:,1) = currentPoseGlobalIMUbody.translation.vector; +posesIMUbody(1).p = positionsIMUbody(:,1); +posesIMUbody(1).R = poses(1).R; + +%% Solver object +isamParams = ISAM2Params; +isamParams.setRelinearizeSkip(1); +isam = gtsam.ISAM2(isamParams); + +initialValues = Values; +initialValues.insert(symbol('x',0), currentPoseGlobal); +initialValues.insert(symbol('v',0), LieVector(currentVelocityGlobal)); +initialValues.insert(symbol('b',0), imuBias.ConstantBias([0;0;0],[0;0;0])); +initialFactors = NonlinearFactorGraph; +initialFactors.add(PriorFactorPose3(symbol('x',0), ... + currentPoseGlobal, noiseModel.Isotropic.Sigma(6, 1.0))); +initialFactors.add(PriorFactorLieVector(symbol('v',0), ... + LieVector(currentVelocityGlobal), noiseModel.Isotropic.Sigma(3, 1.0))); +initialFactors.add(PriorFactorConstantBias(symbol('b',0), ... + imuBias.ConstantBias([0;0;0],[0;0;0]), noiseModel.Isotropic.Sigma(6, 1.0))); + +%% Main loop +i = 2; +lastSummaryTime = 0; +lastSummaryIndex = 0; +currentSummarizedMeasurement = ImuFactorPreintegratedMeasurements(summaryTemplate); +for t = times + %% Create the actual trajectory, using the velocities and + % accelerations in the inertial frame to compute the positions + [ currentPoseGlobal, currentVelocityGlobal ] = imuSimulator.integrateTrajectory( ... + currentPoseGlobal, omega, velocity, velocity, deltaT); + + %% Simulate IMU measurements, considering Coriolis effect + % (in this simple example we neglect gravity and there are no other forces acting on the body) + acc_omega = imuSimulator.calculateIMUMeas_coriolis( ... + omega, omega, velocity, velocity, deltaT); + + %% Accumulate preintegrated measurement + currentSummarizedMeasurement.integrateMeasurement(acc_omega(1:3), acc_omega(4:6), deltaT); + + %% Update solver + if t - lastSummaryTime >= summarizedDeltaT + % Create IMU factor + initialFactors.add(ImuFactor( ... + symbol('x',lastSummaryIndex), symbol('v',lastSummaryIndex), ... + symbol('x',lastSummaryIndex+1), symbol('v',lastSummaryIndex+1), ... + symbol('b',0), currentSummarizedMeasurement, [0;0;1], [0;0;0], ... + noiseModel.Isotropic.Sigma(9, 1e-6))); + + % Predict movement in a straight line (bad initialization) + if lastSummaryIndex > 0 + initialPose = isam.calculateEstimate(symbol('x',lastSummaryIndex)) ... + .compose(Pose3(Rot3, Point3( velocity * t - lastSummaryTime) )); + initialVel = isam.calculateEstimate(symbol('v',lastSummaryIndex)); + else + initialPose = Pose3; + initialVel = LieVector(velocity); + end + initialValues.insert(symbol('x',lastSummaryIndex+1), initialPose); + initialValues.insert(symbol('v',lastSummaryIndex+1), initialVel); + + % Update solver + isam.update(initialFactors, initialValues); + initialFactors = NonlinearFactorGraph; + initialValues = Values; + + lastSummaryIndex = lastSummaryIndex + 1; + lastSummaryTime = t; + currentSummarizedMeasurement = ImuFactorPreintegratedMeasurements(summaryTemplate); + end + + %% Integrate in the body frame + [ currentPoseGlobalIMUbody, currentVelocityGlobalIMUbody ] = imuSimulator.integrateIMUTrajectory_bodyFrame( ... + currentPoseGlobalIMUbody, currentVelocityGlobalIMUbody, acc_omega, deltaT, velocity); + + %% Integrate in the navigation frame + [ currentPoseGlobalIMUnav, currentVelocityGlobalIMUnav ] = imuSimulator.integrateIMUTrajectory_navFrame( ... + currentPoseGlobalIMUnav, currentVelocityGlobalIMUnav, acc_omega, deltaT); + + %% Store data in some structure for statistics and plots + positions(:,i) = currentPoseGlobal.translation.vector; + positionsIMUbody(:,i) = currentPoseGlobalIMUbody.translation.vector; + positionsIMUnav(:,i) = currentPoseGlobalIMUnav.translation.vector; + % - + poses(i).p = positions(:,i); + posesIMUbody(i).p = positionsIMUbody(:,i); + posesIMUnav(i).p = positionsIMUnav(:,i); + % - + poses(i).R = currentPoseGlobal.rotation.matrix; + posesIMUbody(i).R = currentPoseGlobalIMUbody.rotation.matrix; + posesIMUnav(i).R = currentPoseGlobalIMUnav.rotation.matrix; + i = i + 1; +end + +figure(1) +hold on; +plot(positions(1,:), positions(2,:), '-b'); +plot(positionsIMUbody(1,:), positionsIMUbody(2,:), '-r'); +plot(positionsIMUnav(1,:), positionsIMUnav(2,:), ':k'); +plot3DTrajectory(isam.calculateEstimate, 'g-'); +axis equal; +legend('true trajectory', 'traj integrated in body', 'traj integrated in nav') + + diff --git a/matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m b/matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m new file mode 100644 index 000000000..c589bea32 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m @@ -0,0 +1,128 @@ +close all +clc + +import gtsam.*; + +deltaT = 0.001; +summarizedDeltaT = 0.1; +timeElapsed = 1; +times = 0:deltaT:timeElapsed; + +omega = [0;0;2*pi]; +velocity = [1;0;0]; + +g = [0;0;0]; +cor_v = [0;0;0]; + +summaryTemplate = gtsam.ImuFactorPreintegratedMeasurements( ... + gtsam.imuBias.ConstantBias([0;0;0], [0;0;0]), ... + 1e-3 * eye(3), 1e-3 * eye(3), 1e-3 * eye(3)); + +%% Set initial conditions for the true trajectory and for the estimates +% (one estimate is obtained by integrating in the body frame, the other +% by integrating in the navigation frame) +% Initial state (body) +currentPoseGlobal = Pose3; +currentVelocityGlobal = velocity; + +%% Prepare data structures for actual trajectory and estimates +% Actual trajectory +positions = zeros(3, length(times)+1); +positions(:,1) = currentPoseGlobal.translation.vector; +poses(1).p = positions(:,1); +poses(1).R = currentPoseGlobal.rotation.matrix; + +%% Solver object +isamParams = ISAM2Params; +isamParams.setRelinearizeSkip(1); +isam = gtsam.ISAM2(isamParams); + +sigma_init_x = 1.0; +sigma_init_v = 1.0; +sigma_init_b = 1.0; + +initialValues = Values; +initialValues.insert(symbol('x',0), currentPoseGlobal); +initialValues.insert(symbol('v',0), LieVector(currentVelocityGlobal)); +initialValues.insert(symbol('b',0), imuBias.ConstantBias([0;0;0],[0;0;0])); +initialFactors = NonlinearFactorGraph; +% Prior on initial pose +initialFactors.add(PriorFactorPose3(symbol('x',0), ... + currentPoseGlobal, noiseModel.Isotropic.Sigma(6, sigma_init_x))); +% Prior on initial velocity +initialFactors.add(PriorFactorLieVector(symbol('v',0), ... + LieVector(currentVelocityGlobal), noiseModel.Isotropic.Sigma(3, sigma_init_v))); +% Prior on initial bias +initialFactors.add(PriorFactorConstantBias(symbol('b',0), ... + imuBias.ConstantBias([0;0;0],[0;0;0]), noiseModel.Isotropic.Sigma(6, sigma_init_b))); + +%% Main loop +i = 2; +lastSummaryTime = 0; +lastSummaryIndex = 0; +currentSummarizedMeasurement = ImuFactorPreintegratedMeasurements(summaryTemplate); +for t = times + %% Create the ground truth trajectory, using the velocities and accelerations in the inertial frame to compute the positions + [ currentPoseGlobal, currentVelocityGlobal ] = imuSimulator.integrateTrajectory( ... + currentPoseGlobal, omega, velocity, velocity, deltaT); + + %% Simulate IMU measurements, considering Coriolis effect + % (in this simple example we neglect gravity and there are no other forces acting on the body) + acc_omega = imuSimulator.calculateIMUMeas_coriolis( ... + omega, omega, velocity, velocity, deltaT); + + %% Accumulate preintegrated measurement + currentSummarizedMeasurement.integrateMeasurement(acc_omega(1:3), acc_omega(4:6), deltaT); + + %% Update solver + if t - lastSummaryTime >= summarizedDeltaT + + % Create IMU factor + initialFactors.add(ImuFactor( ... + symbol('x',lastSummaryIndex), symbol('v',lastSummaryIndex), ... + symbol('x',lastSummaryIndex+1), symbol('v',lastSummaryIndex+1), ... + symbol('b',0), currentSummarizedMeasurement, g, cor_v, ... + noiseModel.Isotropic.Sigma(9, 1e-6))); + + % Predict movement in a straight line (bad initialization) + if lastSummaryIndex > 0 + initialPose = isam.calculateEstimate(symbol('x',lastSummaryIndex)) ... + .compose(Pose3(Rot3, Point3( velocity * t - lastSummaryTime) )); + initialVel = isam.calculateEstimate(symbol('v',lastSummaryIndex)); + else + initialPose = Pose3; + initialVel = LieVector(velocity); + end + initialValues.insert(symbol('x',lastSummaryIndex+1), initialPose); + initialValues.insert(symbol('v',lastSummaryIndex+1), initialVel); + + key_pose = symbol('x',lastSummaryIndex+1); + + % Update solver + isam.update(initialFactors, initialValues); + initialFactors = NonlinearFactorGraph; + initialValues = Values; + + isam.calculateEstimate(key_pose); + M = isam.marginalCovariance(key_pose); + + lastSummaryIndex = lastSummaryIndex + 1; + lastSummaryTime = t; + currentSummarizedMeasurement = ImuFactorPreintegratedMeasurements(summaryTemplate); + end + + %% Store data in some structure for statistics and plots + positions(:,i) = currentPoseGlobal.translation.vector; + i = i + 1; +end + +figure(1) +hold on; +plot(positions(1,:), positions(2,:), '-b'); +plot3DTrajectory(isam.calculateEstimate, 'g-'); + + +axis equal; +legend('true trajectory', 'traj integrated in body', 'traj integrated in nav') + + diff --git a/matlab/unstable_examples/+imuSimulator/LatLonHRad_to_ECEF.m b/matlab/unstable_examples/+imuSimulator/LatLonHRad_to_ECEF.m new file mode 100644 index 000000000..bcc163eba --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/LatLonHRad_to_ECEF.m @@ -0,0 +1,26 @@ +function pos_ECEF = LatLonHRad_to_ECEF(LatLonH) +% convert latitude, longitude, height to XYZ in ECEF coordinates +% LatLonH(1) : latitude in radian +% LatLonH(2) : longitude in radian +% LatLonH(3) : height in meter +% +% Source: A. Chatfield, "Fundamentals of High Accuracy Inertial +% Navigation", 1997. pp. 10 Eq 1.18 +% + +% constants +a = 6378137.0; %m +e_sqr =0.006694379990141317; +% b = 6356752.3142; % m + +%RAD_PER_DEG = pi/180; +phi = LatLonH(1);%*RAD_PER_DEG; +lambda = LatLonH(2);%*RAD_PER_DEG; +h = LatLonH(3); + +RN = a/sqrt(1 - e_sqr*sin(phi)^2); + +pos_ECEF = zeros(3,1); +pos_ECEF(1) = (RN + h )*cos(phi)*cos(lambda); +pos_ECEF(2) = (RN + h )*cos(phi)*sin(lambda); +pos_ECEF(3) = (RN*(1-e_sqr) + h)*sin(phi) ; \ No newline at end of file diff --git a/matlab/unstable_examples/+imuSimulator/OdometryExample3D.m b/matlab/unstable_examples/+imuSimulator/OdometryExample3D.m new file mode 100644 index 000000000..d28d3c2cb --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/OdometryExample3D.m @@ -0,0 +1,68 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% 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 +% +% @brief Example of a simple 2D localization example +% @author Frank Dellaert +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +% Copied Original file. Modified by David Jensen to use Pose3 instead of +% Pose2. Everything else is the same. + +import gtsam.* + +%% Assumptions +% - Robot poses are facing along the X axis (horizontal, to the right in 2D) +% - The robot moves 2 meters each step +% - The robot is on a grid, moving 2 meters each step + +%% Create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph) +graph = NonlinearFactorGraph; + +%% Add a Gaussian prior on pose x_1 +priorMean = Pose3();%Pose3.Expmap([0.0; 0.0; 0.0; 0.0; 0.0; 0.0]); % prior mean is at origin +priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.3; 0.1; 0.1; 0.1]); % 30cm std on x,y, 0.1 rad on theta +graph.add(PriorFactorPose3(1, priorMean, priorNoise)); % add directly to graph + +%% Add two odometry factors +odometry = Pose3.Expmap([0.0; 0.0; 0.0; 2.0; 0.0; 0.0]); % create a measurement for both factors (the same in this case) +odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.2; 0.1; 0.1; 0.1]); % 20cm std on x,y, 0.1 rad on theta +graph.add(BetweenFactorPose3(1, 2, odometry, odometryNoise)); +graph.add(BetweenFactorPose3(2, 3, odometry, odometryNoise)); + +%% print +graph.print(sprintf('\nFactor graph:\n')); + +%% Initialize to noisy points +initialEstimate = Values; +%initialEstimate.insert(1, Pose3.Expmap([0.2; 0.1; 0.1; 0.5; 0.0; 0.0])); +%initialEstimate.insert(2, Pose3.Expmap([-0.2; 0.1; -0.1; 2.3; 0.1; 0.1])); +%initialEstimate.insert(3, Pose3.Expmap([0.1; -0.1; 0.1; 4.1; 0.1; -0.1])); +%initialEstimate.print(sprintf('\nInitial estimate:\n ')); + +for i=1:3 + deltaPosition = 0.5*rand(3,1) + [1;0;0]; % create random vector with mean = [1 0 0] and sigma = 0.5 + deltaRotation = 0.1*rand(3,1) + [0;0;0]; % create random rotation with mean [0 0 0] and sigma = 0.1 (rad) + deltaPose = Pose3.Expmap([deltaRotation; deltaPosition]); + currentPose = currentPose.compose(deltaPose); + initialEstimate.insert(i, currentPose); +end + +%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd +optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate); +result = optimizer.optimizeSafely(); +result.print(sprintf('\nFinal result:\n ')); + +%% Plot trajectory and covariance ellipses +cla; +hold on; + +plot3DTrajectory(result, [], Marginals(graph, result)); + +axis([-0.6 4.8 -1 1]) +axis equal +view(2) diff --git a/matlab/unstable_examples/+imuSimulator/calculateIMUMeas_coriolis.m b/matlab/unstable_examples/+imuSimulator/calculateIMUMeas_coriolis.m new file mode 100644 index 000000000..c86e40a21 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/calculateIMUMeas_coriolis.m @@ -0,0 +1,14 @@ +function [ acc_omega ] = calculateIMUMeas_coriolis(omega1Body, omega2Body, velocity1Body, velocity2Body, deltaT) + +import gtsam.*; + +% gyro measured rotation rate +measuredOmega = omega1Body; + +% Acceleration measurement (in this simple toy example no other forces +% act on the body and the only acceleration is the centripetal Coriolis acceleration) +measuredAcc = Point3(cross(omega1Body, velocity1Body)).vector; +acc_omega = [ measuredAcc; measuredOmega ]; + +end + diff --git a/matlab/unstable_examples/+imuSimulator/calculateIMUMeasurement.m b/matlab/unstable_examples/+imuSimulator/calculateIMUMeasurement.m new file mode 100644 index 000000000..534b9365e --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/calculateIMUMeasurement.m @@ -0,0 +1,26 @@ +function [ acc_omega ] = calculateIMUMeasurement(omega1Body, omega2Body, velocity1Body, velocity2Body, deltaT, imuFrame) +%CALCULATEIMUMEASUREMENT Calculate measured body frame acceleration in +%frame 1 and measured angular rates in frame 1. + +import gtsam.*; + +% Calculate gyro measured rotation rate by transforming body rotation rate +% into the IMU frame. +measuredOmega = imuFrame.rotation.unrotate(Point3(omega1Body)).vector; + +% Transform both velocities into IMU frame, accounting for the velocity +% induced by rigid body rotation on a lever arm (Coriolis effect). +velocity1inertial = imuFrame.rotation.unrotate( ... + Point3(velocity1Body + cross(omega1Body, imuFrame.translation.vector))).vector; + +imu2in1 = Rot3.Expmap(measuredOmega * deltaT); +velocity2inertial = imu2in1.rotate(imuFrame.rotation.unrotate( ... + Point3(velocity2Body + cross(omega2Body, imuFrame.translation.vector)))).vector; + +% Acceleration in IMU frame +measuredAcc = (velocity2inertial - velocity1inertial) / deltaT; + +acc_omega = [ measuredAcc; measuredOmega ]; + +end + diff --git a/matlab/unstable_examples/+imuSimulator/coriolisExample.m b/matlab/unstable_examples/+imuSimulator/coriolisExample.m new file mode 100644 index 000000000..35d27aa73 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/coriolisExample.m @@ -0,0 +1,508 @@ +%% coriolisExample.m +% Author(s): David Jensen (david.jensen@gtri.gatech.edu) +% This script demonstrates the relationship between object motion in inertial and rotating reference frames. +% For this example, we consider a fixed (inertial) reference frame located at the center of the earth and initially +% coincident with the rotating ECEF reference frame (X towards 0 longitude, Y towards 90 longitude, Z towards 90 latitude), +% which rotates with the earth. +% A body is moving in the positive Z-direction and positive Y-direction with respect to the fixed reference frame. +% Its position is plotted in both the fixed and rotating reference frames to simulate how an observer in each frame would +% experience the body's motion. + +import gtsam.*; + +addpath(genpath('./Libraries')) + +% Check for an external configuarion. This is useful for setting up +% multiple tests +if ~exist('externalCoriolisConfiguration', 'var') + clc + clear all + close all + %% General configuration + navFrameRotating = 1; % 0 = perform navigation in the fixed frame + % 1 = perform navigation in the rotating frame + IMU_type = 1; % IMU type 1 or type 2 + useRealisticValues = 1; % use reaslist values for initial position and earth rotation + record_movie = 0; % 0 = do not record movie + % 1 = record movie of the trajectories. One + % frame per time step (15 fps) + incrementalPlotting = 1; % turn incremental plotting on and off. Turning plotting off increases + % speed for batch testing + estimationEnabled = 1; + + %% Scenario Configuration + deltaT = 0.01; % timestep + timeElapsed = 5; % Total elapsed time + times = 0:deltaT:timeElapsed; + + % Initial Conditions + omegaEarthSeconds = [0;0;7.292115e-5]; % Earth Rotation rate (rad/s) + radiusEarth = 6378.1*1000; % radius of Earth is 6,378.1 km + + if useRealisticValues == 1 + omegaRotatingFrame = omegaEarthSeconds; % rotation of the moving frame wrt fixed frame + omegaFixed = [0;0;0]; % constant rotation rate measurement + accelFixed = [-0.5;0.5;2]; % constant acceleration measurement + g = [0;0;0]; % Gravity + % initial position at some [longitude, latitude] location on earth's + % surface (approximating Earth as a sphere) + initialLongitude = 45; % longitude in degrees + initialLatitude = 30; % latitude in degrees + initialAltitude = 0; % Altitude above Earth's surface in meters + [x, y, z] = sph2cart(initialLongitude * pi/180, initialLatitude * pi/180, radiusEarth + initialAltitude); + initialPosition = [x; y; z]; + initialVelocity = [0; 0; 0];% initial velocity of the body in the rotating frame, + % (ignoring the velocity due to the earth's rotation) + else + omegaRotatingFrame = [0;0;pi/5]; % rotation of the moving frame wrt fixed frame + omegaFixed = [0;0;0]; % constant rotation rate measurement + accelFixed = [0.5;0.5;0.5]; % constant acceleration measurement + g = [0;0;0]; % Gravity + initialPosition = [1; 0; 0];% initial position in both frames + initialVelocity = [0;0;0]; % initial velocity in the rotating frame (ignoring the velocity due to the frame's rotation) + end + + if navFrameRotating == 0 + omegaCoriolisIMU = [0;0;0]; + else + omegaCoriolisIMU = omegaRotatingFrame; + end +end + + +% From Wikipedia Angular Velocity page, dr/dt = W*r, where r is +% position vector and W is angular velocity tensor +% We add the initial velocity in the rotating frame because they +% are the same frame at t=0, so no transformation is needed +angularVelocityTensor = [ 0 -omegaRotatingFrame(3) omegaRotatingFrame(2); + omegaRotatingFrame(3) 0 -omegaRotatingFrame(1); + -omegaRotatingFrame(2) omegaRotatingFrame(1) 0 ]; +initialVelocityFixedFrame = angularVelocityTensor * initialPosition + initialVelocity; +initialVelocityRotatingFrame = initialVelocity; + +% +currentRotatingFrame = Pose3; % rotating frame initially coincides with fixed frame at t=0 +currentPoseFixedGT = Pose3(Rot3, Point3(initialPosition)); +currentPoseRotatingGT = currentPoseFixedGT; % frames coincide for t=0 +currentVelocityFixedGT = initialVelocityFixedFrame; +currentVelocityRotatingGT = initialVelocityRotatingFrame; +% +epsBias = 1e-20; +sigma_init_x = noiseModel.Isotropic.Sigma(6, 1e-10); +sigma_init_v = noiseModel.Isotropic.Sigma(3, 1e-10); +sigma_init_b = noiseModel.Isotropic.Sigma(6, epsBias); + +% Imu metadata +zeroBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); % bias is not of interest and is set to zero +IMU_metadata.AccelerometerSigma = 1e-5; +IMU_metadata.GyroscopeSigma = 1e-7; +IMU_metadata.IntegrationSigma = 1e-10; +IMU_metadata.BiasAccelerometerSigma = epsBias; +IMU_metadata.BiasGyroscopeSigma = epsBias; +IMU_metadata.BiasAccOmegaInit = epsBias; + +%% Initialize storage variables +positionsInFixedGT = zeros(3, length(times)); +velocityInFixedGT = zeros(3, length(times)); + +positionsInRotatingGT = zeros(3, length(times)); +velocityInRotatingGT = zeros(3, length(times)); + +positionsEstimates = zeros(3,length(times)); +velocitiesEstimates = zeros(3,length(times)); + +rotationsErrorVectors = zeros(3,length(times)); % Rotating/Fixed frame selected later + +changePoseRotatingFrame = Pose3.Expmap([omegaRotatingFrame*deltaT; 0; 0; 0]); % rotation of the rotating frame at each time step +h = figure; + +% Solver object +isamParams = ISAM2Params; +isamParams.setFactorization('CHOLESKY'); +isamParams.setRelinearizeSkip(10); +isam = gtsam.ISAM2(isamParams); +newFactors = NonlinearFactorGraph; +newValues = Values; + +% Video recording object +if record_movie == 1 + writerObj = VideoWriter('trajectories.avi'); + writerObj.Quality = 100; + writerObj.FrameRate = 15; %10; + open(writerObj); + set(gca,'nextplot','replacechildren'); + set(gcf,'Renderer','zbuffer'); +end + +%% Print Info about the test +fprintf('\n-------------------------------------------------\n'); +if navFrameRotating == 0 + fprintf('Performing navigation in the Fixed frame.\n'); +else + fprintf('Performing navigation in the Rotating frame.\n'); +end +fprintf('Estimation Enabled = %d\n', estimationEnabled); +fprintf('IMU_type = %d\n', IMU_type); +fprintf('Record Movie = %d\n', record_movie); +fprintf('Realistic Values = %d\n', useRealisticValues); +fprintf('deltaT = %f\n', deltaT); +fprintf('timeElapsed = %f\n', timeElapsed); +fprintf('omegaRotatingFrame = [%f %f %f]\n', omegaRotatingFrame(1), omegaRotatingFrame(2), omegaRotatingFrame(3)); +fprintf('omegaCoriolisIMU = [%f %f %f]\n', omegaCoriolisIMU(1), omegaCoriolisIMU(2), omegaCoriolisIMU(3)); +fprintf('omegaFixed = [%f %f %f]\n', omegaFixed(1), omegaFixed(2), omegaFixed(3)); +fprintf('accelFixed = [%f %f %f]\n', accelFixed(1), accelFixed(2), accelFixed(3)); +fprintf('Initial Velocity = [%f %f %f]\n', initialVelocity(1), initialVelocity(2), initialVelocity(3)); +if(exist('initialLatitude', 'var') && exist('initialLongitude', 'var')) + fprintf('Initial Position\n\t[Long, Lat] = [%f %f] degrees\n\tEFEC = [%f %f %f]\n', ... + initialLongitude, initialLatitude, initialPosition(1), initialPosition(2), initialPosition(3)); +else + fprintf('Initial Position = [%f %f %f]\n', initialPosition(1), initialPosition(2), initialPosition(3)); +end +fprintf('\n'); + +%% Main loop: iterate through the ground truth trajectory, add factors +% and values to the factor graph, and perform inference +for i = 1:length(times) + t = times(i); + + % Create keys for current state + currentPoseKey = symbol('x', i); + currentVelKey = symbol('v', i); + currentBiasKey = symbol('b', i); + + %% Set priors on the first iteration + if(i == 1) + % known initial conditions + currentPoseEstimate = currentPoseFixedGT; + if navFrameRotating == 1 + currentVelocityEstimate = LieVector(currentVelocityRotatingGT); + else + currentVelocityEstimate = LieVector(currentVelocityFixedGT); + end + + % Set Priors + newValues.insert(currentPoseKey, currentPoseEstimate); + newValues.insert(currentVelKey, currentVelocityEstimate); + newValues.insert(currentBiasKey, zeroBias); + % Initial values, same for IMU types 1 and 2 + newFactors.add(PriorFactorPose3(currentPoseKey, currentPoseEstimate, sigma_init_x)); + newFactors.add(PriorFactorLieVector(currentVelKey, currentVelocityEstimate, sigma_init_v)); + newFactors.add(PriorFactorConstantBias(currentBiasKey, zeroBias, sigma_init_b)); + + % Store data + positionsInFixedGT(:,1) = currentPoseFixedGT.translation.vector; + velocityInFixedGT(:,1) = currentVelocityFixedGT; + positionsInRotatingGT(:,1) = currentPoseRotatingGT.translation.vector; + %velocityInRotatingGT(:,1) = currentPoseRotatingGT.velocity.vector; + positionsEstimates(:,i) = currentPoseEstimate.translation.vector; + velocitiesEstimates(:,i) = currentVelocityEstimate.vector; + currentRotatingFrameForPlot(1).p = currentRotatingFrame.translation.vector; + currentRotatingFrameForPlot(1).R = currentRotatingFrame.rotation.matrix; + else + + %% Create ground truth trajectory + % Update the position and velocity + % x_t = x_0 + v_0*dt + 1/2*a_0*dt^2 + % v_t = v_0 + a_0*dt + currentPositionFixedGT = Point3(currentPoseFixedGT.translation.vector ... + + currentVelocityFixedGT * deltaT + 0.5 * accelFixed * deltaT * deltaT); + currentVelocityFixedGT = currentVelocityFixedGT + accelFixed * deltaT; + + currentPoseFixedGT = Pose3(Rot3, currentPositionFixedGT); % constant orientation + + % Rotate pose in fixed frame to get pose in rotating frame + previousPositionRotatingGT = currentPoseRotatingGT.translation.vector; + currentRotatingFrame = currentRotatingFrame.compose(changePoseRotatingFrame); + inverseCurrentRotatingFrame = (currentRotatingFrame.inverse); + currentPoseRotatingGT = inverseCurrentRotatingFrame.compose(currentPoseFixedGT); + currentPositionRotatingGT = currentPoseRotatingGT.translation.vector; + + % Get velocity in rotating frame by treating it like a position and using compose + % Maybe Luca knows a better way to do this within GTSAM. + %currentVelocityPoseFixedGT = Pose3(Rot3, Point3(currentVelocityFixedGT-initialVelocityFixedFrame)); + %currentVelocityPoseRotatingGT = inverseCurrentRotatingFrame.compose(currentVelocityPoseFixedGT); + + %currentRot3RotatingGT = currentRotatingFrame.rotation(); + %currentVelocityRotatingGT = currentRot3RotatingGT.unrotate(Point3(currentVelocityFixedGT)); + % TODO: check if initial velocity in rotating frame is correct + currentVelocityRotatingGT = (currentPositionRotatingGT-previousPositionRotatingGT)/deltaT; + %currentVelocityRotatingGT = (currentPositionRotatingGT - previousPositionRotatingGT ... + % - 0.5 * accelFixed * deltaT * deltaT) / deltaT + accelFixed * deltaT; + + % Store GT (ground truth) poses + positionsInFixedGT(:,i) = currentPoseFixedGT.translation.vector; + velocityInFixedGT(:,i) = currentVelocityFixedGT; + positionsInRotatingGT(:,i) = currentPoseRotatingGT.translation.vector; + velocityInRotatingGT(:,i) = currentVelocityRotatingGT; + currentRotatingFrameForPlot(i).p = currentRotatingFrame.translation.vector; + currentRotatingFrameForPlot(i).R = currentRotatingFrame.rotation.matrix; + + %% Estimate trajectory in rotating frame using GTSAM (ground truth measurements) + % Instantiate preintegrated measurements class + if IMU_type == 1 + currentSummarizedMeasurement = gtsam.ImuFactorPreintegratedMeasurements( ... + zeroBias, ... + IMU_metadata.AccelerometerSigma.^2 * eye(3), ... + IMU_metadata.GyroscopeSigma.^2 * eye(3), ... + IMU_metadata.IntegrationSigma.^2 * eye(3)); + elseif IMU_type == 2 + currentSummarizedMeasurement = gtsam.CombinedImuFactorPreintegratedMeasurements( ... + zeroBias, ... + IMU_metadata.AccelerometerSigma.^2 * eye(3), ... + IMU_metadata.GyroscopeSigma.^2 * eye(3), ... + IMU_metadata.IntegrationSigma.^2 * eye(3), ... + IMU_metadata.BiasAccelerometerSigma.^2 * eye(3), ... + IMU_metadata.BiasGyroscopeSigma.^2 * eye(3), ... + IMU_metadata.BiasAccOmegaInit.^2 * eye(6)); + else + error('imuSimulator:coriolisExample:IMU_typeNotFound', ... + 'IMU_type = %d does not exist.\nAvailable IMU types are 1 and 2\n', IMU_type); + end + + % Add measurement + currentSummarizedMeasurement.integrateMeasurement(accelFixed, omegaFixed, deltaT); + + % Add factors to graph + if IMU_type == 1 + newFactors.add(ImuFactor( ... + currentPoseKey-1, currentVelKey-1, ... + currentPoseKey, currentVelKey, ... + currentBiasKey-1, currentSummarizedMeasurement, g, omegaCoriolisIMU)); + newFactors.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, zeroBias, ... + noiseModel.Isotropic.Sigma(6, epsBias))); + newFactors.add(PriorFactorConstantBias(currentBiasKey, zeroBias, ... + noiseModel.Isotropic.Sigma(6, epsBias))); + elseif IMU_type == 2 + newFactors.add(CombinedImuFactor( ... + currentPoseKey-1, currentVelKey-1, ... + currentPoseKey, currentVelKey, ... + currentBiasKey-1, currentBiasKey, ... + currentSummarizedMeasurement, g, omegaCoriolisIMU, ... + noiseModel.Isotropic.Sigma(15, epsBias))); + % TODO: prior on biases? + end + + % Add values to the graph. Use the current pose and velocity + % estimates as to values when interpreting the next pose and + % velocity estimates + %ImuFactor.Predict(currentPoseEstimate, currentVelocityEstimate, pose_j, vel_j, zeroBias, currentSummarizedMeasurement, g, omegaCoriolisIMU); + newValues.insert(currentPoseKey, currentPoseEstimate); + newValues.insert(currentVelKey, currentVelocityEstimate); + newValues.insert(currentBiasKey, zeroBias); + + %newFactors.print(''); newValues.print(''); + + %% Solve factor graph + if(i > 1 && estimationEnabled) + isam.update(newFactors, newValues); + newFactors = NonlinearFactorGraph; + newValues = Values; + + % Get the new pose, velocity, and bias estimates + currentPoseEstimate = isam.calculateEstimate(currentPoseKey); + currentVelocityEstimate = isam.calculateEstimate(currentVelKey); + currentBias = isam.calculateEstimate(currentBiasKey); + + positionsEstimates(:,i) = currentPoseEstimate.translation.vector; + velocitiesEstimates(:,i) = currentVelocityEstimate.vector; + biasEstimates(:,i) = currentBias.vector; + + % In matrix form: R_error = R_gt'*R_estimate + % Perform Logmap on the rotation matrix to get a vector + if navFrameRotating == 1 + rotationError = Rot3(currentPoseRotatingGT.rotation.matrix' * currentPoseEstimate.rotation.matrix); + else + rotationError = Rot3(currentPoseFixedGT.rotation.matrix' * currentPoseEstimate.rotation.matrix); + end + + rotationsErrorVectors(:,i) = Rot3.Logmap(rotationError); + end + end + + %% incremental plotting for animation (ground truth) + if incrementalPlotting == 1 + figure(h) + %plot_trajectory(currentRotatingFrameForPlot(i),1, '-k', 'Rotating Frame',0.1,0.75,1) + %hold on; + plot3(positionsInFixedGT(1,1:i), positionsInFixedGT(2,1:i), positionsInFixedGT(3,1:i),'r'); + hold on; + plot3(positionsInFixedGT(1,1), positionsInFixedGT(2,1), positionsInFixedGT(3,1), 'xr'); + plot3(positionsInFixedGT(1,i), positionsInFixedGT(2,i), positionsInFixedGT(3,i), 'or'); + + plot3(positionsInRotatingGT(1,1:i), positionsInRotatingGT(2,1:i), positionsInRotatingGT(3,1:i), 'g'); + plot3(positionsInRotatingGT(1,1), positionsInRotatingGT(2,1), positionsInRotatingGT(3,1), 'xg'); + plot3(positionsInRotatingGT(1,i), positionsInRotatingGT(2,i), positionsInRotatingGT(3,i), 'og'); + + if(estimationEnabled) + plot3(positionsEstimates(1,1:i), positionsEstimates(2,1:i), positionsEstimates(3,1:i), 'b'); + plot3(positionsEstimates(1,1), positionsEstimates(2,1), positionsEstimates(3,1), 'xb'); + plot3(positionsEstimates(1,i), positionsEstimates(2,i), positionsEstimates(3,i), 'ob'); + end + + hold off; + xlabel('X axis') + ylabel('Y axis') + zlabel('Z axis') + axis equal + grid on; + %pause(0.1); + + % record frames for movie + if record_movie == 1 + frame = getframe(gcf); + writeVideo(writerObj,frame); + end + end +end + +if record_movie == 1 + close(writerObj); +end + +% Calculate trajectory length +trajectoryLengthEstimated = 0; +trajectoryLengthFixedFrameGT = 0; +trajectoryLengthRotatingFrameGT = 0; +for i = 2:length(positionsEstimates) + trajectoryLengthEstimated = trajectoryLengthEstimated + norm(positionsEstimates(:,i) - positionsEstimates(:,i-1)); + trajectoryLengthFixedFrameGT = trajectoryLengthFixedFrameGT + norm(positionsInFixedGT(:,i) - positionsInFixedGT(:,i-1)); + trajectoryLengthRotatingFrameGT = trajectoryLengthRotatingFrameGT + norm(positionsInRotatingGT(:,i) - positionsInRotatingGT(:,i-1)); +end + +%% Print and plot error results +% Calculate errors for appropriate navigation scheme +if navFrameRotating == 0 + axisPositionsError = positionsInFixedGT - positionsEstimates; + axisVelocityError = velocityInFixedGT - velocitiesEstimates; +else + axisPositionsError = positionsInRotatingGT - positionsEstimates; + axisVelocityError = velocityInRotatingGT - velocitiesEstimates; +end + +% Plot individual axis position errors +figure +plot(times, axisPositionsError); +plotTitle = sprintf('Axis Error in Estimated Position\n(IMU type = %d, omega = [%.2f; %.2f; %.2f])', ... + IMU_type, omegaRotatingFrame(1), omegaRotatingFrame(2), omegaRotatingFrame(3)); +title(plotTitle); +xlabel('Time [s]'); +ylabel('Error (ground_truth - estimate) [m]'); +legend('X axis', 'Y axis', 'Z axis', 'Location', 'NORTHWEST'); + +% Plot 3D position error +figure +positionError3D = sqrt(axisPositionsError(1,:).^2+axisPositionsError(2,:).^2 + axisPositionsError(3,:).^2); +plot(times, positionError3D); +plotTitle = sprintf('3D Error in Estimated Position\n(IMU type = %d, omega = [%.2f; %.2f; %.2f])', ... + IMU_type, omegaRotatingFrame(1), omegaRotatingFrame(2), omegaRotatingFrame(3)); +title(plotTitle); +xlabel('Time [s]'); +ylabel('3D error [meters]'); + +% Plot 3D position error +if navFrameRotating == 0 + trajLen = trajectoryLengthFixedFrameGT; +else + trajLen = trajectoryLengthRotatingFrameGT; +end + +figure +plot(times, (positionError3D/trajLen)*100); +plotTitle = sprintf('3D Error normalized by distance in Estimated Position\n(IMU type = %d, omega = [%.2f; %.2f; %.2f])', ... + IMU_type, omegaRotatingFrame(1), omegaRotatingFrame(2), omegaRotatingFrame(3)); +title(plotTitle); +xlabel('Time [s]'); +ylabel('normalized 3D error [% of distance traveled]'); + +% Plot individual axis velocity errors +figure +plot(times, axisVelocityError); +plotTitle = sprintf('Axis Error in Estimated Velocity\n(IMU type = %d, omega = [%.2f; %.2f; %.2f])', ... + IMU_type, omegaRotatingFrame(1), omegaRotatingFrame(2), omegaRotatingFrame(3)); +title(plotTitle); +xlabel('Time [s]'); +ylabel('Error (ground_truth - estimate) [m/s]'); +legend('X axis', 'Y axis', 'Z axis', 'Location', 'NORTHWEST'); + +% Plot 3D velocity error +figure +velocityError3D = sqrt(axisVelocityError(1,:).^2+axisVelocityError(2,:).^2 + axisVelocityError(3,:).^2); +plot(times, velocityError3D); +plotTitle = sprintf('3D Error in Estimated Velocity\n(IMU type = %d, omega = [%.2f; %.2f; %.2f])', ... + IMU_type, omegaRotatingFrame(1), omegaRotatingFrame(2), omegaRotatingFrame(3)); +title(plotTitle); +xlabel('Time [s]'); +ylabel('3D error [meters/s]'); + +% Plot magnitude of rotation errors +figure +for i = 1:size(rotationsErrorVectors,2) + rotationsErrorMagnitudes(i) = norm(rotationsErrorVectors(:,i)); +end +plot(times,rotationsErrorMagnitudes); +title('Rotation Error'); +xlabel('Time [s]'); +ylabel('Error [rads]'); + + +% Text output for errors +if navFrameRotating == 0 + fprintf('Fixed Frame ground truth trajectory length is %f [m]\n', trajectoryLengthFixedFrameGT); + fprintf('Estimated trajectory length is %f [m]\n', trajectoryLengthEstimated); + fprintf('Final position error = %f [m](%.4f%% of ground truth trajectory length)\n', ... + norm(axisPositionsError(:,end)), norm(axisPositionsError(:,end))/trajectoryLengthFixedFrameGT*100); +else + fprintf('Rotating Frame ground truth trajectory length is %f [m]\n', trajectoryLengthRotatingFrameGT); + fprintf('Estimated trajectory length is %f [m]\n', trajectoryLengthEstimated); + fprintf('Final position error = %f [m](%.4f%% of ground truth trajectory length)\n', ... + norm(axisPositionsError(:,end)), norm(axisPositionsError(:,end))/trajectoryLengthRotatingFrameGT*100); +end +fprintf('Final velocity error = [%f %f %f] [m/s]\n', axisVelocityError(1,end), axisVelocityError(2,end), axisVelocityError(3,end)); +fprintf('Final rotation error = %f [rads]\n', norm(rotationsErrorVectors(:,end))); + +%% Plot final trajectories +figure +[x,y,z] = sphere(30); +if useRealisticValues + mesh(radiusEarth*x,radiusEarth*y, radiusEarth*z) % where (a,b,c) is center of the sphere % sphere for reference +else + mesh(x,y,z); +end +hold on; +axis equal +% Ground truth trajectory in fixed reference frame +plot3(positionsInFixedGT(1,:), positionsInFixedGT(2,:), positionsInFixedGT(3,:),'r','lineWidth',4); +plot3(positionsInFixedGT(1,1), positionsInFixedGT(2,1), positionsInFixedGT(3,1), 'xr','lineWidth',4); +plot3(positionsInFixedGT(1,end), positionsInFixedGT(2,end), positionsInFixedGT(3,end), 'or','lineWidth',4); + +% Ground truth trajectory in rotating reference frame +plot3(positionsInRotatingGT(1,:), positionsInRotatingGT(2,:), positionsInRotatingGT(3,:), '-g','lineWidth',4); +plot3(positionsInRotatingGT(1,1), positionsInRotatingGT(2,1), positionsInRotatingGT(3,1), 'xg','lineWidth',4); +plot3(positionsInRotatingGT(1,end), positionsInRotatingGT(2,end), positionsInRotatingGT(3,end), 'og','lineWidth',4); + +% Estimates +if(estimationEnabled) + plot3(positionsEstimates(1,:), positionsEstimates(2,:), positionsEstimates(3,:), '-b','lineWidth',4); + plot3(positionsEstimates(1,1), positionsEstimates(2,1), positionsEstimates(3,1), 'xb','lineWidth',4); + plot3(positionsEstimates(1,end), positionsEstimates(2,end), positionsEstimates(3,end), 'ob','lineWidth',4); +end + +xlabel('X axis') +ylabel('Y axis') +zlabel('Z axis') +axis equal +grid on; +hold off; + +% TODO: logging rotation errors +%for all time steps +% Rerror = Rgt' * Restimated; +% % transforming rotation matrix to axis-angle representation +% vector_error = Rot3.Logmap(Rerror); +% norm(vector_error) +% +% axis angle: [u,theta], with norm(u)=1 +% vector_error = u * theta; + +% TODO: logging velocity errors +%velocities.. diff --git a/matlab/unstable_examples/+imuSimulator/coriolisTestMonteCarlo.m b/matlab/unstable_examples/+imuSimulator/coriolisTestMonteCarlo.m new file mode 100644 index 000000000..4d02fd03e --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/coriolisTestMonteCarlo.m @@ -0,0 +1,182 @@ +clc +clear all +close all + +% Flag to mark external configuration to the main test script +externalCoriolisConfiguration = 1; + +%% General configuration +navFrameRotating = 0; % 0 = perform navigation in the fixed frame + % 1 = perform navigation in the rotating frame +IMU_type = 1; % IMU type 1 or type 2 +useRealisticValues = 1; % use reaslist values for initial position and earth rotation +record_movie = 0; % 0 = do not record movie + % 1 = record movie of the trajectories. One + % frame per time step (15 fps) +incrementalPlotting = 0; +estimationEnabled = 1; + +%% Scenario Configuration +deltaT = 0.01; % timestep +timeElapsed = 5; % Total elapsed time +times = 0:deltaT:timeElapsed; + +% Initial Conditions +omegaEarthSeconds = [0;0;7.292115e-5]; % Earth Rotation rate (rad/s) +radiusEarth = 6378.1*1000; % radius of Earth is 6,378.1 km + +if useRealisticValues == 1 + omegaRotatingFrame = omegaEarthSeconds; % rotation of the moving frame wrt fixed frame + omegaFixed = [0;0;0]; % constant rotation rate measurement + accelFixed = [0.1;0;1]; % constant acceleration measurement + g = [0;0;0]; % Gravity + + initialLongitude = 45; % longitude in degrees + initialLatitude = 30; % latitude in degrees + initialAltitude = 0; % Altitude above Earth's surface in meters + [x, y, z] = sph2cart(initialLongitude * pi/180, initialLatitude * pi/180, radiusEarth + initialAltitude); + initialPosition = [x; y; z]; + + initialVelocity = [0; 0; 0];% initial velocity of the body in the rotating frame, + % (ignoring the velocity due to the earth's rotation) + +else + omegaRotatingFrame = [0;0;pi/300]; % rotation of the moving frame wrt fixed frame + omegaFixed = [0;0;0]; % constant rotation rate measurement + accelFixed = [1;0;0]; % constant acceleration measurement + g = [0;0;0]; % Gravity + initialPosition = [0; 1; 0];% initial position in both frames + initialVelocity = [0;0;0]; % initial velocity in the rotating frame (ignoring the velocity due to the frame's rotation) +end + +%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +% Run tests with randomly generated initialPosition and accelFixed values +% For each initial position, a number of acceleration vectors are +% generated. For each (initialPosition, accelFixed) pair, the coriolis test +% is run for the following 3 scenarios +% - Navigation performed in fixed frame +% - Navigation performed in rotating frame, including the coriolis effect +% - Navigation performed in rotating frame, ignoring coriolis effect +% + +% Testing configuration +numPosTests = 1; +numAccelTests = 10; +totalNumTests = numPosTests * numAccelTests; + +% Storage variables + +posFinErrorFixed = zeros(numPosTests, numAccelTests); +rotFinErrorFixed = zeros(numPosTests, numAccelTests); +velFinErrorFixed = zeros(numPosTests, numAccelTests); + +posFinErrorRotCoriolis = zeros(numPosTests, numAccelTests); +rotFinErrorRotCoriolis = zeros(numPosTests, numAccelTests); +velFinErrorRotCoriolis = zeros(numPosTests, numAccelTests); + +posFinErrorRot = zeros(numPosTests, numAccelTests); +rotFinErrorRot = zeros(numPosTests, numAccelTests); +velFinErrorRot = zeros(numPosTests, numAccelTests); + + +numErrors = 0; +testIndPos = 1; +testIndAccel = 1; + +while testIndPos <= numPosTests + %generate a random initial position vector + initialLongitude = rand()*360 - 180; % longitude in degrees (-90 to 90) + initialLatitude = rand()*180 - 90; % latitude in degrees (-180 to 180) + initialAltitude = rand()*150; % Altitude above Earth's surface in meters (0-150) + [x, y, z] = sph2cart(initialLongitude * pi/180, initialLatitude * pi/180, radiusEarth + initialAltitude); + initialPosition = [x; y; z]; + + while testIndAccel <= numAccelTests + [testIndPos testIndAccel] + % generate a random acceleration vector + accelFixed = 2*rand(3,1)-ones(3,1); + + %lla = oldTestInfo(testIndPos,testIndAccel).initialPositionLLA; + %initialLongitude = lla(1); + %initialLatitude = lla(2); + %initialAltitude = lla(3); + %initialPosition = oldTestInfo(testIndPos, testIndAccel).initialPositionECEF; + + testInfo(testIndPos, testIndAccel).initialPositionLLA = [initialLongitude, initialLatitude, initialAltitude]; + testInfo(testIndPos, testIndAccel).initialPositionECEF = initialPosition; + testInfo(testIndPos, testIndAccel).accelFixed = accelFixed; + + try + omegaCoriolisIMU = [0;0;0]; + navFrameRotating = 0; + imuSimulator.coriolisExample + posFinErrorFixed(testIndPos, testIndAccel) = norm(axisPositionsError(:,end))/trajectoryLengthFixedFrameGT*100; + rotFinErrorFixed(testIndPos, testIndAccel) = norm(rotationsErrorVectors(:,end)); + velFinErrorFixed(testIndPos, testIndAccel) = norm(axisVelocityError(:,end)); + testInfo(testIndPos, testIndAccel).fixedPositionError = axisPositionsError(:,end); + testInfo(testIndPos, testIndAccel).fixedVelocityError = axisVelocityError(:,end); + testInfo(testIndPos, testIndAccel).fixedRotationError = rotationsErrorVectors(:,end); + testInfo(testIndPos, testIndAccel).fixedEstTrajLength = trajectoryLengthEstimated; + testInfo(testIndPos, testIndAccel).trajLengthFixedFrameGT = trajectoryLengthFixedFrameGT; + testInfo(testIndPos, testIndAccel).trajLengthRotFrameGT = trajectoryLengthRotatingFrameGT; + + close all; + + % Run the same initial conditions but navigating in the rotating frame + % enable coriolis effect by setting: + omegaCoriolisIMU = omegaRotatingFrame; + navFrameRotating = 1; + imuSimulator.coriolisExample + posFinErrorRotCoriolis(testIndPos, testIndAccel) = norm(axisPositionsError(:,end))/trajLen*100; + rotFinErrorRotCoriolis(testIndPos, testIndAccel) = norm(rotationsErrorVectors(:,end)); + velFinErrorRotCoriolis(testIndPos, testIndAccel) = norm(axisVelocityError(:,end)); + testInfo(testIndPos, testIndAccel).rotCoriolisPositionError = axisPositionsError(:,end); + testInfo(testIndPos, testIndAccel).rotCoriolisVelocityError = axisVelocityError(:,end); + testInfo(testIndPos, testIndAccel).rotCoriolisRotationError = rotationsErrorVectors(:,end); + testInfo(testIndPos, testIndAccel).rotCoriolisEstTrajLength = trajectoryLengthEstimated; + + close all; + + % Run the same initial conditions but navigating in the rotating frame + % disable coriolis effect by setting: + omegaCoriolisIMU = [0;0;0]; + navFrameRotating = 1; + imuSimulator.coriolisExample + posFinErrorRot(testIndPos, testIndAccel) = norm(axisPositionsError(:,end))/trajLen*100; + rotFinErrorRot(testIndPos, testIndAccel) = norm(rotationsErrorVectors(:,end)); + velFinErrorRot(testIndPos, testIndAccel) = norm(axisVelocityError(:,end)); + testInfo(testIndPos, testIndAccel).rotPositionError = axisPositionsError(:,end); + testInfo(testIndPos, testIndAccel).rotVelocityError = axisVelocityError(:,end); + testInfo(testIndPos, testIndAccel).rotRotationError = rotationsErrorVectors(:,end); + testInfo(testIndPos, testIndAccel).rotEstTrajLength = trajectoryLengthEstimated; + + close all; + catch + numErrors = numErrors + 1; + fprintf('*ERROR*'); + fprintf('%d tests cancelled due to errors\n', numErrors); + fprintf('restarting test with new accelFixed'); + testIndAccel = testIndAccel - 1; + end + testIndAccel = testIndAccel + 1; + end + testIndAccel = 1; + testIndPos = testIndPos + 1; +end +fprintf('\nTotal: %d tests cancelled due to errors\n', numErrors); + +mean(posFinErrorFixed); +max(posFinErrorFixed); +% box(posFinErrorFixed); + +mean(posFinErrorRotCoriolis); +max(posFinErrorRotCoriolis); +% box(posFinErrorRotCoriolis); + +mean(posFinErrorRot); +max(posFinErrorRot); +% box(posFinErrorRot); + + diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m new file mode 100644 index 000000000..2eddf75ee --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -0,0 +1,325 @@ +import gtsam.*; + +% Test GTSAM covariances on a factor graph with: +% Between Factors +% IMU factors (type 1 and type 2) +% GPS prior factors on poses +% SmartProjectionPoseFactors +% Authors: Luca Carlone, David Jensen +% Date: 2014/4/6 + + +% Check for an extneral configuration, used when running multiple tests +if ~exist('externallyConfigured', 'var') + clc + clear all + close all + + saveResults = 0; + + %% Configuration + % General options + options.useRealData = 1; % controls whether or not to use the real data (if available) as the ground truth traj + options.includeBetweenFactors = 0; % if true, BetweenFactors will be added between consecutive poses + + options.includeIMUFactors = 1; % if true, IMU factors will be added between consecutive states (biases, poses, velocities) + options.imuFactorType = 1; % Set to 1 or 2 to use IMU type 1 or type 2 factors (will default to type 1) + options.imuNonzeroBias = 0; % if true, a nonzero bias is applied to IMU measurements + + options.includeCameraFactors = 1; % if true, SmartProjectionPose3Factors will be used with randomly generated landmarks + options.numberOfLandmarks = 1000; % Total number of visual landmarks (randomly generated in a box around the trajectory) + + options.includeGPSFactors = 0; % if true, GPS factors will be added as priors to poses + options.gpsStartPose = 100; % Pose number to start including GPS factors at + + options.trajectoryLength = 100;%209; % length of the ground truth trajectory + options.subsampleStep = 20; % number of poses to skip when using real data (to reduce computation on long trajectories) + + numMonteCarloRuns = 2; % number of Monte Carlo runs to perform + + % Noise values to be adjusted + sigma_ang = 1e-2; % std. deviation for rotational noise, typical 1e-2 + sigma_cart = 1e-1; % std. deviation for translational noise, typical 1e-1 + sigma_accel = 1e-3; % std. deviation for accelerometer noise, typical 1e-3 + sigma_gyro = 1e-5; % std. deviation for gyroscope noise, typical 1e-5 + sigma_accelBias = 1e-4; % std. deviation for added accelerometer constant bias, typical 1e-3 + sigma_gyroBias = 1e-6; % std. deviation for added gyroscope constant bias, typical 1e-5 + sigma_gps = 1e-4; % std. deviation for noise in GPS position measurements, typical 1e-4 + sigma_camera = 1; % std. deviation for noise in camera measurements (pixels) + + % Set log files + testName = sprintf('sa-%1.2g-sc-%1.2g-sacc-%1.2g-sg-%1.2g',sigma_ang,sigma_cart,sigma_accel,sigma_gyro) + folderName = 'results/' +else + fprintf('Tests have been externally configured.\n'); +end + +%% Between metadata +noiseVectorPose = [sigma_ang * ones(3,1); sigma_cart * ones(3,1)]; +noisePose = noiseModel.Diagonal.Sigmas(noiseVectorPose); + +%% Imu metadata +metadata.imu.epsBias = 1e-10; % was 1e-7 +metadata.imu.g = [0;0;0]; +metadata.imu.omegaCoriolis = [0;0;0]; +metadata.imu.IntegrationSigma = 1e-5; +metadata.imu.zeroBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); +metadata.imu.AccelerometerSigma = sigma_accel; +metadata.imu.GyroscopeSigma = sigma_gyro; +metadata.imu.BiasAccelerometerSigma = metadata.imu.epsBias; % noise on expected change in accelerometer bias over time +metadata.imu.BiasGyroscopeSigma = metadata.imu.epsBias; % noise on expected change in gyroscope bias over time +% noise on initial accelerometer and gyroscope biases +if options.imuNonzeroBias == 1 + metadata.imu.BiasAccOmegaInit = [sigma_accelBias * ones(3,1); sigma_gyroBias * ones(3,1)]; +else + metadata.imu.BiasAccOmegaInit = metadata.imu.epsBias * ones(6,1); +end + +noiseVel = noiseModel.Isotropic.Sigma(3, 1e-2); % was 0.1 +noiseBiasBetween = noiseModel.Diagonal.Sigmas([metadata.imu.BiasAccelerometerSigma * ones(3,1);... + metadata.imu.BiasGyroscopeSigma * ones(3,1)]); % between on biases +noisePriorBias = noiseModel.Diagonal.Sigmas(metadata.imu.BiasAccOmegaInit); + +noiseVectorAccel = metadata.imu.AccelerometerSigma * ones(3,1); +noiseVectorGyro = metadata.imu.GyroscopeSigma * ones(3,1); + +%% GPS metadata +noiseVectorGPS = sigma_gps * ones(3,1); +noiseGPS = noiseModel.Diagonal.Precisions([zeros(3,1); 1/sigma_gps^2 * ones(3,1)]); + +%% Camera metadata +metadata.camera.calibration = Cal3_S2(500,500,0,1920/2,1200/2); % Camera calibration +metadata.camera.xlims = [-100, 650]; % x limits on area for landmark creation +metadata.camera.ylims = [-100, 700]; % y limits on area for landmark creation +metadata.camera.zlims = [-30, 30]; % z limits on area for landmark creation +metadata.camera.visualRange = 100; % maximum distance from the camera that a landmark can be seen (meters) +metadata.camera.bodyPoseCamera = Pose3; % pose of camera in body +metadata.camera.CameraSigma = sigma_camera; +cameraMeasurementNoise = noiseModel.Isotropic.Sigma(2, metadata.camera.CameraSigma); +noiseVectorCamera = metadata.camera.CameraSigma .* ones(2,1); + +% Create landmarks and smart factors +if options.includeCameraFactors == 1 + for i = 1:options.numberOfLandmarks + metadata.camera.gtLandmarkPoints(i) = Point3( ... + [rand() * (metadata.camera.xlims(2)-metadata.camera.xlims(1)) + metadata.camera.xlims(1); ... + rand() * (metadata.camera.ylims(2)-metadata.camera.ylims(1)) + metadata.camera.ylims(1); ... + rand() * (metadata.camera.zlims(2)-metadata.camera.zlims(1)) + metadata.camera.zlims(1)]); + end +end + + +%% Create ground truth trajectory and measurements +[gtValues, gtMeasurements] = imuSimulator.covarianceAnalysisCreateTrajectory(options, metadata); + +%% Create ground truth graph +% Set up noise models +gtNoiseModels.noisePose = noisePose; +gtNoiseModels.noiseVel = noiseVel; +gtNoiseModels.noiseBiasBetween = noiseBiasBetween; +gtNoiseModels.noisePriorPose = noisePose; +gtNoiseModels.noisePriorBias = noisePriorBias; +gtNoiseModels.noiseGPS = noiseGPS; +gtNoiseModels.noiseCamera = cameraMeasurementNoise; + +% Set measurement noise to 0, because this is ground truth +gtMeasurementNoise.poseNoiseVector = zeros(6,1); +gtMeasurementNoise.imu.accelNoiseVector = zeros(3,1); +gtMeasurementNoise.imu.gyroNoiseVector = zeros(3,1); +gtMeasurementNoise.cameraNoiseVector = zeros(2,1); +gtMeasurementNoise.gpsNoiseVector = zeros(3,1); + +% Set IMU biases to zero +metadata.imu.accelConstantBiasVector = zeros(3,1); +metadata.imu.gyroConstantBiasVector = zeros(3,1); + +[gtGraph, projectionFactorSeenBy] = imuSimulator.covarianceAnalysisCreateFactorGraph( ... + gtMeasurements, ... % ground truth measurements + gtValues, ... % ground truth Values + gtNoiseModels, ... % noise models to use in this graph + gtMeasurementNoise, ... % noise to apply to measurements + options, ... % options for the graph (e.g. which factors to include) + metadata); % misc data necessary for factor creation + +%% Display, printing, and plotting of ground truth +%gtGraph.print(sprintf('\nGround Truth Factor graph:\n')); +%gtValues.print(sprintf('\nGround Truth Values:\n ')); + +figure(1) +hold on; + +if options.includeCameraFactors + b = [-1000 2000 -2000 2000 -30 30]; + for i = 1:size(metadata.camera.gtLandmarkPoints,2) + p = metadata.camera.gtLandmarkPoints(i).vector; + if(p(1) > b(1) && p(1) < b(2) && p(2) > b(3) && p(2) < b(4) && p(3) > b(5) && p(3) < b(6)) + plot3(p(1), p(2), p(3), 'k+'); + end + end + pointsToPlot = metadata.camera.gtLandmarkPoints(find(projectionFactorSeenBy > 0)); + for i = 1:length(pointsToPlot) + p = pointsToPlot(i).vector; + plot3(p(1), p(2), p(3), 'gs', 'MarkerSize', 10); + end +end +plot3DPoints(gtValues); +%plot3DTrajectory(gtValues, '-r', [], 1, Marginals(gtGraph, gtValues)); +plot3DTrajectory(gtValues, '-r'); + +axis equal + +% optimize +optimizer = GaussNewtonOptimizer(gtGraph, gtValues); +gtEstimate = optimizer.optimize(); +plot3DTrajectory(gtEstimate, '-k'); +% estimate should match gtValues if graph is correct. +fprintf('Error in ground truth graph at gtValues: %g \n', gtGraph.error(gtValues) ); +fprintf('Error in ground truth graph at gtEstimate: %g \n', gtGraph.error(gtEstimate) ); + +disp('Plotted ground truth') + +%% Monte Carlo Runs + +% Set up noise models +monteCarloNoiseModels.noisePose = noisePose; +monteCarloNoiseModels.noiseVel = noiseVel; +monteCarloNoiseModels.noiseBiasBetween = noiseBiasBetween; +monteCarloNoiseModels.noisePriorPose = noisePose; +monteCarloNoiseModels.noisePriorBias = noisePriorBias; +monteCarloNoiseModels.noiseGPS = noiseGPS; +monteCarloNoiseModels.noiseCamera = cameraMeasurementNoise; + +% Set measurement noise for monte carlo runs +monteCarloMeasurementNoise.poseNoiseVector = zeros(6,1); %noiseVectorPose; +monteCarloMeasurementNoise.imu.accelNoiseVector = noiseVectorAccel; +monteCarloMeasurementNoise.imu.gyroNoiseVector = noiseVectorGyro; +monteCarloMeasurementNoise.gpsNoiseVector = noiseVectorGPS; +monteCarloMeasurementNoise.cameraNoiseVector = noiseVectorCamera; + +for k=1:numMonteCarloRuns + fprintf('Monte Carlo Run %d...\n', k'); + + % Create a random bias for each run + if options.imuNonzeroBias == 1 + metadata.imu.accelConstantBiasVector = metadata.imu.BiasAccOmegaInit(1:3) .* randn(3,1); + metadata.imu.gyroConstantBiasVector = metadata.imu.BiasAccOmegaInit(4:6) .* randn(3,1); + %metadata.imu.accelConstantBiasVector = 1e-2 * ones(3,1); + %metadata.imu.gyroConstantBiasVector = 1e-3 * ones(3,1); + else + metadata.imu.accelConstantBiasVector = zeros(3,1); + metadata.imu.gyroConstantBiasVector = zeros(3,1); + end + + % Create a new graph using noisy measurements + [graph, projectionFactorSeenBy] = imuSimulator.covarianceAnalysisCreateFactorGraph( ... + gtMeasurements, ... % ground truth measurements + gtValues, ... % ground truth Values + monteCarloNoiseModels, ... % noise models to use in this graph + monteCarloMeasurementNoise, ... % noise to apply to measurements + options, ... % options for the graph (e.g. which factors to include) + metadata); % misc data necessary for factor creation + + %graph.print('graph') + + % optimize + optimizer = GaussNewtonOptimizer(graph, gtValues); + estimate = optimizer.optimize(); + figure(1) + plot3DTrajectory(estimate, '-b'); + + marginals = Marginals(graph, estimate); + + % for each pose in the trajectory + for i=0:options.trajectoryLength + % compute estimation errors + currentPoseKey = symbol('x', i); + gtPosition = gtValues.at(currentPoseKey).translation.vector; + estPosition = estimate.at(currentPoseKey).translation.vector; + estR = estimate.at(currentPoseKey).rotation.matrix; + errPosition = estPosition - gtPosition; + + % compute covariances: + cov = marginals.marginalCovariance(currentPoseKey); + covPosition = estR * cov(4:6,4:6) * estR'; + % compute NEES using (estimationError = estimatedValues - gtValues) and estimated covariances + NEES(k,i+1) = errPosition' * inv(covPosition) * errPosition; % distributed according to a Chi square with n = 3 dof + end + + figure(2) + hold on + plot(NEES(k,:),'-b','LineWidth',1.5) +end +%% +ANEES = mean(NEES); +plot(ANEES,'-r','LineWidth',2) +plot(3*ones(size(ANEES,2),1),'k--'); % Expectation(ANEES) = number of dof +box on +set(gca,'Fontsize',16) +title('NEES and ANEES'); +if saveResults + saveas(gcf,horzcat(folderName,'runs-',testName,'.fig'),'fig'); + saveas(gcf,horzcat(folderName,'runs-',testName,'.png'),'png'); +end + +%% +figure(1) +box on +set(gca,'Fontsize',16) +title('Ground truth and estimates for each MC runs'); +if saveResults + saveas(gcf,horzcat(folderName,'gt-',testName,'.fig'),'fig'); + saveas(gcf,horzcat(folderName,'gt-',testName,'.png'),'png'); +end + +%% Let us compute statistics on the overall NEES +n = 3; % position vector dimension +N = numMonteCarloRuns; % number of runs +alpha = 0.01; % confidence level + +% mean_value = n*N; % mean value of the Chi-square distribution +% (we divide by n * N and for this reason we expect ANEES around 1) +r1 = chi2inv(alpha, n * N) / (n * N); +r2 = chi2inv(1-alpha, n * N) / (n * N); + +% output here +fprintf(1, 'r1 = %g\n', r1); +fprintf(1, 'r2 = %g\n', r2); + +figure(3) +hold on +plot(ANEES/n,'-b','LineWidth',2) +plot(ones(size(ANEES,2),1),'r-'); +plot(r1*ones(size(ANEES,2),1),'k-.'); +plot(r2*ones(size(ANEES,2),1),'k-.'); +box on +set(gca,'Fontsize',16) +title('NEES normalized by dof VS bounds'); +if saveResults + saveas(gcf,horzcat(folderName,'ANEES-',testName,'.fig'),'fig'); + saveas(gcf,horzcat(folderName,'ANEES-',testName,'.png'),'png'); + logFile = horzcat(folderName,'log-',testName); + save(logFile) +end + +%% NEES COMPUTATION (Bar-Shalom 2001, Section 5.4) +% the nees for a single experiment (i) is defined as +% NEES_i = xtilda' * inv(P) * xtilda, +% where xtilda in R^n is the estimation +% error, and P is the covariance estimated by the approach we want to test +% +% Average NEES. Given N Monte Carlo simulations, i=1,...,N, the average +% NEES is: +% ANEES = sum(NEES_i)/N +% The quantity N*ANEES is distributed according to a Chi-square +% distribution with N*n degrees of freedom. +% +% For the single run case, N=1, therefore NEES = ANEES is distributed +% according to a chi-square distribution with n degrees of freedom (e.g. n=3 +% if we are testing a position estimate) +% Therefore its mean should be n (difficult to see from a single run) +% and, with probability alpha, it should hold: +% +% NEES in [r1, r2] +% +% where r1 and r2 are built from the Chi-square distribution + diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m new file mode 100644 index 000000000..00ae4b9c2 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m @@ -0,0 +1,199 @@ +function [ graph projectionFactorSeenBy] = covarianceAnalysisCreateFactorGraph( measurements, values, noiseModels, measurementNoise, options, metadata) +% Create a factor graph based on provided measurements, values, and noises. +% Used for covariance analysis scripts. +% 'options' contains fields for including various factor types. +% 'metadata' is a storage variable for miscellaneous factor-specific values +% Authors: Luca Carlone, David Jensen +% Date: 2014/04/16 + +import gtsam.*; + +graph = NonlinearFactorGraph; + +% Iterate through the trajectory +for i=0:length(measurements) + % Get the current pose + currentPoseKey = symbol('x', i); + currentPose = values.at(currentPoseKey); + + if i==0 + %% first time step, add priors + % Pose prior (poses used for all factors) + noisyInitialPoseVector = Pose3.Logmap(currentPose) + measurementNoise.poseNoiseVector .* randn(6,1); + initialPose = Pose3.Expmap(noisyInitialPoseVector); + graph.add(PriorFactorPose3(currentPoseKey, initialPose, noiseModels.noisePose)); + + % IMU velocity and bias priors + if options.includeIMUFactors == 1 + currentVelKey = symbol('v', 0); + currentVel = values.at(currentVelKey).vector; + graph.add(PriorFactorLieVector(currentVelKey, LieVector(currentVel), noiseModels.noiseVel)); + + currentBiasKey = symbol('b', 0); + currentBias = values.at(currentBiasKey); + graph.add(PriorFactorConstantBias(currentBiasKey, currentBias, noiseModels.noisePriorBias)); + end + + %% Create a SmartProjectionFactor for each landmark + projectionFactorSeenBy = []; + if options.includeCameraFactors == 1 + for j=1:options.numberOfLandmarks + SmartProjectionFactors(j) = SmartProjectionPose3Factor(0.01); + % Use constructor with default values, but express the pose of the + % camera as a 90 degree rotation about the X axis +% SmartProjectionFactors(j) = SmartProjectionPose3Factor( ... +% 1, ... % rankTol +% -1, ... % linThreshold +% false, ... % manageDegeneracy +% false, ... % enableEPI +% metadata.camera.bodyPoseCamera); % Pose of camera in body frame + end + projectionFactorSeenBy = zeros(options.numberOfLandmarks,1); + end + + else + + %% Add Between factors + if options.includeBetweenFactors == 1 + prevPoseKey = symbol('x', i-1); + % Create the noisy pose estimate + deltaPose = Pose3.Expmap(measurements(i).deltaVector ... + + (measurementNoise.poseNoiseVector .* randn(6,1))); % added noise + % Add the between factor to the graph + graph.add(BetweenFactorPose3(prevPoseKey, currentPoseKey, deltaPose, noiseModels.noisePose)); + end % end of Between pose factor creation + + %% Add IMU factors + if options.includeIMUFactors == 1 + % Update keys + currentVelKey = symbol('v', i); % not used if includeIMUFactors is false + currentBiasKey = symbol('b', i); % not used if includeIMUFactors is false + + if options.imuFactorType == 1 + use2ndOrderIntegration = true; + % Initialize preintegration + imuMeasurement = gtsam.ImuFactorPreintegratedMeasurements(... + metadata.imu.zeroBias, ... + metadata.imu.AccelerometerSigma.^2 * eye(3), ... + metadata.imu.GyroscopeSigma.^2 * eye(3), ... + metadata.imu.IntegrationSigma.^2 * eye(3), ... + use2ndOrderIntegration); + % Generate IMU measurements with noise + for j=1:length(measurements(i).imu) % all measurements to preintegrate + accelNoise = (measurementNoise.imu.accelNoiseVector .* randn(3,1)); + imuAccel = measurements(i).imu(j).accel ... + + accelNoise ... % added noise + + metadata.imu.accelConstantBiasVector; % constant bias + + gyroNoise = (measurementNoise.imu.gyroNoiseVector .* randn(3,1)); + imuGyro = measurements(i).imu(j).gyro ... + + gyroNoise ... % added noise + + metadata.imu.gyroConstantBiasVector; % constant bias + + % Used for debugging + %fprintf(' A: (meas)[%f %f %f] + (noise)[%f %f %f] + (bias)[%f %f %f] = [%f %f %f]\n', ... + % measurements(i).imu(j).accel(1), measurements(i).imu(j).accel(2), measurements(i).imu(j).accel(3), ... + % accelNoise(1), accelNoise(2), accelNoise(3), ... + % metadata.imu.accelConstantBiasVector(1), metadata.imu.accelConstantBiasVector(2), metadata.imu.accelConstantBiasVector(3), ... + % imuAccel(1), imuAccel(2), imuAccel(3)); + %fprintf(' G: (meas)[%f %f %f] + (noise)[%f %f %f] + (bias)[%f %f %f] = [%f %f %f]\n', ... + % measurements(i).imu(j).gyro(1), measurements(i).imu(j).gyro(2), measurements(i).imu(j).gyro(3), ... + % gyroNoise(1), gyroNoise(2), gyroNoise(3), ... + % metadata.imu.gyroConstantBiasVector(1), metadata.imu.gyroConstantBiasVector(2), metadata.imu.gyroConstantBiasVector(3), ... + % imuGyro(1), imuGyro(2), imuGyro(3)); + + % Preintegrate + imuMeasurement.integrateMeasurement(imuAccel, imuGyro, measurements(i).imu(j).deltaT); + end + %imuMeasurement.print('imuMeasurement'); + + % Add Imu factor + graph.add(ImuFactor(currentPoseKey-1, currentVelKey-1, currentPoseKey, currentVelKey, ... + currentBiasKey-1, imuMeasurement, metadata.imu.g, metadata.imu.omegaCoriolis)); + % Add between factor on biases + graph.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, metadata.imu.zeroBias, ... + noiseModels.noiseBiasBetween)); + end + + if options.imuFactorType == 2 + use2ndOrderIntegration = true; + % Initialize preintegration + imuMeasurement = gtsam.CombinedImuFactorPreintegratedMeasurements(... + metadata.imu.zeroBias, ... + metadata.imu.AccelerometerSigma.^2 * eye(3), ... + metadata.imu.GyroscopeSigma.^2 * eye(3), ... + metadata.imu.IntegrationSigma.^2 * eye(3), ... + metadata.imu.BiasAccelerometerSigma.^2 * eye(3), ... % how bias changes over time + metadata.imu.BiasGyroscopeSigma.^2 * eye(3), ... % how bias changes over time + diag(metadata.imu.BiasAccOmegaInit.^2), ... % prior on bias + use2ndOrderIntegration); + % Generate IMU measurements with noise + for j=1:length(measurements(i).imu) % all measurements to preintegrate + imuAccel = measurements(i).imu(j).accel ... + + (measurementNoise.imu.accelNoiseVector .* randn(3,1))... % added noise + + metadata.imu.accelConstantBiasVector; % constant bias + imuGyro = measurements(i).imu(j).gyro ... + + (measurementNoise.imu.gyroNoiseVector .* randn(3,1))... % added noise + + metadata.imu.gyroConstantBiasVector; % constant bias + + % Preintegrate + imuMeasurement.integrateMeasurement(imuAccel, imuGyro, measurements(i).imu(j).deltaT); + end + + % Add Imu factor + graph.add(CombinedImuFactor( ... + currentPoseKey-1, currentVelKey-1, ... + currentPoseKey, currentVelKey, ... + currentBiasKey-1, currentBiasKey, ... + imuMeasurement, ... + metadata.imu.g, metadata.imu.omegaCoriolis)); + end + + end % end of IMU factor creation + + %% Build Camera Factors + if options.includeCameraFactors == 1 + for j = 1:length(measurements(i).landmarks) + cameraMeasurmentNoise = measurementNoise.cameraNoiseVector .* randn(2,1); + cameraPixelMeasurement = measurements(i).landmarks(j).vector; + % Only add the measurement if it is in the image frame (based on calibration) + if(cameraPixelMeasurement(1) > 0 && cameraPixelMeasurement(2) > 0 ... + && cameraPixelMeasurement(1) < 2*metadata.camera.calibration.px ... + && cameraPixelMeasurement(2) < 2*metadata.camera.calibration.py) + cameraPixelMeasurement = cameraPixelMeasurement + cameraMeasurmentNoise; + SmartProjectionFactors(j).add( ... + Point2(cameraPixelMeasurement), ... + currentPoseKey, noiseModels.noiseCamera, ... + metadata.camera.calibration); + projectionFactorSeenBy(j) = projectionFactorSeenBy(j)+1; + end + end + end % end of Camera factor creation + + %% Add GPS factors + if options.includeGPSFactors == 1 && i >= options.gpsStartPose + gpsPosition = measurements(i).gpsPositionVector ... + + measurementNoise.gpsNoiseVector .* randn(3,1); + graph.add(PriorFactorPose3(currentPoseKey, ... + Pose3(currentPose.rotation, Point3(gpsPosition)), ... + noiseModels.noiseGPS)); + end % end of GPS factor creation + + end % end of else (i=0) + +end % end of for over trajectory + +%% Add Camera Factors to the graph +% Only factors for landmarks that have been viewed at least once are added +% to the graph +%[find(projectionFactorSeenBy ~= 0) projectionFactorSeenBy(find(projectionFactorSeenBy ~= 0))] +if options.includeCameraFactors == 1 + for j = 1:options.numberOfLandmarks + if projectionFactorSeenBy(j) > 0 + graph.add(SmartProjectionFactors(j)); + end + end +end + +end % end of function + diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m new file mode 100644 index 000000000..2cceea753 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m @@ -0,0 +1,150 @@ +function [values, measurements] = covarianceAnalysisCreateTrajectory( options, metadata ) +% Create a trajectory for running covariance analysis scripts. +% 'options' contains fields for including various factor types and setting trajectory length +% 'metadata' is a storage variable for miscellaneous factor-specific values +% Authors: Luca Carlone, David Jensen +% Date: 2014/04/16 + +import gtsam.*; + +values = Values; + +warning('Rotating Pose inside getPoseFromGtScenario! TODO: Use a body_P_sensor transform in the factors') + + +if options.useRealData == 1 + %% Create a ground truth trajectory from Real data (if available) + fprintf('\nUsing real data as ground truth\n'); + gtScenario = load('truth_scen2.mat', 'Time', 'Lat', 'Lon', 'Alt', 'Roll', 'Pitch', 'Heading',... + 'VEast', 'VNorth', 'VUp'); + + % Limit the trajectory length + options.trajectoryLength = min([length(gtScenario.Lat)/options.subsampleStep options.trajectoryLength]); + fprintf('Scenario Ind: '); + + for i=0:options.trajectoryLength + scenarioInd = options.subsampleStep * i + 1; + fprintf('%d, ', scenarioInd); + if (mod(i,12) == 0) fprintf('\n'); end + + %% Generate the current pose + currentPoseKey = symbol('x', i); + currentPose = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioInd); + + %% FOR TESTING - this is currently moved to getPoseFromGtScenario + %currentPose = currentPose.compose(metadata.camera.bodyPoseCamera); + %currentPose = currentPose.compose(Pose3.Expmap([-pi/2;0;0;0;0;0])); + + % add to values + values.insert(currentPoseKey, currentPose); + + %% gt Between measurements + if options.includeBetweenFactors == 1 && i > 0 + prevPose = values.at(currentPoseKey - 1); + deltaPose = prevPose.between(currentPose); + measurements(i).deltaVector = Pose3.Logmap(deltaPose); + end + + %% gt IMU measurements + if options.includeIMUFactors == 1 + currentVelKey = symbol('v', i); + currentBiasKey = symbol('b', i); + deltaT = 1; % amount of time between IMU measurements + if i == 0 + currentVel = [0 0 0]'; + else + % integrate & store intermediate measurements + for j=1:options.subsampleStep % we integrate all the intermediate measurements + previousScenarioInd = options.subsampleStep * (i-1) + 1; + scenarioIndIMU1 = previousScenarioInd+j-1; + scenarioIndIMU2 = previousScenarioInd+j; + IMUPose1 = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioIndIMU1); + IMUPose2 = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioIndIMU2); + IMURot1 = IMUPose1.rotation.matrix; + + IMUdeltaPose = IMUPose1.between(IMUPose2); + IMUdeltaPoseVector = Pose3.Logmap(IMUdeltaPose); + IMUdeltaRotVector = IMUdeltaPoseVector(1:3); + IMUdeltaPositionVector = IMUPose2.translation.vector - IMUPose1.translation.vector; % translation in absolute frame + + measurements(i).imu(j).deltaT = deltaT; + + % gyro rate: Logmap(R_i' * R_i+1) / deltaT + measurements(i).imu(j).gyro = IMUdeltaRotVector./deltaT; + + % deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT; + % acc = (deltaPosition - initialVel * dT) * (2/dt^2) + measurements(i).imu(j).accel = IMURot1' * (IMUdeltaPositionVector - currentVel.*deltaT).*(2/(deltaT*deltaT)); + + % Update velocity + currentVel = currentVel + IMURot1 * measurements(i).imu(j).accel * deltaT; + end + end + + % Add Values: velocity and bias + values.insert(currentVelKey, LieVector(currentVel)); + values.insert(currentBiasKey, metadata.imu.zeroBias); + end + + %% gt GPS measurements + if options.includeGPSFactors == 1 && i > 0 + gpsPositionVector = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioInd).translation.vector; + measurements(i).gpsPositionVector = gpsPositionVector; + end + + %% gt Camera measurements + if options.includeCameraFactors == 1 && i > 0 + % Create the camera based on the current pose and the pose of the + % camera in the body + cameraPose = currentPose.compose(metadata.camera.bodyPoseCamera); + camera = SimpleCamera(cameraPose, metadata.camera.calibration); + %camera = SimpleCamera(currentPose, metadata.camera.calibration); + + % Record measurements if the landmark is within visual range + for j=1:length(metadata.camera.gtLandmarkPoints) + distanceToLandmark = camera.pose.range(metadata.camera.gtLandmarkPoints(j)); + if distanceToLandmark < metadata.camera.visualRange + try + z = camera.project(metadata.camera.gtLandmarkPoints(j)); + measurements(i).landmarks(j) = z; + catch + % point is probably out of the camera's view + end + end + end + end + + end + fprintf('\n'); +else + error('Please use RealData') + %% Create a random trajectory as ground truth + currentPose = Pose3; % initial pose % initial pose + + unsmooth_DP = 0.5; % controls smoothness on translation norm + unsmooth_DR = 0.1; % controls smoothness on rotation norm + + fprintf('\nCreating a random ground truth trajectory\n'); + currentPoseKey = symbol('x', 0); + values.insert(currentPoseKey, currentPose); + + for i=1:options.trajectoryLength + % Update the pose key + currentPoseKey = symbol('x', i); + + % Generate the new measurements + gtDeltaPosition = unsmooth_DP*randn(3,1) + [20;0;0]; % create random vector with mean = [20 0 0] + gtDeltaRotation = unsmooth_DR*randn(3,1) + [0;0;0]; % create random rotation with mean [0 0 0] + measurements.deltaMatrix(i,:) = [gtDeltaRotation; gtDeltaPosition]; + + % Create the next pose + deltaPose = Pose3.Expmap(measurements.deltaMatrix(i,:)'); + currentPose = currentPose.compose(deltaPose); + + % Add the current pose as a value + values.insert(currentPoseKey, currentPose); + end % end of random trajectory creation +end % end of else + +end % end of function + diff --git a/matlab/unstable_examples/+imuSimulator/ct2ENU.m b/matlab/unstable_examples/+imuSimulator/ct2ENU.m new file mode 100644 index 000000000..b7257b664 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/ct2ENU.m @@ -0,0 +1,55 @@ +function [dx,dy,dz]=ct2ENU(dX,dY,dZ,lat,lon) +% CT2LG Converts CT coordinate differences to local geodetic. +% Local origin at lat,lon,h. If lat,lon are vectors, dx,dy,dz +% are referenced to orgin at lat,lon of same index. If +% astronomic lat,lon input, output is in local astronomic +% system. Vectorized in both dx,dy,dz and lat,lon. See also +% LG2CT. +% Version: 2011-02-19 +% Useage: [dx,dy,dz]=ct2lg(dX,dY,dZ,lat,lon) +% Input: dX - vector of X coordinate differences in CT +% dY - vector of Y coordinate differences in CT +% dZ - vector of Z coordinate differences in CT +% lat - lat(s) of local system origin (rad); may be vector +% lon - lon(s) of local system origin (rad); may be vector +% Output: dx - vector of x coordinates in local system (east) +% dy - vector of y coordinates in local system (north) +% dz - vector of z coordinates in local system (up) + +% Copyright (c) 2011, Michael R. Craymer +% All rights reserved. +% Email: mike@craymer.com + +if nargin ~= 5 + warning('Incorrect number of input arguements'); + return +end + +n=length(dX); +if length(lat)==1 + lat=ones(n,1)*lat; + lon=ones(n,1)*lon; +end +R=zeros(3,3,n); + +R(1,1,:)=-sin(lat').*cos(lon'); +R(1,2,:)=-sin(lat').*sin(lon'); +R(1,3,:)=cos(lat'); +R(2,1,:)=sin(lon'); +R(2,2,:)=-cos(lon'); +R(2,3,:)=zeros(1,n); +R(3,1,:)=cos(lat').*cos(lon'); +R(3,2,:)=cos(lat').*sin(lon'); +R(3,3,:)=sin(lat'); + +RR=reshape(R(1,:,:),3,n); +dx_temp=sum(RR'.*[dX dY dZ],2); +RR=reshape(R(2,:,:),3,n); +dy_temp=sum(RR'.*[dX dY dZ],2); +RR=reshape(R(3,:,:),3,n); +dz=sum(RR'.*[dX dY dZ],2); + +dx = -dy_temp; +dy = dx_temp; + + diff --git a/matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m b/matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m new file mode 100644 index 000000000..42dc1db60 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m @@ -0,0 +1,39 @@ +function currentPose = getPoseFromGtScenario(gtScenario,scenarioInd) +% gtScenario contains vectors (Lat, Lon, Alt, Roll, Pitch, Yaw) +% The function picks the index 'scenarioInd' in those vectors and +% computes the corresponding pose by +% 1) Converting (Lat,Lon,Alt) to local coordinates +% 2) Converting (Roll,Pitch,Yaw) to a rotation matrix + +import gtsam.*; + +Org_lat = gtScenario.Lat(1); +Org_lon = gtScenario.Lon(1); +initialPositionECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario.Lat(1); gtScenario.Lon(1); gtScenario.Alt(1)]); + +gtECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario.Lat(scenarioInd); gtScenario.Lon(scenarioInd); gtScenario.Alt(scenarioInd)]); +% truth in ENU +dX = gtECEF(1) - initialPositionECEF(1); +dY = gtECEF(2) - initialPositionECEF(2); +dZ = gtECEF(3) - initialPositionECEF(3); +[xlt, ylt, zlt] = imuSimulator.ct2ENU(dX, dY, dZ,Org_lat, Org_lon); + +gtPosition = Point3([xlt, ylt, zlt]'); +% use the gtsam.Rot3.Ypr constructor (yaw, pitch, roll) from the ground truth data +% yaw = measured positively to the right +% pitch = measured positively up +% roll = measured positively to right +% Assumes vehice X forward, Y right, Z down +% +% In the gtScenario data +% heading (yaw) = measured positively to the left from Y-axis +% pitch = +% roll = +% Coordinate frame is Y forward, X is right, Z is up +gtBodyRotation = Rot3.Ypr(-gtScenario.Heading(scenarioInd), gtScenario.Pitch(scenarioInd), gtScenario.Roll(scenarioInd)); +currentPose = Pose3(gtBodyRotation, gtPosition); + +%% Rotate the pose +currentPose = currentPose.compose(Pose3.Expmap([-pi/2;0;0;0;0;0])); + +end \ No newline at end of file diff --git a/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory.m b/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory.m new file mode 100644 index 000000000..3f72f1215 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory.m @@ -0,0 +1,17 @@ +function [ finalPose, finalVelocityGlobal ] = integrateIMUTrajectory( ... + initialPoseGlobal, initialVelocityGlobal, acc_omegaIMU, deltaT) +%INTEGRATETRAJECTORY Integrate one trajectory step from IMU measurement + +import gtsam.*; + +imu2in1 = Rot3.Expmap(acc_omegaIMU(4:6) * deltaT); +accelGlobal = initialPoseGlobal.rotation().rotate(Point3(acc_omegaIMU(1:3))).vector; + +finalPosition = Point3(initialPoseGlobal.translation.vector ... + + initialVelocityGlobal * deltaT + 0.5 * accelGlobal * deltaT * deltaT); +finalVelocityGlobal = initialVelocityGlobal + accelGlobal * deltaT; +finalRotation = initialPoseGlobal.rotation.compose(imu2in1); +finalPose = Pose3(finalRotation, finalPosition); + +end + diff --git a/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_bodyFrame.m b/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_bodyFrame.m new file mode 100644 index 000000000..50b223060 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_bodyFrame.m @@ -0,0 +1,26 @@ +function [ finalPose, finalVelocityGlobal ] = integrateIMUTrajectory_bodyFrame( ... + initialPoseGlobal, initialVelocityGlobal, acc_omegaIMU, deltaT, velocity1Body) + +% Before integrating in the body frame we need to compensate for the Coriolis +% effect +acc_body = acc_omegaIMU(1:3) - Point3(cross(acc_omegaIMU(4:6), velocity1Body)).vector; +% after compensating for coriolis this will be essentially zero +% since we are moving at constant body velocity + +import gtsam.*; +%% Integrate in the body frame +% Integrate rotations +imu2in1 = Rot3.Expmap(acc_omegaIMU(4:6) * deltaT); +% Integrate positions +finalPositionBody = velocity1Body * deltaT + 0.5 * acc_body * deltaT * deltaT; +finalVelocityBody = velocity1Body + acc_body * deltaT; + +%% Express the integrated quantities in the global frame +finalVelocityGlobal = initialVelocityGlobal + (initialPoseGlobal.rotation().rotate(Point3(finalVelocityBody)).vector() ); +finalPosition = initialPoseGlobal.translation().vector() + initialPoseGlobal.rotation().rotate( Point3(finalPositionBody)).vector() ; +finalRotation = initialPoseGlobal.rotation.compose(imu2in1); +% Include position and rotation in a pose +finalPose = Pose3(finalRotation, Point3(finalPosition) ); + +end + diff --git a/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_navFrame.m b/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_navFrame.m new file mode 100644 index 000000000..b919520ac --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_navFrame.m @@ -0,0 +1,21 @@ +function [ finalPose, finalVelocityGlobal ] = integrateIMUTrajectory_navFrame( ... + initialPoseGlobal, initialVelocityGlobal, acc_omegaIMU, deltaT) +%INTEGRATETRAJECTORY Integrate one trajectory step from IMU measurement + +import gtsam.*; +% Integrate rotations +imu2in1 = Rot3.Expmap(acc_omegaIMU(4:6) * deltaT); +finalRotation = initialPoseGlobal.rotation.compose(imu2in1); + +intermediateRotation = initialPoseGlobal.rotation.compose( Rot3.Expmap(acc_omegaIMU(4:6) * deltaT/2 )); +% Integrate positions (equation (1) in Lupton) +accelGlobal = intermediateRotation.rotate(Point3(acc_omegaIMU(1:3))).vector; +finalPosition = Point3(initialPoseGlobal.translation.vector ... + + initialVelocityGlobal * deltaT + 0.5 * accelGlobal * deltaT * deltaT); +finalVelocityGlobal = initialVelocityGlobal + accelGlobal * deltaT; + +% Include position and rotation in a pose +finalPose = Pose3(finalRotation, finalPosition); + +end + diff --git a/matlab/unstable_examples/+imuSimulator/integrateTrajectory.m b/matlab/unstable_examples/+imuSimulator/integrateTrajectory.m new file mode 100644 index 000000000..e51b622b0 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/integrateTrajectory.m @@ -0,0 +1,24 @@ +function [ finalPose, finalVelocityGlobal ] = integrateTrajectory( ... + initialPose, omega1Body, velocity1Body, velocity2Body, deltaT) +%INTEGRATETRAJECTORY Integrate one trajectory step + +import gtsam.*; +% Rotation: R^1_2 +body2in1 = Rot3.Expmap(omega1Body * deltaT); +% Velocity 2 in frame 1: v^1_2 = R^1_2 v^2_2 +velocity2inertial = body2in1.rotate(Point3(velocity2Body)).vector; +% Acceleration: a^1 = (v^1_2 - v^1_1)/dt +accelBody1 = (velocity2inertial - velocity1Body) / deltaT; + +% Velocity 1 in frame W: v^W_1 = R^W_1 v^1_1 +initialVelocityGlobal = initialPose.rotation().rotate(Point3(velocity1Body)).vector; +% Acceleration in frame W: a^W = R^W_1 a^1 +accelGlobal = initialPose.rotation().rotate(Point3(accelBody1)).vector; + +finalPosition = Point3(initialPose.translation.vector + initialVelocityGlobal * deltaT + 0.5 * accelGlobal * deltaT * deltaT); +finalVelocityGlobal = initialVelocityGlobal + accelGlobal * deltaT; +finalRotation = initialPose.rotation.compose(body2in1); +finalPose = Pose3(finalRotation, finalPosition); + +end + diff --git a/matlab/unstable_examples/+imuSimulator/runConsistencyTests.m b/matlab/unstable_examples/+imuSimulator/runConsistencyTests.m new file mode 100644 index 000000000..697b49a6a --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/runConsistencyTests.m @@ -0,0 +1,173 @@ +% use this script to easily run and save results for multiple consistency +% tests without having to pay attention to the computer every 5 minutes + +import gtsam.*; + +resultsDir = 'results/' +if (~exist(resultsDir, 'dir')) + mkdir(resultsDir); +end + +testOptions = [ ... + % 1 2 3 4 5 6 7 8 9 10 11 12 + % RealData? Between? IMU? IMUType Bias? Camera? #LndMrk GPS? StrtPose TrajLength Subsample #MCRuns + %1 0 1 2 0 0 100 0 100 209 20 100 ;... % 1 + %1 0 1 2 0 0 100 0 100 209 20 100 ;... % 2 + % 1 0 1 2 0 0 100 0 100 209 20 100 ;... % 3 + 1 0 1 2 0 1 100 0 100 209 20 20 ;... % 4 + 1 0 1 2 0 1 100 0 100 209 20 20 ;... % 5 + 1 0 1 2 0 0 100 0 100 209 20 20 ];%... % 6 + % 1 0 1 2 0 0 100 0 100 209 20 100 ;... % 7 + %1 0 1 2 0 0 100 0 100 209 20 1 ;... % 8 + %1 0 1 2 0 0 100 0 100 209 20 1 ]; % 9 + +noises = [ ... + % 1 2 3 4 5 6 7 8 + % sigma_ang sigma_cart sigma_accel sigma_gyro sigma_accelBias sigma_gyroBias sigma_gps sigma_camera + %1e-2 1e-1 1e-3 1e-5 0 0 1e-4 1;... % 1 + %1e-2 1e-1 1e-2 1e-5 0 0 1e-4 1;... % 2 + % 1e-2 1e-1 1e-1 1e-5 0 0 1e-4 1;... % 3 + 1e-2 1e-1 1e-3 1e-4 0 0 1e-4 1;... % 4 + 1e-2 1e-1 1e-3 1e-3 0 0 1e-4 1;... % 5 + 1e-2 1e-1 1e-3 1e-2 0 0 1e-4 1];%... % 6 + % 1e-2 1e-1 1e-3 1e-1 0 0 1e-4 1;... % 7 + %1e-2 1e-1 1e-3 1e-2 1e-3 1e-5 1e-4 1;... % 8 + %1e-2 1e-1 1e-3 1e-2 1e-4 1e-6 1e-4 1]; % 9 + +if(size(testOptions,1) ~= size(noises,1)) + error('testOptions and noises do not have same number of rows'); +end + +% Set flag so the script knows there is an external configuration +externallyConfigured = 1; + +% Set the flag to save the results +saveResults = 0; + +errorRuns = []; + +% Go through tests +for i = 1:size(testOptions,1) + % Clean up from last test + close all; + %clc; + + % Set up variables for test + options.useRealData = testOptions(i,1); + options.includeBetweenFactors = testOptions(i,2); + options.includeIMUFactors = testOptions(i,3); + options.imuFactorType = testOptions(i,4); + options.imuNonzeroBias = testOptions(i,5); + options.includeCameraFactors = testOptions(i,6); + options.numberOfLandmarks = testOptions(i,7); + options.includeGPSFactors = testOptions(i,8); + options.gpsStartPose = testOptions(i,9); + options.trajectoryLength = testOptions(i,10); + options.subsampleStep = testOptions(i,11); + numMonteCarloRuns = testOptions(i,12); + + sigma_ang = noises(i,1); + sigma_cart = noises(i,2); + sigma_accel = noises(i,3); + sigma_gyro = noises(i,4); + sigma_accelBias = noises(i,5); + sigma_gyroBias = noises(i,6); + sigma_gps = noises(i,7); + sigma_camera = noises(i,8); + + % Create folder name + f_between = ''; + f_imu = ''; + f_bias = ''; + f_gps = ''; + f_camera = ''; + f_runs = ''; + + if (options.includeBetweenFactors == 1); + f_between = 'between_'; + end + if (options.includeIMUFactors == 1) + f_imu = sprintf('imu%d_', options.imuFactorType); + if (options.imuNonzeroBias == 1); + f_bias = sprintf('bias_a%1.2g_g%1.2g_', sigma_accelBias, sigma_gyroBias); + end + end + if (options.includeGPSFactors == 1); + f_between = sprintf('gps_%d_', gpsStartPose); + end + if (options.includeCameraFactors == 1) + f_camera = sprintf('camera_%d_', options.numberOfLandmarks); + end + f_runs = sprintf('mc%d', numMonteCarloRuns); + + folderName = [resultsDir f_between f_imu f_bias f_gps f_camera f_runs '/']; + + % make folder if it doesnt exist + if (~exist(folderName, 'dir')) + mkdir(folderName); + end + + testName = sprintf('sa-%1.2g-sc-%1.2g-sacc-%1.2g-sg-%1.2g',sigma_ang,sigma_cart,sigma_accel,sigma_gyro); + + % Run the test + fprintf('Test %d\n\tResults will be saved to:\n\t%s\n\trunning...\n', i, folderName); + fprintf('Test Name: %s\n', testName); + + try + imuSimulator.covarianceAnalysisBetween; + catch + errorRuns = [errorRuns i]; + fprintf('\n*****\n Something went wrong, most likely indeterminant linear system error.\n'); + disp('Test Options:\n'); + disp(testOptions(i,:)); + disp('Noises'); + disp(noises(i,:)); + fprintf('\n*****\n\n'); + end +end + +% Print error summary +fprintf('*************************\n'); +fprintf('%d Runs failed due to errors (data not collected for failed runs)\n', length(errorRuns)); +for i = 1:length(errorRuns) + k = errorRuns(i); + fprintf('\nTest %d:\n', k); + fprintf(' options.useRealData = %d\n', testOptions(k,1)); + fprintf(' options.includeBetweenFactors = %d\n', testOptions(k,2)); + fprintf(' options.includeIMUFactors = %d\n', testOptions(k,3)); + fprintf(' options.imuFactorType = %d\n', testOptions(k,4)); + fprintf(' options.imuNonzeroBias = %d\n', testOptions(k,5)); + fprintf(' options.includeCameraFactors = %d\n', testOptions(k,6)); + fprintf(' numberOfLandmarks = %d\n', testOptions(k,7)); + fprintf(' options.includeGPSFactors = %d\n', testOptions(k,8)); + fprintf(' options.gpsStartPose = %d\n', testOptions(k,9)); + fprintf(' options.trajectoryLength = %d\n', testOptions(k,10)); + fprintf(' options.subsampleStep = %d\n', testOptions(k,11)); + fprintf(' numMonteCarloRuns = %d\n', testOptions(k,12)); + fprintf('\n'); + fprintf(' sigma_ang = %f\n', noises(i,1)); + fprintf(' sigma_cart = %f\n', noises(i,2)); + fprintf(' sigma_accel = %f\n', noises(i,3)); + fprintf(' sigma_gyro = %f\n', noises(i,4)); + fprintf(' sigma_accelBias = %f\n', noises(i,5)); + fprintf(' sigma_gyroBias = %f\n', noises(i,6)); + fprintf(' sigma_gps = %f\n', noises(i,7)); +end + + + + + + + + + + + + + + + + + + diff --git a/matlab/unstable_examples/+imuSimulator/test1onestep.m b/matlab/unstable_examples/+imuSimulator/test1onestep.m new file mode 100644 index 000000000..883569849 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/test1onestep.m @@ -0,0 +1,48 @@ +import gtsam.*; + +deltaT = 0.01; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Constant global velocity w/ lever arm +disp('--------------------------------------------------------'); +disp('Constant global velocity w/ lever arm'); +omega = [0;0;0.1]; +velocity = [1;0;0]; +R = Rot3.Expmap(omega * deltaT); + +velocity2body = R.unrotate(Point3(velocity)).vector; +acc_omegaExpected = [-0.01; 0; 0; 0; 0; 0.1]; +acc_omegaActual = imuSimulator.calculateIMUMeasurement(omega, omega, velocity, velocity2body, deltaT, Pose3(Rot3, Point3([1;0;0]))); +disp('IMU measurement discrepancy:'); +disp(acc_omegaActual - acc_omegaExpected); + +initialPose = Pose3; +finalPoseExpected = Pose3(Rot3.Expmap(omega * deltaT), Point3(velocity * deltaT)); +finalVelocityExpected = velocity; +[ finalPoseActual, finalVelocityActual ] = imuSimulator.integrateTrajectory(initialPose, omega, velocity, velocity2body, deltaT); +disp('Final pose discrepancy:'); +disp(finalPoseExpected.between(finalPoseActual).matrix); +disp('Final velocity discrepancy:'); +disp(finalVelocityActual - finalVelocityExpected); + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Constant body velocity w/ lever arm +disp('--------------------------------------------------------'); +disp('Constant body velocity w/ lever arm'); +omega = [0;0;0.1]; +velocity = [1;0;0]; + +acc_omegaExpected = [-0.01; 0.1; 0; 0; 0; 0.1]; +acc_omegaActual = imuSimulator.calculateIMUMeasurement(omega, omega, velocity, velocity, deltaT, Pose3(Rot3, Point3([1;0;0]))); +disp('IMU measurement discrepancy:'); +disp(acc_omegaActual - acc_omegaExpected); + +initialPose = Pose3; +initialVelocity = velocity; +finalPoseExpected = Pose3.Expmap([ omega; velocity ] * deltaT); +finalVelocityExpected = finalPoseExpected.rotation.rotate(Point3(velocity)).vector; +[ finalPoseActual, finalVelocityActual ] = imuSimulator.integrateTrajectory(initialPose, omega, velocity, velocity, deltaT); +disp('Final pose discrepancy:'); +disp(finalPoseExpected.between(finalPoseActual).matrix); +disp('Final velocity discrepancy:'); +disp(finalVelocityActual - finalVelocityExpected); \ No newline at end of file diff --git a/matlab/unstable_examples/+imuSimulator/test2constglobal.m b/matlab/unstable_examples/+imuSimulator/test2constglobal.m new file mode 100644 index 000000000..19956d08a --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/test2constglobal.m @@ -0,0 +1,34 @@ +import gtsam.*; + +deltaT = 0.01; +timeElapsed = 1000; + +times = 0:deltaT:timeElapsed; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Constant global velocity w/ lever arm +disp('--------------------------------------------------------'); +disp('Constant global velocity w/ lever arm'); +omega = [0;0;0.1]; +velocity = [1;0;0]; + +% Initial state +currentPoseGlobal = Pose3; +currentVelocityGlobal = velocity; + +% Positions +positions = zeros(3, length(times)+1); + +i = 2; +for t = times + velocity1body = currentPoseGlobal.rotation.unrotate(Point3(currentVelocityGlobal)).vector; + R = Rot3.Expmap(omega * deltaT); + velocity2body = currentPoseGlobal.rotation.compose(R).unrotate(Point3(currentVelocityGlobal)).vector; + [ currentPoseGlobal, currentVelocityGlobal ] = imuSimulator.integrateTrajectory(currentPoseGlobal, omega, velocity1body, velocity2body, deltaT); + + positions(:,i) = currentPoseGlobal.translation.vector; + i = i + 1; +end + +figure; +plot(positions(1,:), positions(2,:), '.-'); diff --git a/matlab/unstable_examples/+imuSimulator/test3constbody.m b/matlab/unstable_examples/+imuSimulator/test3constbody.m new file mode 100644 index 000000000..b3ee2edfc --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/test3constbody.m @@ -0,0 +1,96 @@ +clc +clear all +close all + +import gtsam.*; + +addpath(genpath('./Libraries')) + +deltaT = 0.01; +timeElapsed = 25; + +times = 0:deltaT:timeElapsed; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Constant body velocity w/ lever arm +disp('--------------------------------------------------------'); +disp('Constant body velocity w/ lever arm'); +omega = [0;0;0.1]; +velocity = [1;0;0]; +RIMUinBody = Rot3.Rz(-pi/2); +% RIMUinBody = eye(3) +IMUinBody = Pose3(RIMUinBody, Point3([1;0;0])); + +% Initial state (body) +currentPoseGlobal = Pose3; +currentVelocityGlobal = velocity; +% Initial state (IMU) +currentPoseGlobalIMU = Pose3; %currentPoseGlobal.compose(IMUinBody); +%currentVelocityGlobalIMU = IMUinBody.rotation.unrotate(Point3(velocity)).vector; % no Coriolis here? +currentVelocityGlobalIMU = IMUinBody.rotation.unrotate( ... + Point3(velocity + cross(omega, IMUinBody.translation.vector))).vector; + + +% Positions +% body trajectory +positions = zeros(3, length(times)+1); +positions(:,1) = currentPoseGlobal.translation.vector; +poses(1).p = positions(:,1); +poses(1).R = currentPoseGlobal.rotation.matrix; + +% Expected IMU trajectory (from body trajectory and lever arm) +positionsIMUe = zeros(3, length(times)+1); +positionsIMUe(:,1) = IMUinBody.compose(currentPoseGlobalIMU).translation.vector; +posesIMUe(1).p = positionsIMUe(:,1); +posesIMUe(1).R = poses(1).R * IMUinBody.rotation.matrix; + +% Integrated IMU trajectory (from IMU measurements) +positionsIMU = zeros(3, length(times)+1); +positionsIMU(:,1) = IMUinBody.compose(currentPoseGlobalIMU).translation.vector; +posesIMU(1).p = positionsIMU(:,1); +posesIMU(1).R = IMUinBody.compose(currentPoseGlobalIMU).rotation.matrix; + +i = 2; +for t = times + [ currentPoseGlobal, currentVelocityGlobal ] = imuSimulator.integrateTrajectory( ... + currentPoseGlobal, omega, velocity, velocity, deltaT); + + acc_omega = imuSimulator.calculateIMUMeasurement( ... + omega, omega, velocity, velocity, deltaT, IMUinBody); + + [ currentPoseGlobalIMU, currentVelocityGlobalIMU ] = imuSimulator.integrateIMUTrajectory( ... + currentPoseGlobalIMU, currentVelocityGlobalIMU, acc_omega, deltaT); + + % Store data in some structure for statistics and plots + positions(:,i) = currentPoseGlobal.translation.vector; + positionsIMUe(:,i) = currentPoseGlobal.translation.vector + currentPoseGlobal.rotation.matrix * IMUinBody.translation.vector; + positionsIMU(:,i) = IMUinBody.compose(currentPoseGlobalIMU).translation.vector; + + poses(i).p = positions(:,i); + posesIMUe(i).p = positionsIMUe(:,i); + posesIMU(i).p = positionsIMU(:,i); + + poses(i).R = currentPoseGlobal.rotation.matrix; + posesIMUe(i).R = poses(i).R * IMUinBody.rotation.matrix; + posesIMU(i).R = IMUinBody.compose(currentPoseGlobalIMU).rotation.matrix; + i = i + 1; +end + + +figure(1) +plot_trajectory(poses, 50, '-k', 'body trajectory',0.1,0.75,1) +hold on +plot_trajectory(posesIMU, 50, '-r', 'imu trajectory',0.1,0.75,1) + +figure(2) +hold on; +plot(positions(1,:), positions(2,:), '-b'); +plot(positionsIMU(1,:), positionsIMU(2,:), '-r'); +plot(positionsIMUe(1,:), positionsIMUe(2,:), ':k'); +axis equal; + +disp('Mismatch between final integrated IMU position and expected IMU position') +disp(norm(posesIMUe(end).p - posesIMU(end).p)) +disp('Mismatch between final integrated IMU orientation and expected IMU orientation') +disp(norm(posesIMUe(end).R - posesIMU(end).R)) + diff --git a/matlab/unstable_examples/+imuSimulator/test4circle.m b/matlab/unstable_examples/+imuSimulator/test4circle.m new file mode 100644 index 000000000..22ee175dd --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/test4circle.m @@ -0,0 +1,97 @@ +clc +clear all +close all + +import gtsam.*; + +deltaT = 0.001; +timeElapsed = 1; +times = 0:deltaT:timeElapsed; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +disp('--------------------------------------------------------'); +disp('Integration in body frame VS integration in navigation frame'); +disp('TOY EXAMPLE:'); +disp('- Body moves along a circular trajectory with constant rotation rate -omega- and constant -velocity- (in the body frame)'); +disp('- We compare two integration schemes: integating in the navigation frame (similar to Lupton approach) VS integrating in the body frame') +disp('- Navigation frame is assumed inertial for simplicity') +omega = [0;0;2*pi]; +velocity = [1;0;0]; + +%% Set initial conditions for the true trajectory and for the estimates +% (one estimate is obtained by integrating in the body frame, the other +% by integrating in the navigation frame) +% Initial state (body) +currentPoseGlobal = Pose3; +currentVelocityGlobal = velocity; +% Initial state estimate (integrating in navigation frame) +currentPoseGlobalIMUnav = currentPoseGlobal; +currentVelocityGlobalIMUnav = currentVelocityGlobal; +% Initial state estimate (integrating in the body frame) +currentPoseGlobalIMUbody = currentPoseGlobal; +currentVelocityGlobalIMUbody = currentVelocityGlobal; + +%% Prepare data structures for actual trajectory and estimates +% Actual trajectory +positions = zeros(3, length(times)+1); +positions(:,1) = currentPoseGlobal.translation.vector; +poses(1).p = positions(:,1); +poses(1).R = currentPoseGlobal.rotation.matrix; + +% Trajectory estimate (integrated in the navigation frame) +positionsIMUnav = zeros(3, length(times)+1); +positionsIMUnav(:,1) = currentPoseGlobalIMUbody.translation.vector; +posesIMUnav(1).p = positionsIMUnav(:,1); +posesIMUnav(1).R = poses(1).R; + +% Trajectory estimate (integrated in the body frame) +positionsIMUbody = zeros(3, length(times)+1); +positionsIMUbody(:,1) = currentPoseGlobalIMUbody.translation.vector; +posesIMUbody(1).p = positionsIMUbody(:,1); +posesIMUbody(1).R = poses(1).R; + +%% Main loop +i = 2; +for t = times + %% Create the actual trajectory, using the velocities and + % accelerations in the inertial frame to compute the positions + [ currentPoseGlobal, currentVelocityGlobal ] = imuSimulator.integrateTrajectory( ... + currentPoseGlobal, omega, velocity, velocity, deltaT); + + %% Simulate IMU measurements, considering Coriolis effect + % (in this simple example we neglect gravity and there are no other forces acting on the body) + acc_omega = imuSimulator.calculateIMUMeas_coriolis( ... + omega, omega, velocity, velocity, deltaT); + + %% Integrate in the body frame + [ currentPoseGlobalIMUbody, currentVelocityGlobalIMUbody ] = imuSimulator.integrateIMUTrajectory_bodyFrame( ... + currentPoseGlobalIMUbody, currentVelocityGlobalIMUbody, acc_omega, deltaT, velocity); + + %% Integrate in the navigation frame + [ currentPoseGlobalIMUnav, currentVelocityGlobalIMUnav ] = imuSimulator.integrateIMUTrajectory_navFrame( ... + currentPoseGlobalIMUnav, currentVelocityGlobalIMUnav, acc_omega, deltaT); + + %% Store data in some structure for statistics and plots + positions(:,i) = currentPoseGlobal.translation.vector; + positionsIMUbody(:,i) = currentPoseGlobalIMUbody.translation.vector; + positionsIMUnav(:,i) = currentPoseGlobalIMUnav.translation.vector; + % - + poses(i).p = positions(:,i); + posesIMUbody(i).p = positionsIMUbody(:,i); + posesIMUnav(i).p = positionsIMUnav(:,i); + % - + poses(i).R = currentPoseGlobal.rotation.matrix; + posesIMUbody(i).R = currentPoseGlobalIMUbody.rotation.matrix; + posesIMUnav(i).R = currentPoseGlobalIMUnav.rotation.matrix; + i = i + 1; +end + +figure(1) +hold on; +plot(positions(1,:), positions(2,:), '-b'); +plot(positionsIMUbody(1,:), positionsIMUbody(2,:), '-r'); +plot(positionsIMUnav(1,:), positionsIMUnav(2,:), ':k'); +axis equal; +legend('true trajectory', 'traj integrated in body', 'traj integrated in nav') + +