Detailed Description

template<typename M, typename Symmetry>
class gtsam::EquivariantFilter< M, Symmetry >

Equivariant Filter (EqF) for state estimation on Lie groups.

The EqF estimates a Lie group state X ∈ G and a manifold state ξ ∈ M. It uses a symmetry principle where the error dynamics are autonomous in a specific frame.

Both ActionType::Right and ActionType::Left symmetries are supported. The prediction increment acts at the current estimate, while the measurement correction acts at the reference. Their composition sides depend on the action.

Prediction comes in three forms:

Automatic: predict() calculates the Jacobian A from the input orbit.
Explicit A: predictWithJacobian() takes a continuous-time A and discretizes it.
Explicit transition: predictWithTransition() takes an already discretized Phi and Qd, for models derived directly in discrete time.

The filter propagates its error in error coordinates, the tangent space at the reference state xi_ref: the covariance P, the error dynamics matrix A and the process noise Qc all live there. Use covariance() to obtain the covariance in the tangent space at the current state estimate, and actionDifferential() for the map between the two frames.

Template Parameters

M	Manifold type for the physical state.
Symmetry	Functor encoding the group action on the state.

Inheritance diagram for gtsam::EquivariantFilter< M, Symmetry >:

Public Member Functions
	EquivariantFilter (const M &xi_ref, const CovarianceM &Sigma, const G &X0=traits< G >::Identity())
	Initialize the Equivariant Filter.
const Base::Covariance &	errorCovariance () const
	errorCovariance that returns P_, on the equivariant filter error
MatrixM	actionDifferential () const
	Differential of the group action at the reference state.
CovarianceM	covariance () const
	Covariance in the tangent space at the current state.
const G &	groupEstimate () const
template<typename Lift, typename InputOrbit>
MatrixM	computeErrorDynamicsMatrix (const InputOrbit &psi_u) const
	Compute the error dynamics matrix A (Automatic).
template<size_t K = 1>
MatrixM	transitionMatrix (const MatrixM &A, double dt) const
	Discretize continuous-time error dynamics δ̇ = A δ over dt.
template<size_t K = 1, typename Lift, typename InputOrbit>
void	predict (const Lift &lift_u, const InputOrbit &psi_u, const MatrixM &Qc, double dt)
	Propagate the filter state (Automatic).
template<size_t K = 1, typename Lift>
void	predictWithJacobian (const Lift &lift_u, const MatrixM &A, const MatrixM &Qc, double dt)
	Propagate the filter state (Explicit).
template<typename Lift>
void	predictWithTransition (const Lift &lift_u, const MatrixM &Phi, const CovarianceM &Qd, double dt)
	Propagate with an already-discretized transition and process noise.
template<typename Measurement>
void	update (const Measurement &prediction, const Eigen::Matrix< double, traits< Measurement >::dimension, DimM > &H, const Measurement &z, const Eigen::Matrix< double, traits< Measurement >::dimension, traits< Measurement >::dimension > &R)
	Measurement update: Corrects the state and covariance using a pre-calculated predicted measurement and its Jacobian.
template<typename Z, typename Func>
void	update (Func &&h, const Z &z, const Eigen::Matrix< double, traits< Z >::dimension, traits< Z >::dimension > &R)
	Same API as ManifoldEKF for measurement update with model function.
void	updateWithVector (const gtsam::Vector &prediction, const Matrix &H, const gtsam::Vector &z, const Matrix &R)
	Same API as ManifoldEKF for measurement update with vector inputs.
template<typename InnovationLiftFn>
void	updateWithVector (const gtsam::Vector &prediction, const Matrix &H, const gtsam::Vector &z, const Matrix &R, InnovationLiftFn &&innovationLift)
	Vector measurement update using a custom innovation lift delta_x=f(delta_xi).
const M &	state () const
	State on the manifold M is given by the base class.
Public Member Functions inherited from gtsam::ManifoldEKF< M >
	ManifoldEKF (const M &X0, const Covariance &P0)
	Constructor: initialize with state and covariance.
const M &	state () const
const Covariance &	covariance () const
size_t	dimension () const
void	predict (const M &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.
template<typename HMatrix, typename RMatrix>
auto	KalmanGain (const HMatrix &H, const RMatrix &R) const
	Kalman gain K = P H^T S^-1.
template<typename GainMatrix, typename HMatrix, typename RMatrix>
void	JosephUpdate (const GainMatrix &K, const HMatrix &H, const RMatrix &R)
	Joseph-form covariance update in the current tangent space using a precomputed gain.
template<typename Measurement>
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.
template<typename Measurement, typename MeasurementFunction>
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	DimM = Base::Dim
static constexpr int	DimG = traits<G>::dimension
Static Public Attributes inherited from gtsam::ManifoldEKF< M >
static constexpr int	Dim = traits<M>::dimension
	Compile-time dimension of the manifold M.

Public Types
using	Base = ManifoldEKF<M>
using	TangentM = typename Base::TangentVector
using	MatrixM = typename Base::Jacobian
using	CovarianceM = typename Base::Covariance
using	G = typename Symmetry::Group
using	TangentG = typename traits<G>::TangentVector
using	MatrixMG = Eigen::Matrix<double, DimM, DimG>
using	MatrixGM = Eigen::Matrix<double, DimG, DimM>
Public Types inherited from gtsam::ManifoldEKF< M >
using	TangentVector = typename traits<M>::TangentVector
	Tangent vector type for the manifold M.
using	Covariance = Eigen::Matrix<double, Dim, Dim>
	Covariance matrix type (P, Q).
using	Jacobian = Eigen::Matrix<double, Dim, Dim>
	State transition Jacobian type (F).

Protected Member Functions
void	resetReferenceAndGroup (const M &xi_ref, const CovarianceM &P, const G &g)
	Synchronization hooks for derived filters with dynamic runtime structure.
const M &	referenceState () const
	Access current reference state used as the EqF chart origin.
template<typename Lift, typename InputOrbit>
TangentG	liftAtOrigin (const InputOrbit &psi_u, OptionalJacobian< DimG, DimM > D_lift={}) const
	Evaluate the lift at the reference with input u_origin = psi_u(X^-1).
void	propagate (const TangentG &increment, const MatrixM &Phi, const CovarianceM &Qd)
	Advance using a lift evaluated at the current estimate and propagate covariance with an already-discretized transition and process noise.
void	applyCorrection (const TangentG &delta_x)
	Apply an innovation correction, which lives in error coordinates at the reference state, on the opposite side from the prediction increment.
Protected Member Functions inherited from gtsam::ManifoldEKF< M >
void	validateInputs (const gtsam::Vector &prediction, const Matrix &H, const gtsam::Vector &z, const Matrix &R)
	Validate inputs to update.

Additional Inherited Members
Static Protected Member Functions inherited from gtsam::ManifoldEKF< M >
template<typename MatrixType>
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< M >
M	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

◆ EquivariantFilter()

template<typename M, typename Symmetry>

gtsam::EquivariantFilter< M, Symmetry >::EquivariantFilter	(	const M &	xi_ref,
		const CovarianceM &	Sigma,
		const G &	X0 = traits<G>::Identity() )

inline

Initialize the Equivariant Filter.

Parameters

xi_ref	Reference manifold state (origin of lifted coordinates).
Sigma	Initial covariance on the manifold.
X0	Initial group estimate (default: Identity).

Member Function Documentation

◆ actionDifferential()

template<typename M, typename Symmetry>

MatrixM gtsam::EquivariantFilter< M, Symmetry >::actionDifferential ( ) const

inline

Differential of the group action at the reference state.

J = D phi_g|_{xi_ref} maps error coordinates, which live in the tangent space at the reference state, to tangent vectors at the current state estimate. The filter propagates its error in the former frame, so J is the bridge to anything expressed at the current state.

◆ computeErrorDynamicsMatrix()

template<typename M, typename Symmetry>

template<typename Lift, typename InputOrbit>

MatrixM gtsam::EquivariantFilter< M, Symmetry >::computeErrorDynamicsMatrix ( const InputOrbit & psi_u ) const

inline

Compute the error dynamics matrix A (Automatic).

Calculates A = D_phi|_0 * D_lift|_u0, where u0 is the input mapped to the origin.

Concept requirements:

Lift must be callable as Lift(u_origin)(xi_ref, D_lift) where D_lift is an OptionalJacobian of shape DimG x DimM.
InputOrbit must be a group action on the input space with operator() that accepts the current group estimate X and returns the mapped input (no other methods are required by the filter).

Template Parameters

Lift	Functor for the lift Λ(ξ, u).
InputOrbit	Functor for the input orbit ψ_u.

Parameters

psi_u Input Orbit instance.

Returns: MatrixM The calculated error dynamics matrix A.

The lift must reproduce the physical dynamics through the state action. Automatic linearization additionally requires equivariance: Lambda(phi_X(xi), psi_X(u)) = Ad_X Lambda(xi,u) for a left action, or Ad_{X^-1} Lambda(xi,u) for a right action. These are separate requirements, especially for actions with nontrivial stabilizers. See the user guide doc/EquivariantFilter.ipynb for derivations and examples. Explicit prediction accepts a caller-supplied error model without requiring lift equivariance.

◆ covariance()

template<typename M, typename Symmetry>

CovarianceM gtsam::EquivariantFilter< M, Symmetry >::covariance ( ) const

inline

Covariance in the tangent space at the current state.

P_ is the covariance of the error coordinates in the tangent space at the reference state xi_ref. The true state is recovered as xi = phi_g(e) with e = Retract(xi_ref, epsilon), so a perturbation epsilon at the reference appears at the current state as J * epsilon. The covariance therefore pushes forward as J * P_ * J^T.

◆ groupEstimate()

template<typename M, typename Symmetry>

const G & gtsam::EquivariantFilter< M, Symmetry >::groupEstimate ( ) const

inline

Returns: Current group estimate.

◆ liftAtOrigin()

template<typename M, typename Symmetry>

template<typename Lift, typename InputOrbit>

TangentG gtsam::EquivariantFilter< M, Symmetry >::liftAtOrigin	(	const InputOrbit &	psi_u,
		OptionalJacobian< DimG, DimM >	D_lift = {} ) const

inlineprotected

Evaluate the lift at the reference with input u_origin = psi_u(X^-1).

Used by automatic error linearization.

◆ predict()

template<typename M, typename Symmetry>

template<size_t K = 1, typename Lift, typename InputOrbit>

void gtsam::EquivariantFilter< M, Symmetry >::predict	(	const Lift &	lift_u,
		const InputOrbit &	psi_u,
		const MatrixM &	Qc,
		double	dt )

inline

Propagate the filter state (Automatic).

Automatically computes the error dynamics matrix A. Requires the lift equivariance condition documented in computeErrorDynamicsMatrix(). The supplied lift is also evaluated at the current estimate for the mean.

Concept requirements:

Lift is used as Lift(u_origin)(xi_ref_, D_lift) to obtain the lift and its Jacobian w.r.t. the manifold state.
InputOrbit is only used via psi_u(g_.inverse()) to map the current input to the origin; no other methods are needed.

Template Parameters

K	Truncation order for discretization (1 = first order Euler, >1 uses matrix exponential expm(A*dt, K)).
Lift	Functor for the lift Λ(ξ, u).
InputOrbit	Functor for the input orbit ψ_u.

Parameters

lift_u	Lift functor for the current input.
psi_u	Input Orbit for the current input.
Qc	Process noise covariance on the manifold (continuous-time).
dt	Time step.

◆ predictWithJacobian()

template<typename M, typename Symmetry>

template<size_t K = 1, typename Lift>

void gtsam::EquivariantFilter< M, Symmetry >::predictWithJacobian	(	const Lift &	lift_u,
		const MatrixM &	A,
		const MatrixM &	Qc,
		double	dt )

inline

Propagate the filter state (Explicit).

Uses the provided error Jacobian A and process covariance Qc.

Concept requirements:

Lift is only used via Lift(xi_est) to produce a tangent vector. No additional methods are needed for this overload.

The lift is evaluated at the current estimate and must generate the physical dynamics through the declared state action. Prediction composes Exp(Lambda dt) on the left for a left action and on the right for a right action. Equivariance and an input orbit are not required when A is supplied. A and Qc must be expressed in error coordinates at the reference state. Left-action callers that previously supplied body-frame increments for right multiplication must convert their lift to the declared action.

Template Parameters

Lift	Functor for the lift Λ(ξ, u).

Parameters

lift_u	Lift functor for the current input.
A	Error dynamics matrix (DimM x DimM).
Qc	Process noise covariance on the manifold (continuous-time).
dt	Time step.

◆ predictWithTransition()

template<typename M, typename Symmetry>

template<typename Lift>

void gtsam::EquivariantFilter< M, Symmetry >::predictWithTransition	(	const Lift &	lift_u,
		const MatrixM &	Phi,
		const CovarianceM &	Qd,
		double	dt )

inline

Propagate with an already-discretized transition and process noise.

For models derived directly in discrete time, or where a closed-form exp(A dt) is available, this avoids the truncated-series discretization of transitionMatrix(), which is only first order at the default K = 1.

The lift has the same contract as predictWithJacobian(): it is evaluated at the current estimate and generates motion through the state action. No input orbit or equivariance condition is required.

Parameters

lift_u	Lift functor for the current input.
Phi	Discrete transition matrix over dt (DimM x DimM).
Qd	Discrete process noise over dt, in error coordinates.
dt	Time step, used for the mean only.

◆ propagate()

template<typename M, typename Symmetry>

void gtsam::EquivariantFilter< M, Symmetry >::propagate	(	const TangentG &	increment,
		const MatrixM &	Phi,
		const CovarianceM &	Qd )

inlineprotected

Advance using a lift evaluated at the current estimate and propagate covariance with an already-discretized transition and process noise.

Commit g_ after the base class validates the matrix dimensions and updates the manifold state and covariance.

◆ resetReferenceAndGroup()

template<typename M, typename Symmetry>

void gtsam::EquivariantFilter< M, Symmetry >::resetReferenceAndGroup	(	const M &	xi_ref,
		const CovarianceM &	P,
		const G &	g )

inlineprotected

Synchronization hooks for derived filters with dynamic runtime structure.

Most fixed-dimension filters can rely on the public predict/update path only. These functions are primarily for dynamic cases (for example variable-size state/group representations) where derived classes need to resize/reset. Reset reference, covariance, and group estimate; sync manifold state.

◆ transitionMatrix()

template<typename M, typename Symmetry>

template<size_t K = 1>

MatrixM gtsam::EquivariantFilter< M, Symmetry >::transitionMatrix	(	const MatrixM &	A,
		double	dt ) const

inline

Discretize continuous-time error dynamics δ̇ = A δ over dt.

On manifolds (unlike Lie groups) the error stays in a fixed tangent space at the chosen origin, so discretization is just the matrix exponential of A. K mirrors LieGroupEKF: K=1 gives Euler, K>1 calls expm(A*dt, K).

◆ update()

template<typename M, typename Symmetry>

template<typename Measurement>

void gtsam::EquivariantFilter< M, Symmetry >::update	(	const Measurement &	prediction,
		const Eigen::Matrix< double, traits< Measurement >::dimension, DimM > &	H,
		const Measurement &	z,
		const Eigen::Matrix< double, traits< Measurement >::dimension, traits< Measurement >::dimension > &	R )

inline

Measurement update: Corrects the state and covariance using a pre-calculated predicted measurement and its Jacobian.

Overwrites ManifoldEKF::update to modify g_ as well.

Template Parameters

Measurement type of the measurement space.

Parameters

prediction	Predicted measurement.
H	Jacobian of the measurement function h.
z	Observed measurement.
R	Measurement noise covariance.

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

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

Detailed Description

Public Member Functions

Static Public Attributes

Public Types

Protected Member Functions

Additional Inherited Members

Constructor & Destructor Documentation

◆ EquivariantFilter()

Member Function Documentation

◆ actionDifferential()

◆ computeErrorDynamicsMatrix()

◆ covariance()

◆ groupEstimate()

◆ liftAtOrigin()

◆ predict()

◆ predictWithJacobian()

◆ predictWithTransition()

◆ propagate()

◆ resetReferenceAndGroup()

◆ transitionMatrix()

◆ update()