BipedalLocomotion::Estimators::RobotDynamicsEstimator::AccelerometerMeasurementDynamics class

The AccelerometerMeasurementDynamics class is a concrete implementation of the Dynamics.

Please use this element if you want to use the model dynamics of an accelerometer defined, using the kinematics, as the time derivative of the frame linear velocity:

\[ v = J \nu \]

The AccelerometerMeasurementDynamics represents the following equation in the continuous time:

\[ \dot{v}^{accelerometer} = \dot{J} \nu + J \dot{\nu} = \dot{J} \nu + J^{base} \dot{v}^{base} + J^{joints} \ddot{s} \]

where the joint acceleration is given by the forward dynamics equation.

Base classes

class Dynamics
The class Dynamics represents a base class describing a generic model composing the ukf process model or the ukf measurement model.

Constructors, destructors, conversion operators

AccelerometerMeasurementDynamics()
~AccelerometerMeasurementDynamics() virtual

Public functions

auto initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler) -> bool override
Initialize the state dynamics.
auto finalize(const System::VariablesHandler& stateVariableHandler) -> bool override
Finalize the Dynamics.
auto setSubModels(const std::vector<SubModel>& subModelList, const std::vector<std::shared_ptr<KinDynWrapper>>& kinDynWrapperList) -> bool override
Set the KinDynWrapper object.
auto checkStateVariableHandler() -> bool override
Controls whether the variable handler contains the variables on which the dynamics depend.
auto update() -> bool override
Update the content of the element.
void setState(const Eigen::Ref<const Eigen::VectorXd> ukfState) override
Set the state of the ukf needed to update the dynamics of the measurement variable associated to ths object.
void setInput(const UKFInput& ukfInput) override
Set a UKFInput object.

Function documentation

bool BipedalLocomotion::Estimators::RobotDynamicsEstimator::AccelerometerMeasurementDynamics::initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler) override

Initialize the state dynamics.

Parameters
paramHandler pointer to the parameters handler.
Returns True in case of success, false otherwise.

bool BipedalLocomotion::Estimators::RobotDynamicsEstimator::AccelerometerMeasurementDynamics::finalize(const System::VariablesHandler& stateVariableHandler) override

Finalize the Dynamics.

Parameters
stateVariableHandler object describing the variables in the state vector.
Returns true in case of success, false otherwise.

bool BipedalLocomotion::Estimators::RobotDynamicsEstimator::AccelerometerMeasurementDynamics::setSubModels(const std::vector<SubModel>& subModelList, const std::vector<std::shared_ptr<KinDynWrapper>>& kinDynWrapperList) override

Set the KinDynWrapper object.

Parameters
subModelList list of SubModel objects
kinDynWrapperList list of pointers to KinDynWrapper objects.
Returns True in case of success, false otherwise.

bool BipedalLocomotion::Estimators::RobotDynamicsEstimator::AccelerometerMeasurementDynamics::checkStateVariableHandler() override

Controls whether the variable handler contains the variables on which the dynamics depend.

Returns True in case all the dependencies are contained in the variable handler, false otherwise.

bool BipedalLocomotion::Estimators::RobotDynamicsEstimator::AccelerometerMeasurementDynamics::update() override

Update the content of the element.

Returns True in case of success, false otherwise.

void BipedalLocomotion::Estimators::RobotDynamicsEstimator::AccelerometerMeasurementDynamics::setState(const Eigen::Ref<const Eigen::VectorXd> ukfState) override

Set the state of the ukf needed to update the dynamics of the measurement variable associated to ths object.

Parameters
ukfState reference to the ukf state.

void BipedalLocomotion::Estimators::RobotDynamicsEstimator::AccelerometerMeasurementDynamics::setInput(const UKFInput& ukfInput) override

Set a UKFInput object.

Parameters
ukfInput reference to the UKFInput struct.