BipedalLocomotion::Estimators::RobotDynamicsEstimator::UkfState class
#include <BipedalLocomotion/RobotDynamicsEstimator/UkfState.h>

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.

Note

the following parameters are required by the class

Parameter Name	Type	Description	Mandatory
`dT`	`double`	Sampling time.	Yes
`dynamics_list`	`vector<string>`	List of dynamics composing the state model.	Yes

For each dynamics listed in the parameter dynamics_list the user must specified all the parameters required by the dynamics itself but dT since is already specified in the parent group. Moreover the following parameters are required for each dynamics.

Group	Parameter Name	Type	Description	Mandatory
`DYNAMICS_NAME`	`name`	`string`	String representing the name of the dynamics.	Yes
`DYNAMICS_NAME`	`elements`	`std::vector<string>`	Vector of strings representing the elements composing the specific dynamics.	No
`DYNAMICS_NAME`	`covariance`	`std::vector<double>`	Vector of double containing the covariance associated to each element.	Yes
`DYNAMICS_NAME`	`dynamic_model`	`string`	String representing the type of dynamics. The string should match the name of the C++ class.	Yes
`DYNAMICS_NAME`	`friction_k0`	`std::vector<double>`	Vector of double containing the coefficient k0 of the friction model of each element.	No
`DYNAMICS_NAME`	`friction_k1`	`std::vector<double>`	Vector of double containing the coefficient k1 of the friction model of each element.	No
`DYNAMICS_NAME`	`friction_k2`	`std::vector<double>`	Vector of double containing the coefficient k2 of the friction model of each element.	No

DYNAMICS_NAME is a placeholder for the name of the dynamics contained in the dynamics_list list. name can contain only the following values ("ds", "tau_m", "tau_F", "*_ft_sensor", "*_ft_sensor_bias", "*_ft_acc_bias", "*_ftgyro_bias").

The following ini file presents an example of the configuration that can be used to build the UkfState.

# 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.