Detailed Description

Invariant filter: LeftLinearEKF on ExtendedPose3(2+k).

The state is a single dynamic ExtendedPose3 containing (R, p, v, f_1, ..., f_k) in R,p,v order. The prediction step is left-linear, and the contact measurement model has Jacobians that are invariant in the state coordinates.

This is very similar to the Contact-aided invariant filter from

Hartley, Ross, Maani Ghaffari, Ryan M. Eustice, and Jessy W. Grizzle. "Contact-aided invariant extended Kalman filtering for robot state estimation." The International Journal of Robotics Research 39, no. 4 (2020): 402-430.

Inheritance diagram for gtsam::LeggedInvariantEKF:

Public Member Functions
	LeggedInvariantEKF (const NavState &navState0, const Matrix &footholds0, const Matrix &P0, const LeggedEstimatorParams &params, const std::vector< std::string > &footNames={})
	Construct the ExtendedPose3-based EKF.
Matrix	covariance () const
	Return the current full covariance.
size_t	numFeet () const
	Number of feet tracked by the estimator.
const std::vector< std::string > &	footNames () const
	Foot names in state order.
const LeggedEstimatorParams &	params () const
	Shared estimator parameters.
ExtendedPose3d	estimate () const override
	Current estimate in the shared ExtendedPose3d layout.
imuBias::ConstantBias	estimateBias () const override
	Return the fixed IMU bias used by the filter.
void	predict (const Vector3 &omegaBody, const Vector3 &specificForceBody, double dt) override
	Predict forward using one IMU sample.
void	processContacts (const std::vector< ContactMeasurement > &activeContacts) override
	Process all currently active contacts for the current step.
Public Member Functions inherited from gtsam::LeftLinearEKF< ExtendedPose3d >
	LeftLinearEKF (const ExtendedPose3d &X0, const Covariance &P0)
void	predict (const ExtendedPose3d &W, const Phi &phi, const ExtendedPose3d &U, const Covariance &Q)
	General left–linear prediction, updates filter state as follows: X⁺ = W · φ(X) · U P⁺ = A P Aᵀ + Q with A = Ad_{U^{-1}} Φ, Φ := dφ\|_e.
void	predict (const Phi &phi, const ExtendedPose3d &U, const Covariance &Q)
	Special case of predict with W=I, updates filter state as follows: Update: X⁺ = φ(X) · U Covariance: P⁺ = A P Aᵀ + Q with A = Ad_{U^{-1}} Φ, Φ := dφ\|_e.
Public Member Functions inherited from gtsam::LieGroupEKF< G >
	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.
Public Member Functions inherited from gtsam::LeggedEstimator
virtual	~LeggedEstimator ()=default
	Destroy the estimator interface.
void	turnHeightPriorOn (double terrainHeight)
	Enable contact height priors at the supplied terrain height.
void	turnHeightPriorOff ()
	Disable contact height priors.

Static Public Member Functions
static ExtendedPose3d	MakeState (const NavState &navState, const Matrix &footholds)
	Build an ExtendedPose3(2+k) state from base state and world footholds.
static ExtendedPose3d	GravityIncrement (size_t numFeet, const Vector3 &gravity, double dt)
	Build the gravity-only left increment for one prediction step.
static ExtendedPose3d	ImuIncrement (size_t numFeet, const Vector3 &omegaBody, const Vector3 &specificForceBody, double dt)
	Build the IMU-only right increment for one prediction step.
static size_t	FootColumn (size_t foot)
	Return the ExtendedPose3 block index corresponding to a foot number.
Static Public Member Functions inherited from gtsam::LeftLinearEKF< ExtendedPose3d >
static ExtendedPose3d	Dynamics (const ExtendedPose3d &W, const Phi &phi, const ExtendedPose3d &X, const ExtendedPose3d &U, OptionalJacobian< Dim, Dim > A={})
	General left–linear dynamics.
static ExtendedPose3d	Dynamics (const Phi &phi, const ExtendedPose3d &X, const ExtendedPose3d &U, OptionalJacobian< Dim, Dim > A={})
	Left–linear dynamics with W=I.

Public Types
using	EkfBase = LeftLinearEKF<ExtendedPose3d>
using	TangentVector = typename EkfBase::TangentVector
using	Jacobian = typename EkfBase::Jacobian
using	Covariance = typename EkfBase::Covariance
Public Types inherited from gtsam::LeftLinearEKF< ExtendedPose3d >
using	Base
using	TangentVector
using	Jacobian
using	Covariance
Public Types inherited from gtsam::LieGroupEKF< G >
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).

Classes
struct	AutonomousFlow
	Autonomous flow used by the left-linear prediction step. More...

Protected Member Functions
NavState	baseState () const
Matrix	footholdMatrix () const
void	resetFootToMeasurement (size_t foot, const Vector3 &bodyPoint)
void	marginalizeFoot (size_t foot)
virtual void	applyContactUpdate (const std::vector< ContactMeasurement > &activeContacts)
bool	awaitingFullContactInitialization () const
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.
Protected Member Functions inherited from gtsam::LeggedEstimator
const std::optional< double > &	terrainHeight () const
	Optional terrain height used internally for contact height priors.

Additional Inherited Members
Static Public Attributes inherited from gtsam::LeftLinearEKF< ExtendedPose3d >
static constexpr int	Dim
	Compile-time dimension of G.
Static Public Attributes inherited from gtsam::LieGroupEKF< G >
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.
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.
Static Protected Member Functions inherited from gtsam::LeggedEstimator
static ExtendedPose3d	MakeEstimate (const NavState &navState, const Matrix &footholds)
	Build the shared ExtendedPose3d state layout used by all estimators.
static NavState	EstimateNavState (const ExtendedPose3d &estimate)
	Recover the base NavState from an ExtendedPose3d estimate.
static Matrix	EstimateFootholds (const ExtendedPose3d &estimate)
	Recover the foothold matrix from an ExtendedPose3d estimate.
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.

Member Function Documentation

◆ estimate()

ExtendedPose3d gtsam::LeggedInvariantEKF::estimate ( ) const

inlineoverridevirtual

Current estimate in the shared ExtendedPose3d layout.

Implements gtsam::LeggedEstimator.

◆ estimateBias()

imuBias::ConstantBias gtsam::LeggedInvariantEKF::estimateBias ( ) const

inlineoverridevirtual

Return the fixed IMU bias used by the filter.

Implements gtsam::LeggedEstimator.

◆ predict()

void gtsam::LeggedInvariantEKF::predict	(	const Vector3 &	omegaBody,
		const Vector3 &	specificForceBody,
		double	dt )

overridevirtual

Predict forward using one IMU sample.

Implements gtsam::LeggedEstimator.

◆ processContacts()

void gtsam::LeggedInvariantEKF::processContacts ( const std::vector< ContactMeasurement > & activeContacts )

overridevirtual

Process all currently active contacts for the current step.

Implements gtsam::LeggedEstimator.

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

/tmp/gtsam-4.3.0-doxygen.rsXPUS/source/gtsam/navigation/LeggedEstimator.h
/tmp/gtsam-4.3.0-doxygen.rsXPUS/source/gtsam/navigation/LeggedEstimator.cpp

Detailed Description

Public Member Functions

Static Public Member Functions

Public Types

Classes

Protected Member Functions

Additional Inherited Members

Member Function Documentation

◆ estimate()

◆ estimateBias()

◆ predict()

◆ processContacts()