Extended Kalman Filter on a generic manifold M. More...

#include <ManifoldEKF.h>

Public Types
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< M >::TangentVector
	Tangent vector type for the manifold M. More...

Public Member Functions
const Covariance &	covariance () const

int	dimension () const

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

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

const M &	state () const

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)

template<typename Measurement , typename MeasurementFunction >
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 = traits<M>::dimension
	Compile-time dimension of the manifold M. More...

Protected Attributes
int	n_
	Runtime tangent space dimension of M. More...

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

M	X_
	Manifold state estimate. More...

Detailed Description

template<typename M>
class gtsam::ManifoldEKF< M >

Extended Kalman Filter on a generic manifold M.

Template Parameters

M	Manifold type (must satisfy Manifold concept).

This filter maintains a state X in the manifold M and covariance P in the tangent space at X. Prediction requires providing the predicted next state and the state transition Jacobian F. Updates apply a measurement function h and correct the state using the tangent space error.

Handling Static and Dynamic Dimensions: The filter supports manifolds M with either a compile-time fixed dimension or a runtime dynamic dimension. This is determined by gtsam::traits<M>::dimension.

If dimension is an integer (e.g., 3, 6), it's a fixed-size manifold.
If dimension is Eigen::Dynamic, it's a dynamically-sized manifold. In this case, gtsam::traits<M>::GetDimension(const M&) must be available to retrieve the actual dimension at runtime. The internal protected member n_ stores this runtime dimension. Covariance matrices (e.g., P_, method argument Q) and Jacobians (e.g., method argument F) are typed using Covariance and Jacobian typedefs, which are specializations of Eigen::Matrix<double, Dim, Dim>, where Dim is traits<M>::dimension. For dynamically-sized manifolds (Dim == Eigen::Dynamic), these Eigen types represent dynamically-sized matrices.

Definition at line 65 of file ManifoldEKF.h.

Member Typedef Documentation

◆ Covariance

template<typename M >

using gtsam::ManifoldEKF< M >::Covariance = Eigen::Matrix<double, Dim, Dim>

Covariance matrix type (P, Q).

Definition at line 73 of file ManifoldEKF.h.

◆ Jacobian

template<typename M >

using gtsam::ManifoldEKF< M >::Jacobian = Eigen::Matrix<double, Dim, Dim>

State transition Jacobian type (F).

Definition at line 75 of file ManifoldEKF.h.

◆ TangentVector

template<typename M >

using gtsam::ManifoldEKF< M >::TangentVector = typename traits<M>::TangentVector

Tangent vector type for the manifold M.

Definition at line 71 of file ManifoldEKF.h.

Constructor & Destructor Documentation

◆ ManifoldEKF()

template<typename M >

gtsam::ManifoldEKF< M >::ManifoldEKF	(	const M &	X0,
		const Covariance &	P0
	)

inline

Constructor: initialize with state and covariance.

Parameters

X0	Initial state on manifold M.
P0	Initial covariance in the tangent space at X0

Definition at line 83 of file ManifoldEKF.h.

◆ ~ManifoldEKF()

template<typename M >

virtual gtsam::ManifoldEKF< M >::~ManifoldEKF ( )

virtualdefault

Member Function Documentation

◆ covariance()

template<typename M >

const Covariance& gtsam::ManifoldEKF< M >::covariance ( ) const

inline

Returns: current covariance estimate.

Definition at line 111 of file ManifoldEKF.h.

◆ dimension()

template<typename M >

int gtsam::ManifoldEKF< M >::dimension ( ) const

inline

Returns: runtime dimension of the manifold.

Definition at line 114 of file ManifoldEKF.h.

◆ predict()

template<typename M >

void gtsam::ManifoldEKF< M >::predict	(	const M &	X_next,
		const Jacobian &	F,
		const Covariance &	Q
	)

inline

Basic predict step: Updates state and covariance given the predicted next state and the state transition Jacobian F. X_{k+1} = X_next P_{k+1} = F P_k F^T + Q where F = d(local(X_{k+1})) / d(local(X_k)) is the Jacobian of the state transition in local coordinates around X_k.

Parameters

X_next	The predicted state at time k+1 on manifold M.
F	The state transition Jacobian.
Q	Process noise covariance matrix.

Definition at line 128 of file ManifoldEKF.h.

◆ state()

template<typename M >

const M& gtsam::ManifoldEKF< M >::state ( ) const

inline

Returns: current state estimate on manifold M.

Definition at line 108 of file ManifoldEKF.h.

◆ update() [1/2]

template<typename M >

template<typename Measurement >

void gtsam::ManifoldEKF< M >::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
	)

inline

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

Template Parameters

Measurement Type of the measurement vector (e.g., VectorN<m>, Vector).

Parameters

prediction	Predicted measurement.
H	Jacobian of the measurement function h w.r.t. local(X), H = dh/dlocal(X).
z	Observed measurement.
R	Measurement noise covariance.

Definition at line 152 of file ManifoldEKF.h.

◆ update() [2/2]

template<typename M >

template<typename Measurement , typename MeasurementFunction >

void gtsam::ManifoldEKF< M >::update	(	MeasurementFunction &&	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 measurement model function.

Template Parameters

Measurement	Type of the measurement vector.
MeasurementFunction	Functor/lambda providing measurement and its Jacobian. Signature: `Measurement h(const M& x, Jac& H_jacobian)` where H = d(h)/d(local(X)).

Parameters

h	Measurement model function.
z	Observed measurement.
R	Measurement noise covariance.

Definition at line 229 of file ManifoldEKF.h.

Member Data Documentation

◆ Dim

template<typename M >

constexpr int gtsam::ManifoldEKF< M >::Dim = traits<M>::dimension

staticconstexpr

Compile-time dimension of the manifold M.

Definition at line 68 of file ManifoldEKF.h.

◆ n_

template<typename M >

int gtsam::ManifoldEKF< M >::n_

protected

Runtime tangent space dimension of M.

Definition at line 255 of file ManifoldEKF.h.

◆ P_

template<typename M >

Covariance gtsam::ManifoldEKF< M >::P_

protected

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

Definition at line 254 of file ManifoldEKF.h.

◆ X_

template<typename M >

M gtsam::ManifoldEKF< M >::X_

protected

Manifold state estimate.

Definition at line 253 of file ManifoldEKF.h.

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

ManifoldEKF.h

Public Types

Public Member Functions

Static Public Attributes

Protected Attributes

Detailed Description

template<typename M> class gtsam::ManifoldEKF< M >

Member Typedef Documentation

◆ Covariance

◆ Jacobian

◆ TangentVector

Constructor & Destructor Documentation

◆ ManifoldEKF()

◆ ~ManifoldEKF()

Member Function Documentation

◆ covariance()

◆ dimension()

◆ predict()

◆ state()

◆ update() [1/2]

◆ update() [2/2]

Member Data Documentation

◆ Dim

◆ n_

◆ P_

◆ X_

template<typename M>
class gtsam::ManifoldEKF< M >