BipedalLocomotion::Estimators::RobotDynamicsEstimator::UkfState class

UkfState is a concrete class that represents the State of the estimator.

The user should build the dynamic model of the state setting a variable handler describing the variables composing the state, the list of the dynamic model associated to each variable in the variable handler, and the matrix of covariances associated to the state. The user should set also a ukf input provider which provides the inputs needed to update the ukf process dynamics.

Public static functions

static auto build(std::weak_ptr<const ParametersHandler::IParametersHandler> handler, std::shared_ptr<iDynTree::KinDynComputations> kinDynFullModel, const std::vector<SubModel>& subModelList, const std::vector<std::shared_ptr<KinDynWrapper>>& kinDynWrapperList) -> std::unique_ptr<UkfState>
Build the ukf state model.

Public functions

auto initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> handler) -> bool
Initialize the ukf state model.
auto finalize(const System::VariablesHandler& handler) -> bool
Finalize the UkfState.
void setUkfInputProvider(std::shared_ptr<const UkfInputProvider> ukfInputProvider)
setUkfInputProvider set the provider for the ukf input.
auto getStateVariableHandler() const -> const System::VariablesHandler&
getStateVariableHandler access the System::VariablesHandler instance created during the initialization phase.
void propagate(const Eigen::Ref<const Eigen::MatrixXd>& currentStates, Eigen::Ref<Eigen::MatrixXd> propagatedStates) override
propagate implements the prediction phase of the ukf
auto getNoiseCovarianceMatrix() -> Eigen::MatrixXd override
getNoiseCovarianceMatrix access the Eigen::MatrixXd representing the process covariance matrix.
auto setProperty(const std::string& property) -> bool override
setProperty is not implemented.
auto getStateDescription() -> bfl::VectorDescription override
getStateDescription access the bfl::VectorDescription.
auto getStateSize() -> std::size_t
getStateSize access the length of the state vector.
auto getInitialStateCovarianceMatrix() const -> Eigen::Ref<const Eigen::MatrixXd>
getInitialStateCovarianceMatrix access the Eigen::MatrixXd representing the initial state covariance matrix.

Function documentation

static std::unique_ptr<UkfState> BipedalLocomotion::Estimators::RobotDynamicsEstimator::UkfState::build(std::weak_ptr<const ParametersHandler::IParametersHandler> handler, std::shared_ptr<iDynTree::KinDynComputations> kinDynFullModel, const std::vector<SubModel>& subModelList, const std::vector<std::shared_ptr<KinDynWrapper>>& kinDynWrapperList)

Build the ukf state model.

Parameters
handler pointer to the IParametersHandler interface.
kinDynFullModel
subModelList a vector of pairs (SubModel, KinDynWrapper) that will be shared among all the dynamics.
kinDynWrapperList
Returns a std::unique_ptr to the UkfState. In case of issues, an empty BipedalLocomotion::System::VariablesHandler and an invalid pointer will be returned.
# UkfState.ini

dynamics_list                   ("JOINT_VELOCITY", "FRICTION_TORQUE", "RIGHT_LEG_FT", "RIGHT_FOOT_REAR_GYRO_BIAS")

[JOINT_VELOCITY]
name                            "ds"
elements                        ("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll")
covariance                      (1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3)
dynamic_model                   "JointVelocityDynamics"

[FRICTION_TORQUE]
name                            "tau_F"
elements                        ("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll")
covariance                      (1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-1)
dynamic_model                   "FrictionTorqueDynamics"
friction_k0                     (9.106, 5.03, 4.93, 12.88, 14.34, 1.12)
friction_k1                     (200.0, 6.9, 200.0, 59.87, 200.0, 200.0)
friction_k2                     (1.767, 5.64, 0.27, 2.0, 3.0, 0.0)

[RIGHT_LEG_FT]
name                            "r_leg_ft_sensor"
elements                        ("fx", "fy", "fz", "mx", "my", "mz")
covariance                      (1e-3, 1e-3, 1e-3, 1e-4, 1e-4, 1e-4)
dynamic_model                   "ZeroVelocityStateDynamics"

[RIGHT_FOOT_REAR_GYRO_BIAS]
name                            "r_foot_rear_ft_gyro_bias"
elements                        ("x", "y", "z")
covariance                      (8.2e-8, 1e-2, 9.3e-3)
dynamic_model                   "ZeroVelocityStateDynamics"

bool BipedalLocomotion::Estimators::RobotDynamicsEstimator::UkfState::initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> handler)

Initialize the ukf state model.

Parameters
handler pointer to the IParametersHandler interface.
Returns True in case of success, false otherwise.

bool BipedalLocomotion::Estimators::RobotDynamicsEstimator::UkfState::finalize(const System::VariablesHandler& handler)

Finalize the UkfState.

Parameters
handler variable handler.
Returns true in case of success, false otherwise.

void BipedalLocomotion::Estimators::RobotDynamicsEstimator::UkfState::setUkfInputProvider(std::shared_ptr<const UkfInputProvider> ukfInputProvider)

setUkfInputProvider set the provider for the ukf input.

Parameters
ukfInputProvider a pointer to a structure containing the joint positions and the robot base pose, velocity and acceleration.

const System::VariablesHandler& BipedalLocomotion::Estimators::RobotDynamicsEstimator::UkfState::getStateVariableHandler() const

getStateVariableHandler access the System::VariablesHandler instance created during the initialization phase.

Returns the state variable handler containing all the state variables and their sizes and offsets.

Eigen::MatrixXd BipedalLocomotion::Estimators::RobotDynamicsEstimator::UkfState::getNoiseCovarianceMatrix() override

getNoiseCovarianceMatrix access the Eigen::MatrixXd representing the process covariance matrix.

Returns the process noise covariance matrix.

bool BipedalLocomotion::Estimators::RobotDynamicsEstimator::UkfState::setProperty(const std::string& property) override

setProperty is not implemented.

Parameters
property is a string.
Returns false as it is not implemented.

bfl::VectorDescription BipedalLocomotion::Estimators::RobotDynamicsEstimator::UkfState::getStateDescription() override

getStateDescription access the bfl::VectorDescription.

Returns the state vector description.

std::size_t BipedalLocomotion::Estimators::RobotDynamicsEstimator::UkfState::getStateSize()

getStateSize access the length of the state vector.

Returns the length of state vector.

Eigen::Ref<const Eigen::MatrixXd> BipedalLocomotion::Estimators::RobotDynamicsEstimator::UkfState::getInitialStateCovarianceMatrix() const

getInitialStateCovarianceMatrix access the Eigen::MatrixXd representing the initial state covariance matrix.

Returns a Eigen reference to the Eigen Matrix covariance.