This class implements the flexibility estimation of a robot with the hypothesis that the contact positions do not move. This constraint is expressed using fictious measurements but the interface is transparent to this assumption, the state is expressed using classical representation of position, velocity, acceleration, orientation (using (theta x mu) representation) angular velocity (omega) and acceleration (omega dot). More...

#include <model-base-ekf-flex-estimator-imu.hpp>

Inheritance diagram for stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU:

[legend]

Collaboration diagram for stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU:

[legend]

Classes
struct	contactModel
struct	optimization

Public Member Functions
	ModelBaseEKFFlexEstimatorIMU (double dt=0.005)
	The constructor, it requires the value of the time discretization period.
virtual	~ModelBaseEKFFlexEstimatorIMU ()
	Virtual destructor.
void	setContactsNumber (unsigned i)
	Sets the number of contacts can be changed online.
unsigned	getContactsNumber ()
IMUElasticLocalFrameDynamicalSystem	getFunctor ()
virtual stateObservation::Vector	computeAccelerations ()
void	setContactModel (unsigned nb)
virtual void	setMeasurement (const Vector &y)
	Sets the value of the next sensor measurement y_{k+1}.
virtual void	setProcessNoiseCovariance (const Matrix &Q)
	Sets the process covariance matrice.
virtual void	setMeasurementNoiseCovariance (const Matrix &R)
	Sets the measurements covariance matrice.
virtual Matrix	getProcessNoiseCovariance () const
	gets the covariance matrices for the process noises
virtual Matrix	getMeasurementNoiseCovariance () const
	gets the covariance matrices for the sensor noises
virtual Vector	getMomentaDotFromForces ()
virtual Vector	getMomentaDotFromKinematics ()
virtual Vector	getForcesAndMoments ()
stateObservation::Vector	getStateCovariance () const
virtual void	setComBiasGuess (const stateObservation::Vector &x)
virtual void	setFlexibilityGuess (const Matrix &x)
virtual Matrix4	getFlexibility ()
	Gets an estimation of the flexibility in the form of a homogeneous matrix.
virtual const Vector &	getFlexibilityVector ()
	Gets an estimation of the flexibility in the form of a state vector \hat{x_{k+1}}.
virtual stateObservation::Matrix &	computeLocalObservationMatrix ()
virtual stateObservation::Matrix	getAMatrix ()
virtual stateObservation::Matrix	getCMatrix ()
virtual Index	getMeasurementSize () const
virtual Index	getStateSize () const
virtual Index	getInputSize () const
virtual void	setWithForcesMeasurements (bool)
	sets to whether or not the force mesurements are taken into account
bool	getWithForcesMeasurements ()
virtual void	setWithAbsolutePos (bool)
void	setWithUnmodeledForces (bool b)
bool	getWithUnmodeledForces ()
bool	getWithAbsolutePos ()
virtual void	setWithComBias (bool b)
virtual bool	getWithComBias ()
virtual void	setUnmodeledForceVariance (double d)
virtual void	setUnmodeledForceProcessVariance (double d)
virtual void	setForceVariance (double d)
virtual void	setForceVariance (const Matrix3 &C)
virtual void	setAbsolutePosVariance (double d)
virtual void	setSamplingPeriod (double)
	sets the sampling period
void	setOn (bool &b)
	Enable or disable the estimation.
virtual void	setKfe (const Matrix3 &m)
virtual void	setKfv (const Matrix3 &m)
virtual void	setKte (const Matrix3 &m)
virtual void	setKtv (const Matrix3 &m)
virtual void	setKfeRopes (const Matrix3 &m)
virtual void	setKfvRopes (const Matrix3 &m)
virtual void	setKteRopes (const Matrix3 &m)
virtual void	setKtvRopes (const Matrix3 &m)
virtual void	setPe (const stateObservation::Vector3 &Pe)
virtual Matrix	getKfe () const
virtual Matrix	getKfv () const
virtual Matrix	getKte () const
virtual Matrix	getKtv () const
virtual void	resetCovarianceMatrices ()
	Resets the covariance matrices to their original values.
virtual void	resetStateCovarianceMatrix ()
virtual void	setRobotMass (double m)
virtual double	getRobotMass () const
void	setTorquesLimit (const Vector3 &v)
void	setForcesLimit (const Vector3 &v)
virtual stateObservation::Vector3	getForcesLimit () const
virtual stateObservation::Vector3	getTorquesLimit () const
void	setLimitOn (const bool &b)
virtual bool	getLimitOn () const
Public Member Functions inherited from stateObservation::flexibilityEstimation::EKFFlexibilityEstimatorBase
	EKFFlexibilityEstimatorBase (Index stateSize, Index measurementSize, Index inputSize, const Vector &dx=Vector::Zero(0))
virtual	~EKFFlexibilityEstimatorBase ()
	virtual destructor
virtual void	setFlexibilityCovariance (const Matrix &P)
	Sets the covariance matrix of the flexibility Guess.
virtual Matrix	getFlexibilityCovariance () const
	Gets the covariance matrix of the flexibility.
virtual Vector	getMeasurement ()
virtual void	setInput (const Vector &u)
virtual void	setMeasurementInput (const Vector &u)
virtual Vector	getInput ()
virtual Vector	getMeasurementInput ()
virtual const stateObservation::ExtendedKalmanFilter &	getEKF () const
	Gets a const reference on the extended Kalman filter.
virtual stateObservation::ExtendedKalmanFilter &	getEKF ()
	Gets a reference on the extended Kalman filter.
virtual Vector	getSimulatedMeasurement ()
	Gets a simulation of the.
virtual Vector	getInnovation ()
	Get the last vector of inovation of the Kalman filter.
virtual Vector	getPredictedMeasurement ()
	Get the simulated measurement of the predicted state.
virtual Vector	getPrediction ()
	Get the predicted state.
virtual Vector	getLastPredictedMeasurement ()
	Get the last simulated measurement.
virtual Vector	getLastPrediction ()
	Get the last predicted state.
Public Member Functions inherited from stateObservation::flexibilityEstimation::FlexibilityEstimatorBase
virtual	~FlexibilityEstimatorBase ()
	virtual destructor
	FlexibilityEstimatorBase ()
	The constructor.

Static Public Member Functions
static Matrix	getDefaultQ ()
static Matrix6	getDefaultRIMU ()

Protected Member Functions
virtual void	updateMeasurementCovarianceMatrix_ ()
Protected Member Functions inherited from stateObservation::flexibilityEstimation::EKFFlexibilityEstimatorBase
virtual void	setJacobians (const Matrix &A, const Matrix &C)
virtual void	useFiniteDifferencesJacobians (Vector dx)

Protected Attributes
IMUElasticLocalFrameDynamicalSystem	functor_
Vector	x_
Matrix	R_
Matrix	Q_
Matrix	P_
const Index	stateSize_
Index	inputSize_
double	dt_
bool	on_
double	unmodeledForceVariance_
Matrix	forceVariance_
double	absPosVariance_
bool	useFTSensors_
bool	withAbsolutePos_
bool	withComBias_
bool	withUnmodeledForces_
Vector3	limitTorques_
Vector3	limitForces_
bool	limitOn_
struct stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::optimization	op_
Protected Attributes inherited from stateObservation::flexibilityEstimation::EKFFlexibilityEstimatorBase
stateObservation::ExtendedKalmanFilter	ekf_
bool	finiteDifferencesJacobians_
Vector	dx_
Vector	lastX_
TimeIndex	k_

Static Protected Attributes
static const Index	measurementSizeBase_ = 12
static const Index	inputSizeBase_ = IMUElasticLocalFrameDynamicalSystem::input::sizeBase

Detailed Description

This class implements the flexibility estimation of a robot with the hypothesis that the contact positions do not move. This constraint is expressed using fictious measurements but the interface is transparent to this assumption, the state is expressed using classical representation of position, velocity, acceleration, orientation (using (theta x mu) representation) angular velocity (omega) and acceleration (omega dot).

Definition at line 38 of file model-base-ekf-flex-estimator-imu.hpp.

Member Function Documentation

◆ getContactsNumber()

unsigned stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::getContactsNumber ( )

inline

Definition at line 58 of file model-base-ekf-flex-estimator-imu.hpp.

◆ getFunctor()

IMUElasticLocalFrameDynamicalSystem stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::getFunctor ( )

inline

Definition at line 63 of file model-base-ekf-flex-estimator-imu.hpp.

◆ computeAccelerations()

virtual stateObservation::Vector stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::computeAccelerations ( )

inlinevirtual

Definition at line 68 of file model-base-ekf-flex-estimator-imu.hpp.

◆ setMeasurement()

virtual void stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::setMeasurement ( const Vector & y )

virtual

Sets the value of the next sensor measurement y_{k+1}.

Reimplemented from stateObservation::flexibilityEstimation::EKFFlexibilityEstimatorBase.

◆ setProcessNoiseCovariance()

virtual void stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::setProcessNoiseCovariance ( const Matrix & Q )

virtual

Sets the process covariance matrice.

Reimplemented from stateObservation::flexibilityEstimation::EKFFlexibilityEstimatorBase.

◆ setMeasurementNoiseCovariance()

virtual void stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::setMeasurementNoiseCovariance ( const Matrix & R )

virtual

Sets the measurements covariance matrice.

Reimplemented from stateObservation::flexibilityEstimation::EKFFlexibilityEstimatorBase.

◆ getProcessNoiseCovariance()

virtual Matrix stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::getProcessNoiseCovariance ( ) const

virtual

gets the covariance matrices for the process noises

Reimplemented from stateObservation::flexibilityEstimation::EKFFlexibilityEstimatorBase.

◆ getMeasurementNoiseCovariance()

virtual Matrix stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::getMeasurementNoiseCovariance ( ) const

virtual

gets the covariance matrices for the sensor noises

Reimplemented from stateObservation::flexibilityEstimation::EKFFlexibilityEstimatorBase.

◆ getStateCovariance()

stateObservation::Vector stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::getStateCovariance ( ) const

inline

Definition at line 95 of file model-base-ekf-flex-estimator-imu.hpp.

◆ setFlexibilityGuess()

virtual void stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::setFlexibilityGuess ( const Matrix & x )

virtual

Sets a value of the flexibility x_k provided from another source can be used for initialization of the estimator

Implements stateObservation::flexibilityEstimation::EKFFlexibilityEstimatorBase.

◆ getFlexibility()

virtual Matrix4 stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::getFlexibility ( )

virtual

Gets an estimation of the flexibility in the form of a homogeneous matrix.

Implements stateObservation::flexibilityEstimation::EKFFlexibilityEstimatorBase.

◆ getFlexibilityVector()

virtual const Vector & stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::getFlexibilityVector ( )

virtual

Gets an estimation of the flexibility in the form of a state vector \hat{x_{k+1}}.

Reimplemented from stateObservation::flexibilityEstimation::EKFFlexibilityEstimatorBase.

◆ getAMatrix()

virtual stateObservation::Matrix stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::getAMatrix ( )

inlinevirtual

Definition at line 116 of file model-base-ekf-flex-estimator-imu.hpp.

◆ getCMatrix()

virtual stateObservation::Matrix stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::getCMatrix ( )

inlinevirtual

Definition at line 121 of file model-base-ekf-flex-estimator-imu.hpp.

◆ getMeasurementSize()

virtual Index stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::getMeasurementSize ( ) const

virtual

Gets the measurements size this method is pure virtual and reauires to be overloaded in implementation

Implements stateObservation::flexibilityEstimation::EKFFlexibilityEstimatorBase.

◆ getStateSize()

virtual Index stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::getStateSize ( ) const

virtual

Gets the state size this method is pure virtual and reauires to be overloaded in implementation

Implements stateObservation::flexibilityEstimation::EKFFlexibilityEstimatorBase.

◆ getInputSize()

virtual Index stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::getInputSize ( ) const

virtual

Gets the input size this method is pure virtual and reauires to be overloaded in implementation

Implements stateObservation::flexibilityEstimation::EKFFlexibilityEstimatorBase.

◆ getWithUnmodeledForces()

bool stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::getWithUnmodeledForces ( )

inline

Definition at line 141 of file model-base-ekf-flex-estimator-imu.hpp.

◆ getWithAbsolutePos()

bool stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::getWithAbsolutePos ( )

inline

Definition at line 146 of file model-base-ekf-flex-estimator-imu.hpp.

◆ getWithComBias()

virtual bool stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::getWithComBias ( )

inlinevirtual

Definition at line 153 of file model-base-ekf-flex-estimator-imu.hpp.

◆ setForceVariance()

virtual void stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::setForceVariance ( double d )

virtual

sets the force sensor variance, either with a diagonal matrix with constant diagonal, or using a 3x3 matrix

◆ setPe()

virtual void stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::setPe ( const stateObservation::Vector3 & Pe )

inlinevirtual

Definition at line 184 of file model-base-ekf-flex-estimator-imu.hpp.

◆ resetCovarianceMatrices()

virtual void stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::resetCovarianceMatrices ( )

virtual

Resets the covariance matrices to their original values.

Implements stateObservation::flexibilityEstimation::EKFFlexibilityEstimatorBase.

◆ getRobotMass()

virtual double stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::getRobotMass ( ) const

inlinevirtual

Definition at line 199 of file model-base-ekf-flex-estimator-imu.hpp.

◆ setTorquesLimit()

void stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::setTorquesLimit ( const Vector3 & v )

inline

Definition at line 204 of file model-base-ekf-flex-estimator-imu.hpp.

◆ setForcesLimit()

void stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::setForcesLimit ( const Vector3 & v )

inline

Definition at line 209 of file model-base-ekf-flex-estimator-imu.hpp.

◆ getForcesLimit()

virtual stateObservation::Vector3 stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::getForcesLimit ( ) const

inlinevirtual

Definition at line 214 of file model-base-ekf-flex-estimator-imu.hpp.

◆ getTorquesLimit()

virtual stateObservation::Vector3 stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::getTorquesLimit ( ) const

inlinevirtual

Definition at line 219 of file model-base-ekf-flex-estimator-imu.hpp.

◆ setLimitOn()

void stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::setLimitOn ( const bool & b )

inline

Definition at line 224 of file model-base-ekf-flex-estimator-imu.hpp.

◆ getLimitOn()

virtual bool stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::getLimitOn ( ) const

inlinevirtual

Definition at line 229 of file model-base-ekf-flex-estimator-imu.hpp.

Member Data Documentation

◆ functor_

IMUElasticLocalFrameDynamicalSystem stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::functor_

protected

Definition at line 243 of file model-base-ekf-flex-estimator-imu.hpp.

◆ x_

Vector stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::x_

protected

Definition at line 245 of file model-base-ekf-flex-estimator-imu.hpp.

◆ R_

Matrix stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::R_

protected

Definition at line 247 of file model-base-ekf-flex-estimator-imu.hpp.

◆ Q_

Matrix stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::Q_

protected

Definition at line 247 of file model-base-ekf-flex-estimator-imu.hpp.

◆ P_

Matrix stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::P_

protected

Definition at line 247 of file model-base-ekf-flex-estimator-imu.hpp.

◆ stateSize_

const Index stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::stateSize_

protected

Definition at line 249 of file model-base-ekf-flex-estimator-imu.hpp.

◆ measurementSizeBase_

const Index stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::measurementSizeBase_ = 12

staticprotected

Definition at line 251 of file model-base-ekf-flex-estimator-imu.hpp.

◆ inputSizeBase_

const Index stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::inputSizeBase_ = IMUElasticLocalFrameDynamicalSystem::input::sizeBase

staticprotected

Definition at line 253 of file model-base-ekf-flex-estimator-imu.hpp.

◆ inputSize_

Index stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::inputSize_

protected

Definition at line 254 of file model-base-ekf-flex-estimator-imu.hpp.

◆ dt_

double stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::dt_

protected

Definition at line 256 of file model-base-ekf-flex-estimator-imu.hpp.

◆ on_

bool stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::on_

protected

Definition at line 257 of file model-base-ekf-flex-estimator-imu.hpp.

◆ unmodeledForceVariance_

double stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::unmodeledForceVariance_

protected

Definition at line 259 of file model-base-ekf-flex-estimator-imu.hpp.

◆ forceVariance_

Matrix stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::forceVariance_

protected

Definition at line 260 of file model-base-ekf-flex-estimator-imu.hpp.

◆ absPosVariance_

double stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::absPosVariance_

protected

Definition at line 261 of file model-base-ekf-flex-estimator-imu.hpp.

◆ useFTSensors_

bool stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::useFTSensors_

protected

Definition at line 263 of file model-base-ekf-flex-estimator-imu.hpp.

◆ withAbsolutePos_

bool stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::withAbsolutePos_

protected

Definition at line 265 of file model-base-ekf-flex-estimator-imu.hpp.

◆ withComBias_

bool stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::withComBias_

protected

Definition at line 266 of file model-base-ekf-flex-estimator-imu.hpp.

◆ withUnmodeledForces_

bool stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::withUnmodeledForces_

protected

Definition at line 267 of file model-base-ekf-flex-estimator-imu.hpp.

◆ limitTorques_

Vector3 stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::limitTorques_

protected

Definition at line 269 of file model-base-ekf-flex-estimator-imu.hpp.

◆ limitForces_

Vector3 stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::limitForces_

protected

Definition at line 270 of file model-base-ekf-flex-estimator-imu.hpp.

◆ limitOn_

bool stateObservation::flexibilityEstimation::ModelBaseEKFFlexEstimatorIMU::limitOn_

protected

Definition at line 271 of file model-base-ekf-flex-estimator-imu.hpp.

The documentation for this class was generated from the following file:

/build/source/include/state-observation/flexibility-estimation/model-base-ekf-flex-estimator-imu.hpp

Classes

Public Member Functions

Static Public Member Functions

Protected Member Functions

Protected Attributes

Static Protected Attributes

Detailed Description

Member Function Documentation

◆ getContactsNumber()

◆ getFunctor()

◆ computeAccelerations()

◆ setMeasurement()

◆ setProcessNoiseCovariance()

◆ setMeasurementNoiseCovariance()

◆ getProcessNoiseCovariance()

◆ getMeasurementNoiseCovariance()

◆ getStateCovariance()

◆ setFlexibilityGuess()

◆ getFlexibility()

◆ getFlexibilityVector()

◆ getAMatrix()

◆ getCMatrix()

◆ getMeasurementSize()

◆ getStateSize()

◆ getInputSize()

◆ getWithUnmodeledForces()

◆ getWithAbsolutePos()

◆ getWithComBias()

◆ setForceVariance()

◆ setPe()

◆ resetCovarianceMatrices()

◆ getRobotMass()

◆ setTorquesLimit()

◆ setForcesLimit()

◆ getForcesLimit()

◆ getTorquesLimit()

◆ setLimitOn()

◆ getLimitOn()

Member Data Documentation

◆ functor_

◆ x_

◆ R_

◆ Q_

◆ P_

◆ stateSize_

◆ measurementSizeBase_

◆ inputSizeBase_

◆ inputSize_

◆ dt_

◆ on_

◆ unmodeledForceVariance_

◆ forceVariance_

◆ absPosVariance_

◆ useFTSensors_

◆ withAbsolutePos_

◆ withComBias_

◆ withUnmodeledForces_

◆ limitTorques_

◆ limitForces_

◆ limitOn_