gtsam::PreintegratedAhrsMeasurements Class Reference

Detailed Description

PreintegratedAHRSMeasurements accumulates (integrates) the gyroscope measurements (rotation rates) and the corresponding covariance matrix.

Can be built incrementally so as to avoid costly integration at time of factor construction.

Mathematical Formulation

The preintegrated rotation is updated incrementally with each gyroscope measurement. Given a gyroscope measurement \( \omega_k \) at time \( t_k \), the preintegrated rotation \( \Delta R_{ij} \) from time \( t_i \) to \( t_j \) is the product of many small rotations:

\[\Delta R_{ij} = \prod_{k=i}^{j-1} \text{Exp}((\omega_k - b_g) \Delta t) \]

where \( b_g \) is the gyroscope bias, and \( \text{Exp}(\cdot) \) is the exponential map from \( \mathbb{R}^3 \) to SO(3).

This class also propagates the covariance of the preintegrated rotation.

Inheritance diagram for gtsam::PreintegratedAhrsMeasurements:

Public Member Functions
	PreintegratedAhrsMeasurements ()
	Default constructor, only for serialization and wrappers.
	PreintegratedAhrsMeasurements (const std::shared_ptr< Params > &p, const Vector3 &biasHat=Vector3::Zero())
	Default constructor, initialize with no measurements.
	PreintegratedAhrsMeasurements (const std::shared_ptr< Params > &p, const Vector3 &bias_hat, double deltaTij, const Rot3 &deltaRij, const Matrix3 &delRdelBiasOmega, const Matrix3 &preint_meas_cov)
	Non-Default constructor, initialize with measurements.
Params &	p () const
const Vector3 &	biasHat () const
const Matrix3 &	preintMeasCov () const
void	print (const std::string &s="Preintegrated Measurements: ") const
	print
bool	equals (const PreintegratedAhrsMeasurements &expected, double tol=1e-9) const
	equals
void	resetIntegration ()
	Reset integrated quantities to zero.
void	integrateMeasurement (const Vector3 &measuredOmega, double deltaT)
	Add a single gyroscope measurement to the preintegration.
Rot3	predict (const Rot3 &Ri, const Vector3 &bias, gtsam::OptionalJacobian< 3, 3 > H1={}, gtsam::OptionalJacobian< 3, 3 > H2={}) const
	Predict the orientation at time j, given orientation and bias at time i.
Public Member Functions inherited from gtsam::PreintegratedRotation
	PreintegratedRotation ()
	Default constructor for serialization.
	PreintegratedRotation (const std::shared_ptr< Params > &p)
	Default constructor, resets integration to zero.
	PreintegratedRotation (const std::shared_ptr< Params > &p, double deltaTij, const Rot3 &deltaRij, const Matrix3 &delRdelBiasOmega)
	Explicit initialization of all class members.
bool	matchesParamsWith (const PreintegratedRotation &other) const
	check parameters equality: checks whether shared pointer points to same Params object.
const std::shared_ptr< Params > &	params () const
const double &	deltaTij () const
const Rot3 &	deltaRij () const
const Matrix3 &	delRdelBiasOmega () const
void	print (const std::string &s) const
bool	equals (const PreintegratedRotation &other, double tol) const
void	resetIntegration ()
	Re-initialize PreintegratedMeasurements.
void	integrateGyroMeasurement (const Vector3 &measuredOmega, const Vector3 &biasHat, double deltaT, OptionalJacobian< 3, 3 > F={})
	Calculate an incremental rotation given the gyro measurement and a time interval, and update both deltaTij_ and deltaRij_.
Rot3	biascorrectedDeltaRij (const Vector3 &biasOmegaIncr, OptionalJacobian< 3, 3 > H={}) const
	Return a bias corrected version of the integrated rotation.

Protected Attributes
Vector3	biasHat_
	Angular rate bias values used during preintegration.
Matrix3	preintMeasCov_
	Covariance matrix of the preintegrated measurements (first-order propagation from measurementCovariance).
Protected Attributes inherited from gtsam::PreintegratedRotation
std::shared_ptr< Params >	p_
	Parameters.
double	deltaTij_
	Time interval from i to j.
Rot3	deltaRij_
	Preintegrated relative orientation (in frame i).
Matrix3	delRdelBiasOmega_
	Jacobian of preintegrated rotation w.r.t. angular rate bias.

Additional Inherited Members
Public Types inherited from gtsam::PreintegratedRotation
typedef PreintegratedRotationParams	Params

Constructor & Destructor Documentation

PreintegratedAhrsMeasurements() [1/2]

gtsam::PreintegratedAhrsMeasurements::PreintegratedAhrsMeasurements ( const std::shared_ptr< Params > & p, const Vector3 & biasHat = Vector3::Zero() )

inline

Default constructor, initialize with no measurements.

Parameters

bias	Current estimate of rotation rate biases

PreintegratedAhrsMeasurements() [2/2]

gtsam::PreintegratedAhrsMeasurements::PreintegratedAhrsMeasurements ( const std::shared_ptr< Params > & p, const Vector3 & bias_hat, double deltaTij, const Rot3 & deltaRij, const Matrix3 & delRdelBiasOmega, const Matrix3 & preint_meas_cov )

inline

Non-Default constructor, initialize with measurements.

Parameters

p	Parameters for AHRS pre-integration
bias_hat	Current estimate of rotation rate biases
deltaTij	Delta time in pre-integration
deltaRij	Delta rotation in pre-integration
delRdelBiasOmega	Jacobian of rotation wrt. to gyro bias
preint_meas_cov	Pre-integration covariance

Member Function Documentation

integrateMeasurement()

void gtsam::PreintegratedAhrsMeasurements::integrateMeasurement	(	const Vector3 &	measuredOmega,
		double	deltaT )

Add a single gyroscope measurement to the preintegration.

Measurements are taken to be in the sensor frame and conversion to the body frame is handled by body_P_sensor in PreintegratedRotationParams (if provided).

Parameters

measuredOmega	Measured angular velocity (as given by the sensor)
deltaT	Time step

predict()

Rot3 gtsam::PreintegratedAhrsMeasurements::predict ( const Rot3 & Ri, const Vector3 & bias, gtsam::OptionalJacobian< 3, 3 > H1 = {}, gtsam::OptionalJacobian< 3, 3 > H2 = {} ) const

inline

Predict the orientation at time j, given orientation and bias at time i.

Parameters

Ri	orientation at time i
bias	gyroscope bias
H1	optional 3x3 Jacobian wrt Ri
H2	optional 3x3 Jacobian wrt bias

Returns

predicted orientation at time j

---

The documentation for this class was generated from the following files:

• /tmp/gtsam-4.3.0-doxygen.rsXPUS/source/gtsam/navigation/AHRSFactor.h
• /tmp/gtsam-4.3.0-doxygen.rsXPUS/source/gtsam/navigation/AHRSFactor.cpp