Detailed Description

Specialized EKF for IMU-driven on Gal3.

Inheritance diagram for gtsam::Gal3ImuEKF:

Public Member Functions
	Gal3ImuEKF (const Gal3 &X0, const Covariance &P0, const std::shared_ptr< PreintegrationParams > &params, Mode mode=TRACK_TIME_NO_COVARIANCE)
	Construct with initial state/covariance and preintegration params (for gravity and IMU covariances).
void	predict (const Vector3 &omega_b, const Vector3 &f_b, double dt)
	Predict the next state using gyro and accelerometer measurements.
const std::shared_ptr< PreintegrationParams > &	params () const
	Accessors.
const Vector3 &	gravity () const
const Covariance &	processNoise () const
Public Member Functions inherited from gtsam::InvariantEKF< Gal3 >
	InvariantEKF (const Gal3 &X0, const Covariance &P0)
	Constructor: forwards to LeftLinearEKF constructor.
void	predict (const Gal3 &U, const Covariance &Q)
	Predict step via group composition (Left-Invariant): X_{k+1} = X_k * U P_{k+1} = Ad_{U^{-1}} P_k Ad_{U^{-1}}^T + Q where Ad_{U^{-1}} is the Adjoint map of U^{-1}.
void	predict (const TangentVector &u, double dt, const Covariance &Q)
	Predict step via tangent control vector: U = Expmap(u * dt) Then calls predict(U, Q).
void	predict (const Gal3 &W, const Gal3 &U, const Covariance &Q)
	Predict step via left and right group composition (Left-Invariant): X_{k+1} = W * X_k * U P_{k+1} = Ad_{U^{-1}} P_k Ad_{U^{-1}}^T + Q where Ad_{U^{-1}} is the Adjoint map of U^{-1}.
Public Member Functions inherited from gtsam::LeftLinearEKF< G >
	LeftLinearEKF (const G &X0, const Covariance &P0)
template<class Phi, typename = std::enable_if_t<is_automorphism<Phi>::value>>
void	predict (const G &W, const Phi &phi, const G &U, const Covariance &Q)
	General left–linear prediction, updates filter state as follows: X⁺ = W · φ(X) · U P⁺ = A P Aᵀ + Q with A = Ad_{U^{-1}} Φ, Φ := dφ\|_e.
template<class Phi, typename = std::enable_if_t<is_automorphism<Phi>::value>>
void	predict (const Phi &phi, const G &U, const Covariance &Q)
	Special case of predict with W=I, updates filter state as follows: Update: X⁺ = φ(X) · U Covariance: P⁺ = A P Aᵀ + Q with A = Ad_{U^{-1}} Φ, Φ := dφ\|_e.
Public Member Functions inherited from gtsam::LieGroupEKF< G >
	LieGroupEKF (const G &X0, const Covariance &P0)
	Constructor: initialize with state and covariance.
template<size_t K = 1>
Jacobian	transitionMatrix (const TangentVector &xi, const Jacobian &Df, double dt, const G &U, const Jacobian &Dexp) const
	Compute the discrete-time transition matrix Φ corresponding to a continuous-time linearization (Df) over time dt.
template<size_t K = 1, typename Dynamics, typename = enable_if_dynamics<Dynamics>>
G	predictMean (Dynamics &&f, double dt, OptionalJacobian< Dim, Dim > Phi={}) const
	Predict mean and Jacobian Phi with state-dependent dynamics: xi = f(X_k, Df) (tangent vector dynamics and Jacobian Df) U = Expmap(xi * dt, Dexp) (motion increment U and Expmap Jacobian Dexp) X_{k+1} = X_k * U (Predict next state via compose) Phi = Ad_{U^{-1}} + Dexp * Df * dt (K=1 first-order Jacobian) expm((Df - ad(xi))dt) (K>1 matrix exponential discretization).
template<size_t K = 1, typename Dynamics, typename = enable_if_dynamics<Dynamics>>
void	predict (Dynamics &&f, double dt, const Covariance &Q)
	Predict step with state-dependent dynamics: Uses predictMean to compute X_{k+1} and Phi, then updates covariance.
template<size_t K = 1, typename Control, typename Dynamics, typename = enable_if_full_dynamics<Control, Dynamics>>
G	predictMean (Dynamics &&f, const Control &u, double dt, OptionalJacobian< Dim, Dim > Phi={}) const
	Predict mean and Jacobian A with state and control input dynamics: Wraps the dynamics function and calls the state-only predictMean.
template<size_t K = 1, typename Control, typename Dynamics, typename = enable_if_full_dynamics<Control, Dynamics>>
void	predict (Dynamics &&f, const Control &u, double dt, const Covariance &Q)
	Predict step with state and control input dynamics: Wraps the dynamics function and calls the state-only predict.
void	predictWithCompose (const G &U, const Jacobian &J_UX, const Covariance &Q)
	Predict using a precomputed group increment U and its Jacobian J_UX.
void	predict (const G &X_next, const Jacobian &F, const Covariance &Q)
	Expose base class predict method, predict(const M& X_next, const Jacobian& F, const Covariance& Q).
void	update (const Measurement &prediction, const Eigen::Matrix< double, traits< Measurement >::dimension, Dim > &H, const Measurement &z, const Eigen::Matrix< double, traits< Measurement >::dimension, traits< Measurement >::dimension > &R, bool performReset=true)
	Update overloads follow ManifoldEKF.
void	update (MeasurementFunction &&h, const Measurement &z, const Eigen::Matrix< double, traits< Measurement >::dimension, traits< Measurement >::dimension > &R, bool performReset=true)
	Update overloads follow ManifoldEKF.
Public Member Functions inherited from gtsam::ManifoldEKF< G >
	ManifoldEKF (const G &X0, const Covariance &P0)
	Constructor: initialize with state and covariance.
const G &	state () const
const Covariance &	covariance () const
size_t	dimension () const
void	predict (const G &X_next, const Jacobian &F, const Covariance &Q)
	Basic predict step: Updates state and covariance given the predicted next state and the state transition Jacobian F.
auto	KalmanGain (const HMatrix &H, const RMatrix &R) const
	Kalman gain K = P H^T S^-1.
void	JosephUpdate (const GainMatrix &K, const HMatrix &H, const RMatrix &R)
	Joseph-form covariance update in the current tangent space using a precomputed gain.
void	update (const Measurement &prediction, const Eigen::Matrix< double, traits< Measurement >::dimension, Dim > &H, const Measurement &z, const Eigen::Matrix< double, traits< Measurement >::dimension, traits< Measurement >::dimension > &R, bool performReset=true)
	Measurement update: Corrects the state and covariance using a pre-calculated predicted measurement and its Jacobian.
void	update (MeasurementFunction &&h, const Measurement &z, const Eigen::Matrix< double, traits< Measurement >::dimension, traits< Measurement >::dimension > &R, bool performReset=true)
	Measurement update: Corrects the state and covariance using a measurement model function.
void	updateWithVector (const gtsam::Vector &prediction, const Matrix &H, const gtsam::Vector &z, const Matrix &R, bool performReset=true)
	Convenience bridge for wrappers: vector measurement update calling update<Vector>.
void	reset (const TangentVector &eta)
	Reset step: retract the state by a tangent perturbation and, if available, transport the covariance from the old tangent space to the new tangent space.

Static Public Member Functions
static Gal3	Gravity (const Vector3 &g_n, double dt)
	Calculate gravity-only left composition, world-frame increments p = +1/2 g dt^2, v = g dt, t = 0.
static Gal3	TimeZeroingGravity (const Vector3 &g_n, double dt)
	Calculate W: gravity with correction to neutralize time change, Using this W(t_k) together with Imu() yields the exact dynamics update, but t stays 0, and hence we stay within NavState sub-group at all times.
static Gal3	CompensatedGravity (const Vector3 &g_n, double dt, double t_k)
	Calculate W: position-compensated gravity (left composition) that enables tracking absolute time in-state.
static Gal3	Imu (const Vector3 &omega_b, const Vector3 &f_b, double dt)
	Calculate U from raw IMU (no gravity): body-frame increments.
static Gal3	Dynamics (const Vector3 &g_n, const Gal3 &X, const Vector3 &omega_b, const Vector3 &f_b, double dt, Mode mode=TRACK_TIME_WITH_COVARIANCE, OptionalJacobian< 10, 10 > A={})
	Compute the dynamics of the system.
Static Public Member Functions inherited from gtsam::InvariantEKF< Gal3 >
static Gal3	Dynamics (const Gal3 &X, const Gal3 &U, OptionalJacobian< Dim, Dim > A={})
	Dynamics with W=I.
static Gal3	Dynamics (const Gal3 &W, const Gal3 &X, const Gal3 &U, OptionalJacobian< Dim, Dim > A={})
	General dynamics.
Static Public Member Functions inherited from gtsam::LeftLinearEKF< G >
template<class Phi, typename = std::enable_if_t<is_automorphism<Phi>::value>>
static G	Dynamics (const G &W, const Phi &phi, const G &X, const G &U, OptionalJacobian< Dim, Dim > A={})
	General left–linear dynamics.
template<class Phi, typename = std::enable_if_t<is_automorphism<Phi>::value>>
static G	Dynamics (const Phi &phi, const G &X, const G &U, OptionalJacobian< Dim, Dim > A={})
	Left–linear dynamics with W=I.

Public Types
enum	Mode { NO_TIME , TRACK_TIME_NO_COVARIANCE , TRACK_TIME_WITH_COVARIANCE }
	The Gal3 EKF has three modes of operation. More...
using	Base = InvariantEKF<Gal3>
using	TangentVector = typename Base::TangentVector
using	Jacobian = typename Base::Jacobian
using	Covariance = typename Base::Covariance
Public Types inherited from gtsam::InvariantEKF< Gal3 >
using	Base
	Base class type.
using	TangentVector
	Tangent vector type.
using	Jacobian
	Jacobian for group-specific operations like AdjointMap.
using	Covariance
	Covariance matrix type. Eigen::Matrix<double, Dim, Dim>.
Public Types inherited from gtsam::LeftLinearEKF< G >
using	Base = LieGroupEKF<G>
using	TangentVector = typename Base::TangentVector
using	Jacobian = typename Base::Jacobian
using	Covariance = typename Base::Covariance
Public Types inherited from gtsam::LieGroupEKF< G >
using	This = LieGroupEKF<G>
using	Base = ManifoldEKF<G>
	Base class type.
using	Jacobian = typename Base::Jacobian
	Dim x Dim.
using	Covariance = typename Base::Covariance
	Dim x Dim.
using	TangentVector = typename Base::TangentVector
	Tangent vector type.
Public Types inherited from gtsam::ManifoldEKF< G >
using	TangentVector
	Tangent vector type for the manifold M.
using	Covariance
	Covariance matrix type (P, Q).
using	Jacobian
	State transition Jacobian type (F).

Additional Inherited Members
Static Public Attributes inherited from gtsam::InvariantEKF< Gal3 >
static constexpr int	Dim
	Compile-time dimension of G.
Static Public Attributes inherited from gtsam::LeftLinearEKF< G >
static constexpr int	Dim = Base::Dim
	Compile-time dimension of G.
Static Public Attributes inherited from gtsam::LieGroupEKF< G >
static constexpr int	Dim = Base::Dim
	Compile-time dimension of G.
Static Public Attributes inherited from gtsam::ManifoldEKF< G >
static constexpr int	Dim
	Compile-time dimension of the manifold M.
Protected Member Functions inherited from gtsam::ManifoldEKF< G >
void	validateInputs (const gtsam::Vector &prediction, const Matrix &H, const gtsam::Vector &z, const Matrix &R)
	Validate inputs to update.
Static Protected Member Functions inherited from gtsam::ManifoldEKF< G >
static bool	isMatrixOfSize (const MatrixType &matrix, size_t rows, size_t cols)
	Check whether a matrix has the expected runtime dimensions.
Protected Attributes inherited from gtsam::ManifoldEKF< G >
G	X_
	Manifold state estimate.
Covariance	P_
	Covariance (Eigen::Matrix<double, Dim, Dim>).
Jacobian	I_
	Identity matrix sized to the state dimension.
size_t	n_
	Runtime tangent space dimension of M.

Member Enumeration Documentation

◆ Mode

enum gtsam::Gal3ImuEKF::Mode

The Gal3 EKF has three modes of operation.

Enumerator
NO_TIME	Do not track time, state remains in NavState sub-group.
TRACK_TIME_NO_COVARIANCE	Track time, but not its covariance (default).
TRACK_TIME_WITH_COVARIANCE	Track time and its covariance.

Constructor & Destructor Documentation

◆ Gal3ImuEKF()

gtsam::Gal3ImuEKF::Gal3ImuEKF	(	const Gal3 &	X0,
		const Covariance &	P0,
		const std::shared_ptr< PreintegrationParams > &	params,
		Mode	mode = TRACK_TIME_NO_COVARIANCE )

Construct with initial state/covariance and preintegration params (for gravity and IMU covariances).

Parameters

X0	Initial Gal3.
P0	Initial covariance in tangent space at X0.
params	Preintegration parameters providing gravity and options.

Member Function Documentation

◆ CompensatedGravity()

Gal3 gtsam::Gal3ImuEKF::CompensatedGravity	(	const Vector3 &	g_n,
		double	dt,
		double	t_k )

inlinestatic

Calculate W: position-compensated gravity (left composition) that enables tracking absolute time in-state.

Using this W(t_k) together with Imu() yields the exact dynamics update with additionally t_{k+1} = t_k + dt.

◆ Dynamics()

Gal3 gtsam::Gal3ImuEKF::Dynamics	(	const Vector3 &	g_n,
		const Gal3 &	X,
		const Vector3 &	omega_b,
		const Vector3 &	f_b,
		double	dt,
		Mode	mode = TRACK_TIME_WITH_COVARIANCE,
		OptionalJacobian< 10, 10 >	A = {} )

static

Compute the dynamics of the system.

This function computes the next state of the system based on the current state, gravity, body angular velocity, body specific force, and time step. The dynamics are defined as: X_{k+1} = f(X_k; g, omega_b, f_b, dt) = W(g, dt) \phi_dt(X_k) U(omega_b, f_b, dt) where W, \phi, and U are the gravity, (autonomous) position update, and IMU increment functions, respectively.

Parameters

g_n	Gravity vector in the navigation frame.
X	Current Gal3.
omega_b	Body angular velocity measurement (rad/s).
f_b	Body specific force measurement (m/s^2).
dt	Time step in seconds.
A	Optional Jacobian of the dynamics with respect to the state.

Returns: The next Gal3 after applying the dynamics.

◆ predict()

void gtsam::Gal3ImuEKF::predict	(	const Vector3 &	omega_b,
		const Vector3 &	f_b,
		double	dt )

Predict the next state using gyro and accelerometer measurements.

This method updates the state of the system based on the provided body angular velocity (omega_b) and body specific force (f_b) measurements over a given time step.

Parameters

omega_b	Body angular velocity measurement (rad/s).
f_b	Body specific force measurement (m/s^2).
dt	Time step in seconds.

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

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

Detailed Description

Public Member Functions

Static Public Member Functions

Public Types

Additional Inherited Members

Member Enumeration Documentation

◆ Mode

Constructor & Destructor Documentation

◆ Gal3ImuEKF()

Member Function Documentation

◆ CompensatedGravity()

◆ Dynamics()

◆ predict()