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.

Inheritance diagram for gtsam::ManifoldEKF< M >:

Public Member Functions
	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	Dim = traits<M>::dimension
	Compile-time dimension of the manifold M.

Public Types
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	validateInputs (const gtsam::Vector &prediction, const Matrix &H, const gtsam::Vector &z, const Matrix &R)
	Validate inputs to update.

Static Protected Member Functions
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
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

◆ 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

Member Function Documentation

◆ covariance()

template<typename M>

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

inline

Returns: current covariance estimate.

◆ dimension()

template<typename M>

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

inline

Returns: runtime dimension of the manifold.

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

This overload expects a discrete-time process covariance Q already scaled for the step being applied. 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.

◆ reset()

template<typename M>

void gtsam::ManifoldEKF< M >::reset ( const TangentVector & eta )

inline

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.

If the retract supports a Jacobian argument, we compute B and update P <- B P B^T. Otherwise, we leave the covariance unchanged.

◆ state()

template<typename M>

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

inline

Returns: current state estimate on manifold M.

◆ 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,
		bool	performReset = true )

inline

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

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.
performReset	If true (default), performs a reset (transport) after update; otherwise, just retracts the state.

◆ 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,
		bool	performReset = true )

inline

Measurement update: Corrects the state and covariance using a measurement model function.

Template Parameters

Measurement	type of the measurement space.
MeasurementFunction	Functor/lambda providing measurement+Jacobian.

Parameters

h	Measurement model function.
z	Observed measurement.
R	Measurement noise covariance.
performReset	If true (default), transport covariance after retract.

◆ updateWithVector()

template<typename M>

void gtsam::ManifoldEKF< M >::updateWithVector	(	const gtsam::Vector &	prediction,
		const Matrix &	H,
		const gtsam::Vector &	z,
		const Matrix &	R,
		bool	performReset = true )

inline

Convenience bridge for wrappers: vector measurement update calling update<Vector>.

This overload exists to avoid templates in wrappers. It validates sizes and forwards to the templated update with Measurement = gtsam::Vector (dynamic size).

Parameters

prediction	Predicted measurement vector.
H	Measurement Jacobian matrix.
z	Observed measurement vector.
R	Measurement noise covariance matrix.
performReset	If true (default), transport covariance after retract.

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

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

Detailed Description

Public Member Functions

Static Public Attributes

Public Types

Protected Member Functions

Static Protected Member Functions

Protected Attributes

Constructor & Destructor Documentation

◆ ManifoldEKF()

Member Function Documentation

◆ covariance()

◆ dimension()

◆ predict()

◆ reset()

◆ state()

◆ update() [1/2]

◆ update() [2/2]

◆ updateWithVector()