BipedalLocomotion::Estimators::RobotDynamicsEstimator::UkfMeasurement class

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.

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