gtsam
Loading...
Searching...
No Matches
gtsam::LeggedFixedLagSmoother Class Reference

Detailed Description

Fixed-lag smoother over NavState and contact-episode footholds.

The first two classes are single-step estimators that maintain one Gaussian belief at the current time. This final variant instead builds a sliding window over time using BatchFixedLagSmoother. Base states are created only at contact events and are linked by preintegrated ImuFactor2 motion factors, while footholds are represented by contact-episode landmark variables that are created on touchdown and naturally disappear once they fall outside the lag window.

This version is both a more realistic estimator for delayed multi-step inference and an example of how the same measurement ideas can be expressed in GTSAM as a smoother rather than as an EKF. It is deliberately kept in the same file as the filter variants so users can read the progression from sequential EKF, to local graph update, to fixed-lag smoothing in one place.

Inheritance diagram for gtsam::LeggedFixedLagSmoother:

Public Member Functions

 LeggedFixedLagSmoother (const NavState &navState0, const Matrix &footholds0, const Matrix9 &baseCovariance0, const LeggedEstimatorParams &params, double lagSeconds, const std::vector< std::string > &footNames={})
 Construct the fixed-lag smoother variant.
 ~LeggedFixedLagSmoother () override
 Destroy the fixed-lag smoother variant.
size_t numFeet () const
 Number of feet tracked by the smoother front-end.
ExtendedPose3d estimate () const override
 Current estimate in the shared ExtendedPose3d layout.
imuBias::ConstantBias estimateBias () const override
 Return the current single shared IMU bias estimate.
void predict (const Vector3 &omegaBody, const Vector3 &specificForceBody, double dt) override
 Accumulate one IMU sample and advance the dead-reckoned estimate.
void processContacts (const std::vector< ContactMeasurement > &activeContacts) override
 Process the active contacts for the current step.
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.

Additional Inherited Members

Protected Member Functions inherited from gtsam::LeggedEstimator
const std::optional< double > & terrainHeight () const
 Optional terrain height used internally for contact height priors.
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.

Member Function Documentation

◆ estimate()

ExtendedPose3d gtsam::LeggedFixedLagSmoother::estimate ( ) const
overridevirtual

Current estimate in the shared ExtendedPose3d layout.

Implements gtsam::LeggedEstimator.

◆ estimateBias()

imuBias::ConstantBias gtsam::LeggedFixedLagSmoother::estimateBias ( ) const
inlineoverridevirtual

Return the current single shared IMU bias estimate.

Implements gtsam::LeggedEstimator.

◆ predict()

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

Accumulate one IMU sample and advance the dead-reckoned estimate.

Implements gtsam::LeggedEstimator.

◆ processContacts()

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

Process the active contacts for the current step.

Implements gtsam::LeggedEstimator.

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