class
UkfStateUkfState 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::
instance created during the initialization phase.VariablesHandler - 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:: |
# 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::
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. |
---|