#include <ImuKinAnymalOdometryTsif.hpp>

Inheritance diagram for tsif::ImuKinAnymalOdometryTsif:

Public Types
using	Base = ImuKinAnymalOdometryTsifBase

template<RD::ContactEnum contact, FD::ResidualEnum residual>
using	contactToResidualKV = std_utils::KeyValuePair< RD::ContactEnum, FD::ResidualEnum, contact, residual >

template<RD::ContactEnum contact, FD::StateEnum contactState>
using	contactToContactStateKV = std_utils::KeyValuePair< RD::ContactEnum, FD::StateEnum, contact, contactState >

using	mapContactToLandmarkPrediction = std_utils::CompileTimeMap< RD::ContactEnum, FD::ResidualEnum, contactToResidualKV< RD::ContactEnum::LF_FOOT, FD::ResidualEnum::LMK_PRD_LF >, contactToResidualKV< RD::ContactEnum::RF_FOOT, FD::ResidualEnum::LMK_PRD_RF >, contactToResidualKV< RD::ContactEnum::LH_FOOT, FD::ResidualEnum::LMK_PRD_LH >, contactToResidualKV< RD::ContactEnum::RH_FOOT, FD::ResidualEnum::LMK_PRD_RH > >

using	mapContactToContactState = std_utils::CompileTimeMap< RD::ContactEnum, FD::StateEnum, contactToContactStateKV< RD::ContactEnum::LF_FOOT, FD::StateEnum::I_P_LF >, contactToContactStateKV< RD::ContactEnum::RF_FOOT, FD::StateEnum::I_P_RF >, contactToContactStateKV< RD::ContactEnum::LH_FOOT, FD::StateEnum::I_P_LH >, contactToContactStateKV< RD::ContactEnum::RH_FOOT, FD::StateEnum::I_P_RH > >

using	mapContactToLandmarkUpdate = std_utils::CompileTimeMap< RD::ContactEnum, FD::ResidualEnum, contactToResidualKV< RD::ContactEnum::LF_FOOT, FD::ResidualEnum::LMK_UPD_LF >, contactToResidualKV< RD::ContactEnum::RF_FOOT, FD::ResidualEnum::LMK_UPD_RF >, contactToResidualKV< RD::ContactEnum::LH_FOOT, FD::ResidualEnum::LMK_UPD_LH >, contactToResidualKV< RD::ContactEnum::RH_FOOT, FD::ResidualEnum::LMK_UPD_RH > >

using	RD = typename RobotModel::RD

Public Types inherited from tsif::AnymalOdometryTsif< ConcreteRobotDescription_, RobotState_, FilterDefinition_ >
using	Base = FilterWithDefinition< FilterDefinition_ >

using	RobotModel = typename romo::RobotModel< ConcreteRobotDescription_, RobotState_ >

using	RD = typename RobotModel::RD

using	CT = typename ConcreteRobotDescription_::ConcreteTopology

using	ContactEnum = typename CT::ContactEnum

template<typename T >
using	ContactEnumContainer = std_utils::EnumArray< ContactEnum, T >

Public Member Functions
	ImuKinAnymalOdometryTsif (const ros::NodeHandle &node_handle)

	~ImuKinAnymalOdometryTsif () override=default

void	Init (TimePoint t) override

TimePoint	GetLastImuStamp () const override

double	GetLastLandmarkStamp () const override

void	AddMeasurementLinearAccelerationWorldToImuInBase (const TimePoint &time, const Vec3 &B_f_IM) override

void	AddMeasurementAngularVelocityWorldToBaseInBase (const TimePoint &time, const Vec3 &B_omega_IB) override

void	AddMeasurementPositionsBaseToFootInBase (const TimePoint &time, const ContactEnumContainer< Vec3 > &B_r_BSi) override

void	AddMeasurementContacts (const ContactEnumContainer< anymal_state_estimator::Contact > &contacts, bool fakeKinematicsUpdateActive) override

tsif::Vec3	GetPositionWorldToBaseInWorld () const override

tsif::Quat	GetOrientationBaseToWorld () const override

tsif::Vec3	GetLinearVelocityWorldToImuInBase () const override

tsif::Vec3	GetAngularVelocityWorldToBaseInBase () const override

tsif::Vec3	GetLinearAccelerationBiasInBase () const override

tsif::Vec3	GetAngularVelocityBiasInBase () const override

int	GetPositionWorldToBaseInWorldIndex () const override

int	GetOrientationBaseToWorldIndex () const override

int	GetLinearVelocityWorldToImuInBaseIndex () const override

int	GetAngularVelocityWorldToBaseInBaseIndex () const override

int	GetLinearAccelerationBiasInBaseIndex () const override

int	GetAngularVelocityBiasInBaseIndex () const override

Public Member Functions inherited from tsif::AnymalOdometryTsif< ConcreteRobotDescription_, RobotState_, FilterDefinition_ >
	AnymalOdometryTsif ()=default

	~AnymalOdometryTsif () override=default

void	SetInitPose (const kindr::HomTransformQuatD &T_IB_init)

void	SetImuOffset (const Vec3 &B_r_BM)

void	SetInitContacts (const ContactEnumContainer< Vec3 > &B_r_BSi_init)

kindr::HomTransformQuatD &	GetInitPose ()

const kindr::HomTransformQuatD &	GetInitPose () const

Protected Attributes
std::array< double, FD::StateEnum::NUM_STATES >	I_init_

Protected Attributes inherited from tsif::AnymalOdometryTsif< ConcreteRobotDescription_, RobotState_, FilterDefinition_ >
kindr::HomTransformQuatD	T_IB_init_

ContactEnumContainer< Vec3 >	B_r_BSi_init_

Vec3	B_r_BM_

Member Typedef Documentation

◆ Base

using tsif::ImuKinAnymalOdometryTsif::Base = ImuKinAnymalOdometryTsifBase

◆ contactToContactStateKV

template<RD::ContactEnum contact, FD::StateEnum contactState>

using tsif::ImuKinAnymalOdometryTsif::contactToContactStateKV = std_utils::KeyValuePair<RD::ContactEnum, FD::StateEnum, contact, contactState>

◆ contactToResidualKV

template<RD::ContactEnum contact, FD::ResidualEnum residual>

using tsif::ImuKinAnymalOdometryTsif::contactToResidualKV = std_utils::KeyValuePair<RD::ContactEnum, FD::ResidualEnum, contact, residual>

◆ mapContactToContactState

using tsif::ImuKinAnymalOdometryTsif::mapContactToContactState = std_utils::CompileTimeMap<RD::ContactEnum, FD::StateEnum, contactToContactStateKV<RD::ContactEnum::LF_FOOT, FD::StateEnum::I_P_LF>, contactToContactStateKV<RD::ContactEnum::RF_FOOT, FD::StateEnum::I_P_RF>, contactToContactStateKV<RD::ContactEnum::LH_FOOT, FD::StateEnum::I_P_LH>, contactToContactStateKV<RD::ContactEnum::RH_FOOT, FD::StateEnum::I_P_RH> >

◆ mapContactToLandmarkPrediction

using tsif::ImuKinAnymalOdometryTsif::mapContactToLandmarkPrediction = std_utils::CompileTimeMap<RD::ContactEnum, FD::ResidualEnum, contactToResidualKV<RD::ContactEnum::LF_FOOT, FD::ResidualEnum::LMK_PRD_LF>, contactToResidualKV<RD::ContactEnum::RF_FOOT, FD::ResidualEnum::LMK_PRD_RF>, contactToResidualKV<RD::ContactEnum::LH_FOOT, FD::ResidualEnum::LMK_PRD_LH>, contactToResidualKV<RD::ContactEnum::RH_FOOT, FD::ResidualEnum::LMK_PRD_RH> >

◆ mapContactToLandmarkUpdate

using tsif::ImuKinAnymalOdometryTsif::mapContactToLandmarkUpdate = std_utils::CompileTimeMap<RD::ContactEnum, FD::ResidualEnum, contactToResidualKV<RD::ContactEnum::LF_FOOT, FD::ResidualEnum::LMK_UPD_LF>, contactToResidualKV<RD::ContactEnum::RF_FOOT, FD::ResidualEnum::LMK_UPD_RF>, contactToResidualKV<RD::ContactEnum::LH_FOOT, FD::ResidualEnum::LMK_UPD_LH>, contactToResidualKV<RD::ContactEnum::RH_FOOT, FD::ResidualEnum::LMK_UPD_RH> >

◆ RD

using tsif::AnymalOdometryTsif< ConcreteRobotDescription_, RobotState_, FilterDefinition_ >::RD = typename RobotModel::RD

Constructor & Destructor Documentation

◆ ImuKinAnymalOdometryTsif()

tsif::ImuKinAnymalOdometryTsif::ImuKinAnymalOdometryTsif ( const ros::NodeHandle & node_handle )

inlineexplicit

◆ ~ImuKinAnymalOdometryTsif()

tsif::ImuKinAnymalOdometryTsif::~ImuKinAnymalOdometryTsif ( )

overridedefault

Member Function Documentation

◆ AddMeasurementAngularVelocityWorldToBaseInBase()

void tsif::ImuKinAnymalOdometryTsif::AddMeasurementAngularVelocityWorldToBaseInBase	(	const TimePoint &	time,
		const Vec3 &	B_omega_IB
	)

inlineoverridevirtual

Implements tsif::AnymalOdometryTsif< ConcreteRobotDescription_, RobotState_, FilterDefinition_ >.

◆ AddMeasurementContacts()

void tsif::ImuKinAnymalOdometryTsif::AddMeasurementContacts	(	const ContactEnumContainer< anymal_state_estimator::Contact > &	contacts,
		bool	fakeKinematicsUpdateActive
	)