Detailed Description

template<typename G>
class gtsam::InvariantEKF< G >

Left-Invariant Extended Kalman Filter on a Lie group G.

Template Parameters

G	Lie group type (must satisfy LieGroup concept).

This filter inherits from LeftLinearEKF but restricts the prediction interface to only the left-invariant prediction methods:

Prediction via group composition: predict(const G& U, const Covariance& Q)
Prediction via tangent control vector: predict(const TangentVector& u, double dt, const Covariance& Q)

Noise convention mirrors LieGroupEKF:

predict(U, Q) expects Q to be a discrete covariance.
predict(u, dt, Q) interprets Q as continuous-time and scales by dt.

The state-dependent prediction methods from LeftLinearEKF are hidden. The update step remains the same as in ManifoldEKF/LeftLinearEKF. For details on how static and dynamic dimensions are handled, please refer to the ManifoldEKF class documentation.

Inheritance diagram for gtsam::InvariantEKF< G >:

Public Member Functions
	InvariantEKF (const G &X0, const Covariance &P0)
	Constructor: forwards to LeftLinearEKF constructor.
void	predict (const G &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 G &W, const G &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 G	Dynamics (const G &X, const G &U, OptionalJacobian< Dim, Dim > A={})
	Dynamics with W=I.
static G	Dynamics (const G &W, const G &X, const G &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.

Static Public Attributes
static constexpr int	Dim = Base::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.

Public Types
using	Base = LeftLinearEKF<G>
	Base class type.
using	TangentVector = typename Base::TangentVector
	Tangent vector type.
using	Jacobian = typename Base::Jacobian
	Jacobian for group-specific operations like AdjointMap.
using	Covariance = typename Base::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
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 Typedef Documentation

◆ Jacobian

template<typename G>

using gtsam::InvariantEKF< G >::Jacobian = typename Base::Jacobian

Jacobian for group-specific operations like AdjointMap.

Eigen::Matrix<double, Dim, Dim>.

Constructor & Destructor Documentation

◆ InvariantEKF()

template<typename G>

gtsam::InvariantEKF< G >::InvariantEKF	(	const G &	X0,
		const Covariance &	P0 )

inline

Constructor: forwards to LeftLinearEKF constructor.

Parameters

X0	Initial state on Lie group G.
P0	Initial covariance in the tangent space at X0.

Member Function Documentation

◆ Dynamics() [1/2]

template<typename G>

G gtsam::InvariantEKF< G >::Dynamics	(	const G &	W,
		const G &	X,
		const G &	U,
		OptionalJacobian< Dim, Dim >	A = {} )

inlinestatic

General dynamics.

Returns W · X · U, and optional Jacobian A = Ad_{U^{-1}}

◆ Dynamics() [2/2]

template<typename G>

G gtsam::InvariantEKF< G >::Dynamics	(	const G &	X,
		const G &	U,
		OptionalJacobian< Dim, Dim >	A = {} )

inlinestatic

Dynamics with W=I.

Returns φ(X) · U, and optional Jacobian A = Ad_{U^{-1}} Φ, Φ := dφ|_e.

◆ predict() [1/3]

template<typename G>

void gtsam::InvariantEKF< G >::predict	(	const G &	U,
		const Covariance &	Q )

inline

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

Parameters

U	Lie group element representing the motion increment.
Q	Process noise covariance.

◆ predict() [2/3]

template<typename G>

void gtsam::InvariantEKF< G >::predict	(	const G &	W,
		const G &	U,
		const Covariance &	Q )

inline

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

Parameters

W	Lie group element representing the motion increment in world frame.
U	Lie group element representing the motion increment in body frame.
Q	Process noise covariance.

◆ predict() [3/3]

template<typename G>

void gtsam::InvariantEKF< G >::predict	(	const TangentVector &	u,
		double	dt,
		const Covariance &	Q )

inline

Predict step via tangent control vector: U = Expmap(u * dt) Then calls predict(U, Q).

Parameters

u	Tangent space control vector.
dt	Time interval.
Q	Continuous-time process noise covariance matrix (scaled by dt).

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

/tmp/gtsam-4.3.0-doxygen.rsXPUS/source/gtsam/navigation/InvariantEKF.h

Detailed Description

Public Member Functions

Static Public Member Functions

Static Public Attributes

Public Types

Additional Inherited Members

Member Typedef Documentation

◆ Jacobian

Constructor & Destructor Documentation

◆ InvariantEKF()

Member Function Documentation

◆ Dynamics() [1/2]

◆ Dynamics() [2/2]

◆ predict() [1/3]

◆ predict() [2/3]

◆ predict() [3/3]