tabs converted to two spaces
Chris Beall
parent
fdced7dbcf
commit
e799c9ffa9
10
gtsam.h
10
gtsam.h
|
|
@ -907,7 +907,7 @@ class SymbolicBayesTree {
|
||||||
|
|
||||||
//Constructors
|
//Constructors
|
||||||
SymbolicBayesTree();
|
SymbolicBayesTree();
|
||||||
SymbolicBayesTree(const gtsam::SymbolicBayesTree& other);
|
SymbolicBayesTree(const gtsam::SymbolicBayesTree& other);
|
||||||
|
|
||||||
// Testable
|
// Testable
|
||||||
void print(string s);
|
void print(string s);
|
||||||
|
|
@ -920,10 +920,10 @@ class SymbolicBayesTree {
|
||||||
void clear();
|
void clear();
|
||||||
void deleteCachedShortcuts();
|
void deleteCachedShortcuts();
|
||||||
size_t numCachedSeparatorMarginals() const;
|
size_t numCachedSeparatorMarginals() const;
|
||||||
|
|
||||||
gtsam::SymbolicConditional* marginalFactor(size_t key) const;
|
gtsam::SymbolicConditional* marginalFactor(size_t key) const;
|
||||||
gtsam::SymbolicFactorGraph* joint(size_t key1, size_t key2) const;
|
gtsam::SymbolicFactorGraph* joint(size_t key1, size_t key2) const;
|
||||||
gtsam::SymbolicBayesNet* jointBayesNet(size_t key1, size_t key2) const;
|
gtsam::SymbolicBayesNet* jointBayesNet(size_t key1, size_t key2) const;
|
||||||
};
|
};
|
||||||
|
|
||||||
// class SymbolicBayesTreeClique {
|
// class SymbolicBayesTreeClique {
|
||||||
|
|
|
||||||
|
|
@ -154,7 +154,7 @@ struct LieMatrix : public Matrix, public DerivedValue<LieMatrix> {
|
||||||
/** Logmap around identity - just returns with default cast back */
|
/** Logmap around identity - just returns with default cast back */
|
||||||
static inline Vector Logmap(const LieMatrix& p) {
|
static inline Vector Logmap(const LieMatrix& p) {
|
||||||
return Eigen::Map<const Vector>(&p(0,0), p.dim()); }
|
return Eigen::Map<const Vector>(&p(0,0), p.dim()); }
|
||||||
|
|
||||||
/// @}
|
/// @}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
|
||||||
|
|
@ -26,12 +26,12 @@
|
||||||
/*
|
/*
|
||||||
* NOTES:
|
* NOTES:
|
||||||
* - Earth-rate correction:
|
* - Earth-rate correction:
|
||||||
* + Currently the user should supply R_ECEF_to_G, which is the rotation from ECEF to Local-Level system (NED or ENU as defined by the user).
|
* + Currently the user should supply R_ECEF_to_G, which is the rotation from ECEF to Local-Level system (NED or ENU as defined by the user).
|
||||||
* + R_ECEF_to_G can be calculated by approximated values of latitude and longitude of the system.
|
* + R_ECEF_to_G can be calculated by approximated values of latitude and longitude of the system.
|
||||||
* + A relatively small distance is traveled w.r.t. to initial pose is assumed, since R_ECEF_to_G is constant.
|
* + A relatively small distance is traveled w.r.t. to initial pose is assumed, since R_ECEF_to_G is constant.
|
||||||
* Otherwise, R_ECEF_to_G should be updated each time using the current lat-lon.
|
* Otherwise, R_ECEF_to_G should be updated each time using the current lat-lon.
|
||||||
*
|
*
|
||||||
* - Currently, an empty constructed is not enabled so that the user is forced to specify R_ECEF_to_G.
|
* - Currently, an empty constructed is not enabled so that the user is forced to specify R_ECEF_to_G.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
@ -40,11 +40,11 @@ namespace gtsam {
|
||||||
namespace imuBias {
|
namespace imuBias {
|
||||||
|
|
||||||
class ConstantBias : public DerivedValue<ConstantBias> {
|
class ConstantBias : public DerivedValue<ConstantBias> {
|
||||||
private:
|
private:
|
||||||
Vector3 biasAcc_;
|
Vector3 biasAcc_;
|
||||||
Vector3 biasGyro_;
|
Vector3 biasGyro_;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
/// dimension of the variable - used to autodetect sizes
|
/// dimension of the variable - used to autodetect sizes
|
||||||
static const size_t dimension = 6;
|
static const size_t dimension = 6;
|
||||||
|
|
||||||
|
|
@ -144,17 +144,17 @@ namespace imuBias {
|
||||||
/// return dimensionality of tangent space
|
/// return dimensionality of tangent space
|
||||||
inline size_t dim() const { return dimension; }
|
inline size_t dim() const { return dimension; }
|
||||||
|
|
||||||
/** Update the LieVector with a tangent space update */
|
/** Update the LieVector with a tangent space update */
|
||||||
inline ConstantBias retract(const Vector& v) const { return ConstantBias(biasAcc_ + v.head(3), biasGyro_ + v.tail(3)); }
|
inline ConstantBias retract(const Vector& v) const { return ConstantBias(biasAcc_ + v.head(3), biasGyro_ + v.tail(3)); }
|
||||||
|
|
||||||
/** @return the local coordinates of another object */
|
/** @return the local coordinates of another object */
|
||||||
inline Vector localCoordinates(const ConstantBias& b) const { return b.vector() - vector(); }
|
inline Vector localCoordinates(const ConstantBias& b) const { return b.vector() - vector(); }
|
||||||
|
|
||||||
/// @}
|
/// @}
|
||||||
/// @name Group
|
/// @name Group
|
||||||
/// @{
|
/// @{
|
||||||
|
|
||||||
/** identity for group operation */
|
/** identity for group operation */
|
||||||
static ConstantBias identity() { return ConstantBias(); }
|
static ConstantBias identity() { return ConstantBias(); }
|
||||||
|
|
||||||
/** invert the object and yield a new one */
|
/** invert the object and yield a new one */
|
||||||
|
|
@ -213,7 +213,7 @@ namespace imuBias {
|
||||||
|
|
||||||
/// @}
|
/// @}
|
||||||
|
|
||||||
}; // ConstantBias class
|
}; // ConstantBias class
|
||||||
|
|
||||||
|
|
||||||
} // namespace ImuBias
|
} // namespace ImuBias
|
||||||
|
|
|
||||||
|
|
@ -136,12 +136,12 @@ TEST( CombinedImuFactor, PreintegratedMeasurements )
|
||||||
|
|
||||||
// Actual preintegrated values
|
// Actual preintegrated values
|
||||||
ImuFactor::PreintegratedMeasurements expected1(bias, Matrix3::Zero(),
|
ImuFactor::PreintegratedMeasurements expected1(bias, Matrix3::Zero(),
|
||||||
Matrix3::Zero(), Matrix3::Zero());
|
Matrix3::Zero(), Matrix3::Zero());
|
||||||
expected1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
expected1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||||
|
|
||||||
CombinedImuFactor::CombinedPreintegratedMeasurements actual1(bias,
|
CombinedImuFactor::CombinedPreintegratedMeasurements actual1(bias,
|
||||||
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
|
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
|
||||||
Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6));
|
Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6));
|
||||||
|
|
||||||
// const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases
|
// const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases
|
||||||
// const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc
|
// const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc
|
||||||
|
|
@ -193,13 +193,13 @@ TEST( CombinedImuFactor, ErrorWithBiases )
|
||||||
|
|
||||||
|
|
||||||
ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
|
ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
|
||||||
Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity());
|
Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity());
|
||||||
|
|
||||||
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||||
|
|
||||||
CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data(
|
CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data(
|
||||||
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
|
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
|
||||||
Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6 );
|
Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6 );
|
||||||
|
|
||||||
Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||||
|
|
||||||
|
|
@ -224,14 +224,14 @@ TEST( CombinedImuFactor, ErrorWithBiases )
|
||||||
|
|
||||||
|
|
||||||
// Actual Jacobians
|
// Actual Jacobians
|
||||||
Matrix H1a, H2a, H3a, H4a, H5a, H6a;
|
Matrix H1a, H2a, H3a, H4a, H5a, H6a;
|
||||||
(void) Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a, H3a, H4a, H5a, H6a);
|
(void) Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a, H3a, H4a, H5a, H6a);
|
||||||
|
|
||||||
EXPECT(assert_equal(H1e, H1a.topRows(9)));
|
EXPECT(assert_equal(H1e, H1a.topRows(9)));
|
||||||
EXPECT(assert_equal(H2e, H2a.topRows(9)));
|
EXPECT(assert_equal(H2e, H2a.topRows(9)));
|
||||||
EXPECT(assert_equal(H3e, H3a.topRows(9)));
|
EXPECT(assert_equal(H3e, H3a.topRows(9)));
|
||||||
EXPECT(assert_equal(H4e, H4a.topRows(9)));
|
EXPECT(assert_equal(H4e, H4a.topRows(9)));
|
||||||
EXPECT(assert_equal(H5e, H5a.topRows(9)));
|
EXPECT(assert_equal(H5e, H5a.topRows(9)));
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
|
||||||
|
|
@ -39,5 +39,5 @@ TEST( ImuBias, Constructor)
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
|
||||||
|
|
@ -1,8 +1,8 @@
|
||||||
/**
|
/**
|
||||||
* @file testBetweenFactor.cpp
|
* @file testBetweenFactor.cpp
|
||||||
* @brief
|
* @brief
|
||||||
* @author Duy-Nguyen Ta
|
* @author Duy-Nguyen Ta
|
||||||
* @date Aug 2, 2013
|
* @date Aug 2, 2013
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include <gtsam/base/numericalDerivative.h>
|
#include <gtsam/base/numericalDerivative.h>
|
||||||
|
|
|
||||||
|
|
@ -289,7 +289,7 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() {
|
||||||
}
|
}
|
||||||
gttoc(damp);
|
gttoc(damp);
|
||||||
if (lmVerbosity >= LevenbergMarquardtParams::DAMPED)
|
if (lmVerbosity >= LevenbergMarquardtParams::DAMPED)
|
||||||
dampedFactorGraph.print("damped");
|
dampedFactorGraph.print("damped");
|
||||||
result.lambdas++;
|
result.lambdas++;
|
||||||
|
|
||||||
gttic(solve);
|
gttic(solve);
|
||||||
|
|
@ -302,7 +302,7 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() {
|
||||||
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
|
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
|
||||||
std::cout << "linear delta norm = " << newDelta.norm() << std::endl;
|
std::cout << "linear delta norm = " << newDelta.norm() << std::endl;
|
||||||
if (lmVerbosity >= LevenbergMarquardtParams::TRYDELTA)
|
if (lmVerbosity >= LevenbergMarquardtParams::TRYDELTA)
|
||||||
newDelta.print("delta");
|
newDelta.print("delta");
|
||||||
|
|
||||||
// Evaluate the new error
|
// Evaluate the new error
|
||||||
gttic(compute_error);
|
gttic(compute_error);
|
||||||
|
|
@ -310,7 +310,7 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() {
|
||||||
gttoc(compute_error);
|
gttoc(compute_error);
|
||||||
|
|
||||||
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
|
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
|
||||||
std::cout << "next error = " << error << std::endl;
|
std::cout << "next error = " << error << std::endl;
|
||||||
|
|
||||||
if(error < result.error) {
|
if(error < result.error) {
|
||||||
// Keep this change
|
// Keep this change
|
||||||
|
|
|
||||||
|
|
@ -25,7 +25,7 @@ Matrix Z3 = zeros(3, 3);
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e,
|
AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e,
|
||||||
bool flat) :
|
bool flat) :
|
||||||
KF_(9) {
|
KF_(9) {
|
||||||
|
|
||||||
// Initial state
|
// Initial state
|
||||||
|
|
@ -182,8 +182,8 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::aid(
|
||||||
// F(:,k) = mech.x_a + dx_a - bRn*n_g;
|
// F(:,k) = mech.x_a + dx_a - bRn*n_g;
|
||||||
// F(:,k) = mech.x_a + dx_a - bRn*(I+P)*n_g;
|
// F(:,k) = mech.x_a + dx_a - bRn*(I+P)*n_g;
|
||||||
// F(:,k) = mech.x_a + dx_a - b_g - bRn*(rho x n_g); // P = [rho]_x
|
// F(:,k) = mech.x_a + dx_a - b_g - bRn*(rho x n_g); // P = [rho]_x
|
||||||
// Hence, the measurement z = b_g - (mech.x_a - F(:,k)) is predicted
|
// Hence, the measurement z = b_g - (mech.x_a - F(:,k)) is predicted
|
||||||
// from the filter state (dx_a, rho) as dx_a + bRn*(n_g x rho)
|
// from the filter state (dx_a, rho) as dx_a + bRn*(n_g x rho)
|
||||||
// z = b_g - (mech.x_a - F(:,k)) = dx_a + bRn*(n_g x rho)
|
// z = b_g - (mech.x_a - F(:,k)) = dx_a + bRn*(n_g x rho)
|
||||||
z = bRn * n_g_ - measured_b_g;
|
z = bRn * n_g_ - measured_b_g;
|
||||||
// Now the Jacobian H
|
// Now the Jacobian H
|
||||||
|
|
|
||||||
|
|
@ -118,14 +118,14 @@ namespace gtsam {
|
||||||
delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()),
|
delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()),
|
||||||
delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(Matrix::Zero(15,15))
|
delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(Matrix::Zero(15,15))
|
||||||
{
|
{
|
||||||
// COVARIANCE OF: [Integration AccMeasurement OmegaMeasurement BiasAccRandomWalk BiasOmegaRandomWalk (BiasAccInit BiasOmegaInit)] SIZE (21x21)
|
// 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(),
|
measurementCovariance << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
|
||||||
Matrix3::Zero(), measuredAccCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
|
Matrix3::Zero(), measuredAccCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
|
||||||
Matrix3::Zero(), Matrix3::Zero(), measuredOmegaCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
|
Matrix3::Zero(), Matrix3::Zero(), measuredOmegaCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
|
||||||
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
|
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
|
||||||
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasOmegaCovariance, Matrix3::Zero(), Matrix3::Zero(),
|
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasOmegaCovariance, Matrix3::Zero(), Matrix3::Zero(),
|
||||||
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccOmegaInit.block(0,0,3,3), biasAccOmegaInit.block(0,3,3,3),
|
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccOmegaInit.block(0,0,3,3), biasAccOmegaInit.block(0,3,3,3),
|
||||||
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccOmegaInit.block(3,0,3,3), biasAccOmegaInit.block(3,3,3,3);
|
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccOmegaInit.block(3,0,3,3), biasAccOmegaInit.block(3,3,3,3);
|
||||||
}
|
}
|
||||||
|
|
||||||
CombinedPreintegratedMeasurements() :
|
CombinedPreintegratedMeasurements() :
|
||||||
|
|
@ -231,10 +231,10 @@ namespace gtsam {
|
||||||
// overall Jacobian wrt preintegrated measurements (df/dx)
|
// overall Jacobian wrt preintegrated measurements (df/dx)
|
||||||
Matrix F(15,15);
|
Matrix F(15,15);
|
||||||
F << H_pos_pos, H_pos_vel, H_pos_angles, Z_3x3, Z_3x3,
|
F << H_pos_pos, H_pos_vel, H_pos_angles, Z_3x3, Z_3x3,
|
||||||
H_vel_pos, H_vel_vel, H_vel_angles, H_vel_biasacc, Z_3x3,
|
H_vel_pos, H_vel_vel, H_vel_angles, H_vel_biasacc, Z_3x3,
|
||||||
H_angles_pos, H_angles_vel, H_angles_angles, Z_3x3, H_angles_biasomega,
|
H_angles_pos, H_angles_vel, H_angles_angles, Z_3x3, H_angles_biasomega,
|
||||||
Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3,
|
Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3,
|
||||||
Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3;
|
Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3;
|
||||||
|
|
||||||
|
|
||||||
// first order uncertainty propagation
|
// first order uncertainty propagation
|
||||||
|
|
@ -567,18 +567,18 @@ namespace gtsam {
|
||||||
|
|
||||||
if(H6) {
|
if(H6) {
|
||||||
|
|
||||||
H6->resize(15,6);
|
H6->resize(15,6);
|
||||||
(*H6) <<
|
(*H6) <<
|
||||||
// dfP/dBias_j
|
// dfP/dBias_j
|
||||||
Matrix3::Zero(), Matrix3::Zero(),
|
Matrix3::Zero(), Matrix3::Zero(),
|
||||||
// dfV/dBias_j
|
// dfV/dBias_j
|
||||||
Matrix3::Zero(), Matrix3::Zero(),
|
Matrix3::Zero(), Matrix3::Zero(),
|
||||||
// dfR/dBias_j
|
// dfR/dBias_j
|
||||||
Matrix3::Zero(), Matrix3::Zero(),
|
Matrix3::Zero(), Matrix3::Zero(),
|
||||||
//dBiasAcc/dBias_j
|
//dBiasAcc/dBias_j
|
||||||
Matrix3::Identity(), Matrix3::Zero(),
|
Matrix3::Identity(), Matrix3::Zero(),
|
||||||
//dBiasOmega/dBias_j
|
//dBiasOmega/dBias_j
|
||||||
Matrix3::Zero(), Matrix3::Identity();
|
Matrix3::Zero(), Matrix3::Identity();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -39,8 +39,8 @@ namespace gtsam {
|
||||||
* =====
|
* =====
|
||||||
* Concept: Based on [Lupton12tro]
|
* Concept: Based on [Lupton12tro]
|
||||||
* - Pre-integrate IMU measurements using the static function PreIntegrateIMUObservations.
|
* - Pre-integrate IMU measurements using the static function PreIntegrateIMUObservations.
|
||||||
* Pre-integrated quantities are expressed in the body system of t0 - the first time instant (in which pre-integration began).
|
* Pre-integrated quantities are expressed in the body system of t0 - the first time instant (in which pre-integration began).
|
||||||
* All sensor-to-body transformations are performed here.
|
* All sensor-to-body transformations are performed here.
|
||||||
* - If required, calculate inertial solution by calling the static functions: predictPose_inertial, predictVelocity_inertial.
|
* - If required, calculate inertial solution by calling the static functions: predictPose_inertial, predictVelocity_inertial.
|
||||||
* - When the time is right, incorporate pre-integrated IMU data by creating an EquivInertialNavFactor_GlobalVel factor, which will
|
* - When the time is right, incorporate pre-integrated IMU data by creating an EquivInertialNavFactor_GlobalVel factor, which will
|
||||||
* relate between navigation variables at the two time instances (t0 and current time).
|
* relate between navigation variables at the two time instances (t0 and current time).
|
||||||
|
|
@ -54,11 +54,11 @@ namespace gtsam {
|
||||||
* matrices and the process\modeling covariance matrix. The IneritalNavFactor converts this into a
|
* matrices and the process\modeling covariance matrix. The IneritalNavFactor converts this into a
|
||||||
* discrete form using the supplied delta_t between sub-sequential measurements.
|
* discrete form using the supplied delta_t between sub-sequential measurements.
|
||||||
* - Earth-rate correction:
|
* - Earth-rate correction:
|
||||||
* + Currently the user should supply R_ECEF_to_G, which is the rotation from ECEF to the global
|
* + Currently the user should supply R_ECEF_to_G, which is the rotation from ECEF to the global
|
||||||
* frame (Local-Level system: ENU or NED, see above).
|
* frame (Local-Level system: ENU or NED, see above).
|
||||||
* + R_ECEF_to_G can be calculated by approximated values of latitude and longitude of the system.
|
* + R_ECEF_to_G can be calculated by approximated values of latitude and longitude of the system.
|
||||||
* + Currently it is assumed that a relatively small distance is traveled w.r.t. to initial pose, since R_ECEF_to_G is constant.
|
* + Currently it is assumed that a relatively small distance is traveled w.r.t. to initial pose, since R_ECEF_to_G is constant.
|
||||||
* Otherwise, R_ECEF_to_G should be updated each time using the current lat-lon.
|
* Otherwise, R_ECEF_to_G should be updated each time using the current lat-lon.
|
||||||
*
|
*
|
||||||
* - Frame Notation:
|
* - Frame Notation:
|
||||||
* Quantities are written as {Frame of Representation/Destination Frame}_{Quantity Type}_{Quatity Description/Origination Frame}
|
* Quantities are written as {Frame of Representation/Destination Frame}_{Quantity Type}_{Quatity Description/Origination Frame}
|
||||||
|
|
@ -92,260 +92,260 @@ class EquivInertialNavFactor_GlobalVel : public NoiseModelFactor5<POSE, VELOCITY
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
typedef EquivInertialNavFactor_GlobalVel<POSE, VELOCITY, IMUBIAS> This;
|
typedef EquivInertialNavFactor_GlobalVel<POSE, VELOCITY, IMUBIAS> This;
|
||||||
typedef NoiseModelFactor5<POSE, VELOCITY, IMUBIAS, POSE, VELOCITY> Base;
|
typedef NoiseModelFactor5<POSE, VELOCITY, IMUBIAS, POSE, VELOCITY> Base;
|
||||||
|
|
||||||
Vector delta_pos_in_t0_;
|
Vector delta_pos_in_t0_;
|
||||||
Vector delta_vel_in_t0_;
|
Vector delta_vel_in_t0_;
|
||||||
Vector3 delta_angles_;
|
Vector3 delta_angles_;
|
||||||
double dt12_;
|
double dt12_;
|
||||||
|
|
||||||
Vector world_g_;
|
Vector world_g_;
|
||||||
Vector world_rho_;
|
Vector world_rho_;
|
||||||
Vector world_omega_earth_;
|
Vector world_omega_earth_;
|
||||||
|
|
||||||
Matrix Jacobian_wrt_t0_Overall_;
|
Matrix Jacobian_wrt_t0_Overall_;
|
||||||
|
|
||||||
boost::optional<IMUBIAS> Bias_initial_; // Bias used when pre-integrating IMU measurements
|
boost::optional<IMUBIAS> Bias_initial_; // Bias used when pre-integrating IMU measurements
|
||||||
boost::optional<POSE> body_P_sensor_; // The pose of the sensor in the body frame
|
boost::optional<POSE> body_P_sensor_; // The pose of the sensor in the body frame
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// shorthand for a smart pointer to a factor
|
// shorthand for a smart pointer to a factor
|
||||||
typedef typename boost::shared_ptr<EquivInertialNavFactor_GlobalVel> shared_ptr;
|
typedef typename boost::shared_ptr<EquivInertialNavFactor_GlobalVel> shared_ptr;
|
||||||
|
|
||||||
/** default constructor - only use for serialization */
|
/** default constructor - only use for serialization */
|
||||||
EquivInertialNavFactor_GlobalVel() {}
|
EquivInertialNavFactor_GlobalVel() {}
|
||||||
|
|
||||||
/** Constructor */
|
/** Constructor */
|
||||||
EquivInertialNavFactor_GlobalVel(const Key& Pose1, const Key& Vel1, const Key& IMUBias1, const Key& Pose2, const Key& Vel2,
|
EquivInertialNavFactor_GlobalVel(const Key& Pose1, const Key& Vel1, const Key& IMUBias1, const Key& Pose2, const Key& Vel2,
|
||||||
const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0, const Vector3& delta_angles,
|
const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0, const Vector3& delta_angles,
|
||||||
double dt12, const Vector world_g, const Vector world_rho,
|
double dt12, const Vector world_g, const Vector world_rho,
|
||||||
const Vector& world_omega_earth, const noiseModel::Gaussian::shared_ptr& model_equivalent,
|
const Vector& world_omega_earth, const noiseModel::Gaussian::shared_ptr& model_equivalent,
|
||||||
const Matrix& Jacobian_wrt_t0_Overall,
|
const Matrix& Jacobian_wrt_t0_Overall,
|
||||||
boost::optional<IMUBIAS> Bias_initial = boost::none, boost::optional<POSE> body_P_sensor = boost::none) :
|
boost::optional<IMUBIAS> Bias_initial = boost::none, boost::optional<POSE> body_P_sensor = boost::none) :
|
||||||
Base(model_equivalent, Pose1, Vel1, IMUBias1, Pose2, Vel2),
|
Base(model_equivalent, Pose1, Vel1, IMUBias1, Pose2, Vel2),
|
||||||
delta_pos_in_t0_(delta_pos_in_t0), delta_vel_in_t0_(delta_vel_in_t0), delta_angles_(delta_angles),
|
delta_pos_in_t0_(delta_pos_in_t0), delta_vel_in_t0_(delta_vel_in_t0), delta_angles_(delta_angles),
|
||||||
dt12_(dt12), world_g_(world_g), world_rho_(world_rho), world_omega_earth_(world_omega_earth), Jacobian_wrt_t0_Overall_(Jacobian_wrt_t0_Overall),
|
dt12_(dt12), world_g_(world_g), world_rho_(world_rho), world_omega_earth_(world_omega_earth), Jacobian_wrt_t0_Overall_(Jacobian_wrt_t0_Overall),
|
||||||
Bias_initial_(Bias_initial), body_P_sensor_(body_P_sensor) { }
|
Bias_initial_(Bias_initial), body_P_sensor_(body_P_sensor) { }
|
||||||
|
|
||||||
virtual ~EquivInertialNavFactor_GlobalVel() {}
|
virtual ~EquivInertialNavFactor_GlobalVel() {}
|
||||||
|
|
||||||
/** implement functions needed for Testable */
|
/** implement functions needed for Testable */
|
||||||
|
|
||||||
/** print */
|
/** print */
|
||||||
virtual void print(const std::string& s = "EquivInertialNavFactor_GlobalVel", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
|
virtual void print(const std::string& s = "EquivInertialNavFactor_GlobalVel", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
|
||||||
std::cout << s << "("
|
std::cout << s << "("
|
||||||
<< keyFormatter(this->key1()) << ","
|
<< keyFormatter(this->key1()) << ","
|
||||||
<< keyFormatter(this->key2()) << ","
|
<< keyFormatter(this->key2()) << ","
|
||||||
<< keyFormatter(this->key3()) << ","
|
<< keyFormatter(this->key3()) << ","
|
||||||
<< keyFormatter(this->key4()) << ","
|
<< keyFormatter(this->key4()) << ","
|
||||||
<< keyFormatter(this->key5()) << "\n";
|
<< keyFormatter(this->key5()) << "\n";
|
||||||
std::cout << "delta_pos_in_t0: " << this->delta_pos_in_t0_.transpose() << std::endl;
|
std::cout << "delta_pos_in_t0: " << this->delta_pos_in_t0_.transpose() << std::endl;
|
||||||
std::cout << "delta_vel_in_t0: " << this->delta_vel_in_t0_.transpose() << std::endl;
|
std::cout << "delta_vel_in_t0: " << this->delta_vel_in_t0_.transpose() << std::endl;
|
||||||
std::cout << "delta_angles: " << this->delta_angles_ << std::endl;
|
std::cout << "delta_angles: " << this->delta_angles_ << std::endl;
|
||||||
std::cout << "dt12: " << this->dt12_ << std::endl;
|
std::cout << "dt12: " << this->dt12_ << std::endl;
|
||||||
std::cout << "gravity (in world frame): " << this->world_g_.transpose() << std::endl;
|
std::cout << "gravity (in world frame): " << this->world_g_.transpose() << std::endl;
|
||||||
std::cout << "craft rate (in world frame): " << this->world_rho_.transpose() << std::endl;
|
std::cout << "craft rate (in world frame): " << this->world_rho_.transpose() << std::endl;
|
||||||
std::cout << "earth's rotation (in world frame): " << this->world_omega_earth_.transpose() << std::endl;
|
std::cout << "earth's rotation (in world frame): " << this->world_omega_earth_.transpose() << std::endl;
|
||||||
if(this->body_P_sensor_)
|
if(this->body_P_sensor_)
|
||||||
this->body_P_sensor_->print(" sensor pose in body frame: ");
|
this->body_P_sensor_->print(" sensor pose in body frame: ");
|
||||||
this->noiseModel_->print(" noise model");
|
this->noiseModel_->print(" noise model");
|
||||||
}
|
}
|
||||||
|
|
||||||
/** equals */
|
/** equals */
|
||||||
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
|
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
|
||||||
const This *e = dynamic_cast<const This*> (&expected);
|
const This *e = dynamic_cast<const This*> (&expected);
|
||||||
return e != NULL && Base::equals(*e, tol)
|
return e != NULL && Base::equals(*e, tol)
|
||||||
&& (delta_pos_in_t0_ - e->delta_pos_in_t0_).norm() < tol
|
&& (delta_pos_in_t0_ - e->delta_pos_in_t0_).norm() < tol
|
||||||
&& (delta_vel_in_t0_ - e->delta_vel_in_t0_).norm() < tol
|
&& (delta_vel_in_t0_ - e->delta_vel_in_t0_).norm() < tol
|
||||||
&& (delta_angles_ - e->delta_angles_).norm() < tol
|
&& (delta_angles_ - e->delta_angles_).norm() < tol
|
||||||
&& (dt12_ - e->dt12_) < tol
|
&& (dt12_ - e->dt12_) < tol
|
||||||
&& (world_g_ - e->world_g_).norm() < tol
|
&& (world_g_ - e->world_g_).norm() < tol
|
||||||
&& (world_rho_ - e->world_rho_).norm() < tol
|
&& (world_rho_ - e->world_rho_).norm() < tol
|
||||||
&& (world_omega_earth_ - e->world_omega_earth_).norm() < tol
|
&& (world_omega_earth_ - e->world_omega_earth_).norm() < tol
|
||||||
&& ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
|
&& ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
POSE predictPose(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1) const {
|
POSE predictPose(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1) const {
|
||||||
|
|
||||||
// Correct delta_pos_in_t0_ using (Bias1 - Bias_t0)
|
// Correct delta_pos_in_t0_ using (Bias1 - Bias_t0)
|
||||||
Vector delta_BiasAcc = Bias1.accelerometer();
|
Vector delta_BiasAcc = Bias1.accelerometer();
|
||||||
Vector delta_BiasGyro = Bias1.gyroscope();
|
Vector delta_BiasGyro = Bias1.gyroscope();
|
||||||
if (Bias_initial_){
|
if (Bias_initial_){
|
||||||
delta_BiasAcc -= Bias_initial_->accelerometer();
|
delta_BiasAcc -= Bias_initial_->accelerometer();
|
||||||
delta_BiasGyro -= Bias_initial_->gyroscope();
|
delta_BiasGyro -= Bias_initial_->gyroscope();
|
||||||
}
|
}
|
||||||
|
|
||||||
Matrix J_Pos_wrt_BiasAcc = Jacobian_wrt_t0_Overall_.block(4,9,3,3);
|
Matrix J_Pos_wrt_BiasAcc = Jacobian_wrt_t0_Overall_.block(4,9,3,3);
|
||||||
Matrix J_Pos_wrt_BiasGyro = Jacobian_wrt_t0_Overall_.block(4,12,3,3);
|
Matrix J_Pos_wrt_BiasGyro = Jacobian_wrt_t0_Overall_.block(4,12,3,3);
|
||||||
Matrix J_angles_wrt_BiasGyro = Jacobian_wrt_t0_Overall_.block(0,12,3,3);
|
Matrix J_angles_wrt_BiasGyro = Jacobian_wrt_t0_Overall_.block(0,12,3,3);
|
||||||
|
|
||||||
/* Position term */
|
/* Position term */
|
||||||
Vector delta_pos_in_t0_corrected = delta_pos_in_t0_ + J_Pos_wrt_BiasAcc*delta_BiasAcc + J_Pos_wrt_BiasGyro*delta_BiasGyro;
|
Vector delta_pos_in_t0_corrected = delta_pos_in_t0_ + J_Pos_wrt_BiasAcc*delta_BiasAcc + J_Pos_wrt_BiasGyro*delta_BiasGyro;
|
||||||
|
|
||||||
/* Rotation term */
|
/* Rotation term */
|
||||||
Vector delta_angles_corrected = delta_angles_ + J_angles_wrt_BiasGyro*delta_BiasGyro;
|
Vector delta_angles_corrected = delta_angles_ + J_angles_wrt_BiasGyro*delta_BiasGyro;
|
||||||
// Another alternative:
|
// Another alternative:
|
||||||
// Vector delta_angles_corrected = Rot3::Logmap( Rot3::Expmap(delta_angles_)*Rot3::Expmap(J_angles_wrt_BiasGyro*delta_BiasGyro) );
|
// Vector delta_angles_corrected = Rot3::Logmap( Rot3::Expmap(delta_angles_)*Rot3::Expmap(J_angles_wrt_BiasGyro*delta_BiasGyro) );
|
||||||
|
|
||||||
return predictPose_inertial(Pose1, Vel1,
|
return predictPose_inertial(Pose1, Vel1,
|
||||||
delta_pos_in_t0_corrected, delta_angles_corrected,
|
delta_pos_in_t0_corrected, delta_angles_corrected,
|
||||||
dt12_, world_g_, world_rho_, world_omega_earth_);
|
dt12_, world_g_, world_rho_, world_omega_earth_);
|
||||||
}
|
}
|
||||||
|
|
||||||
static inline POSE predictPose_inertial(const POSE& Pose1, const VELOCITY& Vel1,
|
static inline POSE predictPose_inertial(const POSE& Pose1, const VELOCITY& Vel1,
|
||||||
const Vector& delta_pos_in_t0, const Vector3& delta_angles,
|
const Vector& delta_pos_in_t0, const Vector3& delta_angles,
|
||||||
const double dt12, const Vector& world_g, const Vector& world_rho, const Vector& world_omega_earth){
|
const double dt12, const Vector& world_g, const Vector& world_rho, const Vector& world_omega_earth){
|
||||||
|
|
||||||
const POSE& world_P1_body = Pose1;
|
const POSE& world_P1_body = Pose1;
|
||||||
const VELOCITY& world_V1_body = Vel1;
|
const VELOCITY& world_V1_body = Vel1;
|
||||||
|
|
||||||
/* Position term */
|
/* Position term */
|
||||||
Vector body_deltaPos_body = delta_pos_in_t0;
|
Vector body_deltaPos_body = delta_pos_in_t0;
|
||||||
|
|
||||||
Vector world_deltaPos_pls_body = world_P1_body.rotation().matrix() * body_deltaPos_body;
|
Vector world_deltaPos_pls_body = world_P1_body.rotation().matrix() * body_deltaPos_body;
|
||||||
Vector world_deltaPos_body = world_V1_body * dt12 + 0.5*world_g*dt12*dt12 + world_deltaPos_pls_body;
|
Vector world_deltaPos_body = world_V1_body * dt12 + 0.5*world_g*dt12*dt12 + world_deltaPos_pls_body;
|
||||||
|
|
||||||
// Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2.
|
// Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2.
|
||||||
world_deltaPos_body -= 2*skewSymmetric(world_rho + world_omega_earth)*world_V1_body * dt12*dt12;
|
world_deltaPos_body -= 2*skewSymmetric(world_rho + world_omega_earth)*world_V1_body * dt12*dt12;
|
||||||
|
|
||||||
/* TODO: the term dt12*dt12 in 0.5*world_g*dt12*dt12 is not entirely correct:
|
/* TODO: the term dt12*dt12 in 0.5*world_g*dt12*dt12 is not entirely correct:
|
||||||
* the gravity should be canceled from the accelerometer measurements, bust since position
|
* the gravity should be canceled from the accelerometer measurements, bust since position
|
||||||
* is added with a delta velocity from a previous term, the actual delta time is more complicated.
|
* is added with a delta velocity from a previous term, the actual delta time is more complicated.
|
||||||
* Need to figure out this in the future - currently because of this issue we'll get some more error
|
* Need to figure out this in the future - currently because of this issue we'll get some more error
|
||||||
* in Z axis.
|
* in Z axis.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
/* Rotation term */
|
/* Rotation term */
|
||||||
Vector body_deltaAngles_body = delta_angles;
|
Vector body_deltaAngles_body = delta_angles;
|
||||||
|
|
||||||
// Convert earth-related terms into the body frame
|
// Convert earth-related terms into the body frame
|
||||||
Matrix body_R_world(world_P1_body.rotation().inverse().matrix());
|
Matrix body_R_world(world_P1_body.rotation().inverse().matrix());
|
||||||
Vector body_rho = body_R_world * world_rho;
|
Vector body_rho = body_R_world * world_rho;
|
||||||
Vector body_omega_earth = body_R_world * world_omega_earth;
|
Vector body_omega_earth = body_R_world * world_omega_earth;
|
||||||
|
|
||||||
// Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2.
|
// Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2.
|
||||||
body_deltaAngles_body -= (body_rho + body_omega_earth)*dt12;
|
body_deltaAngles_body -= (body_rho + body_omega_earth)*dt12;
|
||||||
|
|
||||||
return POSE(Pose1.rotation() * POSE::Rotation::Expmap(body_deltaAngles_body), Pose1.translation() + typename POSE::Translation(world_deltaPos_body));
|
return POSE(Pose1.rotation() * POSE::Rotation::Expmap(body_deltaAngles_body), Pose1.translation() + typename POSE::Translation(world_deltaPos_body));
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
VELOCITY predictVelocity(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1) const {
|
VELOCITY predictVelocity(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1) const {
|
||||||
|
|
||||||
// Correct delta_vel_in_t0_ using (Bias1 - Bias_t0)
|
// Correct delta_vel_in_t0_ using (Bias1 - Bias_t0)
|
||||||
Vector delta_BiasAcc = Bias1.accelerometer();
|
Vector delta_BiasAcc = Bias1.accelerometer();
|
||||||
Vector delta_BiasGyro = Bias1.gyroscope();
|
Vector delta_BiasGyro = Bias1.gyroscope();
|
||||||
if (Bias_initial_){
|
if (Bias_initial_){
|
||||||
delta_BiasAcc -= Bias_initial_->accelerometer();
|
delta_BiasAcc -= Bias_initial_->accelerometer();
|
||||||
delta_BiasGyro -= Bias_initial_->gyroscope();
|
delta_BiasGyro -= Bias_initial_->gyroscope();
|
||||||
}
|
}
|
||||||
|
|
||||||
Matrix J_Vel_wrt_BiasAcc = Jacobian_wrt_t0_Overall_.block(6,9,3,3);
|
Matrix J_Vel_wrt_BiasAcc = Jacobian_wrt_t0_Overall_.block(6,9,3,3);
|
||||||
Matrix J_Vel_wrt_BiasGyro = Jacobian_wrt_t0_Overall_.block(6,12,3,3);
|
Matrix J_Vel_wrt_BiasGyro = Jacobian_wrt_t0_Overall_.block(6,12,3,3);
|
||||||
|
|
||||||
Vector delta_vel_in_t0_corrected = delta_vel_in_t0_ + J_Vel_wrt_BiasAcc*delta_BiasAcc + J_Vel_wrt_BiasGyro*delta_BiasGyro;
|
Vector delta_vel_in_t0_corrected = delta_vel_in_t0_ + J_Vel_wrt_BiasAcc*delta_BiasAcc + J_Vel_wrt_BiasGyro*delta_BiasGyro;
|
||||||
|
|
||||||
return predictVelocity_inertial(Pose1, Vel1,
|
return predictVelocity_inertial(Pose1, Vel1,
|
||||||
delta_vel_in_t0_corrected,
|
delta_vel_in_t0_corrected,
|
||||||
dt12_, world_g_, world_rho_, world_omega_earth_);
|
dt12_, world_g_, world_rho_, world_omega_earth_);
|
||||||
}
|
}
|
||||||
|
|
||||||
static inline VELOCITY predictVelocity_inertial(const POSE& Pose1, const VELOCITY& Vel1,
|
static inline VELOCITY predictVelocity_inertial(const POSE& Pose1, const VELOCITY& Vel1,
|
||||||
const Vector& delta_vel_in_t0,
|
const Vector& delta_vel_in_t0,
|
||||||
const double dt12, const Vector& world_g, const Vector& world_rho, const Vector& world_omega_earth) {
|
const double dt12, const Vector& world_g, const Vector& world_rho, const Vector& world_omega_earth) {
|
||||||
|
|
||||||
const POSE& world_P1_body = Pose1;
|
const POSE& world_P1_body = Pose1;
|
||||||
const VELOCITY& world_V1_body = Vel1;
|
const VELOCITY& world_V1_body = Vel1;
|
||||||
|
|
||||||
Vector body_deltaVel_body = delta_vel_in_t0;
|
Vector body_deltaVel_body = delta_vel_in_t0;
|
||||||
Vector world_deltaVel_body = world_P1_body.rotation().matrix() * body_deltaVel_body;
|
Vector world_deltaVel_body = world_P1_body.rotation().matrix() * body_deltaVel_body;
|
||||||
|
|
||||||
VELOCITY VelDelta( world_deltaVel_body + world_g * dt12 );
|
VELOCITY VelDelta( world_deltaVel_body + world_g * dt12 );
|
||||||
|
|
||||||
// Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2.
|
// Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2.
|
||||||
VelDelta -= 2*skewSymmetric(world_rho + world_omega_earth)*world_V1_body * dt12;
|
VelDelta -= 2*skewSymmetric(world_rho + world_omega_earth)*world_V1_body * dt12;
|
||||||
|
|
||||||
// Predict
|
// Predict
|
||||||
return Vel1.compose( VelDelta );
|
return Vel1.compose( VelDelta );
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void predict(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, POSE& Pose2, VELOCITY& Vel2) const {
|
void predict(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, POSE& Pose2, VELOCITY& Vel2) const {
|
||||||
Pose2 = predictPose(Pose1, Vel1, Bias1);
|
Pose2 = predictPose(Pose1, Vel1, Bias1);
|
||||||
Vel2 = predictVelocity(Pose1, Vel1, Bias1);
|
Vel2 = predictVelocity(Pose1, Vel1, Bias1);
|
||||||
}
|
}
|
||||||
|
|
||||||
POSE evaluatePoseError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2) const {
|
POSE evaluatePoseError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2) const {
|
||||||
// Predict
|
// Predict
|
||||||
POSE Pose2Pred = predictPose(Pose1, Vel1, Bias1);
|
POSE Pose2Pred = predictPose(Pose1, Vel1, Bias1);
|
||||||
|
|
||||||
// Luca: difference between Pose2 and Pose2Pred
|
// Luca: difference between Pose2 and Pose2Pred
|
||||||
POSE DiffPose( Pose2.rotation().between(Pose2Pred.rotation()), Pose2Pred.translation() - Pose2.translation() );
|
POSE DiffPose( Pose2.rotation().between(Pose2Pred.rotation()), Pose2Pred.translation() - Pose2.translation() );
|
||||||
// DiffPose = Pose2.between(Pose2Pred);
|
// DiffPose = Pose2.between(Pose2Pred);
|
||||||
return DiffPose;
|
return DiffPose;
|
||||||
// Calculate error
|
// Calculate error
|
||||||
//return Pose2.between(Pose2Pred);
|
//return Pose2.between(Pose2Pred);
|
||||||
}
|
}
|
||||||
|
|
||||||
VELOCITY evaluateVelocityError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2) const {
|
VELOCITY evaluateVelocityError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2) const {
|
||||||
// Predict
|
// Predict
|
||||||
VELOCITY Vel2Pred = predictVelocity(Pose1, Vel1, Bias1);
|
VELOCITY Vel2Pred = predictVelocity(Pose1, Vel1, Bias1);
|
||||||
|
|
||||||
// Calculate error
|
// Calculate error
|
||||||
return Vel2.between(Vel2Pred);
|
return Vel2.between(Vel2Pred);
|
||||||
}
|
}
|
||||||
|
|
||||||
Vector evaluateError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2,
|
Vector evaluateError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2,
|
||||||
boost::optional<Matrix&> H1 = boost::none,
|
boost::optional<Matrix&> H1 = boost::none,
|
||||||
boost::optional<Matrix&> H2 = boost::none,
|
boost::optional<Matrix&> H2 = boost::none,
|
||||||
boost::optional<Matrix&> H3 = boost::none,
|
boost::optional<Matrix&> H3 = boost::none,
|
||||||
boost::optional<Matrix&> H4 = boost::none,
|
boost::optional<Matrix&> H4 = boost::none,
|
||||||
boost::optional<Matrix&> H5 = boost::none) const {
|
boost::optional<Matrix&> H5 = boost::none) const {
|
||||||
|
|
||||||
// TODO: Write analytical derivative calculations
|
// TODO: Write analytical derivative calculations
|
||||||
// Jacobian w.r.t. Pose1
|
// Jacobian w.r.t. Pose1
|
||||||
if (H1){
|
if (H1){
|
||||||
Matrix H1_Pose = numericalDerivative11<POSE, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, _1, Vel1, Bias1, Pose2, Vel2), Pose1);
|
Matrix H1_Pose = numericalDerivative11<POSE, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, _1, Vel1, Bias1, Pose2, Vel2), Pose1);
|
||||||
Matrix H1_Vel = numericalDerivative11<VELOCITY, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, _1, Vel1, Bias1, Pose2, Vel2), Pose1);
|
Matrix H1_Vel = numericalDerivative11<VELOCITY, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, _1, Vel1, Bias1, Pose2, Vel2), Pose1);
|
||||||
*H1 = stack(2, &H1_Pose, &H1_Vel);
|
*H1 = stack(2, &H1_Pose, &H1_Vel);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Jacobian w.r.t. Vel1
|
// Jacobian w.r.t. Vel1
|
||||||
if (H2){
|
if (H2){
|
||||||
Matrix H2_Pose = numericalDerivative11<POSE, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1);
|
Matrix H2_Pose = numericalDerivative11<POSE, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1);
|
||||||
Matrix H2_Vel = numericalDerivative11<VELOCITY, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1);
|
Matrix H2_Vel = numericalDerivative11<VELOCITY, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1);
|
||||||
*H2 = stack(2, &H2_Pose, &H2_Vel);
|
*H2 = stack(2, &H2_Pose, &H2_Vel);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Jacobian w.r.t. IMUBias1
|
// Jacobian w.r.t. IMUBias1
|
||||||
if (H3){
|
if (H3){
|
||||||
Matrix H3_Pose = numericalDerivative11<POSE, IMUBIAS>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, _1, Pose2, Vel2), Bias1);
|
Matrix H3_Pose = numericalDerivative11<POSE, IMUBIAS>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, _1, Pose2, Vel2), Bias1);
|
||||||
Matrix H3_Vel = numericalDerivative11<VELOCITY, IMUBIAS>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, _1, Pose2, Vel2), Bias1);
|
Matrix H3_Vel = numericalDerivative11<VELOCITY, IMUBIAS>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, _1, Pose2, Vel2), Bias1);
|
||||||
*H3 = stack(2, &H3_Pose, &H3_Vel);
|
*H3 = stack(2, &H3_Pose, &H3_Vel);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Jacobian w.r.t. Pose2
|
// Jacobian w.r.t. Pose2
|
||||||
if (H4){
|
if (H4){
|
||||||
Matrix H4_Pose = numericalDerivative11<POSE, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, Bias1, _1, Vel2), Pose2);
|
Matrix H4_Pose = numericalDerivative11<POSE, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, Bias1, _1, Vel2), Pose2);
|
||||||
Matrix H4_Vel = numericalDerivative11<VELOCITY, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, Bias1, _1, Vel2), Pose2);
|
Matrix H4_Vel = numericalDerivative11<VELOCITY, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, Bias1, _1, Vel2), Pose2);
|
||||||
*H4 = stack(2, &H4_Pose, &H4_Vel);
|
*H4 = stack(2, &H4_Pose, &H4_Vel);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Jacobian w.r.t. Vel2
|
// Jacobian w.r.t. Vel2
|
||||||
if (H5){
|
if (H5){
|
||||||
Matrix H5_Pose = numericalDerivative11<POSE, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2);
|
Matrix H5_Pose = numericalDerivative11<POSE, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2);
|
||||||
Matrix H5_Vel = numericalDerivative11<VELOCITY, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2);
|
Matrix H5_Vel = numericalDerivative11<VELOCITY, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2);
|
||||||
*H5 = stack(2, &H5_Pose, &H5_Vel);
|
*H5 = stack(2, &H5_Pose, &H5_Vel);
|
||||||
}
|
}
|
||||||
|
|
||||||
Vector ErrPoseVector(POSE::Logmap(evaluatePoseError(Pose1, Vel1, Bias1, Pose2, Vel2)));
|
Vector ErrPoseVector(POSE::Logmap(evaluatePoseError(Pose1, Vel1, Bias1, Pose2, Vel2)));
|
||||||
Vector ErrVelVector(VELOCITY::Logmap(evaluateVelocityError(Pose1, Vel1, Bias1, Pose2, Vel2)));
|
Vector ErrVelVector(VELOCITY::Logmap(evaluateVelocityError(Pose1, Vel1, Bias1, Pose2, Vel2)));
|
||||||
|
|
||||||
return concatVectors(2, &ErrPoseVector, &ErrVelVector);
|
return concatVectors(2, &ErrPoseVector, &ErrVelVector);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -413,137 +413,137 @@ public:
|
||||||
|
|
||||||
static inline void PreIntegrateIMUObservations(const Vector& msr_acc_t, const Vector& msr_gyro_t, const double msr_dt,
|
static inline void PreIntegrateIMUObservations(const Vector& msr_acc_t, const Vector& msr_gyro_t, const double msr_dt,
|
||||||
Vector& delta_pos_in_t0, Vector3& delta_angles, Vector& delta_vel_in_t0, double& delta_t,
|
Vector& delta_pos_in_t0, Vector3& delta_angles, Vector& delta_vel_in_t0, double& delta_t,
|
||||||
const noiseModel::Gaussian::shared_ptr& model_continuous_overall,
|
const noiseModel::Gaussian::shared_ptr& model_continuous_overall,
|
||||||
Matrix& EquivCov_Overall, Matrix& Jacobian_wrt_t0_Overall, const IMUBIAS Bias_t0 = IMUBIAS(),
|
Matrix& EquivCov_Overall, Matrix& Jacobian_wrt_t0_Overall, const IMUBIAS Bias_t0 = IMUBIAS(),
|
||||||
boost::optional<POSE> p_body_P_sensor = boost::none){
|
boost::optional<POSE> p_body_P_sensor = boost::none){
|
||||||
// Note: all delta terms refer to an IMU\sensor system at t0
|
// Note: all delta terms refer to an IMU\sensor system at t0
|
||||||
// Note: Earth-related terms are not accounted here but are incorporated in predict functions.
|
// Note: Earth-related terms are not accounted here but are incorporated in predict functions.
|
||||||
|
|
||||||
POSE body_P_sensor = POSE();
|
POSE body_P_sensor = POSE();
|
||||||
bool flag_use_body_P_sensor = false;
|
bool flag_use_body_P_sensor = false;
|
||||||
if (p_body_P_sensor){
|
if (p_body_P_sensor){
|
||||||
body_P_sensor = *p_body_P_sensor;
|
body_P_sensor = *p_body_P_sensor;
|
||||||
flag_use_body_P_sensor = true;
|
flag_use_body_P_sensor = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
delta_pos_in_t0 = PreIntegrateIMUObservations_delta_pos(msr_dt, delta_pos_in_t0, delta_vel_in_t0);
|
delta_pos_in_t0 = PreIntegrateIMUObservations_delta_pos(msr_dt, delta_pos_in_t0, delta_vel_in_t0);
|
||||||
delta_vel_in_t0 = PreIntegrateIMUObservations_delta_vel(msr_gyro_t, msr_acc_t, msr_dt, delta_angles, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, Bias_t0);
|
delta_vel_in_t0 = PreIntegrateIMUObservations_delta_vel(msr_gyro_t, msr_acc_t, msr_dt, delta_angles, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, Bias_t0);
|
||||||
delta_angles = PreIntegrateIMUObservations_delta_angles(msr_gyro_t, msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor, Bias_t0);
|
delta_angles = PreIntegrateIMUObservations_delta_angles(msr_gyro_t, msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor, Bias_t0);
|
||||||
|
|
||||||
delta_t += msr_dt;
|
delta_t += msr_dt;
|
||||||
|
|
||||||
// Update EquivCov_Overall
|
// Update EquivCov_Overall
|
||||||
Matrix Z_3x3 = zeros(3,3);
|
Matrix Z_3x3 = zeros(3,3);
|
||||||
Matrix I_3x3 = eye(3,3);
|
Matrix I_3x3 = eye(3,3);
|
||||||
|
|
||||||
Matrix H_pos_pos = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0);
|
Matrix H_pos_pos = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0);
|
||||||
Matrix H_pos_vel = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0);
|
Matrix H_pos_vel = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0);
|
||||||
Matrix H_pos_angles = Z_3x3;
|
Matrix H_pos_angles = Z_3x3;
|
||||||
Matrix H_pos_bias = collect(2, &Z_3x3, &Z_3x3);
|
Matrix H_pos_bias = collect(2, &Z_3x3, &Z_3x3);
|
||||||
|
|
||||||
Matrix H_vel_vel = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, _1, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_vel_in_t0);
|
Matrix H_vel_vel = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, _1, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_vel_in_t0);
|
||||||
Matrix H_vel_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, _1, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_angles);
|
Matrix H_vel_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, _1, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_angles);
|
||||||
Matrix H_vel_bias = numericalDerivative11<LieVector, IMUBIAS>(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, _1), Bias_t0);
|
Matrix H_vel_bias = numericalDerivative11<LieVector, IMUBIAS>(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, _1), Bias_t0);
|
||||||
Matrix H_vel_pos = Z_3x3;
|
Matrix H_vel_pos = Z_3x3;
|
||||||
|
|
||||||
Matrix H_angles_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, _1, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_angles);
|
Matrix H_angles_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, _1, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_angles);
|
||||||
Matrix H_angles_bias = numericalDerivative11<LieVector, IMUBIAS>(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor, _1), Bias_t0);
|
Matrix H_angles_bias = numericalDerivative11<LieVector, IMUBIAS>(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor, _1), Bias_t0);
|
||||||
Matrix H_angles_pos = Z_3x3;
|
Matrix H_angles_pos = Z_3x3;
|
||||||
Matrix H_angles_vel = Z_3x3;
|
Matrix H_angles_vel = Z_3x3;
|
||||||
|
|
||||||
Matrix F_angles = collect(4, &H_angles_angles, &H_angles_pos, &H_angles_vel, &H_angles_bias);
|
Matrix F_angles = collect(4, &H_angles_angles, &H_angles_pos, &H_angles_vel, &H_angles_bias);
|
||||||
Matrix F_pos = collect(4, &H_pos_angles, &H_pos_pos, &H_pos_vel, &H_pos_bias);
|
Matrix F_pos = collect(4, &H_pos_angles, &H_pos_pos, &H_pos_vel, &H_pos_bias);
|
||||||
Matrix F_vel = collect(4, &H_vel_angles, &H_vel_pos, &H_vel_vel, &H_vel_bias);
|
Matrix F_vel = collect(4, &H_vel_angles, &H_vel_pos, &H_vel_vel, &H_vel_bias);
|
||||||
Matrix F_bias_a = collect(5, &Z_3x3, &Z_3x3, &Z_3x3, &I_3x3, &Z_3x3);
|
Matrix F_bias_a = collect(5, &Z_3x3, &Z_3x3, &Z_3x3, &I_3x3, &Z_3x3);
|
||||||
Matrix F_bias_g = collect(5, &Z_3x3, &Z_3x3, &Z_3x3, &Z_3x3, &I_3x3);
|
Matrix F_bias_g = collect(5, &Z_3x3, &Z_3x3, &Z_3x3, &Z_3x3, &I_3x3);
|
||||||
Matrix F = stack(5, &F_angles, &F_pos, &F_vel, &F_bias_a, &F_bias_g);
|
Matrix F = stack(5, &F_angles, &F_pos, &F_vel, &F_bias_a, &F_bias_g);
|
||||||
|
|
||||||
|
|
||||||
noiseModel::Gaussian::shared_ptr model_discrete_curr = calc_descrete_noise_model(model_continuous_overall, msr_dt );
|
noiseModel::Gaussian::shared_ptr model_discrete_curr = calc_descrete_noise_model(model_continuous_overall, msr_dt );
|
||||||
Matrix Q_d = inverse(model_discrete_curr->R().transpose() * model_discrete_curr->R() );
|
Matrix Q_d = inverse(model_discrete_curr->R().transpose() * model_discrete_curr->R() );
|
||||||
|
|
||||||
EquivCov_Overall = F * EquivCov_Overall * F.transpose() + Q_d;
|
EquivCov_Overall = F * EquivCov_Overall * F.transpose() + Q_d;
|
||||||
// Luca: force identity covariance matrix (for testing purposes)
|
// Luca: force identity covariance matrix (for testing purposes)
|
||||||
// EquivCov_Overall = Matrix::Identity(15,15);
|
// EquivCov_Overall = Matrix::Identity(15,15);
|
||||||
|
|
||||||
// Update Jacobian_wrt_t0_Overall
|
// Update Jacobian_wrt_t0_Overall
|
||||||
Jacobian_wrt_t0_Overall = F * Jacobian_wrt_t0_Overall;
|
Jacobian_wrt_t0_Overall = F * Jacobian_wrt_t0_Overall;
|
||||||
}
|
}
|
||||||
|
|
||||||
static inline Vector PreIntegrateIMUObservations_delta_pos(const double msr_dt,
|
static inline Vector PreIntegrateIMUObservations_delta_pos(const double msr_dt,
|
||||||
const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0){
|
const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0){
|
||||||
|
|
||||||
// Note: all delta terms refer to an IMU\sensor system at t0
|
// Note: all delta terms refer to an IMU\sensor system at t0
|
||||||
// Note: delta_vel_in_t0 is already in body frame, so no need to use the body_P_sensor transformation here.
|
// Note: delta_vel_in_t0 is already in body frame, so no need to use the body_P_sensor transformation here.
|
||||||
|
|
||||||
return delta_pos_in_t0 + delta_vel_in_t0 * msr_dt;
|
return delta_pos_in_t0 + delta_vel_in_t0 * msr_dt;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt,
|
static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt,
|
||||||
const Vector3& delta_angles, const Vector& delta_vel_in_t0, const bool flag_use_body_P_sensor, const POSE& body_P_sensor,
|
const Vector3& delta_angles, const Vector& delta_vel_in_t0, const bool flag_use_body_P_sensor, const POSE& body_P_sensor,
|
||||||
IMUBIAS Bias_t0 = IMUBIAS()){
|
IMUBIAS Bias_t0 = IMUBIAS()){
|
||||||
|
|
||||||
// Note: all delta terms refer to an IMU\sensor system at t0
|
// Note: all delta terms refer to an IMU\sensor system at t0
|
||||||
|
|
||||||
// Calculate the corrected measurements using the Bias object
|
// Calculate the corrected measurements using the Bias object
|
||||||
Vector AccCorrected = Bias_t0.correctAccelerometer(msr_acc_t);
|
Vector AccCorrected = Bias_t0.correctAccelerometer(msr_acc_t);
|
||||||
Vector body_t_a_body;
|
Vector body_t_a_body;
|
||||||
if (flag_use_body_P_sensor){
|
if (flag_use_body_P_sensor){
|
||||||
Matrix body_R_sensor = body_P_sensor.rotation().matrix();
|
Matrix body_R_sensor = body_P_sensor.rotation().matrix();
|
||||||
|
|
||||||
Vector GyroCorrected(Bias_t0.correctGyroscope(msr_gyro_t));
|
Vector GyroCorrected(Bias_t0.correctGyroscope(msr_gyro_t));
|
||||||
|
|
||||||
Vector body_omega_body = body_R_sensor * GyroCorrected;
|
Vector body_omega_body = body_R_sensor * GyroCorrected;
|
||||||
Matrix body_omega_body__cross = skewSymmetric(body_omega_body);
|
Matrix body_omega_body__cross = skewSymmetric(body_omega_body);
|
||||||
|
|
||||||
body_t_a_body = body_R_sensor * AccCorrected - body_omega_body__cross * body_omega_body__cross * body_P_sensor.translation().vector();
|
body_t_a_body = body_R_sensor * AccCorrected - body_omega_body__cross * body_omega_body__cross * body_P_sensor.translation().vector();
|
||||||
} else{
|
} else{
|
||||||
body_t_a_body = AccCorrected;
|
body_t_a_body = AccCorrected;
|
||||||
}
|
}
|
||||||
|
|
||||||
Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
|
Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
|
||||||
|
|
||||||
return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt;
|
return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt,
|
static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt,
|
||||||
const Vector3& delta_angles, const bool flag_use_body_P_sensor, const POSE& body_P_sensor,
|
const Vector3& delta_angles, const bool flag_use_body_P_sensor, const POSE& body_P_sensor,
|
||||||
IMUBIAS Bias_t0 = IMUBIAS()){
|
IMUBIAS Bias_t0 = IMUBIAS()){
|
||||||
|
|
||||||
// Note: all delta terms refer to an IMU\sensor system at t0
|
// Note: all delta terms refer to an IMU\sensor system at t0
|
||||||
|
|
||||||
// Calculate the corrected measurements using the Bias object
|
// Calculate the corrected measurements using the Bias object
|
||||||
Vector GyroCorrected = Bias_t0.correctGyroscope(msr_gyro_t);
|
Vector GyroCorrected = Bias_t0.correctGyroscope(msr_gyro_t);
|
||||||
|
|
||||||
Vector body_t_omega_body;
|
Vector body_t_omega_body;
|
||||||
if (flag_use_body_P_sensor){
|
if (flag_use_body_P_sensor){
|
||||||
body_t_omega_body = body_P_sensor.rotation().matrix() * GyroCorrected;
|
body_t_omega_body = body_P_sensor.rotation().matrix() * GyroCorrected;
|
||||||
} else {
|
} else {
|
||||||
body_t_omega_body = GyroCorrected;
|
body_t_omega_body = GyroCorrected;
|
||||||
}
|
}
|
||||||
|
|
||||||
Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
|
Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
|
||||||
|
|
||||||
R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt );
|
R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt );
|
||||||
return Rot3::Logmap(R_t_to_t0);
|
return Rot3::Logmap(R_t_to_t0);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
static inline noiseModel::Gaussian::shared_ptr CalcEquivalentNoiseCov(const noiseModel::Gaussian::shared_ptr& gaussian_acc, const noiseModel::Gaussian::shared_ptr& gaussian_gyro,
|
static inline noiseModel::Gaussian::shared_ptr CalcEquivalentNoiseCov(const noiseModel::Gaussian::shared_ptr& gaussian_acc, const noiseModel::Gaussian::shared_ptr& gaussian_gyro,
|
||||||
const noiseModel::Gaussian::shared_ptr& gaussian_process){
|
const noiseModel::Gaussian::shared_ptr& gaussian_process){
|
||||||
|
|
||||||
Matrix cov_acc = inverse( gaussian_acc->R().transpose() * gaussian_acc->R() );
|
Matrix cov_acc = inverse( gaussian_acc->R().transpose() * gaussian_acc->R() );
|
||||||
Matrix cov_gyro = inverse( gaussian_gyro->R().transpose() * gaussian_gyro->R() );
|
Matrix cov_gyro = inverse( gaussian_gyro->R().transpose() * gaussian_gyro->R() );
|
||||||
Matrix cov_process = inverse( gaussian_process->R().transpose() * gaussian_process->R() );
|
Matrix cov_process = inverse( gaussian_process->R().transpose() * gaussian_process->R() );
|
||||||
|
|
||||||
cov_process.block(0,0, 3,3) += cov_gyro;
|
cov_process.block(0,0, 3,3) += cov_gyro;
|
||||||
cov_process.block(6,6, 3,3) += cov_acc;
|
cov_process.block(6,6, 3,3) += cov_acc;
|
||||||
|
|
||||||
return noiseModel::Gaussian::Covariance(cov_process);
|
return noiseModel::Gaussian::Covariance(cov_process);
|
||||||
}
|
}
|
||||||
|
|
||||||
static inline void CalcEquivalentNoiseCov_DifferentParts(const noiseModel::Gaussian::shared_ptr& gaussian_acc, const noiseModel::Gaussian::shared_ptr& gaussian_gyro,
|
static inline void CalcEquivalentNoiseCov_DifferentParts(const noiseModel::Gaussian::shared_ptr& gaussian_acc, const noiseModel::Gaussian::shared_ptr& gaussian_gyro,
|
||||||
const noiseModel::Gaussian::shared_ptr& gaussian_process,
|
const noiseModel::Gaussian::shared_ptr& gaussian_process,
|
||||||
|
|
@ -554,107 +554,107 @@ public:
|
||||||
cov_process_without_acc_gyro = inverse( gaussian_process->R().transpose() * gaussian_process->R() );
|
cov_process_without_acc_gyro = inverse( gaussian_process->R().transpose() * gaussian_process->R() );
|
||||||
}
|
}
|
||||||
|
|
||||||
static inline void Calc_g_rho_omega_earth_NED(const Vector& Pos_NED, const Vector& Vel_NED, const Vector& LatLonHeight_IC, const Vector& Pos_NED_Initial,
|
static inline void Calc_g_rho_omega_earth_NED(const Vector& Pos_NED, const Vector& Vel_NED, const Vector& LatLonHeight_IC, const Vector& Pos_NED_Initial,
|
||||||
Vector& g_NED, Vector& rho_NED, Vector& omega_earth_NED) {
|
Vector& g_NED, Vector& rho_NED, Vector& omega_earth_NED) {
|
||||||
|
|
||||||
Matrix ENU_to_NED = Matrix_(3, 3,
|
Matrix ENU_to_NED = Matrix_(3, 3,
|
||||||
0.0, 1.0, 0.0,
|
0.0, 1.0, 0.0,
|
||||||
1.0, 0.0, 0.0,
|
1.0, 0.0, 0.0,
|
||||||
0.0, 0.0, -1.0);
|
0.0, 0.0, -1.0);
|
||||||
|
|
||||||
Matrix NED_to_ENU = Matrix_(3, 3,
|
Matrix NED_to_ENU = Matrix_(3, 3,
|
||||||
0.0, 1.0, 0.0,
|
0.0, 1.0, 0.0,
|
||||||
1.0, 0.0, 0.0,
|
1.0, 0.0, 0.0,
|
||||||
0.0, 0.0, -1.0);
|
0.0, 0.0, -1.0);
|
||||||
|
|
||||||
// Convert incoming parameters to ENU
|
// Convert incoming parameters to ENU
|
||||||
Vector Pos_ENU = NED_to_ENU * Pos_NED;
|
Vector Pos_ENU = NED_to_ENU * Pos_NED;
|
||||||
Vector Vel_ENU = NED_to_ENU * Vel_NED;
|
Vector Vel_ENU = NED_to_ENU * Vel_NED;
|
||||||
Vector Pos_ENU_Initial = NED_to_ENU * Pos_NED_Initial;
|
Vector Pos_ENU_Initial = NED_to_ENU * Pos_NED_Initial;
|
||||||
|
|
||||||
// Call ENU version
|
// Call ENU version
|
||||||
Vector g_ENU;
|
Vector g_ENU;
|
||||||
Vector rho_ENU;
|
Vector rho_ENU;
|
||||||
Vector omega_earth_ENU;
|
Vector omega_earth_ENU;
|
||||||
Calc_g_rho_omega_earth_ENU(Pos_ENU, Vel_ENU, LatLonHeight_IC, Pos_ENU_Initial, g_ENU, rho_ENU, omega_earth_ENU);
|
Calc_g_rho_omega_earth_ENU(Pos_ENU, Vel_ENU, LatLonHeight_IC, Pos_ENU_Initial, g_ENU, rho_ENU, omega_earth_ENU);
|
||||||
|
|
||||||
// Convert output to NED
|
// Convert output to NED
|
||||||
g_NED = ENU_to_NED * g_ENU;
|
g_NED = ENU_to_NED * g_ENU;
|
||||||
rho_NED = ENU_to_NED * rho_ENU;
|
rho_NED = ENU_to_NED * rho_ENU;
|
||||||
omega_earth_NED = ENU_to_NED * omega_earth_ENU;
|
omega_earth_NED = ENU_to_NED * omega_earth_ENU;
|
||||||
}
|
}
|
||||||
|
|
||||||
static inline void Calc_g_rho_omega_earth_ENU(const Vector& Pos_ENU, const Vector& Vel_ENU, const Vector& LatLonHeight_IC, const Vector& Pos_ENU_Initial,
|
static inline void Calc_g_rho_omega_earth_ENU(const Vector& Pos_ENU, const Vector& Vel_ENU, const Vector& LatLonHeight_IC, const Vector& Pos_ENU_Initial,
|
||||||
Vector& g_ENU, Vector& rho_ENU, Vector& omega_earth_ENU){
|
Vector& g_ENU, Vector& rho_ENU, Vector& omega_earth_ENU){
|
||||||
double R0 = 6.378388e6;
|
double R0 = 6.378388e6;
|
||||||
double e = 1/297;
|
double e = 1/297;
|
||||||
double Re( R0*( 1-e*(sin( LatLonHeight_IC(0) ))*(sin( LatLonHeight_IC(0) )) ) );
|
double Re( R0*( 1-e*(sin( LatLonHeight_IC(0) ))*(sin( LatLonHeight_IC(0) )) ) );
|
||||||
|
|
||||||
// Calculate current lat, lon
|
// Calculate current lat, lon
|
||||||
Vector delta_Pos_ENU(Pos_ENU - Pos_ENU_Initial);
|
Vector delta_Pos_ENU(Pos_ENU - Pos_ENU_Initial);
|
||||||
double delta_lat(delta_Pos_ENU(1)/Re);
|
double delta_lat(delta_Pos_ENU(1)/Re);
|
||||||
double delta_lon(delta_Pos_ENU(0)/(Re*cos(LatLonHeight_IC(0))));
|
double delta_lon(delta_Pos_ENU(0)/(Re*cos(LatLonHeight_IC(0))));
|
||||||
double lat_new(LatLonHeight_IC(0) + delta_lat);
|
double lat_new(LatLonHeight_IC(0) + delta_lat);
|
||||||
double lon_new(LatLonHeight_IC(1) + delta_lon);
|
double lon_new(LatLonHeight_IC(1) + delta_lon);
|
||||||
|
|
||||||
// Rotation of lon about z axis
|
// Rotation of lon about z axis
|
||||||
Rot3 C1(cos(lon_new), sin(lon_new), 0.0,
|
Rot3 C1(cos(lon_new), sin(lon_new), 0.0,
|
||||||
-sin(lon_new), cos(lon_new), 0.0,
|
-sin(lon_new), cos(lon_new), 0.0,
|
||||||
0.0, 0.0, 1.0);
|
0.0, 0.0, 1.0);
|
||||||
|
|
||||||
// Rotation of lat about y axis
|
// Rotation of lat about y axis
|
||||||
Rot3 C2(cos(lat_new), 0.0, sin(lat_new),
|
Rot3 C2(cos(lat_new), 0.0, sin(lat_new),
|
||||||
0.0, 1.0, 0.0,
|
0.0, 1.0, 0.0,
|
||||||
-sin(lat_new), 0.0, cos(lat_new));
|
-sin(lat_new), 0.0, cos(lat_new));
|
||||||
|
|
||||||
Rot3 UEN_to_ENU(0, 1, 0,
|
Rot3 UEN_to_ENU(0, 1, 0,
|
||||||
0, 0, 1,
|
0, 0, 1,
|
||||||
1, 0, 0);
|
1, 0, 0);
|
||||||
|
|
||||||
Rot3 R_ECEF_to_ENU( UEN_to_ENU * C2 * C1 );
|
Rot3 R_ECEF_to_ENU( UEN_to_ENU * C2 * C1 );
|
||||||
|
|
||||||
Vector omega_earth_ECEF(Vector_(3, 0.0, 0.0, 7.292115e-5));
|
Vector omega_earth_ECEF(Vector_(3, 0.0, 0.0, 7.292115e-5));
|
||||||
omega_earth_ENU = R_ECEF_to_ENU.matrix() * omega_earth_ECEF;
|
omega_earth_ENU = R_ECEF_to_ENU.matrix() * omega_earth_ECEF;
|
||||||
|
|
||||||
// Calculating g
|
// Calculating g
|
||||||
double height(LatLonHeight_IC(2));
|
double height(LatLonHeight_IC(2));
|
||||||
double EQUA_RADIUS = 6378137.0; // equatorial radius of the earth; WGS-84
|
double EQUA_RADIUS = 6378137.0; // equatorial radius of the earth; WGS-84
|
||||||
double ECCENTRICITY = 0.0818191908426; // eccentricity of the earth ellipsoid
|
double ECCENTRICITY = 0.0818191908426; // eccentricity of the earth ellipsoid
|
||||||
double e2( pow(ECCENTRICITY,2) );
|
double e2( pow(ECCENTRICITY,2) );
|
||||||
double den( 1-e2*pow(sin(lat_new),2) );
|
double den( 1-e2*pow(sin(lat_new),2) );
|
||||||
double Rm( (EQUA_RADIUS*(1-e2))/( pow(den,(3/2)) ) );
|
double Rm( (EQUA_RADIUS*(1-e2))/( pow(den,(3/2)) ) );
|
||||||
double Rp( EQUA_RADIUS/( sqrt(den) ) );
|
double Rp( EQUA_RADIUS/( sqrt(den) ) );
|
||||||
double Ro( sqrt(Rp*Rm) ); // mean earth radius of curvature
|
double Ro( sqrt(Rp*Rm) ); // mean earth radius of curvature
|
||||||
double g0( 9.780318*( 1 + 5.3024e-3 * pow(sin(lat_new),2) - 5.9e-6 * pow(sin(2*lat_new),2) ) );
|
double g0( 9.780318*( 1 + 5.3024e-3 * pow(sin(lat_new),2) - 5.9e-6 * pow(sin(2*lat_new),2) ) );
|
||||||
double g_calc( g0/( pow(1 + height/Ro, 2) ) );
|
double g_calc( g0/( pow(1 + height/Ro, 2) ) );
|
||||||
g_ENU = Vector_(3, 0.0, 0.0, -g_calc);
|
g_ENU = Vector_(3, 0.0, 0.0, -g_calc);
|
||||||
|
|
||||||
|
|
||||||
// Calculate rho
|
// Calculate rho
|
||||||
double Ve( Vel_ENU(0) );
|
double Ve( Vel_ENU(0) );
|
||||||
double Vn( Vel_ENU(1) );
|
double Vn( Vel_ENU(1) );
|
||||||
double rho_E = -Vn/(Rm + height);
|
double rho_E = -Vn/(Rm + height);
|
||||||
double rho_N = Ve/(Rp + height);
|
double rho_N = Ve/(Rp + height);
|
||||||
double rho_U = Ve*tan(lat_new)/(Rp + height);
|
double rho_U = Ve*tan(lat_new)/(Rp + height);
|
||||||
rho_ENU = Vector_(3, rho_E, rho_N, rho_U);
|
rho_ENU = Vector_(3, rho_E, rho_N, rho_U);
|
||||||
}
|
}
|
||||||
|
|
||||||
static inline noiseModel::Gaussian::shared_ptr calc_descrete_noise_model(const noiseModel::Gaussian::shared_ptr& model, double delta_t){
|
static inline noiseModel::Gaussian::shared_ptr calc_descrete_noise_model(const noiseModel::Gaussian::shared_ptr& model, double delta_t){
|
||||||
/* Q_d (approx)= Q * delta_t */
|
/* Q_d (approx)= Q * delta_t */
|
||||||
/* In practice, square root of the information matrix is represented, so that:
|
/* In practice, square root of the information matrix is represented, so that:
|
||||||
* R_d (approx)= R / sqrt(delta_t)
|
* R_d (approx)= R / sqrt(delta_t)
|
||||||
* */
|
* */
|
||||||
return noiseModel::Gaussian::SqrtInformation(model->R()/sqrt(delta_t));
|
return noiseModel::Gaussian::SqrtInformation(model->R()/sqrt(delta_t));
|
||||||
}
|
}
|
||||||
private:
|
private:
|
||||||
|
|
||||||
/** Serialization function */
|
/** Serialization function */
|
||||||
friend class boost::serialization::access;
|
friend class boost::serialization::access;
|
||||||
template<class ARCHIVE>
|
template<class ARCHIVE>
|
||||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||||
ar & boost::serialization::make_nvp("NonlinearFactor2",
|
ar & boost::serialization::make_nvp("NonlinearFactor2",
|
||||||
boost::serialization::base_object<Base>(*this));
|
boost::serialization::base_object<Base>(*this));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -39,8 +39,8 @@ namespace gtsam {
|
||||||
* =====
|
* =====
|
||||||
* Concept: Based on [Lupton12tro]
|
* Concept: Based on [Lupton12tro]
|
||||||
* - Pre-integrate IMU measurements using the static function PreIntegrateIMUObservations.
|
* - Pre-integrate IMU measurements using the static function PreIntegrateIMUObservations.
|
||||||
* Pre-integrated quantities are expressed in the body system of t0 - the first time instant (in which pre-integration began).
|
* Pre-integrated quantities are expressed in the body system of t0 - the first time instant (in which pre-integration began).
|
||||||
* All sensor-to-body transformations are performed here.
|
* All sensor-to-body transformations are performed here.
|
||||||
* - If required, calculate inertial solution by calling the static functions: predictPose_inertial, predictVelocity_inertial.
|
* - If required, calculate inertial solution by calling the static functions: predictPose_inertial, predictVelocity_inertial.
|
||||||
* - When the time is right, incorporate pre-integrated IMU data by creating an EquivInertialNavFactor_GlobalVel_NoBias factor, which will
|
* - When the time is right, incorporate pre-integrated IMU data by creating an EquivInertialNavFactor_GlobalVel_NoBias factor, which will
|
||||||
* relate between navigation variables at the two time instances (t0 and current time).
|
* relate between navigation variables at the two time instances (t0 and current time).
|
||||||
|
|
@ -54,11 +54,11 @@ namespace gtsam {
|
||||||
* matrices and the process\modeling covariance matrix. The IneritalNavFactor converts this into a
|
* matrices and the process\modeling covariance matrix. The IneritalNavFactor converts this into a
|
||||||
* discrete form using the supplied delta_t between sub-sequential measurements.
|
* discrete form using the supplied delta_t between sub-sequential measurements.
|
||||||
* - Earth-rate correction:
|
* - Earth-rate correction:
|
||||||
* + Currently the user should supply R_ECEF_to_G, which is the rotation from ECEF to the global
|
* + Currently the user should supply R_ECEF_to_G, which is the rotation from ECEF to the global
|
||||||
* frame (Local-Level system: ENU or NED, see above).
|
* frame (Local-Level system: ENU or NED, see above).
|
||||||
* + R_ECEF_to_G can be calculated by approximated values of latitude and longitude of the system.
|
* + R_ECEF_to_G can be calculated by approximated values of latitude and longitude of the system.
|
||||||
* + Currently it is assumed that a relatively small distance is traveled w.r.t. to initial pose, since R_ECEF_to_G is constant.
|
* + Currently it is assumed that a relatively small distance is traveled w.r.t. to initial pose, since R_ECEF_to_G is constant.
|
||||||
* Otherwise, R_ECEF_to_G should be updated each time using the current lat-lon.
|
* Otherwise, R_ECEF_to_G should be updated each time using the current lat-lon.
|
||||||
*
|
*
|
||||||
* - Frame Notation:
|
* - Frame Notation:
|
||||||
* Quantities are written as {Frame of Representation/Destination Frame}_{Quantity Type}_{Quatity Description/Origination Frame}
|
* Quantities are written as {Frame of Representation/Destination Frame}_{Quantity Type}_{Quatity Description/Origination Frame}
|
||||||
|
|
@ -92,222 +92,222 @@ class EquivInertialNavFactor_GlobalVel_NoBias : public NoiseModelFactor4<POSE, V
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
typedef EquivInertialNavFactor_GlobalVel_NoBias<POSE, VELOCITY> This;
|
typedef EquivInertialNavFactor_GlobalVel_NoBias<POSE, VELOCITY> This;
|
||||||
typedef NoiseModelFactor4<POSE, VELOCITY, POSE, VELOCITY> Base;
|
typedef NoiseModelFactor4<POSE, VELOCITY, POSE, VELOCITY> Base;
|
||||||
|
|
||||||
Vector delta_pos_in_t0_;
|
Vector delta_pos_in_t0_;
|
||||||
Vector delta_vel_in_t0_;
|
Vector delta_vel_in_t0_;
|
||||||
Vector3 delta_angles_;
|
Vector3 delta_angles_;
|
||||||
double dt12_;
|
double dt12_;
|
||||||
|
|
||||||
Vector world_g_;
|
Vector world_g_;
|
||||||
Vector world_rho_;
|
Vector world_rho_;
|
||||||
Vector world_omega_earth_;
|
Vector world_omega_earth_;
|
||||||
|
|
||||||
Matrix Jacobian_wrt_t0_Overall_;
|
Matrix Jacobian_wrt_t0_Overall_;
|
||||||
|
|
||||||
boost::optional<POSE> body_P_sensor_; // The pose of the sensor in the body frame
|
boost::optional<POSE> body_P_sensor_; // The pose of the sensor in the body frame
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// shorthand for a smart pointer to a factor
|
// shorthand for a smart pointer to a factor
|
||||||
typedef typename boost::shared_ptr<EquivInertialNavFactor_GlobalVel_NoBias> shared_ptr;
|
typedef typename boost::shared_ptr<EquivInertialNavFactor_GlobalVel_NoBias> shared_ptr;
|
||||||
|
|
||||||
/** default constructor - only use for serialization */
|
/** default constructor - only use for serialization */
|
||||||
EquivInertialNavFactor_GlobalVel_NoBias() {}
|
EquivInertialNavFactor_GlobalVel_NoBias() {}
|
||||||
|
|
||||||
/** Constructor */
|
/** Constructor */
|
||||||
EquivInertialNavFactor_GlobalVel_NoBias(const Key& Pose1, const Key& Vel1, const Key& Pose2, const Key& Vel2,
|
EquivInertialNavFactor_GlobalVel_NoBias(const Key& Pose1, const Key& Vel1, const Key& Pose2, const Key& Vel2,
|
||||||
const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0, const Vector3& delta_angles,
|
const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0, const Vector3& delta_angles,
|
||||||
double dt12, const Vector world_g, const Vector world_rho,
|
double dt12, const Vector world_g, const Vector world_rho,
|
||||||
const Vector& world_omega_earth, const noiseModel::Gaussian::shared_ptr& model_equivalent,
|
const Vector& world_omega_earth, const noiseModel::Gaussian::shared_ptr& model_equivalent,
|
||||||
const Matrix& Jacobian_wrt_t0_Overall,
|
const Matrix& Jacobian_wrt_t0_Overall,
|
||||||
boost::optional<POSE> body_P_sensor = boost::none) :
|
boost::optional<POSE> body_P_sensor = boost::none) :
|
||||||
Base(model_equivalent, Pose1, Vel1, Pose2, Vel2),
|
Base(model_equivalent, Pose1, Vel1, Pose2, Vel2),
|
||||||
delta_pos_in_t0_(delta_pos_in_t0), delta_vel_in_t0_(delta_vel_in_t0), delta_angles_(delta_angles),
|
delta_pos_in_t0_(delta_pos_in_t0), delta_vel_in_t0_(delta_vel_in_t0), delta_angles_(delta_angles),
|
||||||
dt12_(dt12), world_g_(world_g), world_rho_(world_rho), world_omega_earth_(world_omega_earth), Jacobian_wrt_t0_Overall_(Jacobian_wrt_t0_Overall),
|
dt12_(dt12), world_g_(world_g), world_rho_(world_rho), world_omega_earth_(world_omega_earth), Jacobian_wrt_t0_Overall_(Jacobian_wrt_t0_Overall),
|
||||||
body_P_sensor_(body_P_sensor) { }
|
body_P_sensor_(body_P_sensor) { }
|
||||||
|
|
||||||
virtual ~EquivInertialNavFactor_GlobalVel_NoBias() {}
|
virtual ~EquivInertialNavFactor_GlobalVel_NoBias() {}
|
||||||
|
|
||||||
/** implement functions needed for Testable */
|
/** implement functions needed for Testable */
|
||||||
|
|
||||||
/** print */
|
/** print */
|
||||||
virtual void print(const std::string& s = "EquivInertialNavFactor_GlobalVel_NoBias", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
|
virtual void print(const std::string& s = "EquivInertialNavFactor_GlobalVel_NoBias", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
|
||||||
std::cout << s << "("
|
std::cout << s << "("
|
||||||
<< keyFormatter(this->key1()) << ","
|
<< keyFormatter(this->key1()) << ","
|
||||||
<< keyFormatter(this->key2()) << ","
|
<< keyFormatter(this->key2()) << ","
|
||||||
<< keyFormatter(this->key3()) << ","
|
<< keyFormatter(this->key3()) << ","
|
||||||
<< keyFormatter(this->key4()) << "\n";
|
<< keyFormatter(this->key4()) << "\n";
|
||||||
std::cout << "delta_pos_in_t0: " << this->delta_pos_in_t0_.transpose() << std::endl;
|
std::cout << "delta_pos_in_t0: " << this->delta_pos_in_t0_.transpose() << std::endl;
|
||||||
std::cout << "delta_vel_in_t0: " << this->delta_vel_in_t0_.transpose() << std::endl;
|
std::cout << "delta_vel_in_t0: " << this->delta_vel_in_t0_.transpose() << std::endl;
|
||||||
std::cout << "delta_angles: " << this->delta_angles_ << std::endl;
|
std::cout << "delta_angles: " << this->delta_angles_ << std::endl;
|
||||||
std::cout << "dt12: " << this->dt12_ << std::endl;
|
std::cout << "dt12: " << this->dt12_ << std::endl;
|
||||||
std::cout << "gravity (in world frame): " << this->world_g_.transpose() << std::endl;
|
std::cout << "gravity (in world frame): " << this->world_g_.transpose() << std::endl;
|
||||||
std::cout << "craft rate (in world frame): " << this->world_rho_.transpose() << std::endl;
|
std::cout << "craft rate (in world frame): " << this->world_rho_.transpose() << std::endl;
|
||||||
std::cout << "earth's rotation (in world frame): " << this->world_omega_earth_.transpose() << std::endl;
|
std::cout << "earth's rotation (in world frame): " << this->world_omega_earth_.transpose() << std::endl;
|
||||||
if(this->body_P_sensor_)
|
if(this->body_P_sensor_)
|
||||||
this->body_P_sensor_->print(" sensor pose in body frame: ");
|
this->body_P_sensor_->print(" sensor pose in body frame: ");
|
||||||
this->noiseModel_->print(" noise model");
|
this->noiseModel_->print(" noise model");
|
||||||
}
|
}
|
||||||
|
|
||||||
/** equals */
|
/** equals */
|
||||||
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
|
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
|
||||||
const This *e = dynamic_cast<const This*> (&expected);
|
const This *e = dynamic_cast<const This*> (&expected);
|
||||||
return e != NULL && Base::equals(*e, tol)
|
return e != NULL && Base::equals(*e, tol)
|
||||||
&& (delta_pos_in_t0_ - e->delta_pos_in_t0_).norm() < tol
|
&& (delta_pos_in_t0_ - e->delta_pos_in_t0_).norm() < tol
|
||||||
&& (delta_vel_in_t0_ - e->delta_vel_in_t0_).norm() < tol
|
&& (delta_vel_in_t0_ - e->delta_vel_in_t0_).norm() < tol
|
||||||
&& (delta_angles_ - e->delta_angles_).norm() < tol
|
&& (delta_angles_ - e->delta_angles_).norm() < tol
|
||||||
&& (dt12_ - e->dt12_) < tol
|
&& (dt12_ - e->dt12_) < tol
|
||||||
&& (world_g_ - e->world_g_).norm() < tol
|
&& (world_g_ - e->world_g_).norm() < tol
|
||||||
&& (world_rho_ - e->world_rho_).norm() < tol
|
&& (world_rho_ - e->world_rho_).norm() < tol
|
||||||
&& (world_omega_earth_ - e->world_omega_earth_).norm() < tol
|
&& (world_omega_earth_ - e->world_omega_earth_).norm() < tol
|
||||||
&& ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
|
&& ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
POSE predictPose(const POSE& Pose1, const VELOCITY& Vel1) const {
|
POSE predictPose(const POSE& Pose1, const VELOCITY& Vel1) const {
|
||||||
|
|
||||||
/* Position term */
|
/* Position term */
|
||||||
Vector delta_pos_in_t0_corrected = delta_pos_in_t0_;
|
Vector delta_pos_in_t0_corrected = delta_pos_in_t0_;
|
||||||
|
|
||||||
/* Rotation term */
|
/* Rotation term */
|
||||||
Vector delta_angles_corrected = delta_angles_;
|
Vector delta_angles_corrected = delta_angles_;
|
||||||
|
|
||||||
return predictPose_inertial(Pose1, Vel1,
|
return predictPose_inertial(Pose1, Vel1,
|
||||||
delta_pos_in_t0_corrected, delta_angles_corrected,
|
delta_pos_in_t0_corrected, delta_angles_corrected,
|
||||||
dt12_, world_g_, world_rho_, world_omega_earth_);
|
dt12_, world_g_, world_rho_, world_omega_earth_);
|
||||||
}
|
}
|
||||||
|
|
||||||
static inline POSE predictPose_inertial(const POSE& Pose1, const VELOCITY& Vel1,
|
static inline POSE predictPose_inertial(const POSE& Pose1, const VELOCITY& Vel1,
|
||||||
const Vector& delta_pos_in_t0, const Vector3& delta_angles,
|
const Vector& delta_pos_in_t0, const Vector3& delta_angles,
|
||||||
const double dt12, const Vector& world_g, const Vector& world_rho, const Vector& world_omega_earth){
|
const double dt12, const Vector& world_g, const Vector& world_rho, const Vector& world_omega_earth){
|
||||||
|
|
||||||
const POSE& world_P1_body = Pose1;
|
const POSE& world_P1_body = Pose1;
|
||||||
const VELOCITY& world_V1_body = Vel1;
|
const VELOCITY& world_V1_body = Vel1;
|
||||||
|
|
||||||
/* Position term */
|
/* Position term */
|
||||||
Vector body_deltaPos_body = delta_pos_in_t0;
|
Vector body_deltaPos_body = delta_pos_in_t0;
|
||||||
|
|
||||||
Vector world_deltaPos_pls_body = world_P1_body.rotation().matrix() * body_deltaPos_body;
|
Vector world_deltaPos_pls_body = world_P1_body.rotation().matrix() * body_deltaPos_body;
|
||||||
Vector world_deltaPos_body = world_V1_body * dt12 + 0.5*world_g*dt12*dt12 + world_deltaPos_pls_body;
|
Vector world_deltaPos_body = world_V1_body * dt12 + 0.5*world_g*dt12*dt12 + world_deltaPos_pls_body;
|
||||||
|
|
||||||
// Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2.
|
// Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2.
|
||||||
world_deltaPos_body -= 2*skewSymmetric(world_rho + world_omega_earth)*world_V1_body * dt12*dt12;
|
world_deltaPos_body -= 2*skewSymmetric(world_rho + world_omega_earth)*world_V1_body * dt12*dt12;
|
||||||
|
|
||||||
/* TODO: the term dt12*dt12 in 0.5*world_g*dt12*dt12 is not entirely correct:
|
/* TODO: the term dt12*dt12 in 0.5*world_g*dt12*dt12 is not entirely correct:
|
||||||
* the gravity should be canceled from the accelerometer measurements, bust since position
|
* the gravity should be canceled from the accelerometer measurements, bust since position
|
||||||
* is added with a delta velocity from a previous term, the actual delta time is more complicated.
|
* is added with a delta velocity from a previous term, the actual delta time is more complicated.
|
||||||
* Need to figure out this in the future - currently because of this issue we'll get some more error
|
* Need to figure out this in the future - currently because of this issue we'll get some more error
|
||||||
* in Z axis.
|
* in Z axis.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
/* Rotation term */
|
/* Rotation term */
|
||||||
Vector body_deltaAngles_body = delta_angles;
|
Vector body_deltaAngles_body = delta_angles;
|
||||||
|
|
||||||
// Convert earth-related terms into the body frame
|
// Convert earth-related terms into the body frame
|
||||||
Matrix body_R_world(world_P1_body.rotation().inverse().matrix());
|
Matrix body_R_world(world_P1_body.rotation().inverse().matrix());
|
||||||
Vector body_rho = body_R_world * world_rho;
|
Vector body_rho = body_R_world * world_rho;
|
||||||
Vector body_omega_earth = body_R_world * world_omega_earth;
|
Vector body_omega_earth = body_R_world * world_omega_earth;
|
||||||
|
|
||||||
// Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2.
|
// Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2.
|
||||||
body_deltaAngles_body -= (body_rho + body_omega_earth)*dt12;
|
body_deltaAngles_body -= (body_rho + body_omega_earth)*dt12;
|
||||||
|
|
||||||
return POSE(Pose1.rotation() * POSE::Rotation::Expmap(body_deltaAngles_body), Pose1.translation() + typename POSE::Translation(world_deltaPos_body));
|
return POSE(Pose1.rotation() * POSE::Rotation::Expmap(body_deltaAngles_body), Pose1.translation() + typename POSE::Translation(world_deltaPos_body));
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
VELOCITY predictVelocity(const POSE& Pose1, const VELOCITY& Vel1) const {
|
VELOCITY predictVelocity(const POSE& Pose1, const VELOCITY& Vel1) const {
|
||||||
|
|
||||||
|
|
||||||
Vector delta_vel_in_t0_corrected = delta_vel_in_t0_;
|
Vector delta_vel_in_t0_corrected = delta_vel_in_t0_;
|
||||||
|
|
||||||
return predictVelocity_inertial(Pose1, Vel1,
|
return predictVelocity_inertial(Pose1, Vel1,
|
||||||
delta_vel_in_t0_corrected,
|
delta_vel_in_t0_corrected,
|
||||||
dt12_, world_g_, world_rho_, world_omega_earth_);
|
dt12_, world_g_, world_rho_, world_omega_earth_);
|
||||||
}
|
}
|
||||||
|
|
||||||
static inline VELOCITY predictVelocity_inertial(const POSE& Pose1, const VELOCITY& Vel1,
|
static inline VELOCITY predictVelocity_inertial(const POSE& Pose1, const VELOCITY& Vel1,
|
||||||
const Vector& delta_vel_in_t0,
|
const Vector& delta_vel_in_t0,
|
||||||
const double dt12, const Vector& world_g, const Vector& world_rho, const Vector& world_omega_earth) {
|
const double dt12, const Vector& world_g, const Vector& world_rho, const Vector& world_omega_earth) {
|
||||||
|
|
||||||
const POSE& world_P1_body = Pose1;
|
const POSE& world_P1_body = Pose1;
|
||||||
const VELOCITY& world_V1_body = Vel1;
|
const VELOCITY& world_V1_body = Vel1;
|
||||||
|
|
||||||
Vector body_deltaVel_body = delta_vel_in_t0;
|
Vector body_deltaVel_body = delta_vel_in_t0;
|
||||||
Vector world_deltaVel_body = world_P1_body.rotation().matrix() * body_deltaVel_body;
|
Vector world_deltaVel_body = world_P1_body.rotation().matrix() * body_deltaVel_body;
|
||||||
|
|
||||||
VELOCITY VelDelta( world_deltaVel_body + world_g * dt12 );
|
VELOCITY VelDelta( world_deltaVel_body + world_g * dt12 );
|
||||||
|
|
||||||
// Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2.
|
// Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2.
|
||||||
VelDelta -= 2*skewSymmetric(world_rho + world_omega_earth)*world_V1_body * dt12;
|
VelDelta -= 2*skewSymmetric(world_rho + world_omega_earth)*world_V1_body * dt12;
|
||||||
|
|
||||||
// Predict
|
// Predict
|
||||||
return Vel1.compose( VelDelta );
|
return Vel1.compose( VelDelta );
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void predict(const POSE& Pose1, const VELOCITY& Vel1, POSE& Pose2, VELOCITY& Vel2) const {
|
void predict(const POSE& Pose1, const VELOCITY& Vel1, POSE& Pose2, VELOCITY& Vel2) const {
|
||||||
Pose2 = predictPose(Pose1, Vel1);
|
Pose2 = predictPose(Pose1, Vel1);
|
||||||
Vel2 = predictVelocity(Pose1, Vel1);
|
Vel2 = predictVelocity(Pose1, Vel1);
|
||||||
}
|
}
|
||||||
|
|
||||||
POSE evaluatePoseError(const POSE& Pose1, const VELOCITY& Vel1, const POSE& Pose2, const VELOCITY& Vel2) const {
|
POSE evaluatePoseError(const POSE& Pose1, const VELOCITY& Vel1, const POSE& Pose2, const VELOCITY& Vel2) const {
|
||||||
// Predict
|
// Predict
|
||||||
POSE Pose2Pred = predictPose(Pose1, Vel1);
|
POSE Pose2Pred = predictPose(Pose1, Vel1);
|
||||||
|
|
||||||
// Calculate error
|
// Calculate error
|
||||||
return Pose2.between(Pose2Pred);
|
return Pose2.between(Pose2Pred);
|
||||||
}
|
}
|
||||||
|
|
||||||
VELOCITY evaluateVelocityError(const POSE& Pose1, const VELOCITY& Vel1, const POSE& Pose2, const VELOCITY& Vel2) const {
|
VELOCITY evaluateVelocityError(const POSE& Pose1, const VELOCITY& Vel1, const POSE& Pose2, const VELOCITY& Vel2) const {
|
||||||
// Predict
|
// Predict
|
||||||
VELOCITY Vel2Pred = predictVelocity(Pose1, Vel1);
|
VELOCITY Vel2Pred = predictVelocity(Pose1, Vel1);
|
||||||
|
|
||||||
// Calculate error
|
// Calculate error
|
||||||
return Vel2.between(Vel2Pred);
|
return Vel2.between(Vel2Pred);
|
||||||
}
|
}
|
||||||
|
|
||||||
Vector evaluateError(const POSE& Pose1, const VELOCITY& Vel1, const POSE& Pose2, const VELOCITY& Vel2,
|
Vector evaluateError(const POSE& Pose1, const VELOCITY& Vel1, const POSE& Pose2, const VELOCITY& Vel2,
|
||||||
boost::optional<Matrix&> H1 = boost::none,
|
boost::optional<Matrix&> H1 = boost::none,
|
||||||
boost::optional<Matrix&> H2 = boost::none,
|
boost::optional<Matrix&> H2 = boost::none,
|
||||||
boost::optional<Matrix&> H3 = boost::none,
|
boost::optional<Matrix&> H3 = boost::none,
|
||||||
boost::optional<Matrix&> H4 = boost::none) const {
|
boost::optional<Matrix&> H4 = boost::none) const {
|
||||||
|
|
||||||
// TODO: Write analytical derivative calculations
|
// TODO: Write analytical derivative calculations
|
||||||
// Jacobian w.r.t. Pose1
|
// Jacobian w.r.t. Pose1
|
||||||
if (H1){
|
if (H1){
|
||||||
Matrix H1_Pose = numericalDerivative11<POSE, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, _1, Vel1, Pose2, Vel2), Pose1);
|
Matrix H1_Pose = numericalDerivative11<POSE, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, _1, Vel1, Pose2, Vel2), Pose1);
|
||||||
Matrix H1_Vel = numericalDerivative11<VELOCITY, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, _1, Vel1, Pose2, Vel2), Pose1);
|
Matrix H1_Vel = numericalDerivative11<VELOCITY, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, _1, Vel1, Pose2, Vel2), Pose1);
|
||||||
*H1 = stack(2, &H1_Pose, &H1_Vel);
|
*H1 = stack(2, &H1_Pose, &H1_Vel);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Jacobian w.r.t. Vel1
|
// Jacobian w.r.t. Vel1
|
||||||
if (H2){
|
if (H2){
|
||||||
Matrix H2_Pose = numericalDerivative11<POSE, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, _1, Pose2, Vel2), Vel1);
|
Matrix H2_Pose = numericalDerivative11<POSE, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, _1, Pose2, Vel2), Vel1);
|
||||||
Matrix H2_Vel = numericalDerivative11<VELOCITY, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, _1, Pose2, Vel2), Vel1);
|
Matrix H2_Vel = numericalDerivative11<VELOCITY, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, _1, Pose2, Vel2), Vel1);
|
||||||
*H2 = stack(2, &H2_Pose, &H2_Vel);
|
*H2 = stack(2, &H2_Pose, &H2_Vel);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Jacobian w.r.t. Pose2
|
// Jacobian w.r.t. Pose2
|
||||||
if (H3){
|
if (H3){
|
||||||
Matrix H3_Pose = numericalDerivative11<POSE, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, Vel1, _1, Vel2), Pose2);
|
Matrix H3_Pose = numericalDerivative11<POSE, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, Vel1, _1, Vel2), Pose2);
|
||||||
Matrix H3_Vel = numericalDerivative11<VELOCITY, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, Vel1, _1, Vel2), Pose2);
|
Matrix H3_Vel = numericalDerivative11<VELOCITY, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, Vel1, _1, Vel2), Pose2);
|
||||||
*H3 = stack(2, &H3_Pose, &H3_Vel);
|
*H3 = stack(2, &H3_Pose, &H3_Vel);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Jacobian w.r.t. Vel2
|
// Jacobian w.r.t. Vel2
|
||||||
if (H4){
|
if (H4){
|
||||||
Matrix H4_Pose = numericalDerivative11<POSE, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, Vel1, Pose2, _1), Vel2);
|
Matrix H4_Pose = numericalDerivative11<POSE, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, Vel1, Pose2, _1), Vel2);
|
||||||
Matrix H4_Vel = numericalDerivative11<VELOCITY, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, Vel1, Pose2, _1), Vel2);
|
Matrix H4_Vel = numericalDerivative11<VELOCITY, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, Vel1, Pose2, _1), Vel2);
|
||||||
*H4 = stack(2, &H4_Pose, &H4_Vel);
|
*H4 = stack(2, &H4_Pose, &H4_Vel);
|
||||||
}
|
}
|
||||||
|
|
||||||
Vector ErrPoseVector(POSE::Logmap(evaluatePoseError(Pose1, Vel1, Pose2, Vel2)));
|
Vector ErrPoseVector(POSE::Logmap(evaluatePoseError(Pose1, Vel1, Pose2, Vel2)));
|
||||||
Vector ErrVelVector(VELOCITY::Logmap(evaluateVelocityError(Pose1, Vel1, Pose2, Vel2)));
|
Vector ErrVelVector(VELOCITY::Logmap(evaluateVelocityError(Pose1, Vel1, Pose2, Vel2)));
|
||||||
|
|
||||||
return concatVectors(2, &ErrPoseVector, &ErrVelVector);
|
return concatVectors(2, &ErrPoseVector, &ErrVelVector);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -348,126 +348,126 @@ public:
|
||||||
|
|
||||||
static inline void PreIntegrateIMUObservations(const Vector& msr_acc_t, const Vector& msr_gyro_t, const double msr_dt,
|
static inline void PreIntegrateIMUObservations(const Vector& msr_acc_t, const Vector& msr_gyro_t, const double msr_dt,
|
||||||
Vector& delta_pos_in_t0, Vector3& delta_angles, Vector& delta_vel_in_t0, double& delta_t,
|
Vector& delta_pos_in_t0, Vector3& delta_angles, Vector& delta_vel_in_t0, double& delta_t,
|
||||||
const noiseModel::Gaussian::shared_ptr& model_continuous_overall,
|
const noiseModel::Gaussian::shared_ptr& model_continuous_overall,
|
||||||
Matrix& EquivCov_Overall, Matrix& Jacobian_wrt_t0_Overall,
|
Matrix& EquivCov_Overall, Matrix& Jacobian_wrt_t0_Overall,
|
||||||
boost::optional<POSE> p_body_P_sensor = boost::none){
|
boost::optional<POSE> p_body_P_sensor = boost::none){
|
||||||
// Note: all delta terms refer to an IMU\sensor system at t0
|
// Note: all delta terms refer to an IMU\sensor system at t0
|
||||||
// Note: Earth-related terms are not accounted here but are incorporated in predict functions.
|
// Note: Earth-related terms are not accounted here but are incorporated in predict functions.
|
||||||
|
|
||||||
POSE body_P_sensor = POSE();
|
POSE body_P_sensor = POSE();
|
||||||
bool flag_use_body_P_sensor = false;
|
bool flag_use_body_P_sensor = false;
|
||||||
if (p_body_P_sensor){
|
if (p_body_P_sensor){
|
||||||
body_P_sensor = *p_body_P_sensor;
|
body_P_sensor = *p_body_P_sensor;
|
||||||
flag_use_body_P_sensor = true;
|
flag_use_body_P_sensor = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
delta_pos_in_t0 = PreIntegrateIMUObservations_delta_pos(msr_dt, delta_pos_in_t0, delta_vel_in_t0);
|
delta_pos_in_t0 = PreIntegrateIMUObservations_delta_pos(msr_dt, delta_pos_in_t0, delta_vel_in_t0);
|
||||||
delta_vel_in_t0 = PreIntegrateIMUObservations_delta_vel(msr_gyro_t, msr_acc_t, msr_dt, delta_angles, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor);
|
delta_vel_in_t0 = PreIntegrateIMUObservations_delta_vel(msr_gyro_t, msr_acc_t, msr_dt, delta_angles, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor);
|
||||||
delta_angles = PreIntegrateIMUObservations_delta_angles(msr_gyro_t, msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor);
|
delta_angles = PreIntegrateIMUObservations_delta_angles(msr_gyro_t, msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor);
|
||||||
|
|
||||||
delta_t += msr_dt;
|
delta_t += msr_dt;
|
||||||
|
|
||||||
// Update EquivCov_Overall
|
// Update EquivCov_Overall
|
||||||
Matrix Z_3x3 = zeros(3,3);
|
Matrix Z_3x3 = zeros(3,3);
|
||||||
Matrix I_3x3 = eye(3,3);
|
Matrix I_3x3 = eye(3,3);
|
||||||
|
|
||||||
Matrix H_pos_pos = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0);
|
Matrix H_pos_pos = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0);
|
||||||
Matrix H_pos_vel = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0);
|
Matrix H_pos_vel = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0);
|
||||||
Matrix H_pos_angles = Z_3x3;
|
Matrix H_pos_angles = Z_3x3;
|
||||||
|
|
||||||
Matrix H_vel_vel = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, _1, flag_use_body_P_sensor, body_P_sensor), delta_vel_in_t0);
|
Matrix H_vel_vel = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, _1, flag_use_body_P_sensor, body_P_sensor), delta_vel_in_t0);
|
||||||
Matrix H_vel_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, _1, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor), delta_angles);
|
Matrix H_vel_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, _1, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor), delta_angles);
|
||||||
Matrix H_vel_pos = Z_3x3;
|
Matrix H_vel_pos = Z_3x3;
|
||||||
|
|
||||||
Matrix H_angles_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, _1, flag_use_body_P_sensor, body_P_sensor), delta_angles);
|
Matrix H_angles_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, _1, flag_use_body_P_sensor, body_P_sensor), delta_angles);
|
||||||
Matrix H_angles_pos = Z_3x3;
|
Matrix H_angles_pos = Z_3x3;
|
||||||
Matrix H_angles_vel = Z_3x3;
|
Matrix H_angles_vel = Z_3x3;
|
||||||
|
|
||||||
Matrix F_angles = collect(3, &H_angles_angles, &H_angles_pos, &H_angles_vel);
|
Matrix F_angles = collect(3, &H_angles_angles, &H_angles_pos, &H_angles_vel);
|
||||||
Matrix F_pos = collect(3, &H_pos_angles, &H_pos_pos, &H_pos_vel);
|
Matrix F_pos = collect(3, &H_pos_angles, &H_pos_pos, &H_pos_vel);
|
||||||
Matrix F_vel = collect(3, &H_vel_angles, &H_vel_pos, &H_vel_vel);
|
Matrix F_vel = collect(3, &H_vel_angles, &H_vel_pos, &H_vel_vel);
|
||||||
Matrix F = stack(3, &F_angles, &F_pos, &F_vel);
|
Matrix F = stack(3, &F_angles, &F_pos, &F_vel);
|
||||||
|
|
||||||
noiseModel::Gaussian::shared_ptr model_discrete_curr = calc_descrete_noise_model(model_continuous_overall, msr_dt );
|
noiseModel::Gaussian::shared_ptr model_discrete_curr = calc_descrete_noise_model(model_continuous_overall, msr_dt );
|
||||||
Matrix Q_d = inverse(model_discrete_curr->R().transpose() * model_discrete_curr->R() );
|
Matrix Q_d = inverse(model_discrete_curr->R().transpose() * model_discrete_curr->R() );
|
||||||
|
|
||||||
EquivCov_Overall = F * EquivCov_Overall * F.transpose() + Q_d;
|
EquivCov_Overall = F * EquivCov_Overall * F.transpose() + Q_d;
|
||||||
|
|
||||||
// Update Jacobian_wrt_t0_Overall
|
// Update Jacobian_wrt_t0_Overall
|
||||||
Jacobian_wrt_t0_Overall = F * Jacobian_wrt_t0_Overall;
|
Jacobian_wrt_t0_Overall = F * Jacobian_wrt_t0_Overall;
|
||||||
}
|
}
|
||||||
|
|
||||||
static inline Vector PreIntegrateIMUObservations_delta_pos(const double msr_dt,
|
static inline Vector PreIntegrateIMUObservations_delta_pos(const double msr_dt,
|
||||||
const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0){
|
const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0){
|
||||||
|
|
||||||
// Note: all delta terms refer to an IMU\sensor system at t0
|
// Note: all delta terms refer to an IMU\sensor system at t0
|
||||||
// Note: delta_vel_in_t0 is already in body frame, so no need to use the body_P_sensor transformation here.
|
// Note: delta_vel_in_t0 is already in body frame, so no need to use the body_P_sensor transformation here.
|
||||||
|
|
||||||
return delta_pos_in_t0 + delta_vel_in_t0 * msr_dt;
|
return delta_pos_in_t0 + delta_vel_in_t0 * msr_dt;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt,
|
static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt,
|
||||||
const Vector3& delta_angles, const Vector& delta_vel_in_t0, const bool flag_use_body_P_sensor, const POSE& body_P_sensor){
|
const Vector3& delta_angles, const Vector& delta_vel_in_t0, const bool flag_use_body_P_sensor, const POSE& body_P_sensor){
|
||||||
|
|
||||||
// Note: all delta terms refer to an IMU\sensor system at t0
|
// Note: all delta terms refer to an IMU\sensor system at t0
|
||||||
|
|
||||||
// Calculate the corrected measurements using the Bias object
|
// Calculate the corrected measurements using the Bias object
|
||||||
Vector AccCorrected = msr_acc_t;
|
Vector AccCorrected = msr_acc_t;
|
||||||
Vector body_t_a_body;
|
Vector body_t_a_body;
|
||||||
if (flag_use_body_P_sensor){
|
if (flag_use_body_P_sensor){
|
||||||
Matrix body_R_sensor = body_P_sensor.rotation().matrix();
|
Matrix body_R_sensor = body_P_sensor.rotation().matrix();
|
||||||
|
|
||||||
Vector GyroCorrected(msr_gyro_t);
|
Vector GyroCorrected(msr_gyro_t);
|
||||||
|
|
||||||
Vector body_omega_body = body_R_sensor * GyroCorrected;
|
Vector body_omega_body = body_R_sensor * GyroCorrected;
|
||||||
Matrix body_omega_body__cross = skewSymmetric(body_omega_body);
|
Matrix body_omega_body__cross = skewSymmetric(body_omega_body);
|
||||||
|
|
||||||
body_t_a_body = body_R_sensor * AccCorrected - body_omega_body__cross * body_omega_body__cross * body_P_sensor.translation().vector();
|
body_t_a_body = body_R_sensor * AccCorrected - body_omega_body__cross * body_omega_body__cross * body_P_sensor.translation().vector();
|
||||||
} else{
|
} else{
|
||||||
body_t_a_body = AccCorrected;
|
body_t_a_body = AccCorrected;
|
||||||
}
|
}
|
||||||
|
|
||||||
Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
|
Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
|
||||||
|
|
||||||
return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt;
|
return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt,
|
static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt,
|
||||||
const Vector3& delta_angles, const bool flag_use_body_P_sensor, const POSE& body_P_sensor){
|
const Vector3& delta_angles, const bool flag_use_body_P_sensor, const POSE& body_P_sensor){
|
||||||
|
|
||||||
// Note: all delta terms refer to an IMU\sensor system at t0
|
// Note: all delta terms refer to an IMU\sensor system at t0
|
||||||
|
|
||||||
// Calculate the corrected measurements using the Bias object
|
// Calculate the corrected measurements using the Bias object
|
||||||
Vector GyroCorrected = msr_gyro_t;
|
Vector GyroCorrected = msr_gyro_t;
|
||||||
|
|
||||||
Vector body_t_omega_body;
|
Vector body_t_omega_body;
|
||||||
if (flag_use_body_P_sensor){
|
if (flag_use_body_P_sensor){
|
||||||
body_t_omega_body = body_P_sensor.rotation().matrix() * GyroCorrected;
|
body_t_omega_body = body_P_sensor.rotation().matrix() * GyroCorrected;
|
||||||
} else {
|
} else {
|
||||||
body_t_omega_body = GyroCorrected;
|
body_t_omega_body = GyroCorrected;
|
||||||
}
|
}
|
||||||
|
|
||||||
Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
|
Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
|
||||||
|
|
||||||
R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt );
|
R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt );
|
||||||
return Rot3::Logmap(R_t_to_t0);
|
return Rot3::Logmap(R_t_to_t0);
|
||||||
}
|
}
|
||||||
|
|
||||||
static inline noiseModel::Gaussian::shared_ptr CalcEquivalentNoiseCov(const noiseModel::Gaussian::shared_ptr& gaussian_acc, const noiseModel::Gaussian::shared_ptr& gaussian_gyro,
|
static inline noiseModel::Gaussian::shared_ptr CalcEquivalentNoiseCov(const noiseModel::Gaussian::shared_ptr& gaussian_acc, const noiseModel::Gaussian::shared_ptr& gaussian_gyro,
|
||||||
const noiseModel::Gaussian::shared_ptr& gaussian_process){
|
const noiseModel::Gaussian::shared_ptr& gaussian_process){
|
||||||
|
|
||||||
Matrix cov_acc = inverse( gaussian_acc->R().transpose() * gaussian_acc->R() );
|
Matrix cov_acc = inverse( gaussian_acc->R().transpose() * gaussian_acc->R() );
|
||||||
Matrix cov_gyro = inverse( gaussian_gyro->R().transpose() * gaussian_gyro->R() );
|
Matrix cov_gyro = inverse( gaussian_gyro->R().transpose() * gaussian_gyro->R() );
|
||||||
Matrix cov_process = inverse( gaussian_process->R().transpose() * gaussian_process->R() );
|
Matrix cov_process = inverse( gaussian_process->R().transpose() * gaussian_process->R() );
|
||||||
|
|
||||||
cov_process.block(0,0, 3,3) += cov_gyro;
|
cov_process.block(0,0, 3,3) += cov_gyro;
|
||||||
cov_process.block(6,6, 3,3) += cov_acc;
|
cov_process.block(6,6, 3,3) += cov_acc;
|
||||||
|
|
||||||
return noiseModel::Gaussian::Covariance(cov_process);
|
return noiseModel::Gaussian::Covariance(cov_process);
|
||||||
}
|
}
|
||||||
|
|
||||||
static inline void CalcEquivalentNoiseCov_DifferentParts(const noiseModel::Gaussian::shared_ptr& gaussian_acc, const noiseModel::Gaussian::shared_ptr& gaussian_gyro,
|
static inline void CalcEquivalentNoiseCov_DifferentParts(const noiseModel::Gaussian::shared_ptr& gaussian_acc, const noiseModel::Gaussian::shared_ptr& gaussian_gyro,
|
||||||
const noiseModel::Gaussian::shared_ptr& gaussian_process,
|
const noiseModel::Gaussian::shared_ptr& gaussian_process,
|
||||||
|
|
@ -478,107 +478,107 @@ public:
|
||||||
cov_process_without_acc_gyro = inverse( gaussian_process->R().transpose() * gaussian_process->R() );
|
cov_process_without_acc_gyro = inverse( gaussian_process->R().transpose() * gaussian_process->R() );
|
||||||
}
|
}
|
||||||
|
|
||||||
static inline void Calc_g_rho_omega_earth_NED(const Vector& Pos_NED, const Vector& Vel_NED, const Vector& LatLonHeight_IC, const Vector& Pos_NED_Initial,
|
static inline void Calc_g_rho_omega_earth_NED(const Vector& Pos_NED, const Vector& Vel_NED, const Vector& LatLonHeight_IC, const Vector& Pos_NED_Initial,
|
||||||
Vector& g_NED, Vector& rho_NED, Vector& omega_earth_NED) {
|
Vector& g_NED, Vector& rho_NED, Vector& omega_earth_NED) {
|
||||||
|
|
||||||
Matrix ENU_to_NED = Matrix_(3, 3,
|
Matrix ENU_to_NED = Matrix_(3, 3,
|
||||||
0.0, 1.0, 0.0,
|
0.0, 1.0, 0.0,
|
||||||
1.0, 0.0, 0.0,
|
1.0, 0.0, 0.0,
|
||||||
0.0, 0.0, -1.0);
|
0.0, 0.0, -1.0);
|
||||||
|
|
||||||
Matrix NED_to_ENU = Matrix_(3, 3,
|
Matrix NED_to_ENU = Matrix_(3, 3,
|
||||||
0.0, 1.0, 0.0,
|
0.0, 1.0, 0.0,
|
||||||
1.0, 0.0, 0.0,
|
1.0, 0.0, 0.0,
|
||||||
0.0, 0.0, -1.0);
|
0.0, 0.0, -1.0);
|
||||||
|
|
||||||
// Convert incoming parameters to ENU
|
// Convert incoming parameters to ENU
|
||||||
Vector Pos_ENU = NED_to_ENU * Pos_NED;
|
Vector Pos_ENU = NED_to_ENU * Pos_NED;
|
||||||
Vector Vel_ENU = NED_to_ENU * Vel_NED;
|
Vector Vel_ENU = NED_to_ENU * Vel_NED;
|
||||||
Vector Pos_ENU_Initial = NED_to_ENU * Pos_NED_Initial;
|
Vector Pos_ENU_Initial = NED_to_ENU * Pos_NED_Initial;
|
||||||
|
|
||||||
// Call ENU version
|
// Call ENU version
|
||||||
Vector g_ENU;
|
Vector g_ENU;
|
||||||
Vector rho_ENU;
|
Vector rho_ENU;
|
||||||
Vector omega_earth_ENU;
|
Vector omega_earth_ENU;
|
||||||
Calc_g_rho_omega_earth_ENU(Pos_ENU, Vel_ENU, LatLonHeight_IC, Pos_ENU_Initial, g_ENU, rho_ENU, omega_earth_ENU);
|
Calc_g_rho_omega_earth_ENU(Pos_ENU, Vel_ENU, LatLonHeight_IC, Pos_ENU_Initial, g_ENU, rho_ENU, omega_earth_ENU);
|
||||||
|
|
||||||
// Convert output to NED
|
// Convert output to NED
|
||||||
g_NED = ENU_to_NED * g_ENU;
|
g_NED = ENU_to_NED * g_ENU;
|
||||||
rho_NED = ENU_to_NED * rho_ENU;
|
rho_NED = ENU_to_NED * rho_ENU;
|
||||||
omega_earth_NED = ENU_to_NED * omega_earth_ENU;
|
omega_earth_NED = ENU_to_NED * omega_earth_ENU;
|
||||||
}
|
}
|
||||||
|
|
||||||
static inline void Calc_g_rho_omega_earth_ENU(const Vector& Pos_ENU, const Vector& Vel_ENU, const Vector& LatLonHeight_IC, const Vector& Pos_ENU_Initial,
|
static inline void Calc_g_rho_omega_earth_ENU(const Vector& Pos_ENU, const Vector& Vel_ENU, const Vector& LatLonHeight_IC, const Vector& Pos_ENU_Initial,
|
||||||
Vector& g_ENU, Vector& rho_ENU, Vector& omega_earth_ENU){
|
Vector& g_ENU, Vector& rho_ENU, Vector& omega_earth_ENU){
|
||||||
double R0 = 6.378388e6;
|
double R0 = 6.378388e6;
|
||||||
double e = 1/297;
|
double e = 1/297;
|
||||||
double Re( R0*( 1-e*(sin( LatLonHeight_IC(0) ))*(sin( LatLonHeight_IC(0) )) ) );
|
double Re( R0*( 1-e*(sin( LatLonHeight_IC(0) ))*(sin( LatLonHeight_IC(0) )) ) );
|
||||||
|
|
||||||
// Calculate current lat, lon
|
// Calculate current lat, lon
|
||||||
Vector delta_Pos_ENU(Pos_ENU - Pos_ENU_Initial);
|
Vector delta_Pos_ENU(Pos_ENU - Pos_ENU_Initial);
|
||||||
double delta_lat(delta_Pos_ENU(1)/Re);
|
double delta_lat(delta_Pos_ENU(1)/Re);
|
||||||
double delta_lon(delta_Pos_ENU(0)/(Re*cos(LatLonHeight_IC(0))));
|
double delta_lon(delta_Pos_ENU(0)/(Re*cos(LatLonHeight_IC(0))));
|
||||||
double lat_new(LatLonHeight_IC(0) + delta_lat);
|
double lat_new(LatLonHeight_IC(0) + delta_lat);
|
||||||
double lon_new(LatLonHeight_IC(1) + delta_lon);
|
double lon_new(LatLonHeight_IC(1) + delta_lon);
|
||||||
|
|
||||||
// Rotation of lon about z axis
|
// Rotation of lon about z axis
|
||||||
Rot3 C1(cos(lon_new), sin(lon_new), 0.0,
|
Rot3 C1(cos(lon_new), sin(lon_new), 0.0,
|
||||||
-sin(lon_new), cos(lon_new), 0.0,
|
-sin(lon_new), cos(lon_new), 0.0,
|
||||||
0.0, 0.0, 1.0);
|
0.0, 0.0, 1.0);
|
||||||
|
|
||||||
// Rotation of lat about y axis
|
// Rotation of lat about y axis
|
||||||
Rot3 C2(cos(lat_new), 0.0, sin(lat_new),
|
Rot3 C2(cos(lat_new), 0.0, sin(lat_new),
|
||||||
0.0, 1.0, 0.0,
|
0.0, 1.0, 0.0,
|
||||||
-sin(lat_new), 0.0, cos(lat_new));
|
-sin(lat_new), 0.0, cos(lat_new));
|
||||||
|
|
||||||
Rot3 UEN_to_ENU(0, 1, 0,
|
Rot3 UEN_to_ENU(0, 1, 0,
|
||||||
0, 0, 1,
|
0, 0, 1,
|
||||||
1, 0, 0);
|
1, 0, 0);
|
||||||
|
|
||||||
Rot3 R_ECEF_to_ENU( UEN_to_ENU * C2 * C1 );
|
Rot3 R_ECEF_to_ENU( UEN_to_ENU * C2 * C1 );
|
||||||
|
|
||||||
Vector omega_earth_ECEF(Vector_(3, 0.0, 0.0, 7.292115e-5));
|
Vector omega_earth_ECEF(Vector_(3, 0.0, 0.0, 7.292115e-5));
|
||||||
omega_earth_ENU = R_ECEF_to_ENU.matrix() * omega_earth_ECEF;
|
omega_earth_ENU = R_ECEF_to_ENU.matrix() * omega_earth_ECEF;
|
||||||
|
|
||||||
// Calculating g
|
// Calculating g
|
||||||
double height(LatLonHeight_IC(2));
|
double height(LatLonHeight_IC(2));
|
||||||
double EQUA_RADIUS = 6378137.0; // equatorial radius of the earth; WGS-84
|
double EQUA_RADIUS = 6378137.0; // equatorial radius of the earth; WGS-84
|
||||||
double ECCENTRICITY = 0.0818191908426; // eccentricity of the earth ellipsoid
|
double ECCENTRICITY = 0.0818191908426; // eccentricity of the earth ellipsoid
|
||||||
double e2( pow(ECCENTRICITY,2) );
|
double e2( pow(ECCENTRICITY,2) );
|
||||||
double den( 1-e2*pow(sin(lat_new),2) );
|
double den( 1-e2*pow(sin(lat_new),2) );
|
||||||
double Rm( (EQUA_RADIUS*(1-e2))/( pow(den,(3/2)) ) );
|
double Rm( (EQUA_RADIUS*(1-e2))/( pow(den,(3/2)) ) );
|
||||||
double Rp( EQUA_RADIUS/( sqrt(den) ) );
|
double Rp( EQUA_RADIUS/( sqrt(den) ) );
|
||||||
double Ro( sqrt(Rp*Rm) ); // mean earth radius of curvature
|
double Ro( sqrt(Rp*Rm) ); // mean earth radius of curvature
|
||||||
double g0( 9.780318*( 1 + 5.3024e-3 * pow(sin(lat_new),2) - 5.9e-6 * pow(sin(2*lat_new),2) ) );
|
double g0( 9.780318*( 1 + 5.3024e-3 * pow(sin(lat_new),2) - 5.9e-6 * pow(sin(2*lat_new),2) ) );
|
||||||
double g_calc( g0/( pow(1 + height/Ro, 2) ) );
|
double g_calc( g0/( pow(1 + height/Ro, 2) ) );
|
||||||
g_ENU = Vector_(3, 0.0, 0.0, -g_calc);
|
g_ENU = Vector_(3, 0.0, 0.0, -g_calc);
|
||||||
|
|
||||||
|
|
||||||
// Calculate rho
|
// Calculate rho
|
||||||
double Ve( Vel_ENU(0) );
|
double Ve( Vel_ENU(0) );
|
||||||
double Vn( Vel_ENU(1) );
|
double Vn( Vel_ENU(1) );
|
||||||
double rho_E = -Vn/(Rm + height);
|
double rho_E = -Vn/(Rm + height);
|
||||||
double rho_N = Ve/(Rp + height);
|
double rho_N = Ve/(Rp + height);
|
||||||
double rho_U = Ve*tan(lat_new)/(Rp + height);
|
double rho_U = Ve*tan(lat_new)/(Rp + height);
|
||||||
rho_ENU = Vector_(3, rho_E, rho_N, rho_U);
|
rho_ENU = Vector_(3, rho_E, rho_N, rho_U);
|
||||||
}
|
}
|
||||||
|
|
||||||
static inline noiseModel::Gaussian::shared_ptr calc_descrete_noise_model(const noiseModel::Gaussian::shared_ptr& model, double delta_t){
|
static inline noiseModel::Gaussian::shared_ptr calc_descrete_noise_model(const noiseModel::Gaussian::shared_ptr& model, double delta_t){
|
||||||
/* Q_d (approx)= Q * delta_t */
|
/* Q_d (approx)= Q * delta_t */
|
||||||
/* In practice, square root of the information matrix is represented, so that:
|
/* In practice, square root of the information matrix is represented, so that:
|
||||||
* R_d (approx)= R / sqrt(delta_t)
|
* R_d (approx)= R / sqrt(delta_t)
|
||||||
* */
|
* */
|
||||||
return noiseModel::Gaussian::SqrtInformation(model->R()/sqrt(delta_t));
|
return noiseModel::Gaussian::SqrtInformation(model->R()/sqrt(delta_t));
|
||||||
}
|
}
|
||||||
private:
|
private:
|
||||||
|
|
||||||
/** Serialization function */
|
/** Serialization function */
|
||||||
friend class boost::serialization::access;
|
friend class boost::serialization::access;
|
||||||
template<class ARCHIVE>
|
template<class ARCHIVE>
|
||||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||||
ar & boost::serialization::make_nvp("NonlinearFactor2",
|
ar & boost::serialization::make_nvp("NonlinearFactor2",
|
||||||
boost::serialization::base_object<Base>(*this));
|
boost::serialization::base_object<Base>(*this));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -44,11 +44,11 @@ namespace gtsam {
|
||||||
* matrices and the process\modeling covariance matrix. The IneritalNavFactor converts this into a
|
* matrices and the process\modeling covariance matrix. The IneritalNavFactor converts this into a
|
||||||
* discrete form using the supplied delta_t between sub-sequential measurements.
|
* discrete form using the supplied delta_t between sub-sequential measurements.
|
||||||
* - Earth-rate correction:
|
* - Earth-rate correction:
|
||||||
* + Currently the user should supply R_ECEF_to_G, which is the rotation from ECEF to the global
|
* + Currently the user should supply R_ECEF_to_G, which is the rotation from ECEF to the global
|
||||||
* frame (Local-Level system: ENU or NED, see above).
|
* frame (Local-Level system: ENU or NED, see above).
|
||||||
* + R_ECEF_to_G can be calculated by approximated values of latitude and longitude of the system.
|
* + R_ECEF_to_G can be calculated by approximated values of latitude and longitude of the system.
|
||||||
* + Currently it is assumed that a relatively small distance is traveled w.r.t. to initial pose, since R_ECEF_to_G is constant.
|
* + Currently it is assumed that a relatively small distance is traveled w.r.t. to initial pose, since R_ECEF_to_G is constant.
|
||||||
* Otherwise, R_ECEF_to_G should be updated each time using the current lat-lon.
|
* Otherwise, R_ECEF_to_G should be updated each time using the current lat-lon.
|
||||||
*
|
*
|
||||||
* - Frame Notation:
|
* - Frame Notation:
|
||||||
* Quantities are written as {Frame of Representation/Destination Frame}_{Quantity Type}_{Quatity Description/Origination Frame}
|
* Quantities are written as {Frame of Representation/Destination Frame}_{Quantity Type}_{Quatity Description/Origination Frame}
|
||||||
|
|
@ -81,70 +81,70 @@ class InertialNavFactor_GlobalVelocity : public NoiseModelFactor5<POSE, VELOCITY
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
typedef InertialNavFactor_GlobalVelocity<POSE, VELOCITY, IMUBIAS> This;
|
typedef InertialNavFactor_GlobalVelocity<POSE, VELOCITY, IMUBIAS> This;
|
||||||
typedef NoiseModelFactor5<POSE, VELOCITY, IMUBIAS, POSE, VELOCITY> Base;
|
typedef NoiseModelFactor5<POSE, VELOCITY, IMUBIAS, POSE, VELOCITY> Base;
|
||||||
|
|
||||||
Vector measurement_acc_;
|
Vector measurement_acc_;
|
||||||
Vector measurement_gyro_;
|
Vector measurement_gyro_;
|
||||||
double dt_;
|
double dt_;
|
||||||
|
|
||||||
Vector world_g_;
|
Vector world_g_;
|
||||||
Vector world_rho_;
|
Vector world_rho_;
|
||||||
Vector world_omega_earth_;
|
Vector world_omega_earth_;
|
||||||
|
|
||||||
boost::optional<POSE> body_P_sensor_; // The pose of the sensor in the body frame
|
boost::optional<POSE> body_P_sensor_; // The pose of the sensor in the body frame
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// shorthand for a smart pointer to a factor
|
// shorthand for a smart pointer to a factor
|
||||||
typedef typename boost::shared_ptr<InertialNavFactor_GlobalVelocity> shared_ptr;
|
typedef typename boost::shared_ptr<InertialNavFactor_GlobalVelocity> shared_ptr;
|
||||||
|
|
||||||
/** default constructor - only use for serialization */
|
/** default constructor - only use for serialization */
|
||||||
InertialNavFactor_GlobalVelocity() {}
|
InertialNavFactor_GlobalVelocity() {}
|
||||||
|
|
||||||
/** Constructor */
|
/** Constructor */
|
||||||
InertialNavFactor_GlobalVelocity(const Key& Pose1, const Key& Vel1, const Key& IMUBias1, const Key& Pose2, const Key& Vel2,
|
InertialNavFactor_GlobalVelocity(const Key& Pose1, const Key& Vel1, const Key& IMUBias1, const Key& Pose2, const Key& Vel2,
|
||||||
const Vector& measurement_acc, const Vector& measurement_gyro, const double measurement_dt, const Vector world_g, const Vector world_rho,
|
const Vector& measurement_acc, const Vector& measurement_gyro, const double measurement_dt, const Vector world_g, const Vector world_rho,
|
||||||
const Vector& world_omega_earth, const noiseModel::Gaussian::shared_ptr& model_continuous, boost::optional<POSE> body_P_sensor = boost::none) :
|
const Vector& world_omega_earth, const noiseModel::Gaussian::shared_ptr& model_continuous, boost::optional<POSE> body_P_sensor = boost::none) :
|
||||||
Base(calc_descrete_noise_model(model_continuous, measurement_dt ),
|
Base(calc_descrete_noise_model(model_continuous, measurement_dt ),
|
||||||
Pose1, Vel1, IMUBias1, Pose2, Vel2), measurement_acc_(measurement_acc), measurement_gyro_(measurement_gyro),
|
Pose1, Vel1, IMUBias1, Pose2, Vel2), measurement_acc_(measurement_acc), measurement_gyro_(measurement_gyro),
|
||||||
dt_(measurement_dt), world_g_(world_g), world_rho_(world_rho), world_omega_earth_(world_omega_earth), body_P_sensor_(body_P_sensor) { }
|
dt_(measurement_dt), world_g_(world_g), world_rho_(world_rho), world_omega_earth_(world_omega_earth), body_P_sensor_(body_P_sensor) { }
|
||||||
|
|
||||||
virtual ~InertialNavFactor_GlobalVelocity() {}
|
virtual ~InertialNavFactor_GlobalVelocity() {}
|
||||||
|
|
||||||
/** implement functions needed for Testable */
|
/** implement functions needed for Testable */
|
||||||
|
|
||||||
/** print */
|
/** print */
|
||||||
virtual void print(const std::string& s = "InertialNavFactor_GlobalVelocity", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
|
virtual void print(const std::string& s = "InertialNavFactor_GlobalVelocity", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
|
||||||
std::cout << s << "("
|
std::cout << s << "("
|
||||||
<< keyFormatter(this->key1()) << ","
|
<< keyFormatter(this->key1()) << ","
|
||||||
<< keyFormatter(this->key2()) << ","
|
<< keyFormatter(this->key2()) << ","
|
||||||
<< keyFormatter(this->key3()) << ","
|
<< keyFormatter(this->key3()) << ","
|
||||||
<< keyFormatter(this->key4()) << ","
|
<< keyFormatter(this->key4()) << ","
|
||||||
<< keyFormatter(this->key5()) << "\n";
|
<< keyFormatter(this->key5()) << "\n";
|
||||||
std::cout << "acc measurement: " << this->measurement_acc_.transpose() << std::endl;
|
std::cout << "acc measurement: " << this->measurement_acc_.transpose() << std::endl;
|
||||||
std::cout << "gyro measurement: " << this->measurement_gyro_.transpose() << std::endl;
|
std::cout << "gyro measurement: " << this->measurement_gyro_.transpose() << std::endl;
|
||||||
std::cout << "dt: " << this->dt_ << std::endl;
|
std::cout << "dt: " << this->dt_ << std::endl;
|
||||||
std::cout << "gravity (in world frame): " << this->world_g_.transpose() << std::endl;
|
std::cout << "gravity (in world frame): " << this->world_g_.transpose() << std::endl;
|
||||||
std::cout << "craft rate (in world frame): " << this->world_rho_.transpose() << std::endl;
|
std::cout << "craft rate (in world frame): " << this->world_rho_.transpose() << std::endl;
|
||||||
std::cout << "earth's rotation (in world frame): " << this->world_omega_earth_.transpose() << std::endl;
|
std::cout << "earth's rotation (in world frame): " << this->world_omega_earth_.transpose() << std::endl;
|
||||||
if(this->body_P_sensor_)
|
if(this->body_P_sensor_)
|
||||||
this->body_P_sensor_->print(" sensor pose in body frame: ");
|
this->body_P_sensor_->print(" sensor pose in body frame: ");
|
||||||
this->noiseModel_->print(" noise model");
|
this->noiseModel_->print(" noise model");
|
||||||
}
|
}
|
||||||
|
|
||||||
/** equals */
|
/** equals */
|
||||||
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
|
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
|
||||||
const This *e = dynamic_cast<const This*> (&expected);
|
const This *e = dynamic_cast<const This*> (&expected);
|
||||||
return e != NULL && Base::equals(*e, tol)
|
return e != NULL && Base::equals(*e, tol)
|
||||||
&& (measurement_acc_ - e->measurement_acc_).norm() < tol
|
&& (measurement_acc_ - e->measurement_acc_).norm() < tol
|
||||||
&& (measurement_gyro_ - e->measurement_gyro_).norm() < tol
|
&& (measurement_gyro_ - e->measurement_gyro_).norm() < tol
|
||||||
&& (dt_ - e->dt_) < tol
|
&& (dt_ - e->dt_) < tol
|
||||||
&& (world_g_ - e->world_g_).norm() < tol
|
&& (world_g_ - e->world_g_).norm() < tol
|
||||||
&& (world_rho_ - e->world_rho_).norm() < tol
|
&& (world_rho_ - e->world_rho_).norm() < tol
|
||||||
&& (world_omega_earth_ - e->world_omega_earth_).norm() < tol
|
&& (world_omega_earth_ - e->world_omega_earth_).norm() < tol
|
||||||
&& ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
|
&& ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
|
||||||
}
|
}
|
||||||
|
|
||||||
POSE predictPose(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1) const {
|
POSE predictPose(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1) const {
|
||||||
// Calculate the corrected measurements using the Bias object
|
// Calculate the corrected measurements using the Bias object
|
||||||
|
|
@ -225,12 +225,12 @@ public:
|
||||||
}
|
}
|
||||||
|
|
||||||
/** implement functions needed to derive from Factor */
|
/** implement functions needed to derive from Factor */
|
||||||
Vector evaluateError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2,
|
Vector evaluateError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2,
|
||||||
boost::optional<Matrix&> H1 = boost::none,
|
boost::optional<Matrix&> H1 = boost::none,
|
||||||
boost::optional<Matrix&> H2 = boost::none,
|
boost::optional<Matrix&> H2 = boost::none,
|
||||||
boost::optional<Matrix&> H3 = boost::none,
|
boost::optional<Matrix&> H3 = boost::none,
|
||||||
boost::optional<Matrix&> H4 = boost::none,
|
boost::optional<Matrix&> H4 = boost::none,
|
||||||
boost::optional<Matrix&> H5 = boost::none) const {
|
boost::optional<Matrix&> H5 = boost::none) const {
|
||||||
|
|
||||||
// TODO: Write analytical derivative calculations
|
// TODO: Write analytical derivative calculations
|
||||||
// Jacobian w.r.t. Pose1
|
// Jacobian w.r.t. Pose1
|
||||||
|
|
@ -268,24 +268,24 @@ public:
|
||||||
*H5 = stack(2, &H5_Pose, &H5_Vel);
|
*H5 = stack(2, &H5_Pose, &H5_Vel);
|
||||||
}
|
}
|
||||||
|
|
||||||
Vector ErrPoseVector(POSE::Logmap(evaluatePoseError(Pose1, Vel1, Bias1, Pose2, Vel2)));
|
Vector ErrPoseVector(POSE::Logmap(evaluatePoseError(Pose1, Vel1, Bias1, Pose2, Vel2)));
|
||||||
Vector ErrVelVector(VELOCITY::Logmap(evaluateVelocityError(Pose1, Vel1, Bias1, Pose2, Vel2)));
|
Vector ErrVelVector(VELOCITY::Logmap(evaluateVelocityError(Pose1, Vel1, Bias1, Pose2, Vel2)));
|
||||||
|
|
||||||
return concatVectors(2, &ErrPoseVector, &ErrVelVector);
|
return concatVectors(2, &ErrPoseVector, &ErrVelVector);
|
||||||
}
|
}
|
||||||
|
|
||||||
static inline noiseModel::Gaussian::shared_ptr CalcEquivalentNoiseCov(const noiseModel::Gaussian::shared_ptr& gaussian_acc, const noiseModel::Gaussian::shared_ptr& gaussian_gyro,
|
static inline noiseModel::Gaussian::shared_ptr CalcEquivalentNoiseCov(const noiseModel::Gaussian::shared_ptr& gaussian_acc, const noiseModel::Gaussian::shared_ptr& gaussian_gyro,
|
||||||
const noiseModel::Gaussian::shared_ptr& gaussian_process){
|
const noiseModel::Gaussian::shared_ptr& gaussian_process){
|
||||||
|
|
||||||
Matrix cov_acc = inverse( gaussian_acc->R().transpose() * gaussian_acc->R() );
|
Matrix cov_acc = inverse( gaussian_acc->R().transpose() * gaussian_acc->R() );
|
||||||
Matrix cov_gyro = inverse( gaussian_gyro->R().transpose() * gaussian_gyro->R() );
|
Matrix cov_gyro = inverse( gaussian_gyro->R().transpose() * gaussian_gyro->R() );
|
||||||
Matrix cov_process = inverse( gaussian_process->R().transpose() * gaussian_process->R() );
|
Matrix cov_process = inverse( gaussian_process->R().transpose() * gaussian_process->R() );
|
||||||
|
|
||||||
cov_process.block(0,0, 3,3) += cov_gyro;
|
cov_process.block(0,0, 3,3) += cov_gyro;
|
||||||
cov_process.block(6,6, 3,3) += cov_acc;
|
cov_process.block(6,6, 3,3) += cov_acc;
|
||||||
|
|
||||||
return noiseModel::Gaussian::Covariance(cov_process);
|
return noiseModel::Gaussian::Covariance(cov_process);
|
||||||
}
|
}
|
||||||
|
|
||||||
static inline void Calc_g_rho_omega_earth_NED(const Vector& Pos_NED, const Vector& Vel_NED, const Vector& LatLonHeight_IC, const Vector& Pos_NED_Initial,
|
static inline void Calc_g_rho_omega_earth_NED(const Vector& Pos_NED, const Vector& Vel_NED, const Vector& LatLonHeight_IC, const Vector& Pos_NED_Initial,
|
||||||
Vector& g_NED, Vector& rho_NED, Vector& omega_earth_NED) {
|
Vector& g_NED, Vector& rho_NED, Vector& omega_earth_NED) {
|
||||||
|
|
@ -317,78 +317,78 @@ public:
|
||||||
omega_earth_NED = ENU_to_NED * omega_earth_ENU;
|
omega_earth_NED = ENU_to_NED * omega_earth_ENU;
|
||||||
}
|
}
|
||||||
|
|
||||||
static inline void Calc_g_rho_omega_earth_ENU(const Vector& Pos_ENU, const Vector& Vel_ENU, const Vector& LatLonHeight_IC, const Vector& Pos_ENU_Initial,
|
static inline void Calc_g_rho_omega_earth_ENU(const Vector& Pos_ENU, const Vector& Vel_ENU, const Vector& LatLonHeight_IC, const Vector& Pos_ENU_Initial,
|
||||||
Vector& g_ENU, Vector& rho_ENU, Vector& omega_earth_ENU){
|
Vector& g_ENU, Vector& rho_ENU, Vector& omega_earth_ENU){
|
||||||
double R0 = 6.378388e6;
|
double R0 = 6.378388e6;
|
||||||
double e = 1/297;
|
double e = 1/297;
|
||||||
double Re( R0*( 1-e*(sin( LatLonHeight_IC(0) ))*(sin( LatLonHeight_IC(0) )) ) );
|
double Re( R0*( 1-e*(sin( LatLonHeight_IC(0) ))*(sin( LatLonHeight_IC(0) )) ) );
|
||||||
|
|
||||||
// Calculate current lat, lon
|
// Calculate current lat, lon
|
||||||
Vector delta_Pos_ENU(Pos_ENU - Pos_ENU_Initial);
|
Vector delta_Pos_ENU(Pos_ENU - Pos_ENU_Initial);
|
||||||
double delta_lat(delta_Pos_ENU(1)/Re);
|
double delta_lat(delta_Pos_ENU(1)/Re);
|
||||||
double delta_lon(delta_Pos_ENU(0)/(Re*cos(LatLonHeight_IC(0))));
|
double delta_lon(delta_Pos_ENU(0)/(Re*cos(LatLonHeight_IC(0))));
|
||||||
double lat_new(LatLonHeight_IC(0) + delta_lat);
|
double lat_new(LatLonHeight_IC(0) + delta_lat);
|
||||||
double lon_new(LatLonHeight_IC(1) + delta_lon);
|
double lon_new(LatLonHeight_IC(1) + delta_lon);
|
||||||
|
|
||||||
// Rotation of lon about z axis
|
// Rotation of lon about z axis
|
||||||
Rot3 C1(cos(lon_new), sin(lon_new), 0.0,
|
Rot3 C1(cos(lon_new), sin(lon_new), 0.0,
|
||||||
-sin(lon_new), cos(lon_new), 0.0,
|
-sin(lon_new), cos(lon_new), 0.0,
|
||||||
0.0, 0.0, 1.0);
|
0.0, 0.0, 1.0);
|
||||||
|
|
||||||
// Rotation of lat about y axis
|
// Rotation of lat about y axis
|
||||||
Rot3 C2(cos(lat_new), 0.0, sin(lat_new),
|
Rot3 C2(cos(lat_new), 0.0, sin(lat_new),
|
||||||
0.0, 1.0, 0.0,
|
0.0, 1.0, 0.0,
|
||||||
-sin(lat_new), 0.0, cos(lat_new));
|
-sin(lat_new), 0.0, cos(lat_new));
|
||||||
|
|
||||||
Rot3 UEN_to_ENU(0, 1, 0,
|
Rot3 UEN_to_ENU(0, 1, 0,
|
||||||
0, 0, 1,
|
0, 0, 1,
|
||||||
1, 0, 0);
|
1, 0, 0);
|
||||||
|
|
||||||
Rot3 R_ECEF_to_ENU( UEN_to_ENU * C2 * C1 );
|
Rot3 R_ECEF_to_ENU( UEN_to_ENU * C2 * C1 );
|
||||||
|
|
||||||
Vector omega_earth_ECEF(Vector_(3, 0.0, 0.0, 7.292115e-5));
|
Vector omega_earth_ECEF(Vector_(3, 0.0, 0.0, 7.292115e-5));
|
||||||
omega_earth_ENU = R_ECEF_to_ENU.matrix() * omega_earth_ECEF;
|
omega_earth_ENU = R_ECEF_to_ENU.matrix() * omega_earth_ECEF;
|
||||||
|
|
||||||
// Calculating g
|
// Calculating g
|
||||||
double height(LatLonHeight_IC(2));
|
double height(LatLonHeight_IC(2));
|
||||||
double EQUA_RADIUS = 6378137.0; // equatorial radius of the earth; WGS-84
|
double EQUA_RADIUS = 6378137.0; // equatorial radius of the earth; WGS-84
|
||||||
double ECCENTRICITY = 0.0818191908426; // eccentricity of the earth ellipsoid
|
double ECCENTRICITY = 0.0818191908426; // eccentricity of the earth ellipsoid
|
||||||
double e2( pow(ECCENTRICITY,2) );
|
double e2( pow(ECCENTRICITY,2) );
|
||||||
double den( 1-e2*pow(sin(lat_new),2) );
|
double den( 1-e2*pow(sin(lat_new),2) );
|
||||||
double Rm( (EQUA_RADIUS*(1-e2))/( pow(den,(3/2)) ) );
|
double Rm( (EQUA_RADIUS*(1-e2))/( pow(den,(3/2)) ) );
|
||||||
double Rp( EQUA_RADIUS/( sqrt(den) ) );
|
double Rp( EQUA_RADIUS/( sqrt(den) ) );
|
||||||
double Ro( sqrt(Rp*Rm) ); // mean earth radius of curvature
|
double Ro( sqrt(Rp*Rm) ); // mean earth radius of curvature
|
||||||
double g0( 9.780318*( 1 + 5.3024e-3 * pow(sin(lat_new),2) - 5.9e-6 * pow(sin(2*lat_new),2) ) );
|
double g0( 9.780318*( 1 + 5.3024e-3 * pow(sin(lat_new),2) - 5.9e-6 * pow(sin(2*lat_new),2) ) );
|
||||||
double g_calc( g0/( pow(1 + height/Ro, 2) ) );
|
double g_calc( g0/( pow(1 + height/Ro, 2) ) );
|
||||||
g_ENU = Vector_(3, 0.0, 0.0, -g_calc);
|
g_ENU = Vector_(3, 0.0, 0.0, -g_calc);
|
||||||
|
|
||||||
|
|
||||||
// Calculate rho
|
// Calculate rho
|
||||||
double Ve( Vel_ENU(0) );
|
double Ve( Vel_ENU(0) );
|
||||||
double Vn( Vel_ENU(1) );
|
double Vn( Vel_ENU(1) );
|
||||||
double rho_E = -Vn/(Rm + height);
|
double rho_E = -Vn/(Rm + height);
|
||||||
double rho_N = Ve/(Rp + height);
|
double rho_N = Ve/(Rp + height);
|
||||||
double rho_U = Ve*tan(lat_new)/(Rp + height);
|
double rho_U = Ve*tan(lat_new)/(Rp + height);
|
||||||
rho_ENU = Vector_(3, rho_E, rho_N, rho_U);
|
rho_ENU = Vector_(3, rho_E, rho_N, rho_U);
|
||||||
}
|
}
|
||||||
|
|
||||||
static inline noiseModel::Gaussian::shared_ptr calc_descrete_noise_model(const noiseModel::Gaussian::shared_ptr& model, double delta_t){
|
static inline noiseModel::Gaussian::shared_ptr calc_descrete_noise_model(const noiseModel::Gaussian::shared_ptr& model, double delta_t){
|
||||||
/* Q_d (approx)= Q * delta_t */
|
/* Q_d (approx)= Q * delta_t */
|
||||||
/* In practice, square root of the information matrix is represented, so that:
|
/* In practice, square root of the information matrix is represented, so that:
|
||||||
* R_d (approx)= R / sqrt(delta_t)
|
* R_d (approx)= R / sqrt(delta_t)
|
||||||
* */
|
* */
|
||||||
return noiseModel::Gaussian::SqrtInformation(model->R()/std::sqrt(delta_t));
|
return noiseModel::Gaussian::SqrtInformation(model->R()/std::sqrt(delta_t));
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
/** Serialization function */
|
/** Serialization function */
|
||||||
friend class boost::serialization::access;
|
friend class boost::serialization::access;
|
||||||
template<class ARCHIVE>
|
template<class ARCHIVE>
|
||||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||||
ar & boost::serialization::make_nvp("NonlinearFactor2",
|
ar & boost::serialization::make_nvp("NonlinearFactor2",
|
||||||
boost::serialization::base_object<Base>(*this));
|
boost::serialization::base_object<Base>(*this));
|
||||||
}
|
}
|
||||||
|
|
||||||
}; // \class GaussMarkov1stOrderFactor
|
}; // \class GaussMarkov1stOrderFactor
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -92,8 +92,8 @@ TEST (AHRS, init) {
|
||||||
*/
|
*/
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
int main() {
|
int main() {
|
||||||
TestResult tr;
|
TestResult tr;
|
||||||
return TestRegistry::runAllTests(tr);
|
return TestRegistry::runAllTests(tr);
|
||||||
}
|
}
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -31,11 +31,11 @@ using namespace gtsam;
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
TEST( EquivInertialNavFactor_GlobalVel, Constructor)
|
TEST( EquivInertialNavFactor_GlobalVel, Constructor)
|
||||||
{
|
{
|
||||||
Key poseKey1(11);
|
Key poseKey1(11);
|
||||||
Key poseKey2(12);
|
Key poseKey2(12);
|
||||||
Key velKey1(21);
|
Key velKey1(21);
|
||||||
Key velKey2(22);
|
Key velKey2(22);
|
||||||
Key biasKey1(31);
|
Key biasKey1(31);
|
||||||
|
|
||||||
// IMU accumulation variables
|
// IMU accumulation variables
|
||||||
Vector delta_pos_in_t0 = Vector_(3, 0.0, 0.0, 0.0);
|
Vector delta_pos_in_t0 = Vector_(3, 0.0, 0.0, 0.0);
|
||||||
|
|
@ -46,16 +46,16 @@ TEST( EquivInertialNavFactor_GlobalVel, Constructor)
|
||||||
Matrix Jacobian_wrt_t0_Overall = eye(15);
|
Matrix Jacobian_wrt_t0_Overall = eye(15);
|
||||||
imuBias::ConstantBias bias1 = imuBias::ConstantBias();
|
imuBias::ConstantBias bias1 = imuBias::ConstantBias();
|
||||||
|
|
||||||
// Earth Terms (gravity, etc)
|
// Earth Terms (gravity, etc)
|
||||||
Vector3 g(0.0, 0.0, -9.80);
|
Vector3 g(0.0, 0.0, -9.80);
|
||||||
Vector3 rho(0.0, 0.0, 0.0);
|
Vector3 rho(0.0, 0.0, 0.0);
|
||||||
Vector3 omega_earth(0.0, 0.0, 0.0);
|
Vector3 omega_earth(0.0, 0.0, 0.0);
|
||||||
|
|
||||||
// IMU Noise Model
|
// IMU Noise Model
|
||||||
SharedGaussian imu_model = noiseModel::Gaussian::Covariance(EquivCov_Overall.block(0,0,9,9));
|
SharedGaussian imu_model = noiseModel::Gaussian::Covariance(EquivCov_Overall.block(0,0,9,9));
|
||||||
|
|
||||||
// Constructor
|
// Constructor
|
||||||
EquivInertialNavFactor_GlobalVel<Pose3, LieVector, imuBias::ConstantBias> factor(
|
EquivInertialNavFactor_GlobalVel<Pose3, LieVector, imuBias::ConstantBias> factor(
|
||||||
poseKey1, velKey1, biasKey1, poseKey2, velKey2,
|
poseKey1, velKey1, biasKey1, poseKey2, velKey2,
|
||||||
delta_pos_in_t0, delta_vel_in_t0, delta_angles, delta_t,
|
delta_pos_in_t0, delta_vel_in_t0, delta_angles, delta_t,
|
||||||
g, rho, omega_earth, imu_model, Jacobian_wrt_t0_Overall, bias1);
|
g, rho, omega_earth, imu_model, Jacobian_wrt_t0_Overall, bias1);
|
||||||
|
|
@ -63,5 +63,5 @@ TEST( EquivInertialNavFactor_GlobalVel, Constructor)
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
|
||||||
|
|
@ -30,9 +30,9 @@ using namespace std;
|
||||||
using namespace gtsam;
|
using namespace gtsam;
|
||||||
|
|
||||||
gtsam::Rot3 world_R_ECEF(
|
gtsam::Rot3 world_R_ECEF(
|
||||||
0.31686, 0.51505, 0.79645,
|
0.31686, 0.51505, 0.79645,
|
||||||
0.85173, -0.52399, 0,
|
0.85173, -0.52399, 0,
|
||||||
0.41733, 0.67835, -0.60471);
|
0.41733, 0.67835, -0.60471);
|
||||||
|
|
||||||
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5));
|
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5));
|
||||||
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
|
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
|
||||||
|
|
@ -49,24 +49,24 @@ gtsam::LieVector predictionErrorVel(const Pose3& p1, const LieVector& v1, const
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
TEST( InertialNavFactor_GlobalVelocity, Constructor)
|
TEST( InertialNavFactor_GlobalVelocity, Constructor)
|
||||||
{
|
{
|
||||||
gtsam::Key Pose1(11);
|
gtsam::Key Pose1(11);
|
||||||
gtsam::Key Pose2(12);
|
gtsam::Key Pose2(12);
|
||||||
gtsam::Key Vel1(21);
|
gtsam::Key Vel1(21);
|
||||||
gtsam::Key Vel2(22);
|
gtsam::Key Vel2(22);
|
||||||
gtsam::Key Bias1(31);
|
gtsam::Key Bias1(31);
|
||||||
|
|
||||||
Vector measurement_acc(Vector_(3,0.1,0.2,0.4));
|
Vector measurement_acc(Vector_(3,0.1,0.2,0.4));
|
||||||
Vector measurement_gyro(Vector_(3, -0.2, 0.5, 0.03));
|
Vector measurement_gyro(Vector_(3, -0.2, 0.5, 0.03));
|
||||||
|
|
||||||
double measurement_dt(0.1);
|
double measurement_dt(0.1);
|
||||||
Vector world_g(Vector_(3, 0.0, 0.0, 9.81));
|
Vector world_g(Vector_(3, 0.0, 0.0, 9.81));
|
||||||
Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system
|
Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system
|
||||||
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5));
|
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5));
|
||||||
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
|
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
|
||||||
|
|
||||||
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
|
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
|
||||||
|
|
||||||
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
|
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
@ -78,20 +78,20 @@ TEST( InertialNavFactor_GlobalVelocity, Equals)
|
||||||
gtsam::Key Vel2(22);
|
gtsam::Key Vel2(22);
|
||||||
gtsam::Key Bias1(31);
|
gtsam::Key Bias1(31);
|
||||||
|
|
||||||
Vector measurement_acc(Vector_(3,0.1,0.2,0.4));
|
Vector measurement_acc(Vector_(3,0.1,0.2,0.4));
|
||||||
Vector measurement_gyro(Vector_(3, -0.2, 0.5, 0.03));
|
Vector measurement_gyro(Vector_(3, -0.2, 0.5, 0.03));
|
||||||
|
|
||||||
double measurement_dt(0.1);
|
double measurement_dt(0.1);
|
||||||
Vector world_g(Vector_(3, 0.0, 0.0, 9.81));
|
Vector world_g(Vector_(3, 0.0, 0.0, 9.81));
|
||||||
Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system
|
Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system
|
||||||
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5));
|
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5));
|
||||||
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
|
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
|
||||||
|
|
||||||
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
|
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
|
||||||
|
|
||||||
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
|
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
|
||||||
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> g(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
|
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> g(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
|
||||||
CHECK(assert_equal(f, g, 1e-5));
|
CHECK(assert_equal(f, g, 1e-5));
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
@ -140,31 +140,31 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVel)
|
||||||
gtsam::Key VelKey2(22);
|
gtsam::Key VelKey2(22);
|
||||||
gtsam::Key BiasKey1(31);
|
gtsam::Key BiasKey1(31);
|
||||||
|
|
||||||
double measurement_dt(0.1);
|
double measurement_dt(0.1);
|
||||||
Vector world_g(Vector_(3, 0.0, 0.0, 9.81));
|
Vector world_g(Vector_(3, 0.0, 0.0, 9.81));
|
||||||
Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system
|
Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system
|
||||||
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5));
|
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5));
|
||||||
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
|
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
|
||||||
|
|
||||||
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
|
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
|
||||||
|
|
||||||
|
|
||||||
// First test: zero angular motion, some acceleration
|
// First test: zero angular motion, some acceleration
|
||||||
Vector measurement_acc(Vector_(3,0.1,0.2,0.3-9.81));
|
Vector measurement_acc(Vector_(3,0.1,0.2,0.3-9.81));
|
||||||
Vector measurement_gyro(Vector_(3, 0.0, 0.0, 0.0));
|
Vector measurement_gyro(Vector_(3, 0.0, 0.0, 0.0));
|
||||||
|
|
||||||
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
|
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
|
||||||
|
|
||||||
Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00));
|
Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00));
|
||||||
Pose3 Pose2(Rot3(), Point3(2.05, 0.95, 3.04));
|
Pose3 Pose2(Rot3(), Point3(2.05, 0.95, 3.04));
|
||||||
LieVector Vel1(3, 0.50, -0.50, 0.40);
|
LieVector Vel1(3, 0.50, -0.50, 0.40);
|
||||||
LieVector Vel2(3, 0.51, -0.48, 0.43);
|
LieVector Vel2(3, 0.51, -0.48, 0.43);
|
||||||
imuBias::ConstantBias Bias1;
|
imuBias::ConstantBias Bias1;
|
||||||
|
|
||||||
Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
|
Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
|
||||||
Vector ExpectedErr(zero(9));
|
Vector ExpectedErr(zero(9));
|
||||||
|
|
||||||
CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5));
|
CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5));
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
@ -176,30 +176,30 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRot)
|
||||||
gtsam::Key VelKey2(22);
|
gtsam::Key VelKey2(22);
|
||||||
gtsam::Key BiasKey1(31);
|
gtsam::Key BiasKey1(31);
|
||||||
|
|
||||||
double measurement_dt(0.1);
|
double measurement_dt(0.1);
|
||||||
Vector world_g(Vector_(3, 0.0, 0.0, 9.81));
|
Vector world_g(Vector_(3, 0.0, 0.0, 9.81));
|
||||||
Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system
|
Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system
|
||||||
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5));
|
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5));
|
||||||
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
|
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
|
||||||
|
|
||||||
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
|
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
|
||||||
|
|
||||||
// Second test: zero angular motion, some acceleration
|
// Second test: zero angular motion, some acceleration
|
||||||
Vector measurement_acc(Vector_(3,0.0,0.0,0.0-9.81));
|
Vector measurement_acc(Vector_(3,0.0,0.0,0.0-9.81));
|
||||||
Vector measurement_gyro(Vector_(3, 0.1, 0.2, 0.3));
|
Vector measurement_gyro(Vector_(3, 0.1, 0.2, 0.3));
|
||||||
|
|
||||||
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
|
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
|
||||||
|
|
||||||
Pose3 Pose1(Rot3(), Point3(2.0,1.0,3.0));
|
Pose3 Pose1(Rot3(), Point3(2.0,1.0,3.0));
|
||||||
Pose3 Pose2(Rot3::Expmap(measurement_gyro*measurement_dt), Point3(2.0,1.0,3.0));
|
Pose3 Pose2(Rot3::Expmap(measurement_gyro*measurement_dt), Point3(2.0,1.0,3.0));
|
||||||
LieVector Vel1(3,0.0,0.0,0.0);
|
LieVector Vel1(3,0.0,0.0,0.0);
|
||||||
LieVector Vel2(3,0.0,0.0,0.0);
|
LieVector Vel2(3,0.0,0.0,0.0);
|
||||||
imuBias::ConstantBias Bias1;
|
imuBias::ConstantBias Bias1;
|
||||||
|
|
||||||
Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
|
Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
|
||||||
Vector ExpectedErr(zero(9));
|
Vector ExpectedErr(zero(9));
|
||||||
|
|
||||||
CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5));
|
CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5));
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
@ -211,67 +211,67 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVel)
|
||||||
gtsam::Key VelKey2(22);
|
gtsam::Key VelKey2(22);
|
||||||
gtsam::Key BiasKey1(31);
|
gtsam::Key BiasKey1(31);
|
||||||
|
|
||||||
double measurement_dt(0.1);
|
double measurement_dt(0.1);
|
||||||
Vector world_g(Vector_(3, 0.0, 0.0, 9.81));
|
Vector world_g(Vector_(3, 0.0, 0.0, 9.81));
|
||||||
Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system
|
Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system
|
||||||
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5));
|
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5));
|
||||||
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
|
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
|
||||||
|
|
||||||
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
|
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
|
||||||
|
|
||||||
// Second test: zero angular motion, some acceleration - generated in matlab
|
// Second test: zero angular motion, some acceleration - generated in matlab
|
||||||
Vector measurement_acc(Vector_(3, 6.501390843381716, -6.763926150509185, -2.300389940090343));
|
Vector measurement_acc(Vector_(3, 6.501390843381716, -6.763926150509185, -2.300389940090343));
|
||||||
Vector measurement_gyro(Vector_(3, 0.1, 0.2, 0.3));
|
Vector measurement_gyro(Vector_(3, 0.1, 0.2, 0.3));
|
||||||
|
|
||||||
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
|
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
|
||||||
|
|
||||||
Rot3 R1(0.487316618, 0.125253866, 0.86419557,
|
Rot3 R1(0.487316618, 0.125253866, 0.86419557,
|
||||||
0.580273724, 0.693095498, -0.427669306,
|
0.580273724, 0.693095498, -0.427669306,
|
||||||
-0.652537293, 0.709880342, 0.265075427);
|
-0.652537293, 0.709880342, 0.265075427);
|
||||||
Point3 t1(2.0,1.0,3.0);
|
Point3 t1(2.0,1.0,3.0);
|
||||||
Pose3 Pose1(R1, t1);
|
Pose3 Pose1(R1, t1);
|
||||||
LieVector Vel1(3,0.5,-0.5,0.4);
|
LieVector Vel1(3,0.5,-0.5,0.4);
|
||||||
Rot3 R2(0.473618898, 0.119523052, 0.872582019,
|
Rot3 R2(0.473618898, 0.119523052, 0.872582019,
|
||||||
0.609241153, 0.67099888, -0.422594037,
|
0.609241153, 0.67099888, -0.422594037,
|
||||||
-0.636011287, 0.731761397, 0.244979388);
|
-0.636011287, 0.731761397, 0.244979388);
|
||||||
Point3 t2 = t1.compose( Point3(Vel1*measurement_dt) );
|
Point3 t2 = t1.compose( Point3(Vel1*measurement_dt) );
|
||||||
Pose3 Pose2(R2, t2);
|
Pose3 Pose2(R2, t2);
|
||||||
Vector dv = measurement_dt * (R1.matrix() * measurement_acc + world_g);
|
Vector dv = measurement_dt * (R1.matrix() * measurement_acc + world_g);
|
||||||
LieVector Vel2 = Vel1.compose( dv );
|
LieVector Vel2 = Vel1.compose( dv );
|
||||||
imuBias::ConstantBias Bias1;
|
imuBias::ConstantBias Bias1;
|
||||||
|
|
||||||
Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
|
Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
|
||||||
Vector ExpectedErr(zero(9));
|
Vector ExpectedErr(zero(9));
|
||||||
|
|
||||||
// TODO: Expected values need to be updated for global velocity version
|
// TODO: Expected values need to be updated for global velocity version
|
||||||
CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5));
|
CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
///* VADIM - START ************************************************************************* */
|
///* VADIM - START ************************************************************************* */
|
||||||
//LieVector predictionRq(const LieVector angles, const LieVector q) {
|
//LieVector predictionRq(const LieVector angles, const LieVector q) {
|
||||||
// return (Rot3().RzRyRx(angles) * q).vector();
|
// return (Rot3().RzRyRx(angles) * q).vector();
|
||||||
//}
|
//}
|
||||||
//
|
//
|
||||||
//TEST (InertialNavFactor_GlobalVelocity, Rotation_Deriv ) {
|
//TEST (InertialNavFactor_GlobalVelocity, Rotation_Deriv ) {
|
||||||
// LieVector angles(Vector_(3, 3.001, -1.0004, 2.0005));
|
// LieVector angles(Vector_(3, 3.001, -1.0004, 2.0005));
|
||||||
// Rot3 R1(Rot3().RzRyRx(angles));
|
// Rot3 R1(Rot3().RzRyRx(angles));
|
||||||
// LieVector q(Vector_(3, 5.8, -2.2, 4.105));
|
// LieVector q(Vector_(3, 5.8, -2.2, 4.105));
|
||||||
// Rot3 qx(0.0, -q[2], q[1],
|
// Rot3 qx(0.0, -q[2], q[1],
|
||||||
// q[2], 0.0, -q[0],
|
// q[2], 0.0, -q[0],
|
||||||
// -q[1], q[0],0.0);
|
// -q[1], q[0],0.0);
|
||||||
// Matrix J_hyp( -(R1*qx).matrix() );
|
// Matrix J_hyp( -(R1*qx).matrix() );
|
||||||
//
|
//
|
||||||
// gtsam::Matrix J_expected;
|
// gtsam::Matrix J_expected;
|
||||||
//
|
//
|
||||||
// LieVector v(predictionRq(angles, q));
|
// LieVector v(predictionRq(angles, q));
|
||||||
//
|
//
|
||||||
// J_expected = gtsam::numericalDerivative11<LieVector, LieVector>(boost::bind(&predictionRq, _1, q), angles);
|
// J_expected = gtsam::numericalDerivative11<LieVector, LieVector>(boost::bind(&predictionRq, _1, q), angles);
|
||||||
//
|
//
|
||||||
// cout<<"J_hyp"<<J_hyp<<endl;
|
// cout<<"J_hyp"<<J_hyp<<endl;
|
||||||
// cout<<"J_expected"<<J_expected<<endl;
|
// cout<<"J_expected"<<J_expected<<endl;
|
||||||
//
|
//
|
||||||
// CHECK( gtsam::assert_equal(J_expected, J_hyp, 1e-6));
|
// CHECK( gtsam::assert_equal(J_expected, J_hyp, 1e-6));
|
||||||
//}
|
//}
|
||||||
///* VADIM - END ************************************************************************* */
|
///* VADIM - END ************************************************************************* */
|
||||||
|
|
||||||
|
|
@ -284,82 +284,82 @@ TEST (InertialNavFactor_GlobalVelocity, Jacobian ) {
|
||||||
gtsam::Key VelKey2(22);
|
gtsam::Key VelKey2(22);
|
||||||
gtsam::Key BiasKey1(31);
|
gtsam::Key BiasKey1(31);
|
||||||
|
|
||||||
double measurement_dt(0.01);
|
double measurement_dt(0.01);
|
||||||
Vector world_g(Vector_(3, 0.0, 0.0, 9.81));
|
Vector world_g(Vector_(3, 0.0, 0.0, 9.81));
|
||||||
Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system
|
Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system
|
||||||
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5));
|
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5));
|
||||||
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
|
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
|
||||||
|
|
||||||
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
|
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
|
||||||
|
|
||||||
Vector measurement_acc(Vector_(3, 6.501390843381716, -6.763926150509185, -2.300389940090343));
|
Vector measurement_acc(Vector_(3, 6.501390843381716, -6.763926150509185, -2.300389940090343));
|
||||||
Vector measurement_gyro(Vector_(3, 3.14, 3.14/2, -3.14));
|
Vector measurement_gyro(Vector_(3, 3.14, 3.14/2, -3.14));
|
||||||
|
|
||||||
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> factor(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
|
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> factor(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
|
||||||
|
|
||||||
Rot3 R1(0.487316618, 0.125253866, 0.86419557,
|
Rot3 R1(0.487316618, 0.125253866, 0.86419557,
|
||||||
0.580273724, 0.693095498, -0.427669306,
|
0.580273724, 0.693095498, -0.427669306,
|
||||||
-0.652537293, 0.709880342, 0.265075427);
|
-0.652537293, 0.709880342, 0.265075427);
|
||||||
Point3 t1(2.0,1.0,3.0);
|
Point3 t1(2.0,1.0,3.0);
|
||||||
Pose3 Pose1(R1, t1);
|
Pose3 Pose1(R1, t1);
|
||||||
LieVector Vel1(3,0.5,-0.5,0.4);
|
LieVector Vel1(3,0.5,-0.5,0.4);
|
||||||
Rot3 R2(0.473618898, 0.119523052, 0.872582019,
|
Rot3 R2(0.473618898, 0.119523052, 0.872582019,
|
||||||
0.609241153, 0.67099888, -0.422594037,
|
0.609241153, 0.67099888, -0.422594037,
|
||||||
-0.636011287, 0.731761397, 0.244979388);
|
-0.636011287, 0.731761397, 0.244979388);
|
||||||
Point3 t2(2.052670960415706, 0.977252139079380, 2.942482135362800);
|
Point3 t2(2.052670960415706, 0.977252139079380, 2.942482135362800);
|
||||||
Pose3 Pose2(R2, t2);
|
Pose3 Pose2(R2, t2);
|
||||||
LieVector Vel2(3,0.510000000000000, -0.480000000000000, 0.430000000000000);
|
LieVector Vel2(3,0.510000000000000, -0.480000000000000, 0.430000000000000);
|
||||||
imuBias::ConstantBias Bias1;
|
imuBias::ConstantBias Bias1;
|
||||||
|
|
||||||
Matrix H1_actual, H2_actual, H3_actual, H4_actual, H5_actual;
|
Matrix H1_actual, H2_actual, H3_actual, H4_actual, H5_actual;
|
||||||
|
|
||||||
Vector ActualErr(factor.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2, H1_actual, H2_actual, H3_actual, H4_actual, H5_actual));
|
Vector ActualErr(factor.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2, H1_actual, H2_actual, H3_actual, H4_actual, H5_actual));
|
||||||
|
|
||||||
// Checking for Pose part in the jacobians
|
// Checking for Pose part in the jacobians
|
||||||
// ******
|
// ******
|
||||||
Matrix H1_actualPose(H1_actual.block(0,0,6,H1_actual.cols()));
|
Matrix H1_actualPose(H1_actual.block(0,0,6,H1_actual.cols()));
|
||||||
Matrix H2_actualPose(H2_actual.block(0,0,6,H2_actual.cols()));
|
Matrix H2_actualPose(H2_actual.block(0,0,6,H2_actual.cols()));
|
||||||
Matrix H3_actualPose(H3_actual.block(0,0,6,H3_actual.cols()));
|
Matrix H3_actualPose(H3_actual.block(0,0,6,H3_actual.cols()));
|
||||||
Matrix H4_actualPose(H4_actual.block(0,0,6,H4_actual.cols()));
|
Matrix H4_actualPose(H4_actual.block(0,0,6,H4_actual.cols()));
|
||||||
Matrix H5_actualPose(H5_actual.block(0,0,6,H5_actual.cols()));
|
Matrix H5_actualPose(H5_actual.block(0,0,6,H5_actual.cols()));
|
||||||
|
|
||||||
// Calculate the Jacobian matrices H1 until H5 using the numerical derivative function
|
// Calculate the Jacobian matrices H1 until H5 using the numerical derivative function
|
||||||
gtsam::Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose, H5_expectedPose;
|
gtsam::Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose, H5_expectedPose;
|
||||||
H1_expectedPose = gtsam::numericalDerivative11<Pose3, Pose3>(boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1);
|
H1_expectedPose = gtsam::numericalDerivative11<Pose3, Pose3>(boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1);
|
||||||
H2_expectedPose = gtsam::numericalDerivative11<Pose3, LieVector>(boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1);
|
H2_expectedPose = gtsam::numericalDerivative11<Pose3, LieVector>(boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1);
|
||||||
H3_expectedPose = gtsam::numericalDerivative11<Pose3, imuBias::ConstantBias>(boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1);
|
H3_expectedPose = gtsam::numericalDerivative11<Pose3, imuBias::ConstantBias>(boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1);
|
||||||
H4_expectedPose = gtsam::numericalDerivative11<Pose3, Pose3>(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2);
|
H4_expectedPose = gtsam::numericalDerivative11<Pose3, Pose3>(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2);
|
||||||
H5_expectedPose = gtsam::numericalDerivative11<Pose3, LieVector>(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2);
|
H5_expectedPose = gtsam::numericalDerivative11<Pose3, LieVector>(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2);
|
||||||
|
|
||||||
// Verify they are equal for this choice of state
|
// Verify they are equal for this choice of state
|
||||||
CHECK( gtsam::assert_equal(H1_expectedPose, H1_actualPose, 1e-5));
|
CHECK( gtsam::assert_equal(H1_expectedPose, H1_actualPose, 1e-5));
|
||||||
CHECK( gtsam::assert_equal(H2_expectedPose, H2_actualPose, 1e-5));
|
CHECK( gtsam::assert_equal(H2_expectedPose, H2_actualPose, 1e-5));
|
||||||
CHECK( gtsam::assert_equal(H3_expectedPose, H3_actualPose, 2e-3));
|
CHECK( gtsam::assert_equal(H3_expectedPose, H3_actualPose, 2e-3));
|
||||||
CHECK( gtsam::assert_equal(H4_expectedPose, H4_actualPose, 1e-5));
|
CHECK( gtsam::assert_equal(H4_expectedPose, H4_actualPose, 1e-5));
|
||||||
CHECK( gtsam::assert_equal(H5_expectedPose, H5_actualPose, 1e-5));
|
CHECK( gtsam::assert_equal(H5_expectedPose, H5_actualPose, 1e-5));
|
||||||
|
|
||||||
// Checking for Vel part in the jacobians
|
// Checking for Vel part in the jacobians
|
||||||
// ******
|
// ******
|
||||||
Matrix H1_actualVel(H1_actual.block(6,0,3,H1_actual.cols()));
|
Matrix H1_actualVel(H1_actual.block(6,0,3,H1_actual.cols()));
|
||||||
Matrix H2_actualVel(H2_actual.block(6,0,3,H2_actual.cols()));
|
Matrix H2_actualVel(H2_actual.block(6,0,3,H2_actual.cols()));
|
||||||
Matrix H3_actualVel(H3_actual.block(6,0,3,H3_actual.cols()));
|
Matrix H3_actualVel(H3_actual.block(6,0,3,H3_actual.cols()));
|
||||||
Matrix H4_actualVel(H4_actual.block(6,0,3,H4_actual.cols()));
|
Matrix H4_actualVel(H4_actual.block(6,0,3,H4_actual.cols()));
|
||||||
Matrix H5_actualVel(H5_actual.block(6,0,3,H5_actual.cols()));
|
Matrix H5_actualVel(H5_actual.block(6,0,3,H5_actual.cols()));
|
||||||
|
|
||||||
// Calculate the Jacobian matrices H1 until H5 using the numerical derivative function
|
// Calculate the Jacobian matrices H1 until H5 using the numerical derivative function
|
||||||
gtsam::Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel, H5_expectedVel;
|
gtsam::Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel, H5_expectedVel;
|
||||||
H1_expectedVel = gtsam::numericalDerivative11<LieVector, Pose3>(boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1);
|
H1_expectedVel = gtsam::numericalDerivative11<LieVector, Pose3>(boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1);
|
||||||
H2_expectedVel = gtsam::numericalDerivative11<LieVector, LieVector>(boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1);
|
H2_expectedVel = gtsam::numericalDerivative11<LieVector, LieVector>(boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1);
|
||||||
H3_expectedVel = gtsam::numericalDerivative11<LieVector, imuBias::ConstantBias>(boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1);
|
H3_expectedVel = gtsam::numericalDerivative11<LieVector, imuBias::ConstantBias>(boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1);
|
||||||
H4_expectedVel = gtsam::numericalDerivative11<LieVector, Pose3>(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2);
|
H4_expectedVel = gtsam::numericalDerivative11<LieVector, Pose3>(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2);
|
||||||
H5_expectedVel = gtsam::numericalDerivative11<LieVector, LieVector>(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2);
|
H5_expectedVel = gtsam::numericalDerivative11<LieVector, LieVector>(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2);
|
||||||
|
|
||||||
// Verify they are equal for this choice of state
|
// Verify they are equal for this choice of state
|
||||||
CHECK( gtsam::assert_equal(H1_expectedVel, H1_actualVel, 1e-5));
|
CHECK( gtsam::assert_equal(H1_expectedVel, H1_actualVel, 1e-5));
|
||||||
CHECK( gtsam::assert_equal(H2_expectedVel, H2_actualVel, 1e-5));
|
CHECK( gtsam::assert_equal(H2_expectedVel, H2_actualVel, 1e-5));
|
||||||
CHECK( gtsam::assert_equal(H3_expectedVel, H3_actualVel, 1e-5));
|
CHECK( gtsam::assert_equal(H3_expectedVel, H3_actualVel, 1e-5));
|
||||||
CHECK( gtsam::assert_equal(H4_expectedVel, H4_actualVel, 1e-5));
|
CHECK( gtsam::assert_equal(H4_expectedVel, H4_actualVel, 1e-5));
|
||||||
CHECK( gtsam::assert_equal(H5_expectedVel, H5_actualVel, 1e-5));
|
CHECK( gtsam::assert_equal(H5_expectedVel, H5_actualVel, 1e-5));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -679,5 +679,5 @@ TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) {
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
|
||||||
|
|
@ -28,9 +28,9 @@ using namespace std;
|
||||||
using namespace gtsam;
|
using namespace gtsam;
|
||||||
|
|
||||||
gtsam::Rot3 world_R_ECEF(
|
gtsam::Rot3 world_R_ECEF(
|
||||||
0.31686, 0.51505, 0.79645,
|
0.31686, 0.51505, 0.79645,
|
||||||
0.85173, -0.52399, 0,
|
0.85173, -0.52399, 0,
|
||||||
0.41733, 0.67835, -0.60471);
|
0.41733, 0.67835, -0.60471);
|
||||||
|
|
||||||
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5));
|
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5));
|
||||||
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
|
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue