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

Detailed Description

Fixed-lag smoother with CombinedImuFactor bias evolution.

This variant keeps the same contact-episode smoother structure as LeggedFixedLagSmoother, but replaces the ImuFactor2 motion model with event-to-event CombinedImuFactor links. Pose, velocity, and bias are stored as separate graph variables at each contact event, and the bias is allowed to evolve with a random walk between events.

In practice this is the smoother analogue of moving from a fixed, subtractive IMU bias estimate to an explicitly estimated bias trajectory. It serves as an example of how to upgrade a contact-event smoother from simple preintegration with one shared bias key to combined preintegration with per-event bias states.

Inheritance diagram for gtsam::LeggedCombinedFixedLagSmoother:

Public Member Functions

 LeggedCombinedFixedLagSmoother (const NavState &navState0, const Matrix &footholds0, const Matrix9 &baseCovariance0, const LeggedEstimatorParams &params, double lagSeconds, const std::vector< std::string > &footNames={})
 Construct the combined-IMU fixed-lag smoother variant.
 ~LeggedCombinedFixedLagSmoother () override
 Destroy the combined-IMU 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 per-window bias estimate at the latest event.
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::LeggedCombinedFixedLagSmoother::estimate ( ) const
overridevirtual

Current estimate in the shared ExtendedPose3d layout.

Implements gtsam::LeggedEstimator.

◆ estimateBias()

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

Return the current per-window bias estimate at the latest event.

Implements gtsam::LeggedEstimator.

◆ predict()

void gtsam::LeggedCombinedFixedLagSmoother::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::LeggedCombinedFixedLagSmoother::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: