Detailed Description

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

Extended Kalman Filter on a Lie group G, derived from ManifoldEKF.

Template Parameters

G	Lie group type (must satisfy LieGroup concept).

This filter specializes ManifoldEKF for Lie groups, offering predict methods with state-dependent dynamics functions. Use the InvariantEKF class for prediction via group composition. For details on how static and dynamic dimensions are handled, please refer to the ManifoldEKF class documentation.

Update API: inherited from ManifoldEKF (update(prediction, H, z, R), update(h, z, R), and updateWithVector).

Noise convention:

Overloads without dt (e.g., predict(X_next, F, Q) inherited from ManifoldEKF) expect Q to be a discrete covariance already scaled for the step being applied.
Overloads with dt interpret Q as a continuous-time covariance.

Inheritance diagram for gtsam::LieGroupEKF< G >:

Public Member Functions
	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 Attributes
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	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.

Constructor & Destructor Documentation

◆ LieGroupEKF()

template<typename G>

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

inline

Constructor: initialize with state and covariance.

Parameters

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

Member Function Documentation

◆ predict() [1/2]

template<typename G>

template<size_t K = 1, typename Control, typename Dynamics, typename = enable_if_full_dynamics<Control, Dynamics>>

void gtsam::LieGroupEKF< G >::predict	(	Dynamics &&	f,
		const Control &	u,
		double	dt,
		const Covariance &	Q )

inline

Predict step with state and control input dynamics: Wraps the dynamics function and calls the state-only predict.

xi = f(X_k, u, Df)

Template Parameters

K	Truncation order for the matrix exponential (K=1 recovers first-order).
Control	Control input type.
Dynamics	Functor signature: TangentVector f(const G&, const Control&, OptionalJacobian<Dim,Dim>&)

Parameters

f	Dynamics functor.
u	Control input.
dt	Time step.
Q	Continuous-time process noise covariance (will be scaled by dt).

◆ predict() [2/2]

template<typename G>

template<size_t K = 1, typename Dynamics, typename = enable_if_dynamics<Dynamics>>

void gtsam::LieGroupEKF< G >::predict	(	Dynamics &&	f,
		double	dt,
		const Covariance &	Q )

inline

Predict step with state-dependent dynamics: Uses predictMean to compute X_{k+1} and Phi, then updates covariance.

X_{k+1}, Phi = predictMean(f, dt) P_{k+1} = Phi P_k Phi^T + Q

Template Parameters

K	Truncation order for expm (K=1 for first-order).
Dynamics	Functor signature: TangentVector f(const G&, OptionalJacobian<Dim,Dim>&)

Parameters

f	Dynamics functor.
dt	Time step.
Q	Continuous-time process noise covariance (will be scaled by dt).

◆ predictMean() [1/2]

template<typename G>

template<size_t K = 1, typename Control, typename Dynamics, typename = enable_if_full_dynamics<Control, Dynamics>>

G gtsam::LieGroupEKF< G >::predictMean	(	Dynamics &&	f,
		const Control &	u,
		double	dt,
		OptionalJacobian< Dim, Dim >	Phi = {} ) const

inline

Predict mean and Jacobian A with state and control input dynamics: Wraps the dynamics function and calls the state-only predictMean.

xi = f(X_k, u, Df)

Template Parameters

K	Truncation order for expm (K=1 for first-order).
Control	Control input type.
Dynamics	Functor signature: TangentVector f(const G&, const Control&, OptionalJacobian<Dim,Dim>&)

Parameters

f	Dynamics functor.
u	Control input.
dt	Time step.
A	Optional pointer to store the computed state transition Jacobian A.

Returns: Predicted state X_{k+1}.

◆ predictMean() [2/2]

template<typename G>

template<size_t K = 1, typename Dynamics, typename = enable_if_dynamics<Dynamics>>

G gtsam::LieGroupEKF< G >::predictMean	(	Dynamics &&	f,
		double	dt,
		OptionalJacobian< Dim, Dim >	Phi = {} ) const

inline

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 Parameters

K	Truncation order for expm (K=1 for first-order).
Dynamics	TangentVector f(const G&, OptionalJacobian<Dim,Dim>&)

Parameters

f	Dynamics function computing tangent vector xi and its Jacobian Df.
dt	Time step.
Phi	OptionalJacobian to store the computed transition matrix Phi.

Returns: Predicted state X_{k+1}.

◆ predictWithCompose()

template<typename G>

void gtsam::LieGroupEKF< G >::predictWithCompose	(	const G &	U,
		const Jacobian &	J_UX,
		const Covariance &	Q )

inline

Predict using a precomputed group increment U and its Jacobian J_UX.

Contract:

Input state X_k on G with covariance P_k in local coordinates
Precomputed increment U = U(X_k) in G
Jacobian J_UX = d(u_left)/d(local(X)) at X_k, where u_left = Log(U)
Process noise Q expressed in the same coordinates as u_left

Update performed:

X_{k+1} = X_k ∘ U
A = Ad_{U^{-1}} + J_UX
P_{k+1} = A P_k A^T + Q

Notes: This API is intended for custom integrators that construct U(X) directly (e.g., second-order kinematics), which may not be expressible as an Euler step on the tangent used by predict/predictMean.

◆ transitionMatrix()

template<typename G>

template<size_t K = 1>

Jacobian gtsam::LieGroupEKF< G >::transitionMatrix	(	const TangentVector &	xi,
		const Jacobian &	Df,
		double	dt,
		const G &	U,
		const Jacobian &	Dexp ) const

inline

Compute the discrete-time transition matrix Φ corresponding to a continuous-time linearization (Df) over time dt.

Template Parameters

K	Truncation order for expm (K=1 for first-order).

Parameters

xi	Tangent increment (used only for Lie groups).
Df	Jacobian of dynamics w.r.t. local coordinates.
dt	Time step.
U	Increment Expmap(xi * dt) (ignored for Matrix case).
Dexp	Jacobian returned by Expmap (ignored for Matrix case).

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

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

Detailed Description

Public Member Functions

Static Public Attributes

Public Types

Additional Inherited Members

Constructor & Destructor Documentation

◆ LieGroupEKF()

Member Function Documentation

◆ predict() [1/2]

◆ predict() [2/2]

◆ predictMean() [1/2]

◆ predictMean() [2/2]

◆ predictWithCompose()

◆ transitionMatrix()