12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'origin/feature/new_imu_factors' into develop

release/4.3a0
cbeall3 2014-06-20 13:32:34 -04:00
parent a7cd94c9dc 950e95b015
commit a34dff1397
35 changed files with 3014 additions and 118 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
gtsam.h
View File

@ -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;

186
gtsam/navigation/CombinedImuFactor.h
View File

@ -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)

162
gtsam/navigation/ImuFactor.h
View File

@ -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)

4
gtsam/navigation/tests/testCombinedImuFactor.cpp
View File

@ -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>

14
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -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>

19
gtsam_unstable/gtsam_unstable.h
View File

@ -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>

7
matlab/unstable_examples/+imuSimulator/+lib/antisim.m Normal file
View File

@ -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];

140
matlab/unstable_examples/+imuSimulator/+lib/arrow3d.m Normal file
View File

@ -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

14
matlab/unstable_examples/+imuSimulator/+lib/getxyz.m Normal file
View File

@ -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

19
matlab/unstable_examples/+imuSimulator/+lib/plot_trajectory.m Normal file
View File

@ -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)

54
matlab/unstable_examples/+imuSimulator/+lib/ref_frame_plot.m Normal file
View File

@ -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;

82
matlab/unstable_examples/+imuSimulator/+lib/rotatePoints.m Normal file
View File

@ -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

14
matlab/unstable_examples/+imuSimulator/+lib/uth2rot.m Normal file
View File

@ -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;

146
matlab/unstable_examples/+imuSimulator/IMUComparison.m Normal file
View File

@ -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')

128
matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m Normal file
View File

@ -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')

26
matlab/unstable_examples/+imuSimulator/LatLonHRad_to_ECEF.m Normal file
View File

@ -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) ;

68
matlab/unstable_examples/+imuSimulator/OdometryExample3D.m Normal file
View File

@ -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)

14
matlab/unstable_examples/+imuSimulator/calculateIMUMeas_coriolis.m Normal file
View File

@ -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

26
matlab/unstable_examples/+imuSimulator/calculateIMUMeasurement.m Normal file
View File

@ -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

508
matlab/unstable_examples/+imuSimulator/coriolisExample.m Normal file
View File

@ -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..

182
matlab/unstable_examples/+imuSimulator/coriolisTestMonteCarlo.m Normal file
View File

@ -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);

325
matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m Normal file
View File

@ -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

199
matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m Normal file
View File

@ -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

150
matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m Normal file
View File

@ -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

55
matlab/unstable_examples/+imuSimulator/ct2ENU.m Normal file
View File

@ -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;

39
matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m Normal file
View File

@ -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

17
matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory.m Normal file
View File

@ -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

26
matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_bodyFrame.m Normal file
View File

@ -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

21
matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_navFrame.m Normal file
View File

@ -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

24
matlab/unstable_examples/+imuSimulator/integrateTrajectory.m Normal file
View File

@ -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

173
matlab/unstable_examples/+imuSimulator/runConsistencyTests.m Normal file
View File

@ -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

48
matlab/unstable_examples/+imuSimulator/test1onestep.m Normal file
View File

@ -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);

34
matlab/unstable_examples/+imuSimulator/test2constglobal.m Normal file
View File

@ -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,:), '.-');

96
matlab/unstable_examples/+imuSimulator/test3constbody.m Normal file
View File

@ -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))

97
matlab/unstable_examples/+imuSimulator/test4circle.m Normal file
View File

@ -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')