AnymalStateEstimator
The state estimator for anymal.
anymal_state_estimator_lwf::AnymalFilterLwf< ConcreteDescription_, RobotState_, LwfModelType_ > Class Template Reference

The lightweight filter implementation of AnymalFilter. More...

#include <AnymalFilterLwf.hpp>

Inheritance diagram for anymal_state_estimator_lwf::AnymalFilterLwf< ConcreteDescription_, RobotState_, LwfModelType_ >:
Inheritance graph

Public Types

using Base = anymal_state_estimator::AnymalFilter< ConcreteDescription_, RobotState_ >
 
using Contact = anymal_state_estimator::Contact
 
template<typename ValueType_ >
using ContactEnumContainer = std_utils::EnumArray< ContactEnum, ValueType_ >
 
using CT = typename ConcreteDescription_::ConcreteTopology
 
using ContactEnum = typename CT::ContactEnum
 
using JointEnum = typename CT::JointEnum
 
using JointState = std_utils::EnumArray< JointEnum, any_measurements::ExtendedJointState >
 
using PoseCovarianceMatrix = Eigen::Matrix< double, RD::getNumSpatialDof(), RD::getNumSpatialDof()>
 
using RD = typename RobotModel::RD
 
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

 AnymalFilterLwf ()=delete
 
 AnymalFilterLwf (any_node::Node::NodeHandlePtr nh)
 
 ~AnymalFilterLwf () override
 
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
 
StateStatus getStatus () const override
 
kindr::RotationQuaternionPD getOrientationBaseToWorld () const override
 
kindr::Position3D getPositionWorldToBaseInWorldFrame () const override
 
kindr::LocalAngularVelocityPD getAngularVelocityBaseInBaseFrame () const override
 
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
 
kindr::Velocity3D getLinearVelocityBaseInWorldFrame () const 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 addVariablesToLog ()
 
virtual void setModelPtr (std::shared_ptr< RobotModel > modelPtr)
 
virtual void updateFullContactFlag (const bool hasFullContact)
 
any_measurements::Time getLastEstimatedStateStamp () const
 

Protected Types

using mtFilter = LWFM::LegPosAndPoseFilter< RD::getNumLegs(), LwfModelType_ >
 
using mtPredictionMeas = typename mtFilter::mtPrediction::mtMeas
 
using mtPoseMeas = typename std::tuple_element< 0, decltype(filter_.mUpdates_)>::type::mtMeas
 
using mtKinUpdate = typename std::tuple_element< 1, decltype(filter_.mUpdates_)>::type
 
using mtKinMeas = typename std::tuple_element< 1, decltype(filter_.mUpdates_)>::type::mtMeas
 
using mtOutput = LWFM::StandardOutput
 

Protected Member Functions

kindr::RotationQuaternionPD getOrientationOdomToBase () const
 
kindr::Velocity3D getLinearVelocityInBaseFrame () const
 

Protected Attributes

mtFilter filter_
 
LwfModelType_ * filterKinematicModelPtr_
 This needs to be protected by mutexFilter_. More...
 
LWFM::TransformStandardPoseCF transformStandardPoseCF_
 
mtOutput alignedPoseOutput_
 This holds C_BG, G_r_GB, which can be stored in the transform T_MG = [inv(C_BG), G_r_GB]. More...
 
Eigen::MatrixXd alignedPoseOutputCov_
 
mtOutput odometryOutput_
 This holds C_BO, O_r_OB, which can be stored in the transform T_OB = [inv(C_BO), O_r_OB]. More...
 
LWFM::OdometryToBaseOutputCT< mtKinUpdateodometryToBaseOutputCT_
 
Eigen::MatrixXd odometryOutputCov_
 
any_measurements::Time prevMeasImuStamp_
 
bool isInitialized_ {false}
 
- 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 LwfModelType_>
class anymal_state_estimator_lwf::AnymalFilterLwf< ConcreteDescription_, RobotState_, LwfModelType_ >

The lightweight filter implementation of AnymalFilter.

Template Parameters
ConcreteDescription_romo::ConcreteDescription
RobotState_romo::RobotState
LwfModelType_kinematic model of the robot

Member Typedef Documentation

template<typename ConcreteDescription_ , typename RobotState_ , typename LwfModelType_ >
using anymal_state_estimator_lwf::AnymalFilterLwf< ConcreteDescription_, RobotState_, LwfModelType_ >::Base = anymal_state_estimator::AnymalFilter<ConcreteDescription_, RobotState_>
template<typename ConcreteDescription_ , typename RobotState_ , typename LwfModelType_ >
using anymal_state_estimator_lwf::AnymalFilterLwf< ConcreteDescription_, RobotState_, LwfModelType_ >::Contact = anymal_state_estimator::Contact
template<typename ConcreteDescription_ , typename RobotState_ , typename LwfModelType_ >
using anymal_state_estimator::AnymalFilter< ConcreteDescription_, RobotState_ >::ContactEnum = typename CT::ContactEnum
template<typename ConcreteDescription_ , typename RobotState_ , typename LwfModelType_ >
template<typename ValueType_ >
using anymal_state_estimator_lwf::AnymalFilterLwf< ConcreteDescription_, RobotState_, LwfModelType_ >::ContactEnumContainer = std_utils::EnumArray<ContactEnum, ValueType_>
template<typename ConcreteDescription_ , typename RobotState_ , typename LwfModelType_ >
using anymal_state_estimator::AnymalFilter< ConcreteDescription_, RobotState_ >::CT = typename ConcreteDescription_::ConcreteTopology
template<typename ConcreteDescription_ , typename RobotState_ , typename LwfModelType_ >
using anymal_state_estimator::AnymalFilter< ConcreteDescription_, RobotState_ >::JointEnum = typename CT::JointEnum
template<typename ConcreteDescription_ , typename RobotState_ , typename LwfModelType_ >
using anymal_state_estimator::AnymalFilter< ConcreteDescription_, RobotState_ >::JointState = std_utils::EnumArray<JointEnum, any_measurements::ExtendedJointState>
template<typename ConcreteDescription_ , typename RobotState_ , typename LwfModelType_ >
using anymal_state_estimator_lwf::AnymalFilterLwf< ConcreteDescription_, RobotState_, LwfModelType_ >::mtFilter = LWFM::LegPosAndPoseFilter<RD::getNumLegs(), LwfModelType_>
protected
template<typename ConcreteDescription_ , typename RobotState_ , typename LwfModelType_ >
using anymal_state_estimator_lwf::AnymalFilterLwf< ConcreteDescription_, RobotState_, LwfModelType_ >::mtKinMeas = typename std::tuple_element<1, decltype(filter_.mUpdates_)>::type::mtMeas
protected
template<typename ConcreteDescription_ , typename RobotState_ , typename LwfModelType_ >
using anymal_state_estimator_lwf::AnymalFilterLwf< ConcreteDescription_, RobotState_, LwfModelType_ >::mtKinUpdate = typename std::tuple_element<1, decltype(filter_.mUpdates_)>::type
protected
template<typename ConcreteDescription_ , typename RobotState_ , typename LwfModelType_ >
using anymal_state_estimator_lwf::AnymalFilterLwf< ConcreteDescription_, RobotState_, LwfModelType_ >::mtOutput = LWFM::StandardOutput
protected
template<typename ConcreteDescription_ , typename RobotState_ , typename LwfModelType_ >
using anymal_state_estimator_lwf::AnymalFilterLwf< ConcreteDescription_, RobotState_, LwfModelType_ >::mtPoseMeas = typename std::tuple_element<0, decltype(filter_.mUpdates_)>::type::mtMeas
protected
template<typename ConcreteDescription_ , typename RobotState_ , typename LwfModelType_ >
using anymal_state_estimator_lwf::AnymalFilterLwf< ConcreteDescription_, RobotState_, LwfModelType_ >::mtPredictionMeas = typename mtFilter::mtPrediction::mtMeas
protected
template<typename ConcreteDescription_ , typename RobotState_ , typename LwfModelType_ >
using anymal_state_estimator::AnymalFilter< ConcreteDescription_, RobotState_ >::PoseCovarianceMatrix = Eigen::Matrix<double, RD::getNumSpatialDof(), RD::getNumSpatialDof()>
template<typename ConcreteDescription_ , typename RobotState_ , typename LwfModelType_ >
using anymal_state_estimator::AnymalFilter< ConcreteDescription_, RobotState_ >::RD = typename RobotModel::RD
template<typename ConcreteDescription_ , typename RobotState_ , typename LwfModelType_ >
using anymal_state_estimator::AnymalFilter< ConcreteDescription_, RobotState_ >::StateStatus = anymal_description::StateStatus

Constructor & Destructor Documentation

template<typename ConcreteDescription_ , typename RobotState_ , typename LwfModelType_ >
anymal_state_estimator_lwf::AnymalFilterLwf< ConcreteDescription_, RobotState_, LwfModelType_ >::AnymalFilterLwf ( )
delete
template<typename ConcreteDescription_ , typename RobotState_ , typename LwfModelType_ >
anymal_state_estimator_lwf::AnymalFilterLwf< ConcreteDescription_, RobotState_, LwfModelType_ >::AnymalFilterLwf ( any_node::Node::NodeHandlePtr  nh)
explicit
template<typename ConcreteDescription_ , typename RobotState_ , typename LwfModelType_ >
anymal_state_estimator_lwf::AnymalFilterLwf< ConcreteDescription_, RobotState_, LwfModelType_ >::~AnymalFilterLwf ( )
override

Member Function Documentation

template<typename ConcreteDescription_ , typename RobotState_ , typename LwfModelType_ >
kindr::LocalAngularVelocityPD anymal_state_estimator_lwf::AnymalFilterLwf< ConcreteDescription_, RobotState_, LwfModelType_ >::getAngularVelocityBaseInBaseFrame ( ) const
overridevirtual
template<typename ConcreteDescription_ , typename RobotState_ , typename LwfModelType_ >
PoseCovarianceMatrix anymal_state_estimator_lwf::AnymalFilterLwf< ConcreteDescription_, RobotState_, LwfModelType_ >::getEstPoseInOdomCovariance ( ) const
overridevirtual
template<typename ConcreteDescription_ , typename RobotState_ , typename LwfModelType_ >
PoseCovarianceMatrix anymal_state_estimator_lwf::AnymalFilterLwf< ConcreteDescription_, RobotState_, LwfModelType_ >::getEstTwistCovariance ( ) const
overridevirtual
template<typename ConcreteDescription_ , typename RobotState_ , typename LwfModelType_ >
Eigen::Matrix<double, 3, 1> anymal_state_estimator_lwf::AnymalFilterLwf< ConcreteDescription_, RobotState_, LwfModelType_ >::getImuAngularVelocityBias ( ) const
overridevirtual
template<typename ConcreteDescription_ , typename RobotState_ , typename LwfModelType_ >
Eigen::Matrix<double, 3, 1> anymal_state_estimator_lwf::AnymalFilterLwf< ConcreteDescription_, RobotState_, LwfModelType_ >::getImuLinearAccelerationBias ( ) const
overridevirtual
template<typename ConcreteDescription_ , typename RobotState_ , typename LwfModelType_ >
any_measurements::Time anymal_state_estimator_lwf::AnymalFilterLwf< ConcreteDescription_, RobotState_, LwfModelType_ >::getLastImuStamp ( ) const
overridevirtual
template<typename ConcreteDescription_ , typename RobotState_ , typename LwfModelType_ >
kindr::Velocity3D anymal_state_estimator_lwf::AnymalFilterLwf< ConcreteDescription_, RobotState_, LwfModelType_ >::getLinearVelocityBaseInWorldFrame ( ) const
overridevirtual
template<typename ConcreteDescription_ , typename RobotState_ , typename LwfModelType_ >
kindr::Velocity3D anymal_state_estimator_lwf::AnymalFilterLwf< ConcreteDescription_, RobotState_, LwfModelType_ >::getLinearVelocityInBaseFrame ( ) const
protected
template<typename ConcreteDescription_ , typename RobotState_ , typename LwfModelType_ >
kindr::RotationQuaternionPD anymal_state_estimator_lwf::AnymalFilterLwf< ConcreteDescription_, RobotState_, LwfModelType_ >::getOrientationBaseToWorld ( ) const
overridevirtual
template<typename ConcreteDescription_ , typename RobotState_ , typename LwfModelType_ >
kindr::RotationQuaternionPD anymal_state_estimator_lwf::AnymalFilterLwf< ConcreteDescription_, RobotState_, LwfModelType_ >::getOrientationOdomToBase ( ) const
protected
template<typename ConcreteDescription_ , typename RobotState_ , typename LwfModelType_ >
kindr::Position3D anymal_state_estimator_lwf::AnymalFilterLwf< ConcreteDescription_, RobotState_, LwfModelType_ >::getPositionWorldToBaseInWorldFrame ( ) const
overridevirtual
template<typename ConcreteDescription_ , typename RobotState_ , typename LwfModelType_ >
StateStatus anymal_state_estimator_lwf::AnymalFilterLwf< ConcreteDescription_, RobotState_, LwfModelType_ >::getStatus ( ) const
overridevirtual
template<typename ConcreteDescription_ , typename RobotState_ , typename LwfModelType_ >
bool anymal_state_estimator_lwf::AnymalFilterLwf< ConcreteDescription_, RobotState_, LwfModelType_ >::initFilter ( const kindr::HomTransformQuatD &  poseBaseToOdom)
overridevirtual
template<typename ConcreteDescription_ , typename RobotState_ , typename LwfModelType_ >
void anymal_state_estimator_lwf::AnymalFilterLwf< ConcreteDescription_, RobotState_, LwfModelType_ >::processImuReadings ( const any_measurements::ImuWithCovariance &  imu)
overridevirtual
template<typename ConcreteDescription_ , typename RobotState_ , typename LwfModelType_ >
void anymal_state_estimator_lwf::AnymalFilterLwf< ConcreteDescription_, RobotState_, LwfModelType_ >::processKinematics ( const JointState joints,
const ContactEnumContainer< Contact > &  contacts,
const bool  fakeKinematicsUpdateActive 
)
overridevirtual
template<typename ConcreteDescription_ , typename RobotState_ , typename LwfModelType_ >
bool anymal_state_estimator_lwf::AnymalFilterLwf< ConcreteDescription_, RobotState_, LwfModelType_ >::resetFilter ( const kindr::HomTransformQuatD &  poseBaseToOdom)
overridevirtual
template<typename ConcreteDescription_ , typename RobotState_ , typename LwfModelType_ >
void anymal_state_estimator_lwf::AnymalFilterLwf< ConcreteDescription_, RobotState_, LwfModelType_ >::updateFilter ( )
overridevirtual

Member Data Documentation

template<typename ConcreteDescription_ , typename RobotState_ , typename LwfModelType_ >
mtOutput anymal_state_estimator_lwf::AnymalFilterLwf< ConcreteDescription_, RobotState_, LwfModelType_ >::alignedPoseOutput_
protected

This holds C_BG, G_r_GB, which can be stored in the transform T_MG = [inv(C_BG), G_r_GB].

template<typename ConcreteDescription_ , typename RobotState_ , typename LwfModelType_ >
Eigen::MatrixXd anymal_state_estimator_lwf::AnymalFilterLwf< ConcreteDescription_, RobotState_, LwfModelType_ >::alignedPoseOutputCov_
protected
template<typename ConcreteDescription_ , typename RobotState_ , typename LwfModelType_ >
mtFilter anymal_state_estimator_lwf::AnymalFilterLwf< ConcreteDescription_, RobotState_, LwfModelType_ >::filter_
protected
template<typename ConcreteDescription_ , typename RobotState_ , typename LwfModelType_ >
LwfModelType_* anymal_state_estimator_lwf::AnymalFilterLwf< ConcreteDescription_, RobotState_, LwfModelType_ >::filterKinematicModelPtr_
protected

This needs to be protected by mutexFilter_.

template<typename ConcreteDescription_ , typename RobotState_ , typename LwfModelType_ >
bool anymal_state_estimator_lwf::AnymalFilterLwf< ConcreteDescription_, RobotState_, LwfModelType_ >::isInitialized_ {false}
protected
template<typename ConcreteDescription_ , typename RobotState_ , typename LwfModelType_ >
mtOutput anymal_state_estimator_lwf::AnymalFilterLwf< ConcreteDescription_, RobotState_, LwfModelType_ >::odometryOutput_
protected

This holds C_BO, O_r_OB, which can be stored in the transform T_OB = [inv(C_BO), O_r_OB].

template<typename ConcreteDescription_ , typename RobotState_ , typename LwfModelType_ >
Eigen::MatrixXd anymal_state_estimator_lwf::AnymalFilterLwf< ConcreteDescription_, RobotState_, LwfModelType_ >::odometryOutputCov_
protected
template<typename ConcreteDescription_ , typename RobotState_ , typename LwfModelType_ >
LWFM::OdometryToBaseOutputCT<mtKinUpdate> anymal_state_estimator_lwf::AnymalFilterLwf< ConcreteDescription_, RobotState_, LwfModelType_ >::odometryToBaseOutputCT_
protected
template<typename ConcreteDescription_ , typename RobotState_ , typename LwfModelType_ >
any_measurements::Time anymal_state_estimator_lwf::AnymalFilterLwf< ConcreteDescription_, RobotState_, LwfModelType_ >::prevMeasImuStamp_
protected
template<typename ConcreteDescription_ , typename RobotState_ , typename LwfModelType_ >
LWFM::TransformStandardPoseCF anymal_state_estimator_lwf::AnymalFilterLwf< ConcreteDescription_, RobotState_, LwfModelType_ >::transformStandardPoseCF_
protected

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