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

#include <LieGroupEKF.h>

Inheritance diagram for gtsam::LieGroupEKF< G >:

Public Types
using	Base = ManifoldEKF< G >
	Base class type. More...

using	Covariance = typename Base::Covariance

template<typename Dynamics >
using	enable_if_dynamics = std::enable_if_t< !std::is_convertible_v< Dynamics, TangentVector > &&std::is_invocable_r_v< TangentVector, Dynamics, const G &, OptionalJacobian< Dim, Dim > & > >

template<typename Control , typename Dynamics >
using	enable_if_full_dynamics = std::enable_if_t< std::is_invocable_r_v< TangentVector, Dynamics, const G &, const Control &, OptionalJacobian< Dim, Dim > & > >

using	Jacobian = typename Base::Jacobian
	Jacobian for group operations (Adjoint, Expmap derivatives, F). More...

using	TangentVector = typename Base::TangentVector

Public Types inherited from gtsam::ManifoldEKF< G >
using	Covariance = Eigen::Matrix< double, Dim, Dim >
	Covariance matrix type (P, Q). More...

using	Jacobian = Eigen::Matrix< double, Dim, Dim >
	State transition Jacobian type (F). More...

using	TangentVector = typename traits< G >::TangentVector
	Tangent vector type for the manifold M. More...

Public Member Functions
	LieGroupEKF (const G &X0, const Covariance &P0)

template<typename Control , typename Dynamics , typename = enable_if_full_dynamics<Control, Dynamics>>
void	predict (Dynamics &&f, const Control &u, double dt, const Covariance &Q)

template<typename Dynamics , typename = enable_if_dynamics<Dynamics>>
void	predict (Dynamics &&f, double dt, const Covariance &Q)

template<typename Control , typename Dynamics , typename = enable_if_full_dynamics<Control, Dynamics>>
G	predictMean (Dynamics &&f, const Control &u, double dt, OptionalJacobian< Dim, Dim > A={}) const

template<typename Dynamics , typename = enable_if_dynamics<Dynamics>>
G	predictMean (Dynamics &&f, double dt, OptionalJacobian< Dim, Dim > A={}) const

Public Member Functions inherited from gtsam::ManifoldEKF< G >
const Covariance &	covariance () const

int	dimension () const

	ManifoldEKF (const G &X0, const Covariance &P0)

void	predict (const G &X_next, const Jacobian &F, const Covariance &Q)

const G &	state () const

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)

void	update (MeasurementFunction &&h, const Measurement &z, const Eigen::Matrix< double, traits< Measurement >::dimension, traits< Measurement >::dimension > &R)

virtual	~ManifoldEKF ()=default

Static Public Attributes
static constexpr int	Dim = Base::Dim
	Compile-time dimension of G. More...

Static Public Attributes inherited from gtsam::ManifoldEKF< G >
static constexpr int	Dim
	Compile-time dimension of the manifold M. More...

Additional Inherited Members
Protected Attributes inherited from gtsam::ManifoldEKF< G >
int	n_
	Runtime tangent space dimension of M. More...

Covariance	P_
	Covariance (Eigen::Matrix<double, Dim, Dim>). More...

G	X_
	Manifold state estimate. More...

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.

Definition at line 49 of file LieGroupEKF.h.

Member Typedef Documentation

◆ Base

template<typename G >

using gtsam::LieGroupEKF< G >::Base = ManifoldEKF<G>

Base class type.

Definition at line 51 of file LieGroupEKF.h.

◆ Covariance

template<typename G >

using gtsam::LieGroupEKF< G >::Covariance = typename Base::Covariance

Definition at line 56 of file LieGroupEKF.h.

◆ enable_if_dynamics

template<typename G >

template<typename Dynamics >

using gtsam::LieGroupEKF< G >::enable_if_dynamics = std::enable_if_t< !std::is_convertible_v<Dynamics, TangentVector>&& std::is_invocable_r_v<TangentVector, Dynamics, const G&, OptionalJacobian<Dim, Dim>&> >

SFINAE check for correctly typed state-dependent dynamics function. Signature: TangentVector f(const G& X, OptionalJacobian<Dim, Dim> Df) Df = d(xi)/d(local(X))

Definition at line 76 of file LieGroupEKF.h.

◆ enable_if_full_dynamics

template<typename G >

template<typename Control , typename Dynamics >

using gtsam::LieGroupEKF< G >::enable_if_full_dynamics = std::enable_if_t< std::is_invocable_r_v<TangentVector, Dynamics, const G&, const Control&, OptionalJacobian<Dim, Dim>&> >

SFINAE check for state- and control-dependent dynamics function. Signature: TangentVector f(const G& X, const Control& u, OptionalJacobian<Dim, Dim> Df)

Definition at line 145 of file LieGroupEKF.h.

◆ Jacobian

template<typename G >

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

Jacobian for group operations (Adjoint, Expmap derivatives, F).

Definition at line 55 of file LieGroupEKF.h.

◆ TangentVector

template<typename G >

using gtsam::LieGroupEKF< G >::TangentVector = typename Base::TangentVector

Tangent vector type for G.

Definition at line 53 of file LieGroupEKF.h.

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.

Definition at line 63 of file LieGroupEKF.h.

Member Function Documentation

◆ predict() [1/2]

template<typename G >

template<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

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	Process noise covariance.

Definition at line 179 of file LieGroupEKF.h.

◆ predict() [2/2]

template<typename G >

template<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 A, then updates covariance. X_{k+1}, A = predictMean(f, dt) P_{k+1} = A P_k A^T + Q

Template Parameters

Dynamics Functor signature: TangentVector f(const G&, OptionalJacobian<Dim,Dim>&)

Parameters

f	Dynamics functor.
dt	Time step.
Q	Process noise covariance.

Definition at line 129 of file LieGroupEKF.h.

◆ predictMean() [1/2]

template<typename G >

template<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 >	A = `{}`
	)		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

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

Definition at line 161 of file LieGroupEKF.h.

◆ predictMean() [2/2]

template<typename G >

template<typename Dynamics , typename = enable_if_dynamics<Dynamics>>

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

inline

Predict mean and Jacobian A with state-dependent dynamics: xi = f(X_k, Df) (Compute tangent vector dynamics and Jacobian Df) U = Expmap(xi * dt, Dexp) (Compute motion increment U and Expmap Jacobian Dexp) X_{k+1} = X_k * U (Predict next state) A = Ad_{U^{-1}} + Dexp * Df * dt (Compute full state transition Jacobian)

Template Parameters

Dynamics Functor signature: TangentVector f(const G&, OptionalJacobian<Dim,Dim>&)

Parameters

f	Dynamics functor returning tangent vector xi and its Jacobian Df w.r.t. local(X).
dt	Time step.
A	OptionalJacobian to store the computed state transition Jacobian A.

Returns: Predicted state X_{k+1}.

Definition at line 92 of file LieGroupEKF.h.

Member Data Documentation

◆ Dim

template<typename G >

constexpr int gtsam::LieGroupEKF< G >::Dim = Base::Dim

staticconstexpr

Compile-time dimension of G.

Definition at line 52 of file LieGroupEKF.h.

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

LieGroupEKF.h

Public Types

Public Member Functions

Static Public Attributes

Additional Inherited Members

Detailed Description

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

Member Typedef Documentation

◆ Base

◆ Covariance

◆ enable_if_dynamics

◆ enable_if_full_dynamics

◆ Jacobian

◆ TangentVector

Constructor & Destructor Documentation

◆ LieGroupEKF()

Member Function Documentation

◆ predict() [1/2]

◆ predict() [2/2]

◆ predictMean() [1/2]

◆ predictMean() [2/2]

Member Data Documentation

◆ Dim

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