Merge remote-tracking branch 'origin/feature/new_imu_factors' into develop
						commit
						a34dff1397
					
				
							
								
								
									
										15
									
								
								gtsam.h
								
								
								
								
							
							
						
						
									
										15
									
								
								gtsam.h
								
								
								
								
							|  | @ -2299,7 +2299,8 @@ virtual class ConstantBias : gtsam::Value { | |||
| #include <gtsam/navigation/ImuFactor.h> | ||||
| 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; | ||||
|  |  | |||
|  | @ -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<Pose3> 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<CombinedImuFactor> shared_ptr; | ||||
| #if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5 | ||||
|     typedef typename boost::shared_ptr<CombinedImuFactor> shared_ptr; | ||||
| #else | ||||
|       typedef boost::shared_ptr<CombinedImuFactor> 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<const Pose3&> 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<const Pose3&> 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<const This*> (&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<const Pose3&> body_P_sensor = boost::none) | ||||
|         const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional<const Pose3&> 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)
 | ||||
|  |  | |||
|  | @ -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<Pose3> 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<ImuFactor> shared_ptr; | ||||
| #else | ||||
|     typedef boost::shared_ptr<ImuFactor> 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<const Pose3&> 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<const Pose3&> 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<const This*> (&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<const Pose3&> body_P_sensor = boost::none) | ||||
|         const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional<const Pose3&> 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)
 | ||||
|  |  | |||
|  | @ -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 <gtsam/linear/GaussianFactorGraph.h> | ||||
|  |  | |||
|  | @ -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 <gtsam/linear/GaussianFactorGraph.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 <gtsam_unstable/slam/SmartProjectionPoseFactor.h> | ||||
| template<POSE, LANDMARK, CALIBRATION> | ||||
| 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<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_S2> SmartProjectionPose3Factor; | ||||
| 
 | ||||
| #include <gtsam/slam/RangeFactor.h> | ||||
| template<POSE, POINT> | ||||
|  |  | |||
|  | @ -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]; | ||||
| 
 | ||||
|  | @ -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 | ||||
|      | ||||
|      | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
|  | @ -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 | ||||
|  | @ -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)   | ||||
|  | @ -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; | ||||
|  | @ -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 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
|  | @ -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; | ||||
| 
 | ||||
|  | @ -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') | ||||
| 
 | ||||
| 
 | ||||
|  | @ -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') | ||||
| 
 | ||||
| 
 | ||||
|  | @ -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) ; | ||||
|  | @ -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) | ||||
|  | @ -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 | ||||
| 
 | ||||
|  | @ -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 | ||||
| 
 | ||||
|  | @ -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.. | ||||
|  | @ -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); | ||||
| 
 | ||||
| 
 | ||||
|  | @ -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 | ||||
| 
 | ||||
|  | @ -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 | ||||
| 
 | ||||
|  | @ -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 | ||||
| 
 | ||||
|  | @ -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; | ||||
| 
 | ||||
| 
 | ||||
|  | @ -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 | ||||
|  | @ -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 | ||||
| 
 | ||||
|  | @ -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 | ||||
| 
 | ||||
|  | @ -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 | ||||
| 
 | ||||
|  | @ -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 | ||||
| 
 | ||||
|  | @ -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 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
|  | @ -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); | ||||
|  | @ -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,:), '.-'); | ||||
|  | @ -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)) | ||||
| 
 | ||||
|  | @ -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') | ||||
| 
 | ||||
| 
 | ||||
		Loading…
	
		Reference in New Issue