BipedalLocomotion::Estimators::FloatingBaseEstimator class
#include <BipedalLocomotion/FloatingBaseEstimators/FloatingBaseEstimator.h>

FloatingBaseEstimator class contains the bare-bones implementation for a LeggedOdometry based or Extended Kalman Filter based floating base estimation algorithms for bipedal robots.

Base classes

template<class Output>
class BipedalLocomotion::System::Source<FloatingBaseEstimators::Output>
Source is a template specialization of Advanceable and represents a block that does not contains input.

Derived classes

class InvariantEKFBaseEstimator
InvariantEKFBaseEstimator class implements the Contact Aided Invariant EKF algorithm, developed by Ross Hartley, Maani Ghaffari, Ryan M.
class LeggedOdometry
Floating base estimation algorithm using only kinematic measurements by assuming atleast one frame on the robot is in contact with the environment.

Public types

class ModelComputations
iDynTree based model-specific computations class This is class is used in a required configuration step for the estimator All the model related kinematics and dynamics computations specific to the floating base estimator are contained within this class.

Constructors, destructors, conversion operators

FloatingBaseEstimator()
~FloatingBaseEstimator() virtual

Public functions

auto initialize(std::weak_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> handler, std::shared_ptr<iDynTree::KinDynComputations> kindyn) -> bool
Configure generic parameters and the model.
auto initialize(std::weak_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> handler) -> bool virtual
Configure generic parameters, calling this overloaded method assumes model information is not going to be used.
auto setIMUMeasurement(const Eigen::Vector3d& accMeas, const Eigen::Vector3d& gyroMeas) -> bool
Set the polled IMU measurement.
auto setContacts(const bool& lfInContact, const bool& rfInContact) -> bool
Set feet contact states.
auto setContactStatus(const std::string& name, const bool& contactStatus, const std::chrono::nanoseconds& switchTime, const std::chrono::nanoseconds& = std::chrono::nanoseconds::zero()) -> bool
Set contact status.
auto setKinematics(const Eigen::VectorXd& encoders, const Eigen::VectorXd& encoderSpeeds) -> bool
Set kinematic measurements.
auto setLandmarkRelativePose(const int& landmarkID, const Eigen::Quaterniond& quat, const Eigen::Vector3d& pos, const std::chrono::nanoseconds& timeNow) -> bool
Set the relative pose of a landmark relative to the base link.
auto advance() -> bool final
Compute one step of the estimator.
auto resetEstimator(const FloatingBaseEstimators::InternalState& newState) -> bool virtual
Reset the internal state of the estimator.
auto resetEstimator(const Eigen::Quaterniond& newBaseOrientation, const Eigen::Vector3d& newBasePosition) -> bool virtual
Reset the base pose estimate and consequently the internal state of the estimator.
auto getOutput() const -> const FloatingBaseEstimators::Output& final
Get estimator outputs.
auto isOutputValid() const -> bool final
Determines the validity of the object retrieved with get()
auto modelComputations() -> ModelComputations&
Get ModelComputations object by reference.

Protected types

enum class State { NotInitialized, Initialized, Running }
Enumerator used to determine the running state of the estimator.

Protected functions

auto customInitialization(std::weak_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> handler) -> bool virtual
These custom parameter specifications should be specified by the derived class.
auto predictState(const FloatingBaseEstimators::Measurements& meas, const double& dt) -> bool virtual
Propagate the states through the prediction model, if there exists any (eg.
auto updateKinematics(FloatingBaseEstimators::Measurements& meas, const double& dt) -> bool virtual
Update the predicted state estimates using kinematics and contact measurements.
auto updateLandmarkRelativePoses(FloatingBaseEstimators::Measurements& meas, const double& dt) -> bool virtual
Update the predicted state estimates using relative pose measurements of static landmarks in the environment.
auto updateBaseStateFromIMUState(const FloatingBaseEstimators::InternalState& state, const FloatingBaseEstimators::Measurements& meas, manif::SE3d& basePose, Eigen::Ref<Eigen::Matrix<double, 6, 1>> baseTwist) -> bool
Wrapper method for getting base state from internal IMU state.
auto setupOptions(std::weak_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> handler) -> bool
Setup estimator options.
auto setupSensorDevs(std::weak_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> handler) -> bool
Setup measurement noise standard deviations The parameters in the SensorsStdDev group are,.
auto setupInitialStates(std::weak_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> handler) -> bool
Setup initial states.
auto setupPriorDevs(std::weak_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> handler) -> bool
Setup initial state standard deviations.

Protected variables

ModelComputations m_modelComp
Model computations object.
FloatingBaseEstimators::Options m_options
Struct holding estimator options.
FloatingBaseEstimators::Measurements m_meas
FloatingBaseEstimators::Measurements m_measPrev
Struct holding the latest measurements that were set to the estimator.
FloatingBaseEstimators::InternalState m_state
FloatingBaseEstimators::InternalState m_statePrev
Structs holding the curent and previous internal states of the estimation algorithm.
FloatingBaseEstimators::Output m_estimatorOut
Struct holding outputs of the estimator.
FloatingBaseEstimators::StateStdDev m_priors
FloatingBaseEstimators::StateStdDev m_stateStdDev
Struct holding standard deviations associated to prior state estimates.
FloatingBaseEstimators::SensorsStdDev m_sensorsDev
Struct holding standard deviations associated to sensor measurements.
State m_estimatorState
State of the estimator.
double m_dt
Fixed time step of the estimator, in seconds.
bool m_useIMUForAngVelEstimate
Use IMU measurements as internal state imu angular velocity by default set to true for strap down IMU based EKF implementations, if IMU measurements not used, corresponding impl can set to false.
bool m_useIMUVelForBaseVelComputation
Compute base velocity using inertnal state IMU velocity.
bool m_useModelInfo
Flag to enable running the estimator without using the URDF model information.
bool m_isInvEKF
Flag to maintain soon to be deprecated functionalities currently existing only in InvEKFBaseEstimator.

Enum documentation

enum class BipedalLocomotion::Estimators::FloatingBaseEstimator::State protected

Enumerator used to determine the running state of the estimator.

Enumerators
NotInitialized

The estimator is not initialized yet call FloatingBaseEstimator::initialze method to initialize it.
Initialized

The estimator is initialized and ready to be used.
Running

The estimator is running.

Function documentation

bool BipedalLocomotion::Estimators::FloatingBaseEstimator::initialize(std::weak_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> handler, std::shared_ptr<iDynTree::KinDynComputations> kindyn)

Configure generic parameters and the model.

Parameters
handler in configure the generic parameters for the estimator
kindyn in shared pointer of iDynTree kindyncomputations object (model will be loaded internally)
Returns True in case of success, false otherwise.

The generic parameters include,

  • sampling_period_in_s [PARAMETER|REQUIRED|Sampling period of the estimator in seconds]
  • ModelInfo [GROUP|REQUIRED|URDF Model specific parameters used by the estimator]
    • base_link [PARAMETER|REQUIRED|base link frame from the URDF model| Exists in "ModelInfo" GROUP]
    • base_link_imu [PARAMETER|REQUIRED|IMU frame rigidly attached to thebase link from the URDF model| Exists in "ModelInfo" GROUP]
    • left_foot_contact_frame [PARAMETER|REQUIRED|left foot contact frame from the URDF model| Exists in "ModelInfo" GROUP]

    • right_foot_contact_frame [PARAMETER|REQUIRED|right foot contact frame from the URDF model| Exists in "ModelInfo" GROUP]

bool BipedalLocomotion::Estimators::FloatingBaseEstimator::initialize(std::weak_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> handler) virtual

Configure generic parameters, calling this overloaded method assumes model information is not going to be used.

Parameters
handler in configure the generic parameters for the estimator
Returns True in case of success, false otherwise.

bool BipedalLocomotion::Estimators::FloatingBaseEstimator::setIMUMeasurement(const Eigen::Vector3d& accMeas, const Eigen::Vector3d& gyroMeas)

Set the polled IMU measurement.

Parameters
accMeas in linear accelerometer measurement expressed in the local IMU frame (m/s^2)
gyroMeas in gyroscope measurement expressed in the local IMU frame (rad/s)
Returns True in case of success, false otherwise.

bool BipedalLocomotion::Estimators::FloatingBaseEstimator::setContacts(const bool& lfInContact, const bool& rfInContact)

Set feet contact states.

Parameters
lfInContact in left foot contact state [0, 1]
rfInContact in right foot contact state [0, 1]
Returns True in case of success, false otherwise.

bool BipedalLocomotion::Estimators::FloatingBaseEstimator::setContactStatus(const std::string& name, const bool& contactStatus, const std::chrono::nanoseconds& switchTime, const std::chrono::nanoseconds& = std::chrono::nanoseconds::zero())

Set contact status.

Parameters
name in contact frame name
contactStatus in flag to check active contact
switchTime in time of switching contact

bool BipedalLocomotion::Estimators::FloatingBaseEstimator::setKinematics(const Eigen::VectorXd& encoders, const Eigen::VectorXd& encoderSpeeds)

Set kinematic measurements.

Parameters
encoders in joint positions measured through encoders
encoderSpeeds in joint velocities measured through encoders
Returns True in case of success, false otherwise.

bool BipedalLocomotion::Estimators::FloatingBaseEstimator::setLandmarkRelativePose(const int& landmarkID, const Eigen::Quaterniond& quat, const Eigen::Vector3d& pos, const std::chrono::nanoseconds& timeNow)

Set the relative pose of a landmark relative to the base link.

Parameters
landmarkID in unique landmark ID
quat in relative orientation of the landmark as a quaternion
pos in relative position of the landmark
timeNow in relative position of the landmark
Returns true in case of success, false otherwise

bool BipedalLocomotion::Estimators::FloatingBaseEstimator::advance() final

Compute one step of the estimator.

Returns True in case of success, false otherwise.

bool BipedalLocomotion::Estimators::FloatingBaseEstimator::resetEstimator(const FloatingBaseEstimators::InternalState& newState) virtual

Reset the internal state of the estimator.

Parameters
newState in Internal state of the estimator
Returns True in case of success, false otherwise.

bool BipedalLocomotion::Estimators::FloatingBaseEstimator::resetEstimator(const Eigen::Quaterniond& newBaseOrientation, const Eigen::Vector3d& newBasePosition) virtual

Reset the base pose estimate and consequently the internal state of the estimator.

Parameters
newBaseOrientation in base link orientation as a Eigen quaternion
newBasePosition in base link position
Returns True in case of success, false otherwise.

const FloatingBaseEstimators::Output& BipedalLocomotion::Estimators::FloatingBaseEstimator::getOutput() const final

Get estimator outputs.

Returns A struct containing he estimated internal states of the estiamtor and the associated covariance matrix

bool BipedalLocomotion::Estimators::FloatingBaseEstimator::isOutputValid() const final

Determines the validity of the object retrieved with get()

Returns True in case of success, false otherwise.

ModelComputations& BipedalLocomotion::Estimators::FloatingBaseEstimator::modelComputations()

Get ModelComputations object by reference.

Returns ModelComputations object providing information between considered model related quantities in the estimator like the base link, IMU, feet contact frames.

bool BipedalLocomotion::Estimators::FloatingBaseEstimator::customInitialization(std::weak_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> handler) virtual protected

These custom parameter specifications should be specified by the derived class.

Parameters
handler in configure the custom parameters for the estimator
Returns bool

bool BipedalLocomotion::Estimators::FloatingBaseEstimator::predictState(const FloatingBaseEstimators::Measurements& meas, const double& dt) virtual protected

Propagate the states through the prediction model, if there exists any (eg.

Parameters
meas in measurements passed as exogeneous inputs to the prediction model
dt in sampling period in seconds
Returns True in case of success, false otherwise.

a strap-down IMU model)

bool BipedalLocomotion::Estimators::FloatingBaseEstimator::updateKinematics(FloatingBaseEstimators::Measurements& meas, const double& dt) virtual protected

Update the predicted state estimates using kinematics and contact measurements.

Parameters
meas in measurements to update the predicted states
dt in sampling period in seconds
Returns True in case of success, false otherwise.

bool BipedalLocomotion::Estimators::FloatingBaseEstimator::updateLandmarkRelativePoses(FloatingBaseEstimators::Measurements& meas, const double& dt) virtual protected

Update the predicted state estimates using relative pose measurements of static landmarks in the environment.

Parameters
meas in measurements to update the predicted states
dt in sampling period in seconds
Returns True in case of success, false otherwise.

bool BipedalLocomotion::Estimators::FloatingBaseEstimator::updateBaseStateFromIMUState(const FloatingBaseEstimators::InternalState& state, const FloatingBaseEstimators::Measurements& meas, manif::SE3d& basePose, Eigen::Ref<Eigen::Matrix<double, 6, 1>> baseTwist) protected

Wrapper method for getting base state from internal IMU state.

Parameters
state in internal state of the estimator
meas in previous measurement to the estimator
basePose in base pose as an iDynTree Transform object
baseTwist in mixe- trivialized base velocity as an iDynTree Twist object

bool BipedalLocomotion::Estimators::FloatingBaseEstimator::setupOptions(std::weak_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> handler) protected

Setup estimator options.

Parameters
handler in parameter handler
Returns True in case of success, false otherwise.

The parameters in the Options group are,

  • enable_imu_bias_estimation [PARAMETER|-|Enable/disable IMU bias estimation]
  • enable_static_imu_bias_initialization [PARAMETER|-|Enable/disable IMU bias initialization assuming static configuration]
  • nr_samples_for_imu_bias_initialization [PARAMETER|REQUIRED, if staticImuBiasInitializationEnabled is set to true|Number of samples for static bias initialization]
  • enable_ekf_update [PARAMETER|-|Enable/disable update step of EKF (not recommended to set to false)]

  • acceleration_due_to_gravity [PARAMETER|-|Acceleration due to gravity, 3d vector]

bool BipedalLocomotion::Estimators::FloatingBaseEstimator::setupSensorDevs(std::weak_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> handler) protected

Setup measurement noise standard deviations The parameters in the SensorsStdDev group are,.

Parameters
handler in parameter handler
Returns True in case of success, false otherwise.
  • accelerometer_measurement_noise_std_dev [PARAMETER|REQUIRED|Accelerometer measurement white noise deviation]
  • gyroscope_measurement_noise_std_dev [PARAMETER|REQUIRED|Gyroscope measurement white noise deviation]
  • accelerometer_measurement_bias_noise_std_dev [PARAMETER|REQUIRED, if imuBiasEstimationEnabled is set to true|Accelerometer measurement bias noise deviation]
  • gyroscope_measurement_bias_noise_std_dev [PARAMETER|REQUIRED, if imuBiasEstimationEnabled is set to true|Gyroscope measurement bias noise deviation]
  • contact_foot_linear_velocity_noise_std_dev [PARAMETER|REQUIRED|Linear velocity white noise deviation when foot is in contact]
  • contact_foot_angular_velocity_noise_std_dev [PARAMETER|REQUIRED|Angular velocity white noise deviation when foot is in contact]
  • swing_foot_linear_velocity_noise_std_dev [PARAMETER|REQUIRED|Linear velocity white noise deviation when foot is swinging off contact]
  • swing_foot_angular_velocity_noise_std_dev [PARAMETER|REQUIRED|Angular velocity white noise deviation when foot is swinging off contact]
  • forward_kinematic_measurement_noise_std_dev [PARAMETER|REQUIRED|White noise deviation in relative poses computed through forward kinematics]

  • encoders_measurement_noise_std_dev [PARAMETER|REQUIRED|Encoder measurement white noise deviation]

bool BipedalLocomotion::Estimators::FloatingBaseEstimator::setupInitialStates(std::weak_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> handler) protected

Setup initial states.

Parameters
handler in parameter handler
Returns True in case of success, false otherwise.

The parameters in the InitialStates group are,

  • imu_orientation_quaternion_wxyz [PARAMETER|REQUIRED|Initial IMU orientation wrt inertial frame, as a quaternion]
  • imu_position_xyz [PARAMETER|REQUIRED|Initial IMU position wrt inertial frame]
  • imu_linear_velocity_xyz [PARAMETER|REQUIRED|Initial IMU linear velocity wrt inertial frame, in a mixed-velocity representation]
  • l_contact_frame_orientation_quaternion_wxyz [PARAMETER|REQUIRED|Initial left foot contact frame orientation wrt inertial frame, as a quaternion]
  • l_contact_frame_position_xyz [PARAMETER|REQUIRED|Initial left foot contact frame position wrt inertial frame]
  • r_contact_frame_orientation_quaternion_wxyz [PARAMETER|REQUIRED|Initial right foot contact frame orientation wrt inertial frame, as a quaternion]
  • r_contact_frame_position_xyz [PARAMETER|REQUIRED|Initial right foot contact frame position wrt inertial frame]
  • accelerometer_bias [PARAMETER|REQUIRED, if imuBiasEstimationEnabled is set to true|Initial accelerometer bias expressed in IMU frame]

  • gyroscope_bias [PARAMETER|REQUIRED, if imuBiasEstimationEnabled is set to true|Initial gyroscope bias expressed in IMU frame]

bool BipedalLocomotion::Estimators::FloatingBaseEstimator::setupPriorDevs(std::weak_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> handler) protected

Setup initial state standard deviations.

Parameters
handler in parameter handler
Returns True in case of success, false otherwise.

The parameters in the PriorsStdDev group are,

  • imu_orientation_quaternion_wxyz [PARAMETER|REQUIRED|Initial IMU orientation as wrt inertial frame]
  • imu_position [PARAMETER|REQUIRED|Initial IMU position deviation wrt inertial frame]
  • imu_linear_velocity [PARAMETER|REQUIRED|Initial IMU linear velocity deviation wrt inertial frame, in a mixed-velocity representation]
  • l_contact_frame_orientation_quaternion [PARAMETER|REQUIRED|Initial left foot contact frame orientation deviation wrt inertial frame]
  • l_contact_frame_position [PARAMETER|REQUIRED|Initial left foot contact frame position deviation wrt inertial frame]
  • r_contact_frame_orientation_quaternion [PARAMETER|REQUIRED|Initial right foot contact frame orientation deviation wrt inertial frame]
  • r_contact_frame_position [PARAMETER|REQUIRED|Initial right foot contact frame position deviation wrt inertial frame]
  • accelerometer_bias [PARAMETER|REQUIRED, if imuBiasEstimationEnabled is set to true|Initial accelerometer bias devitaion expressed in IMU frame]

  • gyroscope_bias [PARAMETER|REQUIRED, if imuBiasEstimationEnabled is set to true|Initial gyroscope bias deviation expressed in IMU frame]

Variable documentation

bool BipedalLocomotion::Estimators::FloatingBaseEstimator::m_useIMUVelForBaseVelComputation protected

Compute base velocity using inertnal state IMU velocity.

by default set to true for strap down IMU based EKF implementations, if IMU measurements not used, corresponding impl can set to false