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

UkfMeasurement is a concrete class that represents the Measurement of the estimator.

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

Public static functions

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

Public functions

auto initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> handler) -> bool: Initialize the ukf measurement model.
auto finalize(const System::VariablesHandler& handler) -> bool: Finalize the UkfMeasurement.
void setUkfInputProvider(std::shared_ptr<const UkfInputProvider> ukfInputProvider): setUkfInputProvider set the provider for the ukf input.
auto getMeasurementVariableHandler() const -> const System::VariablesHandler&: getMeasurementVariableHandler access the System::VariablesHandler instance created during the initialization phase.
auto predictedMeasure(const Eigen::Ref<const Eigen::MatrixXd>& currentState) const -> std::pair<bool, bfl::Data> override: predictedMeasure predict the new measurement depending on the state computed by the predict step.
auto getNoiseCovarianceMatrix() const -> std::pair<bool, 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 getMeasurementDescription() const -> bfl::VectorDescription override: getMeasurementDescription access the bfl::VectorDescription.
auto getInputDescription() const -> bfl::VectorDescription override: getInputDescription access the bfl::VectorDescription.
auto getMeasurementSize() -> std::size_t: getMeasurementSize access the length of the measurement vector
void setStateVariableHandler(System::VariablesHandler stateVariableHandler): Set a System::VariableHandler describing the variables composing the state.
auto innovation(const bfl::Data& predictedMeasurements, const bfl::Data& measurements) const -> std::pair<bool, bfl::Data> override: innovation computes the innovation step of the ukf update as the difference between the predicted_measurement and the measurement.
auto measure(const bfl::Data& data = bfl::Data()) const -> std::pair<bool, bfl::Data> override: measure get the updated measurement.
auto freeze(const bfl::Data& data = bfl::Data()) -> bool override: freeze update the measurement using data from sensors.
void setStateNameMapping(const std::map<std::string, std::string>& stateToUkfNames): setMeasurementNameMapping set the mapping between the name of the measurement and the name of the variable in the ukf.

Function documentation

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

Build the ukf measurement model.

Parameters
handler	pointer to the IParametersHandler interface.
stateVariableHandler	a variable handler describing the variables in the state vector of the ukf.
kinDynFullModel
subModelList	a vector of SubModel objects.
kinDynWrapperList	a vector of pointers to KinDynWrapper objects
Returns	a std::unique_ptr to the UkfMeasurement. 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
`sampling_time`	`double`	Sampling time.	Yes
`dynamics_list`	`vector<string>`	List of dynamics composing the measurement 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`	`use_bias`	`boolean`	Boolean saying if an additive bias must be used in the dynamic model.	No
`DYNAMICS_NAME`	`use_bias`	`boolean`	Boolean saying if an additive bias must be used in the dynamic model.	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", "i_m", "*_ft_sensor", "*_ft_acc", "*_ft_gyro").

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

# UkfMeasurement.ini

dynamics_list                   ("JOINT_VELOCITY", "MOTOR_CURRENT", "RIGHT_LEG_FT", "RIGHT_FOOT_REAR_GYRO")

[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                   "ConstantMeasurementModel"

[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                   "ConstantMeasurementModel"

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

Initialize the ukf measurement model.

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

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

Finalize the UkfMeasurement.

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

void BipedalLocomotion::Estimators::RobotDynamicsEstimator::UkfMeasurement::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::UkfMeasurement::getMeasurementVariableHandler() const

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

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

std::pair<bool, bfl::Data> BipedalLocomotion::Estimators::RobotDynamicsEstimator::UkfMeasurement::predictedMeasure(const Eigen::Ref<const Eigen::MatrixXd>& currentState) const override

predictedMeasure predict the new measurement depending on the state computed by the predict step.

Returns	a std::pair<bool, bfl::Data> where the bool value says if the measurement prediciton is done correctly and the bfl::Data is the predicted measure.

std::pair<bool, Eigen::MatrixXd> BipedalLocomotion::Estimators::RobotDynamicsEstimator::UkfMeasurement::getNoiseCovarianceMatrix() const override

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

Returns	a boolean value and the measurement noise covariance matrix.

bool BipedalLocomotion::Estimators::RobotDynamicsEstimator::UkfMeasurement::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::UkfMeasurement::getMeasurementDescription() const override

getMeasurementDescription access the bfl::VectorDescription.

Returns	the measurement vector description.

bfl::VectorDescription BipedalLocomotion::Estimators::RobotDynamicsEstimator::UkfMeasurement::getInputDescription() const override

getInputDescription access the bfl::VectorDescription.

Returns	the input vector description.

std::size_t BipedalLocomotion::Estimators::RobotDynamicsEstimator::UkfMeasurement::getMeasurementSize()

getMeasurementSize access the length of the measurement vector

Returns	the length of measurement vector

void BipedalLocomotion::Estimators::RobotDynamicsEstimator::UkfMeasurement::setStateVariableHandler(System::VariablesHandler stateVariableHandler)

Set a System::VariableHandler describing the variables composing the state.

Parameters
stateVariableHandler	is the variable handler

std::pair<bool, bfl::Data> BipedalLocomotion::Estimators::RobotDynamicsEstimator::UkfMeasurement::innovation(const bfl::Data& predictedMeasurements, const bfl::Data& measurements) const override

innovation computes the innovation step of the ukf update as the difference between the predicted_measurement and the measurement.

Parameters
predictedMeasurements
measurements	is a `blf::Data` reference representing the measurements coming from the user.
Returns	a `std::pair<bool, bfl::Data>` where the boolean value is always true and the `bfl::Data` is the innovation term.

std::pair<bool, bfl::Data> BipedalLocomotion::Estimators::RobotDynamicsEstimator::UkfMeasurement::measure(const bfl::Data& data = bfl::Data()) const override

measure get the updated measurement.

Parameters
data	is a `const blf::Data` reference and is optional parameter.
Returns	a pair of a boolean value which is always true and the measurements.

bool BipedalLocomotion::Estimators::RobotDynamicsEstimator::UkfMeasurement::freeze(const bfl::Data& data = bfl::Data()) override

freeze update the measurement using data from sensors.

Parameters
data	is a generic object representing data coming from sensors.
Returns	true.

void BipedalLocomotion::Estimators::RobotDynamicsEstimator::UkfMeasurement::setStateNameMapping(const std::map<std::string, std::string>& stateToUkfNames)

setMeasurementNameMapping set the mapping between the name of the measurement and the name of the variable in the ukf.