AnymalStateEstimator
The state estimator for anymal.
anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ > Class Template Reference

The two state information filter implementation of AnymalFilter. More...

#include <AnymalFilterTsif.hpp>

Inheritance diagram for anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >:
Inheritance graph

Public Types

using Base = anymal_state_estimator::AnymalFilter< ConcreteDescription_, RobotState_ >
 
using BodyEnum = typename CT::BodyEnum
 
template<typename ValueType_ >
using ContactEnumContainer = std_utils::EnumArray< ContactEnum, ValueType_ >
 
using CT = typename ConcreteDescription_::ConcreteTopology
 
using ContactEnum = typename CT::ContactEnum
 
using JointState = std_utils::EnumArray< JointEnum, any_measurements::ExtendedJointState >
 
using PoseCovarianceMatrix = Eigen::Matrix< double, RD::getNumSpatialDof(), RD::getNumSpatialDof()>
 
using RD = typename RobotModel::RD
 
using RobotModel = romo::RobotModel< ConcreteDescription_, RobotState_ >
 
using StateStatus = anymal_description::StateStatus
 
- Public Types inherited from anymal_state_estimator::AnymalFilter< ConcreteDescription_, RobotState_ >
using RobotModel = romo::RobotModel< ConcreteDescription_, RobotState_ >
 
using RD = typename RobotModel::RD
 
using CT = typename ConcreteDescription_::ConcreteTopology
 
using JointEnum = typename CT::JointEnum
 
using ContactEnum = typename CT::ContactEnum
 
using StateStatus = anymal_description::StateStatus
 
using PoseCovarianceMatrix = Eigen::Matrix< double, RD::getNumSpatialDof(), RD::getNumSpatialDof()>
 
using JointState = std_utils::EnumArray< JointEnum, any_measurements::ExtendedJointState >
 
template<typename ValueType_ >
using ContactEnumContainer = std_utils::EnumArray< ContactEnum, ValueType_ >
 

Public Member Functions

 AnymalFilterTsif (any_node::Node::NodeHandlePtr nh)
 
 ~AnymalFilterTsif () override=default
 
bool initFilter (const kindr::HomTransformQuatD &poseBaseToOdom) override
 
bool resetFilter (const kindr::HomTransformQuatD &poseBaseToOdom) override
 
void processImuReadings (const any_measurements::ImuWithCovariance &imu) override
 
void processKinematics (const JointState &joints, const ContactEnumContainer< Contact > &contacts, const bool fakeKinematicsUpdateActive) override
 
void updateFilter () override
 
void addVariablesToLog () override
 
StateStatus getStatus () const override
 
kindr::RotationQuaternionPD getOrientationBaseToWorld () const override
 
kindr::Position3D getPositionWorldToBaseInWorldFrame () const override
 
kindr::Velocity3D getLinearVelocityBaseInWorldFrame () const override
 
kindr::LocalAngularVelocityPD getAngularVelocityBaseInBaseFrame () const override
 
tsif::Vec3 getAngularVelocityBaseInBaseFrameTsif () const
 
Eigen::Matrix< double, 3, 1 > getImuLinearAccelerationBias () const override
 
Eigen::Matrix< double, 3, 1 > getImuAngularVelocityBias () const override
 
PoseCovarianceMatrix getEstPoseInOdomCovariance () const override
 
PoseCovarianceMatrix getEstTwistCovariance () const override
 
any_measurements::Time getLastImuStamp () const override
 
void setModelPtr (std::shared_ptr< RobotModel > modelPtr) override
 
- Public Member Functions inherited from anymal_state_estimator::AnymalFilter< ConcreteDescription_, RobotState_ >
 AnymalFilter ()=delete
 
 AnymalFilter (any_node::Node::NodeHandlePtr nh)
 
virtual ~AnymalFilter ()=default
 
virtual void updateFullContactFlag (const bool hasFullContact)
 
any_measurements::Time getLastEstimatedStateStamp () const
 

Protected Member Functions

virtual void readParameters ()
 
virtual void processJointStates (const JointState &joints, const bool fakeKinematicsUpdateActive)
 
virtual void processContacts (const ContactEnumContainer< Contact > &contacts, const bool fakeKinematicsUpdateActive)
 
void setOutput ()
 

Protected Attributes

std::shared_ptr< RobotModelrobotModelPtr_ {nullptr}
 
OdometryFilter_ odometryFilter_
 
PoseInitializer poseInitializer_
 
tsif::Vec3 previousLinearAccelerationMeasurement_
 
ContactEnumContainer< tsif::Vec3 > measuredFootholds_
 
tsif::Mat3 imuToBaseRotationMatrix_
 
tsif::Vec3 baseToImuPosition_
 
tsif::Vec3 positionFootToContactInWorld_
 
bool initialActuatorReadingsSet_
 
bool initialOdometryFilterPoseSet_
 
bool odometryFilterReadyToUpdate_
 
bool setOutputFromExtrapolation_
 
tsif::Vec3 imuLinearAccelerationBiasCovarianceDiagonal_ {tsif::Vec3::Zero()}
 
tsif::Vec3 imuAngularVelocityBiasCovarianceDiagonal_ {tsif::Vec3::Zero()}
 
tsif::MatX odometryStateCovariance_ {tsif::ImuKinAnymalOdometryTsif::State::Dim(), tsif::ImuKinAnymalOdometryTsif::State::Dim()}
 
tsif::Vec3 estPositionWorldToBaseInWorld_ {tsif::Vec3::Zero()}
 
tsif::Quat estOrientationBaseToWorld_ {tsif::Quat::Identity()}
 
tsif::Vec3 estLinVelWorldToImuInBase_ {tsif::Vec3::Zero()}
 
tsif::Vec3 estAngVelWorldToBaseInBase_ {tsif::Vec3::Zero()}
 
tsif::Vec3 imuLinearAccBias_ {tsif::Vec3::Zero()}
 
tsif::Vec3 imuAngularVelBias_ {tsif::Vec3::Zero()}
 
any_measurements::Time prevMeasImuStamp_
 
- Protected Attributes inherited from anymal_state_estimator::AnymalFilter< ConcreteDescription_, RobotState_ >
any_node::Node::NodeHandlePtr nh_
 
Eigen::Vector3d previousAngularVelocityMeasurement_ = Eigen::Vector3d::Zero()
 
any_measurements::Time lastEstimatedStateStamp_
 
bool initialFullContactSet_ {false}
 

Detailed Description

template<typename ConcreteDescription_, typename RobotState_, typename OdometryFilter_>
class anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >

The two state information filter implementation of AnymalFilter.

Template Parameters
ConcreteDescription_romo::ConcreteDescription
RobotState_romo::RobotState
OdometryFilter_tsif::AnymalOdometryTsif

Member Typedef Documentation

template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
using anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::Base = anymal_state_estimator::AnymalFilter<ConcreteDescription_, RobotState_>
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
using anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::BodyEnum = typename CT::BodyEnum
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
using anymal_state_estimator::AnymalFilter< ConcreteDescription_, RobotState_ >::ContactEnum = typename CT::ContactEnum
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
template<typename ValueType_ >
using anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::ContactEnumContainer = std_utils::EnumArray<ContactEnum, ValueType_>
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
using anymal_state_estimator::AnymalFilter< ConcreteDescription_, RobotState_ >::CT = typename ConcreteDescription_::ConcreteTopology
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
using anymal_state_estimator::AnymalFilter< ConcreteDescription_, RobotState_ >::JointState = std_utils::EnumArray<JointEnum, any_measurements::ExtendedJointState>
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
using anymal_state_estimator::AnymalFilter< ConcreteDescription_, RobotState_ >::PoseCovarianceMatrix = Eigen::Matrix<double, RD::getNumSpatialDof(), RD::getNumSpatialDof()>
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
using anymal_state_estimator::AnymalFilter< ConcreteDescription_, RobotState_ >::RD = typename RobotModel::RD
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
using anymal_state_estimator::AnymalFilter< ConcreteDescription_, RobotState_ >::RobotModel = romo::RobotModel<ConcreteDescription_, RobotState_>
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
using anymal_state_estimator::AnymalFilter< ConcreteDescription_, RobotState_ >::StateStatus = anymal_description::StateStatus

Constructor & Destructor Documentation

template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::AnymalFilterTsif ( any_node::Node::NodeHandlePtr  nh)
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::~AnymalFilterTsif ( )
overridedefault

Member Function Documentation

template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
void anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::addVariablesToLog ( )
overridevirtual
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
kindr::LocalAngularVelocityPD anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::getAngularVelocityBaseInBaseFrame ( ) const
overridevirtual
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
tsif::Vec3 anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::getAngularVelocityBaseInBaseFrameTsif ( ) const
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
PoseCovarianceMatrix anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::getEstPoseInOdomCovariance ( ) const
overridevirtual
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
PoseCovarianceMatrix anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::getEstTwistCovariance ( ) const
overridevirtual
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
Eigen::Matrix<double, 3, 1> anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::getImuAngularVelocityBias ( ) const
overridevirtual
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
Eigen::Matrix<double, 3, 1> anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::getImuLinearAccelerationBias ( ) const
overridevirtual
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
any_measurements::Time anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::getLastImuStamp ( ) const
overridevirtual
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
kindr::Velocity3D anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::getLinearVelocityBaseInWorldFrame ( ) const
overridevirtual
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
kindr::RotationQuaternionPD anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::getOrientationBaseToWorld ( ) const
overridevirtual
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
kindr::Position3D anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::getPositionWorldToBaseInWorldFrame ( ) const
overridevirtual
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
StateStatus anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::getStatus ( ) const
overridevirtual
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
bool anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::initFilter ( const kindr::HomTransformQuatD &  poseBaseToOdom)
overridevirtual
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
virtual void anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::processContacts ( const ContactEnumContainer< Contact > &  contacts,
const bool  fakeKinematicsUpdateActive 
)
protectedvirtual
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
void anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::processImuReadings ( const any_measurements::ImuWithCovariance &  imu)
overridevirtual
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
virtual void anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::processJointStates ( const JointState joints,
const bool  fakeKinematicsUpdateActive 
)
protectedvirtual
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
void anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::processKinematics ( const JointState joints,
const ContactEnumContainer< Contact > &  contacts,
const bool  fakeKinematicsUpdateActive 
)
overridevirtual
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
virtual void anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::readParameters ( )
protectedvirtual
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
bool anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::resetFilter ( const kindr::HomTransformQuatD &  poseBaseToOdom)
overridevirtual
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
void anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::setModelPtr ( std::shared_ptr< RobotModel modelPtr)
inlineoverridevirtual
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
void anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::setOutput ( )
protected
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
void anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::updateFilter ( )
overridevirtual

Member Data Documentation

template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
tsif::Vec3 anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::baseToImuPosition_
protected
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
tsif::Vec3 anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::estAngVelWorldToBaseInBase_ {tsif::Vec3::Zero()}
protected
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
tsif::Vec3 anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::estLinVelWorldToImuInBase_ {tsif::Vec3::Zero()}
protected
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
tsif::Quat anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::estOrientationBaseToWorld_ {tsif::Quat::Identity()}
protected
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
tsif::Vec3 anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::estPositionWorldToBaseInWorld_ {tsif::Vec3::Zero()}
protected
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
tsif::Vec3 anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::imuAngularVelBias_ {tsif::Vec3::Zero()}
protected
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
tsif::Vec3 anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::imuAngularVelocityBiasCovarianceDiagonal_ {tsif::Vec3::Zero()}
protected
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
tsif::Vec3 anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::imuLinearAccBias_ {tsif::Vec3::Zero()}
protected
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
tsif::Vec3 anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::imuLinearAccelerationBiasCovarianceDiagonal_ {tsif::Vec3::Zero()}
protected
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
tsif::Mat3 anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::imuToBaseRotationMatrix_
protected
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
bool anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::initialActuatorReadingsSet_
protected
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
bool anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::initialOdometryFilterPoseSet_
protected
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
ContactEnumContainer<tsif::Vec3> anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::measuredFootholds_
protected
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
OdometryFilter_ anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::odometryFilter_
protected
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
bool anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::odometryFilterReadyToUpdate_
protected
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
tsif::MatX anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::odometryStateCovariance_ {tsif::ImuKinAnymalOdometryTsif::State::Dim(), tsif::ImuKinAnymalOdometryTsif::State::Dim()}
protected
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
PoseInitializer anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::poseInitializer_
protected
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
tsif::Vec3 anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::positionFootToContactInWorld_
protected
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
tsif::Vec3 anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::previousLinearAccelerationMeasurement_
protected
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
any_measurements::Time anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::prevMeasImuStamp_
protected
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
std::shared_ptr<RobotModel> anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::robotModelPtr_ {nullptr}
protected
template<typename ConcreteDescription_ , typename RobotState_ , typename OdometryFilter_ >
bool anymal_state_estimator_tsif::AnymalFilterTsif< ConcreteDescription_, RobotState_, OdometryFilter_ >::setOutputFromExtrapolation_
protected

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