stateObservation::KineticsObserver Class Reference

This observer estimates the kinematics, the external forces, the bias on the gyrometers measurements, and the contacts forces and pose. More...

#include <state-observation/dynamics-estimators/kinetics-observer.hpp>

Inheritance diagram for stateObservation::KineticsObserver:

Collaboration diagram for stateObservation::KineticsObserver:

Classes
struct	AbsoluteOriSensor
struct	AbsolutePoseSensor
struct	Contact
struct	IMU
struct	Opt
	a structure to optimize computations More...
struct	Sensor

Public Types
typedef kine::Kinematics	Kinematics
typedef kine::LocalKinematics	LocalKinematics
typedef kine::Orientation	Orientation

Public Member Functions
Constructors and destructors
	KineticsObserver (unsigned maxContacts=4, unsigned maxNumberOfIMU=1)
	Construct a new Kinetics Observer. More...
virtual	~KineticsObserver ()
	Destroy the Kinetics Observer. More...
Setting and getting parameters
For initialization and update of parameters that should not evolve over time on a sample basis.
double	getSamplingTime () const
	Get the Sampling Time. More...
void	setSamplingTime (double)
	Set the Sampling Time. More...
void	setWithUnmodeledWrench (bool b=true)
	Set if the unmodeled and unmeasured external wrench should be estimated. More...
void	setWithAccelerationEstimation (bool b=true)
	Sets if the estimation computes also the accelerations. More...
bool	getWithAccelerationEstimation () const
	Returns if the estimation computes also the accelerations. More...
void	setWithGyroBias (bool b=true)
	Set if the gyrometers bias is computed or not. This parameter is global for all the IMUs. More...
void	setMass (double)
	Set the total mass of the robot. This can be changed online. More...
double	getMass () const
	Returns the mass of the robot. More...
const IndexedMatrix3 &	getInertiaMatrix () const
	Returns the global inertia matrix of the robot at the center of mass. More...
const IndexedMatrix3 &	getInertiaMatrixDot () const
	Returns the derivative of the global inertia matrix of the robot at the center of mass. More...
const IndexedVector3 &	getAngularMomentum () const
	Returns the angular momentum of the robot at the center of mass. More...
const IndexedVector3 &	getAngularMomentumDot () const
	Returns the derivative of the angular momentum of the robot at the center of mass. More...
const IndexedVector3 &	getCenterOfMass () const
	Returns the position of the CoM of the robot in the user frame. More...
const IndexedVector3 &	getCenterOfMassDot () const
	Returns the linear velocity of the CoM of the robot in the user frame. More...
const IndexedVector3 &	getCenterOfMassDotDot () const
	Returns the linear acceleration of the CoM of the robot in the user frame. More...
Vector6	getAdditionalWrench () const
	Returns the input additional wrench, expressed in the centroid frame. More...
Setting kinematic sensors
These are the methods to be called at each iteration to give the control inputs and the sensor measurement for IMUs and absolute pose sensors. //////////////////////////////////////////////////////////
Index	setIMU (const Vector3 &accelero, const Vector3 &gyrometer, const Kinematics &userImuKinematics, Index num=-1)
	Set the measurements of an IMU and give the Kinematic of the IMU in the user frame. More...
Index	setIMU (const Vector3 &accelero, const Vector3 &gyrometer, const Matrix3 &acceleroCov, const Matrix3 &gyroCov, const Kinematics &userImuKinematics, Index num=-1)
	Provides also the associated covariance matrices More...
void	setIMUDefaultCovarianceMatrix (const Matrix3 &acceleroCov, const Matrix3 &gyroCov)
	set the default covariance matrix for IMU. More...
void	setAbsolutePoseSensor (const Kinematics &measurement)
	Set an Absolute Pose Sensor measurement The measurement is the kinematics namely position and orientation of the observed frame in the global frame. More...
void	setAbsolutePoseSensor (const Kinematics &measurement, const Matrix6 &CovarianceMatrix)
	Set an Absolute Pose Sensor measurement The measurement is the kinematics namely position and orientation of the observed frame in the global frame. More...
void	setAbsolutePoseSensorDefaultCovarianceMatrix (const Matrix6 &covMat)
	Set the Absolute Pose Sensor Default Covariance Matrix. More...
void	setAbsoluteOriSensor (const Orientation &measurement)
	Set an Absolute Orientation Sensor measurement The measurement is the orientation of the observed frame in the global frame. More...
void	setAbsoluteOriSensor (const Orientation &measurement, const Matrix3 &CovarianceMatrix)
	Set an Absolute Orientation Sensor measurement The measurement is the orientation of the observed frame in the global frame. More...
void	setAbsoluteOriSensorDefaultCovarianceMatrix (const Matrix3 &covMat)
	Set the Absolute Orientation Sensor Default Covariance Matrix. More...
Adding, managing and deleting contacts
This class does NOT detect new contacts with the environment. Use these classes instead. Call these only when a new contact is created or removed from the environment, otherwise the contacts will remain the same at each new iteration.
Index	addContact (const Kinematics &pose, const Matrix12 &initialCovarianceMatrix, const Matrix12 &processCovarianceMatrix, Index contactNumber=-1, const Matrix3 &linearStiffness=Matrix3::Constant(-1), const Matrix3 &linearDamping=Matrix3::Constant(-1), const Matrix3 &angularStiffness=Matrix3::Constant(-1), const Matrix3 &angularDamping=Matrix3::Constant(-1))
	Set a new contact with the environment. More...
Index	addContact (const Kinematics &pose, Index contactNumber=-1, const Matrix3 &linearStiffness=Matrix3::Constant(-1), const Matrix3 &linearDamping=Matrix3::Constant(-1), const Matrix3 &angularStiffness=Matrix3::Constant(-1), const Matrix3 &angularDamping=Matrix3::Constant(-1))
	Set a new contact with the environment (use default covariance matrices) More...
void	removeContact (Index contactnbr)
	Remove a contact. More...
void	clearContacts ()
	remove all the contacts More...
Index	getNumberOfSetContacts () const
	Get the Current Number Of Contacts. More...
std::vector< Index >	getListOfContacts () const
	Get the List Of Contact ids. More...
Updating contact information
Calling one of the two following methods (updateContactWithWrenchSensor() and updateContactWithNoSensor()) is MANDATORY for every contact and at every iteration. If the contact is equipped with wrench sensor call updateContactWithWrenchSensor() If not call updateContactWithNoSensor() if the contact is lost, it needs to be explicitely removed using removeContact()
void	updateContactWithNoSensor (const Kinematics &localKine, unsigned contactNumber)
	Update the contact when it is NOT equipped with wrench sensor. More...
void	updateContactWithWrenchSensor (const Vector6 &wrenchMeasurement, const Kinematics &localKine, unsigned contactNumber)
	Update the contact when it is equipped with wrench sensor. More...
void	updateContactWithWrenchSensor (const Vector6 &wrenchMeasurement, const Matrix6 &wrenchCovMatrix, const Kinematics &localKine, unsigned contactNumber)
	Update the contact when it is equipped with wrench sensor. More...
void	setContactWrenchSensorDefaultCovarianceMatrix (const Matrix6 &wrenchSensorCovMat)
	Set the Contact Wrench Sensor Default Covariance Matrix. More...
Setting additional inputs to the dynamical system
It is highly recommended to set these inputs at each iteration ////////////////////////////////////////////////
void	setCenterOfMass (const Vector3 &com, const Vector3 &com_dot, const Vector3 &com_dot_dot)
	Set the Center Of Mass kinematics expressed in the user frame. More...
void	setCenterOfMass (const Vector3 &com, const Vector3 &com_dot)
	Set the Center Of Mass kinematics expressed in the user frame. More...
void	setCenterOfMass (const Vector3 &com)
	Set the Center Of Mass kinematics expressed in the user frame. More...
void	setCoMInertiaMatrix (const Matrix3 &I, const Matrix3 &I_dot)
	Set the 3x3 inertia matrix and its derivative expressed in the user frame. More...
void	setCoMInertiaMatrix (const Matrix3 &I)
	Set the 3x3 inertia matrix expressed in the user frame. More...
void	setCoMInertiaMatrix (const Vector6 &I, const Vector6 &I_dot)
	Set the inertia matrix and its derivative as a Vector6 expressed in the user frame. More...
void	setCoMInertiaMatrix (const Vector6 &I)
	Set the inertia matrix as a Vector6 expressed in the user frame. More...
void	setCoMAngularMomentum (const Vector3 &sigma, const Vector3 &sigma_dot)
	Set the Angular Momentum around the CoM and its derviative expressed in the user frame. More...
void	setCoMAngularMomentum (const Vector3 &sigma)
	Set the Angular Momentum around the CoM expressed in the user frame. More...
void	setAdditionalWrench (const Vector3 &force, const Vector3 &torque)
	Set any Additional resultant known wrench (e.g. measured external forces and moments but no contact ones) expressed in the local estimated frame. More...
Running and getting the estimations
/////////////////////////////////////////////////////////
void	updateMeasurements ()
	Updates the measurements. More...
const Vector &	update ()
	Runs the estimation. More...
void	convertWrenchFromUserToCentroid (const Vector3 &forceUserFrame, const Vector3 &momentUserFrame, Vector3 &forceCentroidFrame, Vector3 &momentCentroidFrame)
	Returns the predicted Kinematics object of the centroid in the world frame at the time of the measurement predictions. More...
LocalKinematics	getLocalCentroidKinematics () const
	Get the estimated local Kinematics of the centroid frame in the world frame (local, which means expressed in the centroid frame). More...
Kinematics	getGlobalCentroidKinematics () const
	Get the estimated Kinematics of the centroid frame in the world frame. More...
LocalKinematics	estimateAccelerations ()
	gets the Kinematics that include the linear and angular accelerations. More...
LocalKinematics	getLocalKinematicsOf (const Kinematics &userBodyKine)
	Get the local kinematics of a given frame (in the user frame) in the centroid frame. More...
Kinematics	getGlobalKinematicsOf (const Kinematics &userBodyKin) const
	Get the global kinematics of a given frame (in the user frame) in the centroid frame. More...
Vector6	getContactWrench (Index contactNbr) const
	Get the Estimated Contact Wrench This is useful in the case of uncertain wrench sensors or when contact force measurement is not available. More...
Kinematics	getContactPosition (Index contactNbr) const
	Get the Contact 6D pose n in the global frame. More...
Vector6	getUnmodeledWrench () const
	Get the Unmodeled External Wrench (requires setWithUnmodeledWrench() to true before to update()) More...
Set values for state components
/////////////////////////////////////////////////////////// These methods allow to update some parts of the state of the system based on guesses obtained independently
void	setWorldCentroidStateKinematics (const LocalKinematics &localKine, bool resetContactWrenches=true, bool resetCovariance=true)
	Set the State Kinematics. More...
void	setWorldCentroidStateKinematics (const Kinematics &kine, bool resetCovariance=true)
	Set the State Kinematics. More...
void	setStateContact (Index index, Kinematics worldContactRestPose, const Vector6 &wrench, bool resetCovariance=true)
	Set the state contact kinematics and wrench. More...
void	setGyroBias (const Vector3 &, unsigned numberOfIMU=1, bool resetCovariance=true)
	Set the Gyro Bias Allows to initializa the value of the gyro bias of the IMU corresponding to the numberOfIMU. More...
void	setStateUnmodeledWrench (const Vector6 &, bool resetCovariance=true)
	Set the State Unmodeled Wrench. More...
Estimator resets
This allows to reset default values for specific parameters of the estimator
void	resetSensorsDefaultCovMats ()
	Reset the default values for the sensors covariance matrices. More...
void	resetInputs ()
	reset all the sensor inputs and provided contact information but keeps the contacts themselves More...
Covariance matrices operations
void	setKinematicsInitCovarianceDefault (const Matrix &)
	Set the Default value for Kinematics Init Covariance. More...
void	setKinematicsInitCovarianceDefault (const Matrix3 &P_pos, const Matrix3 &P_ori, const Matrix3 &P_linVel, const Matrix3 &P_angVel)
	Set the Default value for Kinematics Init Covariance. More...
void	setGyroBiasInitCovarianceDefault (const Matrix3 &covMat)
	Set the Default value for Gyro Bias Init Covariance. More...
void	setUnmodeledWrenchInitCovMatDefault (const Matrix6 &initCovMat)
	Set the default value for init Unmodeled Wrench covariance matrix. More...
void	setContactInitCovMatDefault (const Matrix12 &contactCovMat)
	Set the default valut for the Initial Covariance Matrix of the contact in the state. More...
void	setKinematicsStateCovariance (const Matrix &)
	Set the Kinematics State Covariance. More...
void	setGyroBiasStateCovariance (const Matrix3 &covMat, unsigned imuNumber)
	Set the Gyro Bias State Covariance. More...
void	setUnmodeledWrenchStateCovMat (const Matrix6 &newCovMat)
	Set the Unmodeled Wrench State Cov Mat. More...
void	setContactStateCovMat (Index contactNbr, const Matrix12 &contactCovMat)
	Set the Contact State Covariance Matrix. More...
void	setKinematicsProcessCovarianceDefault (const Matrix12 &)
	Set the default Kinematics Process Covariance. More...
void	setKinematicsProcessCovarianceDefault (const Matrix3 &P_pos, const Matrix3 &P_ori, const Matrix3 &P_linVel, const Matrix3 &P_angVel)
	Set the default Kinematics Process Covariance. More...
void	setGyroBiasProcessCovarianceDefault (const Matrix3 &covMat)
	Set the default Gyro Bias Process Covariance. More...
void	setUnmodeledWrenchProcessCovarianceDefault (const Matrix6 &covMat)
	Set the default Unmodeled Wrench Process Covariance. More...
void	setContactProcessCovarianceDefault (const Matrix12 &covMat)
	Set the default contact Process Covariance. More...
void	setKinematicsProcessCovariance (const Matrix12 &)
	Set the Kinematics Process Covariance. More...
void	setGyroBiasProcessCovariance (const Matrix3 &covMat, unsigned imuNumber)
	Set the Gyro Bias Process Covariance. More...
void	setUnmodeledWrenchProcessCovMat (const Matrix6 &processCovMat)
	Set the Unmodeled Wrench Process Covariance Mattix. More...
void	setContactProcessCovMat (Index contactNbr, const Matrix12 &contactCovMat)
	Set the Contact Process Covariance Matrix. More...
void	resetStateCovarianceMat ()
	Resets the covariance matrices to their original values. More...
void	resetStateKinematicsCovMat ()
void	resetStateGyroBiasCovMat (Index i)
void	resetStateUnmodeledWrenchCovMat ()
void	resetStateContactsCovMat ()
void	resetStateContactCovMat (Index contactNbr)
void	resetProcessCovarianceMat ()
void	resetProcessKinematicsCovMat ()
void	resetProcessGyroBiasCovMat (Index i)
void	resetProcessUnmodeledWrenchCovMat ()
void	resetProcessContactsCovMat ()
void	resetProcessContactCovMat (Index contactNbr)
State vector representation operations (advanced use)
@ this is constituted with Getters and setters for the state vector (from getStateSize() to setStateVector()) Getters for the indexes of the state Vector (from kineIndex() to contactWrenchIndex()) Getters for the indexes of the tangent state Vector (from kineIndexTangent() to contactWrenchIndextangent())
Index	getStateSize () const
	Get the State Vector Size. More...
Index	getStateTangentSize () const
	Get the State Vector Tangent Size. More...
Index	getMeasurementSize () const
	Get the Measurement vector Size. More...
Matrix	getStateCovarianceMat () const
	Get the State Covariance matrix. More...
void	setStateCovarianceMat (const Matrix &P)
	Set the State Covariance Matrix This is useful in case of a setting a guess on a whole state vect9or. More...
void	setProcessNoiseCovarianceMat (const Matrix &Q)
	Set the covariance matrices for the process noises. More...
const Vector &	getCurrentStateVector () const
	Gets the current value of the state estimation in the form of a state vector \(\hat{x_{k}}\). More...
TimeIndex	getStateVectorTimeIndex () const
	Get the State Vector Internal Time Index This is for advanced use but may be used to check how many states have been estimated up to now. More...
void	setInitWorldCentroidStateVector (const Vector &initStateVector)
	Initializes the state vector. More...
void	setStateVector (const Vector &newvalue, bool resetCovariance=true)
	Set a value of the state x_k provided from another source. More...
Vector	getMeasurementVector ()
	Get the Measurement Vector. More...
Index	kineIndex () const
	Get the kinematics index of the state vector. More...
Index	posIndex () const
	Get the position index of the state vector. More...
Index	oriIndex () const
	Get the orientation index of the state vector. More...
Index	linVelIndex () const
	Get the linear velocity index of the state vector. More...
Index	angVelIndex () const
	Get the angular velocity index of the state vector. More...
Index	gyroBiasIndex (Index IMUNumber) const
	Get the gyro bias index of the state vector. More...
Index	unmodeledWrenchIndex () const
	Get the unmodeled external wrench index of the state vector. More...
Index	unmodeledForceIndex () const
	Get the unmodeled external linear force index of the state vector. More...
Index	unmodeledTorqueIndex () const
	Get the unmodeled external torque force index of the state vector. More...
Index	contactsIndex () const
	Get the index for the contact segment in the state vector. More...
Index	contactIndex (Index contactNbr) const
	Get the index of a specific contact in the sate vector. More...
Index	contactKineIndex (Index contactNbr) const
	Get the index of the kinematics of a specific contact in the sate vector. More...
Index	contactPosIndex (Index contactNbr) const
	Get the index of the position of a specific contact in the sate vector. More...
Index	contactOriIndex (Index contactNbr) const
	Get the index of the orientation of a specific contact in the sate vector. More...
Index	contactForceIndex (Index contactNbr) const
	Get the index of the linear force of a specific contact in the sate vector. More...
Index	contactTorqueIndex (Index contactNbr) const
	Get the index of the toraue of a specific contact in the sate vector. More...
Index	contactWrenchIndex (Index contactNbr) const
	Get the index of the wrench of a specific contact in the sate vector. More...
Index	kineIndexTangent () const
	Get the kinematics index of the tangent state vector. More...
Index	posIndexTangent () const
	Get the position index of the tangent state vector. More...
Index	oriIndexTangent () const
	Get the orientation index of the tangent state vector. More...
Index	linVelIndexTangent () const
	Get the linear velocity index of the tangent state vector. More...
Index	angVelIndexTangent () const
	Get the angular velocity index of the tangent state vector. More...
Index	gyroBiasIndexTangent (Index IMUNumber) const
	Get the gyro bias index of the tangent state vector. More...
Index	unmodeledWrenchIndexTangent () const
	Get the unmodeled external wrench index of the tangent state vector. More...
Index	unmodeledForceIndexTangent () const
	Get the unmodeled external linear force index of the tangent state vector. More...
Index	unmodeledTorqueIndexTangent () const
	Get the unmodeled external torque force index of the tangent state vector. More...
Index	contactsIndexTangent () const
	Get the index for the contact segment in the tangent state vector. More...
Index	contactIndexTangent (Index contactNbr) const
	Get the index of a specific contact in the tangent sate vector. More...
Index	contactKineIndexTangent (Index contactNbr) const
	Get the index of the kinematics of a specific contact in the tangent sate vector. More...
Index	contactPosIndexTangent (Index contactNbr) const
	Get the index of the position of a specific contact in the tangent sate vector. More...
Index	contactOriIndexTangent (Index contactNbr) const
	Get the index of the orientation of a specific contact in the tangent sate vector. More...
Index	contactForceIndexTangent (Index contactNbr) const
	Get the index of the linear force of a specific contact in the tangent sate vector. More...
Index	contactTorqueIndexTangent (Index contactNbr) const
	Get the index of the toraue of a specific contact in the tangent sate vector. More...
Index	contactWrenchIndexTangent (Index contactNbr) const
	Get the index of the wrench of a specific contact in the sate tangent vector. More...
Getters for the extended Kalman filter (advanced use)
const ExtendedKalmanFilter &	getEKF () const
	Gets a const reference on the extended Kalman filter. More...
ExtendedKalmanFilter &	getEKF ()
Vector6	getCentroidContactWrench (Index numContact) const
	Returns the wrench exerted at the contact, expressed in the frame of the centroid. More...
Kinematics	getCentroidContactInputPose (Index numContact) const
	Returns the pose of the contact in the centroid frame, given as an input when updating the contact (obtained from its pose in the user frame). More...
Kinematics	getWorldContactPoseFromCentroid (Index numContact) const
	Returns the pose of the contact in the world frame, obtained from the state pose of the centroid in the world frame. More...
Kinematics	getContactStateRestKinematics (Index numContact) const
	Returns the estimated rest pose of the contact in the world frame. More...
Kinematics	getUserContactInputPose (Index numContact) const
	Returns the pose of the contact in the user frame, given as an input when updating the contact. More...
Index	getIMUMeasIndexByNum (Index num) const
	Get the measurement index of the required IMU : allows to access its corresponding measurements in the measurement vector for example. More...
Index	getContactMeasIndexByNum (Index num) const
bool	getContactIsSetByNum (Index num) const

Public Attributes
bool	nanDetected_ = false

Static Public Attributes
static constexpr Index	sizeAcceleroSignal = 3
static constexpr Index	sizeGyroSignal = 3
static constexpr Index	sizeIMUSignal = sizeAcceleroSignal + sizeGyroSignal
static constexpr Index	sizePos = 3
static constexpr Index	sizePosTangent = 3
static constexpr Index	sizeOri = 4
static constexpr Index	sizeOriTangent = 3
static constexpr Index	sizeLinVel = sizePos
static constexpr Index	sizeLinVelTangent = sizeLinVel
static constexpr Index	sizeLinAccTangent = sizeLinVelTangent
static constexpr Index	sizeAngVel = sizeOriTangent
static constexpr Index	sizeAngVelTangent = sizeAngVel
static constexpr Index	sizeGyroBias = sizeGyroSignal
static constexpr Index	sizeGyroBiasTangent = sizeGyroBias
static constexpr Index	sizeForce = 3
static constexpr Index	sizeForceTangent = sizeForce
static constexpr Index	sizeTorque = 3
static constexpr Index	sizeTorqueTangent = sizeTorque
static constexpr Index	sizeWrench = sizeForce + sizeTorque
static constexpr Index	sizeStateKine = sizePos + sizeOri + sizeLinVel + sizeAngVel
static constexpr Index	sizeStateBase = sizeStateKine + sizeForce + sizeTorque
static constexpr Index	sizeStateKineTangent = sizePos + sizeOriTangent + sizeLinVel + sizeAngVel
static constexpr Index	sizeStateTangentBase = sizeStateKineTangent + sizeForce + sizeTorque
static constexpr Index	sizePose = sizePos + sizeOri
static constexpr Index	sizePoseTangent = sizePos + sizeOriTangent
static constexpr Index	sizeContactKine = sizePose
static constexpr Index	sizeContactKineTangent = sizePoseTangent
static constexpr Index	sizeContact = sizeContactKine + sizeWrench
static constexpr Index	sizeContactTangent = sizeContactKineTangent + sizeWrench
static constexpr Kinematics::Flags::Byte	flagsStateKine
static constexpr Kinematics::Flags::Byte	flagsContactKine
static constexpr Kinematics::Flags::Byte	flagsPoseKine
static constexpr Kinematics::Flags::Byte	flagsPosKine = Kinematics::Flags::position
static constexpr Kinematics::Flags::Byte	flagsIMUKine
static const double	defaultMass
static const double	statePoseInitVarianceDefault
static const double	stateOriInitVarianceDefault
static const double	stateLinVelInitVarianceDefault
static const double	stateAngVelInitVarianceDefault
static const double	gyroBiasInitVarianceDefault
static const double	unmodeledWrenchInitVarianceDefault
static const double	contactForceInitVarianceDefault
static const double	contactTorqueInitVarianceDefault
static const double	statePoseProcessVarianceDefault
static const double	stateOriProcessVarianceDefault
static const double	stateLinVelProcessVarianceDefault
static const double	stateAngVelProcessVarianceDefault
static const double	gyroBiasProcessVarianceDefault
static const double	unmodeledWrenchProcessVarianceDefault
static const double	contactPositionProcessVarianceDefault
static const double	contactOrientationProcessVarianceDefault
static const double	contactForceProcessVarianceDefault
static const double	contactTorqueProcessVarianceDefault
static const double	acceleroVarianceDefault
static const double	gyroVarianceDefault
static const double	forceSensorVarianceDefault
static const double	torqueSensorVarianceDefault
static const double	positionSensorVarianceDefault
static const double	orientationSensorVarianceDefault
static const double	linearStiffnessDefault
static const double	angularStiffnessDefault
static const double	linearDampingDefault
static const double	angularDampingDefault

Protected Types
typedef std::vector< IMU, Eigen::aligned_allocator< IMU > >	VectorIMU
typedef VectorIMU::iterator	VectorIMUIterator
typedef VectorIMU::const_iterator	VectorIMUConstIterator
typedef std::vector< Contact, Eigen::aligned_allocator< Contact > >	VectorContact
typedef VectorContact::iterator	VectorContactIterator
typedef VectorContact::const_iterator	VectorContactConstIterator

Protected Member Functions
virtual Vector	stateDynamics (const Vector &x, const Vector &u, TimeIndex k)
	Applies the state-transition model to the given state vector using the given input to predict the future state. More...
virtual Vector	measureDynamics (const Vector &x, const Vector &u, TimeIndex k)
	Applies the measurement model to the given state vector using the given input to predict the sensor measurements. More...
void	addUnmodeledAndContactWrench_ (const Vector &centroidStateVector, Vector3 &force, Vector3 &torque)
	Adds the unmodeled and contact wrenches from the state to the given wrench. More...
void	addUnmodeledWrench_ (const Vector &centroidStateVector, Vector3 &force, Vector3 &torque)
	Adds the unmodeled wrench from the state to the given wrench. More...
void	addContactWrench_ (const Kinematics &centroidContactKine, const Vector3 &centroidContactForce, const Vector3 &centroidContactTorque, Vector3 &totalCentroidForce, Vector3 &totalCentroidTorque)
	adds the contribution of a contact wrench at the centroid to the total wrench More...
void	computeLocalAccelerations_ (LocalKinematics &localStateKine, const Vector3 &totalForceLocal, const Vector3 &totalMomentLocal, Vector3 &linAcc, Vector3 &angAcc)
	Computes the local accelerations of the centroid frame in the world frame and adds them to its local kinematics. More...
void	computeContactForce_ (VectorContactIterator i, LocalKinematics &worldCentroidStateKinematics, Kinematics &worldRestContactPose, Vector3 &contactForce, Vector3 &contactTorque)
	Computes the force exerted at a contact using the visco-elastic model on the given state vector. More...
void	computeContactForces_ (LocalKinematics &worldCentroidStateKinematics, Vector3 &contactForce, Vector3 &contactTorque)
	Computes the force exerted at a contact using the visco-elastic model on the given state vector. Compute the resulting wrench for all currently set contacts. More...
virtual void	setProcessNoise (NoiseBase *)
	Sets a noise which disturbs the state dynamics. More...
virtual void	resetProcessNoise ()
	Removes the process noise. More...
virtual NoiseBase *	getProcessNoise () const
	Gets the process noise. More...
virtual void	setMeasurementNoise (NoiseBase *)
	Sets a noise which disturbs the measurements. More...
virtual void	resetMeasurementNoise ()
	Removes the measurement noise. More...
virtual NoiseBase *	getMeasurementNoise () const
	Gets a pointer on the measurement noise. More...
virtual Index	getInputSize () const
	Gets the input size. More...
void	stateNaNCorrection_ ()
void	updateLocalKineAndContacts_ ()
	update of the state kinematics worldCentroidStateKinematics_ and of the contacts pose with the newly estimated state More...
void	updateGlobalKine_ ()
	updates the global kinematics of the centroid from the local ones, that can be more interpretable More...
void	startNewIteration_ ()
void	convertUserToCentroidFrame_ (const Kinematics &userKine, Kinematics &centroidKine, TimeIndex k_data)
	Converts a LocalKinematics object from the user's frame to the centroid's frame, which is used for most of the computations. More...
Kinematics	convertUserToCentroidFrame_ (const Kinematics &userKine, TimeIndex k_data)
	Converts a Kinematics object from the user's frame to the centroid's frame, which is used for most of the computations. More...
Index	gyroBiasIndex (VectorIMUConstIterator i) const
	Getters for the indexes of the state Vector using private types. More...
Index	gyroBiasIndexTangent (VectorIMUConstIterator i) const
Index	contactIndex (VectorContactConstIterator i) const
Index	contactKineIndex (VectorContactConstIterator i) const
Index	contactPosIndex (VectorContactConstIterator i) const
Index	contactOriIndex (VectorContactConstIterator i) const
Index	contactForceIndex (VectorContactConstIterator i) const
Index	contactTorqueIndex (VectorContactConstIterator i) const
Index	contactWrenchIndex (VectorContactConstIterator i) const
Index	contactIndexTangent (VectorContactConstIterator i) const
	Getters for the indexes of the state Vector using private types. More...
Index	contactKineIndexTangent (VectorContactConstIterator i) const
Index	contactPosIndexTangent (VectorContactConstIterator i) const
Index	contactOriIndexTangent (VectorContactConstIterator i) const
Index	contactForceIndexTangent (VectorContactConstIterator i) const
Index	contactTorqueIndexTangent (VectorContactConstIterator i) const
Index	contactWrenchIndexTangent (VectorContactConstIterator i) const
Protected Member Functions inherited from stateObservation::DynamicalSystemFunctorBase
void	assertStateVector_ (const Vector &v)
void	assertInputVector_ (const Vector &v)
	DynamicalSystemFunctorBase ()
virtual	~DynamicalSystemFunctorBase ()
virtual void	reset ()
virtual bool	checkStateVector (const Vector &)
	Gives a boolean answer on whether or not the vector is correctly sized to be a state vector. More...
virtual bool	checkInputvector (const Vector &)
	Gives a boolean answer on whether or not the vector is correctly sized to be an input vector. More...

Protected Attributes
unsigned	maxContacts_
unsigned	maxImuNumber_
AbsolutePoseSensor	absPoseSensor_
AbsoluteOriSensor	absOriSensor_
VectorContact	contacts_
VectorIMU	imuSensors_
Index	stateSize_
Index	stateTangentSize_
Index	measurementSize_
Index	measurementTangentSize_
Vector	worldCentroidStateVector_
Vector	worldCentroidStateVectorDx_
Vector	oldWorldCentroidStateVector_
LocalKinematics	worldCentroidStateKinematics_
Kinematics	worldCentroidKinematics_
Vector3	additionalForce_
Vector3	additionalTorque_
Vector3	initTotalCentroidForce_
Vector3	initTotalCentroidTorque_
Vector	measurementVector_
Matrix	measurementCovMatrix_
stateObservation::ExtendedKalmanFilter	ekf_
bool	finiteDifferencesJacobians_
bool	withGyroBias_
bool	withUnmodeledWrench_
bool	withAccelerationEstimation_
IndexedVector3	com_
IndexedVector3	comd_
IndexedVector3	comdd_
IndexedVector3	sigma_
IndexedVector3	sigmad_
IndexedMatrix3	I_
IndexedMatrix3	Id_
TimeIndex	k_est_
TimeIndex	k_data_
double	mass_
double	dt_
NoiseBase *	processNoise_
NoiseBase *	measurementNoise_
Index	numberOfContactRealSensors_
Index	currentIMUSensorNumber_
Matrix3	linearStiffnessMatDefault_
	Default Stiffness and damping. More...
Matrix3	angularStiffnessMatDefault_
Matrix3	linearDampingMatDefault_
Matrix3	angularDampingMatDefault_
Matrix3	acceleroCovMatDefault_
Matrix3	gyroCovMatDefault_
Matrix6	contactWrenchSensorCovMatDefault_
Matrix6	absPoseSensorCovMatDefault_
Matrix3	absOriSensorCovMatDefault_
Matrix3	statePosInitCovMat_
Matrix3	stateOriInitCovMat_
Matrix3	stateLinVelInitCovMat_
Matrix3	stateAngVelInitCovMat_
Matrix3	gyroBiasInitCovMat_
Matrix6	unmodeledWrenchInitCovMat_
Matrix12	contactInitCovMatDefault_
Matrix3	statePosProcessCovMat_
Matrix3	stateOriProcessCovMat_
Matrix3	stateLinVelProcessCovMat_
Matrix3	stateAngVelProcessCovMat_
Matrix3	gyroBiasProcessCovMat_
Matrix6	unmodeledWrenchProcessCovMat_
Matrix3	contactPositionProcessCovMat_
Matrix3	contactOrientationProcessCovMat_
Matrix3	contactForceProcessCovMat_
Matrix3	contactTorqueProcessCovMat_
Matrix12	contactProcessCovMatDefault_
Matrix12	stateKinematicsInitCovMat_
Matrix12	stateKinematicsProcessCovMat_
struct stateObservation::KineticsObserver::Opt	opt_

Static Protected Attributes
static const double	defaultdx
	default derivation steps More...

State vector representation arithmetics and derivation (advanced use)
Vector	stateSum (const Vector &stateVector, const Vector &tangentVector)
	the sum operator for the state vector More...
virtual void	stateSum (const Vector &stateVector, const Vector &tangentVector, Vector &sum)
	the sum operator for the state vector This version does not allocate a new vector More...
Vector	stateDifference (const Vector &stateVector1, const Vector &stateVector2)
	the difference operator for the state statevector1 ⊖ statevector2 More...
virtual void	stateDifference (const Vector &stateVector1, const Vector &stateVector2, Vector &difference)
	the difference operator for the state statevector1 ⊖ statevector2 This version prevents a nwe vector allocation More...
virtual void	measurementDifference (const Vector &measureVector1, const Vector &measureVector2, Vector &difference)
	the difference operator for the measurement statevector1 ⊖ statevector2 More...
virtual void	useFiniteDifferencesJacobians (bool b=true)
	Define if we use dinite differences Jacobian or analytic. More...
virtual void	setFiniteDifferenceStep (const Vector &dx)
	Set the Finite Difference time step. More...
virtual Matrix	computeAMatrix ()
virtual Matrix	computeCMatrix ()
void	computeLocalAccelerations (const Vector &x, Vector &acceleration)
	computes the local acceleration from the given state vector More...
int	testAccelerationsJacobians (KineticsObserver &ko, int errcode, double relativeErrorThreshold, double threshold)
	Comparison between the Jacobians of the linear and angular accelerations with respect to the state, obtained with finite differences and analyticially. More...
int	testAnalyticalAJacobianVsFD (KineticsObserver &ko, int errcode, double relativeErrorThreshold, double threshold)
	Comparison between the analytical Jacobian matrix A and the one obtained by finite differences. Used to test the analytical method. More...
int	testAnalyticalCJacobianVsFD (KineticsObserver &ko, int errcode, double relativeErrorThreshold, double threshold)
int	testOrientationsJacobians (KineticsObserver &ko, int errcode, double relativeErrorThreshold, double threshold)
	Comparison between the Jacobians of orientation integration with respect to an increment vector delta, obtained with finite differences and analyticially. More...

Detailed Description

This observer estimates the kinematics, the external forces, the bias on the gyrometers measurements, and the contacts forces and pose.

Our observer estimates the localkinematics of the centroid's frame within the world frame. The reason to choose the centroid's frame is that it simplifies many expressions, for example the expressions of the accelerations. This estimation is based on the assumption of viscoelastic contacts and using three kinds of measurements: IMUs, Force/Torque measurements (contact and other ones) and any absolute position measurements. Inputs are given in a frame whose choice is at the user's discretion, we therefore call it the user frame.

Member Typedef Documentation

Kinematics

typedef kine::Kinematics stateObservation::KineticsObserver::Kinematics

LocalKinematics

typedef kine::LocalKinematics stateObservation::KineticsObserver::LocalKinematics

Orientation

typedef kine::Orientation stateObservation::KineticsObserver::Orientation

VectorContact

typedef std::vector<Contact, Eigen::aligned_allocator<Contact> > stateObservation::KineticsObserver::VectorContact

protected

VectorContactConstIterator

typedef VectorContact::const_iterator stateObservation::KineticsObserver::VectorContactConstIterator

protected

VectorContactIterator

typedef VectorContact::iterator stateObservation::KineticsObserver::VectorContactIterator

protected

VectorIMU

typedef std::vector<IMU, Eigen::aligned_allocator<IMU> > stateObservation::KineticsObserver::VectorIMU

protected

VectorIMUConstIterator

typedef VectorIMU::const_iterator stateObservation::KineticsObserver::VectorIMUConstIterator

protected

VectorIMUIterator

typedef VectorIMU::iterator stateObservation::KineticsObserver::VectorIMUIterator

protected

Constructor & Destructor Documentation

KineticsObserver()

stateObservation::KineticsObserver::KineticsObserver	(	unsigned	maxContacts = 4,
		unsigned	maxNumberOfIMU = 1
	)

Construct a new Kinetics Observer.

Parameters

maxContacts	maximum number of contacts between the robot and the environment. These do not include the additional forces nor the estimated unmodeled forces
maxNumberOfIMU	the maximum number of IMUs. They don't have to give measurements at each iterations and they don't have to be synchronized

~KineticsObserver()

virtual stateObservation::KineticsObserver::~KineticsObserver ( )

virtual

Destroy the Kinetics Observer.

Member Function Documentation

addContact() [1/2]

Index stateObservation::KineticsObserver::addContact	(	const Kinematics &	pose,
		const Matrix12 &	initialCovarianceMatrix,
		const Matrix12 &	processCovarianceMatrix,
		Index	contactNumber = -1,
		const Matrix3 &	linearStiffness = Matrix3::Constant(-1),
		const Matrix3 &	linearDamping = Matrix3::Constant(-1),
		const Matrix3 &	angularStiffness = Matrix3::Constant(-1),
		const Matrix3 &	angularDamping = Matrix3::Constant(-1)
	)

Set a new contact with the environment.

Parameters

pose	is the initial guess on the position of the contact in the WORLD frame. Only position and orientation are enough. If the contact is compliant, you need to set the "rest" pose of the contact (i.e. the pose that gives zero reaction force)
initialCovarianceMatrix	is the covariance matrix expressing the uncertainty in the pose of the initial guess in the 6x6 upper left corner ( if no good initial guess is available give a rough position with a high initial covariance matrix, if the position is certain, set it to zero.) and the initial wrench in the 6x6 lower right corner.
processCovarianceMatrix	is the covariance matrix expressing the rate at which the contact slides or drifts in the 6x6 upper left corner (set to zero for no sliding) and the certainty in the reaction force model (viscoelastic) in the prediction of the contact force
contactNumber	the number id of the contact to add. If no predefined id, use -1 (default) in order to set the number automatically
linearStiffness	the linear stiffness of the contact viscoelastic model, if unknown, set to Matrix3::Constant(-1) (default) to use the default one
linearDamping	the linear damping of the contact viscoelastic model, if unknown, set to Matrix3::Constant(-1) (default) to use the default one
angularStiffness	the angular stiffness of the contact viscoelastic model, if unknown, set to Matrix3::Constant(-1) (default) to use the default one
angularDamping	the angular damping of the contact viscoelastic model, if unknown, set to Matrix3::Constant(-1) (default) to use the default one

Returns

int the id number of the contact just added (returns contactNumber if it is positive)

addContact() [2/2]

Index stateObservation::KineticsObserver::addContact	(	const Kinematics &	pose,
		Index	contactNumber = -1,
		const Matrix3 &	linearStiffness = Matrix3::Constant(-1),
		const Matrix3 &	linearDamping = Matrix3::Constant(-1),
		const Matrix3 &	angularStiffness = Matrix3::Constant(-1),
		const Matrix3 &	angularDamping = Matrix3::Constant(-1)
	)

Set a new contact with the environment (use default covariance matrices)

Parameters

pose	is the initial guess on the position of the contact in the WORLD frame. Only position and orientation are enough
contactNumber	the number id of the contact to add. If no predefined id, use -1 (default) in order to set the number automatically
linearStiffness	the linear stiffness of the contact viscoelastic model, if unknown, set to Matrix3::Constant(-1) (default) to use the default one
linearDamping	the linear damping of the contact viscoelastic model, if unknown, set to Matrix3::Constant(-1) (default) to use the default one
angularStiffness	the angular stiffness of the contact viscoelastic model, if unknown, set to Matrix3::Constant(-1) (default) to use the default one
angularDamping	the angular damping of the contact viscoelastic model, if unknown, set to Matrix3:::Constant(-1) (default) to use the default one

Returns

int the id number of the contact just added (returns contactNumber if it is positive)

addContactWrench_()

void stateObservation::KineticsObserver::addContactWrench_ ( const Kinematics &  centroidContactKine, const Vector3 &  centroidContactForce, const Vector3 &  centroidContactTorque, Vector3 &  totalCentroidForce, Vector3 &  totalCentroidTorque  )

protected

adds the contribution of a contact wrench at the centroid to the total wrench

Parameters

centroidContactKine	The Kinematics of the current contact at the centroid
centroidContactForce	The contact force at the centroid to add to the total force
centroidContactTorque	The contact torque at the centroid to add to the total torque
totalCentroidForce	The total force exerced at the centroid, that will be completed with the contact's force
totalCentroidTorque	The total torque exerced at the centroid, that will be completed with the contact's torque

addUnmodeledAndContactWrench_()

void stateObservation::KineticsObserver::addUnmodeledAndContactWrench_ ( const Vector &  centroidStateVector, Vector3 &  force, Vector3 &  torque  )

protected

Adds the unmodeled and contact wrenches from the state to the given wrench.

Parameters

centroidStateVector	The current state vector
force	The force we want to add the forces to. Must be expressed in the centroid frame.
torque	The torque we want to add the torques to. Must be expressed in the centroid frame.

addUnmodeledWrench_()

void stateObservation::KineticsObserver::addUnmodeledWrench_ ( const Vector &  centroidStateVector, Vector3 &  force, Vector3 &  torque  )

protected

Adds the unmodeled wrench from the state to the given wrench.

Parameters

centroidStateVector	The current state vector
force	The force we want to add the unmodeled force to. Must be expressed in the centroid frame.
torque	The torque we want to add the unmodeled torque to. Must be expressed in the centroid frame.

angVelIndex()

Index stateObservation::KineticsObserver::angVelIndex ( ) const

inline

Get the angular velocity index of the state vector.

Returns

Index

angVelIndexTangent()

Index stateObservation::KineticsObserver::angVelIndexTangent ( ) const

inline

Get the angular velocity index of the tangent state vector.

Returns

Index

clearContacts()

void stateObservation::KineticsObserver::clearContacts

(

)

remove all the contacts

computeAMatrix()

virtual Matrix stateObservation::KineticsObserver::computeAMatrix ( )

virtual

computeCMatrix()

virtual Matrix stateObservation::KineticsObserver::computeCMatrix ( )

virtual

computeContactForce_()

void stateObservation::KineticsObserver::computeContactForce_ ( VectorContactIterator  i, LocalKinematics &  worldCentroidStateKinematics, Kinematics &  worldRestContactPose, Vector3 &  contactForce, Vector3 &  contactTorque  )

protected

Computes the force exerted at a contact using the visco-elastic model on the given state vector.

Parameters

i	Contact to estimate
worldCentroidStateKinematics	State vector used in the visco-elastic model
worldRestContactPose	Rest pose of the contact
contactForce	Empty vector of the contact force to estimate
contactTorque	Empty vector of the contact force to estimate

computeContactForces_()

void stateObservation::KineticsObserver::computeContactForces_ ( LocalKinematics &  worldCentroidStateKinematics, Vector3 &  contactForce, Vector3 &  contactTorque  )

protected

Computes the force exerted at a contact using the visco-elastic model on the given state vector. Compute the resulting wrench for all currently set contacts.

Parameters

worldCentroidStateKinematics	State vector used in the visco-elastic model
contactForce	Empty vector of the contact force to estimate
contactTorque	Empty vector of the contact force to estimate

computeLocalAccelerations()

void stateObservation::KineticsObserver::computeLocalAccelerations	(	const Vector &	x,
		Vector &	acceleration
	)

computes the local acceleration from the given state vector

computeLocalAccelerations_()

void stateObservation::KineticsObserver::computeLocalAccelerations_ ( LocalKinematics &  localStateKine, const Vector3 &  totalForceLocal, const Vector3 &  totalMomentLocal, Vector3 &  linAcc, Vector3 &  angAcc  )

protected

Computes the local accelerations of the centroid frame in the world frame and adds them to its local kinematics.

Parameters

localStateKine	The local kinematics of the centroid frame in the world frame that still don't contain the accelerations.
centroidContactForce	The contact force at the centroid to add to the total force
totalForceLocal	Total force exerted on the centroid.
totalMomentLocal	Total torque exerted on the centroid.
linAcc	The empty vector of the linear acceleration we want to compute.
angAcc	The empty vector of the angular acceleration we want to compute.

contactForceIndex() [1/2]

Index stateObservation::KineticsObserver::contactForceIndex ( Index  contactNbr ) const

inline

Get the index of the linear force of a specific contact in the sate vector.

Parameters

contactNbr

The contact number id

Returns

Index

contactForceIndex() [2/2]

Index stateObservation::KineticsObserver::contactForceIndex ( VectorContactConstIterator  i ) const

inlineprotected

contactForceIndexTangent() [1/2]

Index stateObservation::KineticsObserver::contactForceIndexTangent ( Index  contactNbr ) const

inline

Get the index of the linear force of a specific contact in the tangent sate vector.

Parameters

contactNbr

The contact number id

Returns

Index

contactForceIndexTangent() [2/2]

Index stateObservation::KineticsObserver::contactForceIndexTangent ( VectorContactConstIterator  i ) const

inlineprotected

contactIndex() [1/2]

Index stateObservation::KineticsObserver::contactIndex ( Index  contactNbr ) const

inline

Get the index of a specific contact in the sate vector.

Parameters

contactNbr

The contact number id

Returns

Index

contactIndex() [2/2]

Index stateObservation::KineticsObserver::contactIndex ( VectorContactConstIterator  i ) const

inlineprotected

contactIndexTangent() [1/2]

Index stateObservation::KineticsObserver::contactIndexTangent ( Index  contactNbr ) const

inline

Get the index of a specific contact in the tangent sate vector.

Parameters

contactNbr

The contact number id

Returns

Index

contactIndexTangent() [2/2]

Index stateObservation::KineticsObserver::contactIndexTangent ( VectorContactConstIterator  i ) const

inlineprotected

Getters for the indexes of the state Vector using private types.

contactKineIndex() [1/2]

Index stateObservation::KineticsObserver::contactKineIndex ( Index  contactNbr ) const

inline

Get the index of the kinematics of a specific contact in the sate vector.

Parameters

contactNbr

The contact number id

Returns

Index

contactKineIndex() [2/2]

Index stateObservation::KineticsObserver::contactKineIndex ( VectorContactConstIterator  i ) const

inlineprotected

contactKineIndexTangent() [1/2]

Index stateObservation::KineticsObserver::contactKineIndexTangent ( Index  contactNbr ) const

inline

Get the index of the kinematics of a specific contact in the tangent sate vector.

Parameters

contactNbr

The contact number id

Returns

Index

contactKineIndexTangent() [2/2]

Index stateObservation::KineticsObserver::contactKineIndexTangent ( VectorContactConstIterator  i ) const

inlineprotected

contactOriIndex() [1/2]

Index stateObservation::KineticsObserver::contactOriIndex ( Index  contactNbr ) const

inline

Get the index of the orientation of a specific contact in the sate vector.

Parameters

contactNbr

The contact number id

Returns

Index

contactOriIndex() [2/2]

Index stateObservation::KineticsObserver::contactOriIndex ( VectorContactConstIterator  i ) const

inlineprotected

contactOriIndexTangent() [1/2]

Index stateObservation::KineticsObserver::contactOriIndexTangent ( Index  contactNbr ) const

inline

Get the index of the orientation of a specific contact in the tangent sate vector.

Parameters

contactNbr

The contact number id

Returns

Index

contactOriIndexTangent() [2/2]

Index stateObservation::KineticsObserver::contactOriIndexTangent ( VectorContactConstIterator  i ) const

inlineprotected

contactPosIndex() [1/2]

Index stateObservation::KineticsObserver::contactPosIndex ( Index  contactNbr ) const

inline

Get the index of the position of a specific contact in the sate vector.

Parameters

contactNbr

The contact number id

Returns

Index

contactPosIndex() [2/2]

Index stateObservation::KineticsObserver::contactPosIndex ( VectorContactConstIterator  i ) const

inlineprotected

contactPosIndexTangent() [1/2]

Index stateObservation::KineticsObserver::contactPosIndexTangent ( Index  contactNbr ) const

inline

Get the index of the position of a specific contact in the tangent sate vector.

Parameters

contactNbr

The contact number id

Returns

Index

contactPosIndexTangent() [2/2]

Index stateObservation::KineticsObserver::contactPosIndexTangent ( VectorContactConstIterator  i ) const

inlineprotected

contactsIndex()

Index stateObservation::KineticsObserver::contactsIndex ( ) const

inline

Get the index for the contact segment in the state vector.

Returns

Index

contactsIndexTangent()

Index stateObservation::KineticsObserver::contactsIndexTangent ( ) const

inline

Get the index for the contact segment in the tangent state vector.

Returns

Index

contactTorqueIndex() [1/2]

Index stateObservation::KineticsObserver::contactTorqueIndex ( Index  contactNbr ) const

inline

Get the index of the toraue of a specific contact in the sate vector.

Parameters

contactNbr

The contact number id

Returns

Index

contactTorqueIndex() [2/2]

Index stateObservation::KineticsObserver::contactTorqueIndex ( VectorContactConstIterator  i ) const

inlineprotected

contactTorqueIndexTangent() [1/2]

Index stateObservation::KineticsObserver::contactTorqueIndexTangent ( Index  contactNbr ) const

inline

Get the index of the toraue of a specific contact in the tangent sate vector.

Parameters

contactNbr

The contact number id

Returns

Index

contactTorqueIndexTangent() [2/2]

Index stateObservation::KineticsObserver::contactTorqueIndexTangent ( VectorContactConstIterator  i ) const

inlineprotected

contactWrenchIndex() [1/2]

Index stateObservation::KineticsObserver::contactWrenchIndex ( Index  contactNbr ) const

inline

Get the index of the wrench of a specific contact in the sate vector.

Parameters

contactNbr

The contact number id

Returns

Index

contactWrenchIndex() [2/2]

Index stateObservation::KineticsObserver::contactWrenchIndex ( VectorContactConstIterator  i ) const

inlineprotected

contactWrenchIndexTangent() [1/2]

Index stateObservation::KineticsObserver::contactWrenchIndexTangent ( Index  contactNbr ) const

inline

Get the index of the wrench of a specific contact in the sate tangent vector.

Parameters

contactNbr

The contact number id

Returns

Index

contactWrenchIndexTangent() [2/2]

Index stateObservation::KineticsObserver::contactWrenchIndexTangent ( VectorContactConstIterator  i ) const

inlineprotected

convertUserToCentroidFrame_() [1/2]

void stateObservation::KineticsObserver::convertUserToCentroidFrame_ ( const Kinematics &  userKine, Kinematics &  centroidKine, TimeIndex  k_data  )

protected

Converts a LocalKinematics object from the user's frame to the centroid's frame, which is used for most of the computations.

Parameters

userKine	the LocalKinematics object expressed in the user's frame. It is likely to correspond to the IMU's LocalKinematics, defined by the user in its frame.
centroidKine	the LocalKinematics object corresponding to the converted LocalKinematics in the centroid's frame.

convertUserToCentroidFrame_() [2/2]

Kinematics stateObservation::KineticsObserver::convertUserToCentroidFrame_ ( const Kinematics &  userKine, TimeIndex  k_data  )

protected

Converts a Kinematics object from the user's frame to the centroid's frame, which is used for most of the computations.

Parameters

userKine

the Kinematics object expressed in the user's frame. It is likely to correspond to the contact's Kinematics, defined by the user in its frame.

convertWrenchFromUserToCentroid()

void stateObservation::KineticsObserver::convertWrenchFromUserToCentroid	(	const Vector3 &	forceUserFrame,
		const Vector3 &	momentUserFrame,
		Vector3 &	forceCentroidFrame,
		Vector3 &	momentCentroidFrame
	)

Returns the predicted Kinematics object of the centroid in the world frame at the time of the measurement predictions.

Converts a given wrench from the user to the centroid frame

Performs the conversion of a wrench {force, torque} from the user frame to the centroid frame.

estimateAccelerations()

LocalKinematics stateObservation::KineticsObserver::estimateAccelerations

(

)

gets the Kinematics that include the linear and angular accelerations.

This method computes the estimated accelerations from the observed state of the robot. It means this acceleration is filtered by the model

Returns

Kinematics

getAdditionalWrench()

Vector6 stateObservation::KineticsObserver::getAdditionalWrench

(

)

const

Returns the input additional wrench, expressed in the centroid frame.

Returns

The input additional wrench, expressed in the centroid frame.

getAngularMomentum()

const IndexedVector3& stateObservation::KineticsObserver::getAngularMomentum

(

)

const

Returns the angular momentum of the robot at the center of mass.

Returns

The angular momentum of the robot at the center of mass.

getAngularMomentumDot()

const IndexedVector3& stateObservation::KineticsObserver::getAngularMomentumDot

(

)

const

Returns the derivative of the angular momentum of the robot at the center of mass.

Returns

The derivative of the angular momentum of the robot at the center of mass.

getCenterOfMass()

const IndexedVector3& stateObservation::KineticsObserver::getCenterOfMass

(

)

const

Returns the position of the CoM of the robot in the user frame.

Returns

The position of the center of mass of the robot in the user frame.

getCenterOfMassDot()

const IndexedVector3& stateObservation::KineticsObserver::getCenterOfMassDot

(

)

const

Returns the linear velocity of the CoM of the robot in the user frame.

Returns

The linear velocity of the center of mass of the robot in the user frame.

getCenterOfMassDotDot()

const IndexedVector3& stateObservation::KineticsObserver::getCenterOfMassDotDot

(

)

const

Returns the linear acceleration of the CoM of the robot in the user frame.

Returns

The linear acceleration of the center of mass of the robot in the user frame.

getCentroidContactInputPose()

Kinematics stateObservation::KineticsObserver::getCentroidContactInputPose

(

Index

numContact

)

const

Returns the pose of the contact in the centroid frame, given as an input when updating the contact (obtained from its pose in the user frame).

Returns

Kinematics

getCentroidContactWrench()

Vector6 stateObservation::KineticsObserver::getCentroidContactWrench

(

Index

numContact

)

const

Returns the wrench exerted at the contact, expressed in the frame of the centroid.

Returns

Vector6

getContactIsSetByNum()

bool stateObservation::KineticsObserver::getContactIsSetByNum

(

Index

num

)

const

getContactMeasIndexByNum()

Index stateObservation::KineticsObserver::getContactMeasIndexByNum

(

Index

num

)

const

getContactPosition()

Kinematics stateObservation::KineticsObserver::getContactPosition

(

Index

contactNbr

)

const

Get the Contact 6D pose n in the global frame.

The contact position may be uncertain, this estimator uses the input data, the kinematic and the dynamic models to estimate the position of the contact in the environment This position is the "rest position" of the contact and therefore is different from what is obtained using forward kinematics because it does not include the contact deformation due to the contact forces

Parameters

contactNbr

The contact number id

Returns

Kinematics The pose

getContactStateRestKinematics()

Kinematics stateObservation::KineticsObserver::getContactStateRestKinematics

(

Index

numContact

)

const

Returns the estimated rest pose of the contact in the world frame.

Returns

Kinematics

getContactWrench()

Vector6 stateObservation::KineticsObserver::getContactWrench

(

Index

contactNbr

)

const

Get the Estimated Contact Wrench This is useful in the case of uncertain wrench sensors or when contact force measurement is not available.

get the contact force provided by the estimator which is different from a contact sensor measurement

Parameters

contactNbr

Returns

Vector6 Wrench

getCurrentStateVector()

const Vector& stateObservation::KineticsObserver::getCurrentStateVector

(

)

const

Gets the current value of the state estimation in the form of a state vector \(\hat{x_{k}}\).

//////////////////////////////// Getters and setters for the state vectors ////////////////////////////////

Returns

const Vector&

getEKF() [1/2]

ExtendedKalmanFilter& stateObservation::KineticsObserver::getEKF

(

)

Gets a reference on the extended Kalman filter modifying this object may lead to instabilities

getEKF() [2/2]

const ExtendedKalmanFilter& stateObservation::KineticsObserver::getEKF

(

)

const

Gets a const reference on the extended Kalman filter.

getGlobalCentroidKinematics()

Kinematics stateObservation::KineticsObserver::getGlobalCentroidKinematics

(

)

const

Get the estimated Kinematics of the centroid frame in the world frame.

It includes the linear and angular position and velocity but not the accelerations by default. To get the acceleration call estimateAccelerations(). This method does NOT update the estimation, for this use update().

Returns

Kinematics

getGlobalKinematicsOf()

Kinematics stateObservation::KineticsObserver::getGlobalKinematicsOf

(

const Kinematics &

userBodyKin

)

const

Get the global kinematics of a given frame (in the user frame) in the centroid frame.

The kinematics are linear and angular positions, velocities and optionally accelerations.

Parameters

kine

Returns

Kinematics

getIMUMeasIndexByNum()

Index stateObservation::KineticsObserver::getIMUMeasIndexByNum

(

Index

num

)

const

Get the measurement index of the required IMU : allows to access its corresponding measurements in the measurement vector for example.

Returns

Index

getInertiaMatrix()

const IndexedMatrix3& stateObservation::KineticsObserver::getInertiaMatrix

(

)

const

Returns the global inertia matrix of the robot at the center of mass.

Returns

The global inertia matrix of the robot at the center of mass.

getInertiaMatrixDot()

const IndexedMatrix3& stateObservation::KineticsObserver::getInertiaMatrixDot

(

)

const

Returns the derivative of the global inertia matrix of the robot at the center of mass.

Returns

The derivative of the global inertia matrix of the robot at the center of mass.

getInputSize()

virtual Index stateObservation::KineticsObserver::getInputSize ( ) const

protectedvirtual

Gets the input size.

Implements stateObservation::DynamicalSystemFunctorBase.

getListOfContacts()

std::vector<Index> stateObservation::KineticsObserver::getListOfContacts

(

)

const

Get the List Of Contact ids.

Returns

std::vector<int> a vector listing the contact ids

getLocalCentroidKinematics()

LocalKinematics stateObservation::KineticsObserver::getLocalCentroidKinematics

(

)

const

Get the estimated local Kinematics of the centroid frame in the world frame (local, which means expressed in the centroid frame).

the kinematics are the main output of this observer. It includes the linear and angular position and velocity but not the accelerations by default. To get the acceleration call estimateAccelerations(). This method does NOT update the estimation, for this use update().

Returns

Kinematics

getLocalKinematicsOf()

LocalKinematics stateObservation::KineticsObserver::getLocalKinematicsOf

(

const Kinematics &

userBodyKine

)

Get the local kinematics of a given frame (in the user frame) in the centroid frame.

The kinematics are linear and angular positions, velocities and optionally accelerations.

Parameters

userBodyKine

Returns

LocalKinematics

getMass()

double stateObservation::KineticsObserver::getMass

(

)

const

Returns the mass of the robot.

Returns

the mass of the robot.

getMeasurementNoise()

virtual NoiseBase* stateObservation::KineticsObserver::getMeasurementNoise ( ) const

protectedvirtual

Gets a pointer on the measurement noise.

getMeasurementSize()

Index stateObservation::KineticsObserver::getMeasurementSize ( ) const

virtual

Get the Measurement vector Size.

Returns

Index

Implements stateObservation::DynamicalSystemFunctorBase.

getMeasurementVector()

Vector stateObservation::KineticsObserver::getMeasurementVector

(

)

Get the Measurement Vector.

Returns

Vector

getNumberOfSetContacts()

Index stateObservation::KineticsObserver::getNumberOfSetContacts

(

)

const

Get the Current Number Of Contacts.

Returns

Index The current number of contacts

getProcessNoise()

virtual NoiseBase* stateObservation::KineticsObserver::getProcessNoise ( ) const

protectedvirtual

Gets the process noise.

getSamplingTime()

double stateObservation::KineticsObserver::getSamplingTime

(

)

const

Get the Sampling Time.

Returns

const double &

getStateCovarianceMat()

Matrix stateObservation::KineticsObserver::getStateCovarianceMat

(

)

const

Get the State Covariance matrix.

Returns

Matrix

getStateSize()

Index stateObservation::KineticsObserver::getStateSize ( ) const

virtual

Get the State Vector Size.

//////////////////////////////// Getters and setters for the state vector and full covariance matrices ////////////////////////////////

Returns

Index

Implements stateObservation::DynamicalSystemFunctorBase.

getStateTangentSize()

Index stateObservation::KineticsObserver::getStateTangentSize

(

)

const

Get the State Vector Tangent Size.

Returns

Index

getStateVectorTimeIndex()

TimeIndex stateObservation::KineticsObserver::getStateVectorTimeIndex

(

)

const

Get the State Vector Internal Time Index This is for advanced use but may be used to check how many states have been estimated up to now.

Returns

TimeIndex

getUnmodeledWrench()

Vector6 stateObservation::KineticsObserver::getUnmodeledWrench

(

)

const

Get the Unmodeled External Wrench (requires setWithUnmodeledWrench() to true before to update())

In the presence of unmodeled and unmeasured external forces and moments, the dynamics of the robot behaves differently, this difference can be exploited to estimate the resultant of these forces and moments.

Returns

Vector6

getUserContactInputPose()

Kinematics stateObservation::KineticsObserver::getUserContactInputPose

(

Index

numContact

)

const

Returns the pose of the contact in the user frame, given as an input when updating the contact.

Returns

Kinematics

getWithAccelerationEstimation()

bool stateObservation::KineticsObserver::getWithAccelerationEstimation

(

)

const

Returns if the estimation computes also the accelerations.

Returns

True if the acceleration is also estimated. Returns false otherwise.

getWorldContactPoseFromCentroid()

Kinematics stateObservation::KineticsObserver::getWorldContactPoseFromCentroid

(

Index

numContact

)

const

Returns the pose of the contact in the world frame, obtained from the state pose of the centroid in the world frame.

Returns

Kinematics

gyroBiasIndex() [1/2]

Index stateObservation::KineticsObserver::gyroBiasIndex ( Index  IMUNumber ) const

inline

Get the gyro bias index of the state vector.

Returns

Index

gyroBiasIndex() [2/2]

Index stateObservation::KineticsObserver::gyroBiasIndex ( VectorIMUConstIterator  i ) const

inlineprotected

Getters for the indexes of the state Vector using private types.

gyroBiasIndexTangent() [1/2]

Index stateObservation::KineticsObserver::gyroBiasIndexTangent ( Index  IMUNumber ) const

inline

Get the gyro bias index of the tangent state vector.

Returns

Index

gyroBiasIndexTangent() [2/2]

Index stateObservation::KineticsObserver::gyroBiasIndexTangent ( VectorIMUConstIterator  i ) const

inlineprotected

kineIndex()

Index stateObservation::KineticsObserver::kineIndex ( ) const

inline

Get the kinematics index of the state vector.

/////////////////////////////////////////////////////////// Getters for the indexes of the state Vector //////////////////////////////////////////////////////////

Returns

unsigned

kineIndexTangent()

Index stateObservation::KineticsObserver::kineIndexTangent ( ) const

inline

Get the kinematics index of the tangent state vector.

Getters for the indexes of the tangent state Vector //////////////////////////////////////////////////////////

Returns

Index

linVelIndex()

Index stateObservation::KineticsObserver::linVelIndex ( ) const

inline

Get the linear velocity index of the state vector.

Returns

Index

linVelIndexTangent()

Index stateObservation::KineticsObserver::linVelIndexTangent ( ) const

inline

Get the linear velocity index of the tangent state vector.

Returns

Index

measureDynamics()

virtual Vector stateObservation::KineticsObserver::measureDynamics ( const Vector &  x, const Vector &  u, TimeIndex  k  )

protectedvirtual

Applies the measurement model to the given state vector using the given input to predict the sensor measurements.

Parameters

x	The current state vector
u	The current input vector
k	The current time index

Returns

Vector&

Implements stateObservation::DynamicalSystemFunctorBase.

measurementDifference()

virtual void stateObservation::KineticsObserver::measurementDifference ( const Vector &  measureVector1, const Vector &  measureVector2, Vector &  difference  )

virtual

the difference operator for the measurement statevector1 ⊖ statevector2

it amounts at a time derivation betweeen state vectors with a time step of one second. Therefore the result is a tangent vector

Parameters

measureVector1	Operator 1
measureVector2	Operator 2
difference	The result

Reimplemented from stateObservation::StateVectorArithmetics.

oriIndex()

Index stateObservation::KineticsObserver::oriIndex ( ) const

inline

Get the orientation index of the state vector.

Returns

Index

oriIndexTangent()

Index stateObservation::KineticsObserver::oriIndexTangent ( ) const

inline

Get the orientation index of the tangent state vector.

Returns

Index

posIndex()

Index stateObservation::KineticsObserver::posIndex ( ) const

inline

Get the position index of the state vector.

Returns

Index

posIndexTangent()

Index stateObservation::KineticsObserver::posIndexTangent ( ) const

inline

Get the position index of the tangent state vector.

Returns

Index

removeContact()

void stateObservation::KineticsObserver::removeContact

(

Index

contactnbr

)

Remove a contact.

Parameters

contactnbr

the number of the contact to remove

resetInputs()

void stateObservation::KineticsObserver::resetInputs

(

)

reset all the sensor inputs and provided contact information but keeps the contacts themselves

resetMeasurementNoise()

virtual void stateObservation::KineticsObserver::resetMeasurementNoise ( )

protectedvirtual

Removes the measurement noise.

resetProcessContactCovMat()

void stateObservation::KineticsObserver::resetProcessContactCovMat

(

Index

contactNbr

)

resetProcessContactsCovMat()

void stateObservation::KineticsObserver::resetProcessContactsCovMat

(

)

resetProcessCovarianceMat()

void stateObservation::KineticsObserver::resetProcessCovarianceMat

(

)

resetProcessGyroBiasCovMat()

void stateObservation::KineticsObserver::resetProcessGyroBiasCovMat

(

Index

i

)

resetProcessKinematicsCovMat()

void stateObservation::KineticsObserver::resetProcessKinematicsCovMat

(

)

resetProcessNoise()

virtual void stateObservation::KineticsObserver::resetProcessNoise ( )

protectedvirtual

Removes the process noise.

resetProcessUnmodeledWrenchCovMat()

void stateObservation::KineticsObserver::resetProcessUnmodeledWrenchCovMat

(

)

resetSensorsDefaultCovMats()

void stateObservation::KineticsObserver::resetSensorsDefaultCovMats

(

)

Reset the default values for the sensors covariance matrices.

this is useful in case of misbehavior of the estimator or the sensors

resetStateContactCovMat()

void stateObservation::KineticsObserver::resetStateContactCovMat

(

Index

contactNbr

)

resetStateContactsCovMat()

void stateObservation::KineticsObserver::resetStateContactsCovMat

(

)

resetStateCovarianceMat()

void stateObservation::KineticsObserver::resetStateCovarianceMat

(

)

Resets the covariance matrices to their original values.

resetStateGyroBiasCovMat()

void stateObservation::KineticsObserver::resetStateGyroBiasCovMat

(

Index

i

)

resetStateKinematicsCovMat()

void stateObservation::KineticsObserver::resetStateKinematicsCovMat

(

)

resetStateUnmodeledWrenchCovMat()

void stateObservation::KineticsObserver::resetStateUnmodeledWrenchCovMat

(

)

setAbsoluteOriSensor() [1/2]

void stateObservation::KineticsObserver::setAbsoluteOriSensor

(

const Orientation &

measurement

)

Set an Absolute Orientation Sensor measurement The measurement is the orientation of the observed frame in the global frame.

The overload with the measurement only uses default covariance matrix.

Parameters

measurement

The measurement in the form of an Orientation object

setAbsoluteOriSensor() [2/2]

void stateObservation::KineticsObserver::setAbsoluteOriSensor	(	const Orientation &	measurement,
		const Matrix3 &	CovarianceMatrix
	)

Set an Absolute Orientation Sensor measurement The measurement is the orientation of the observed frame in the global frame.

This version sets the Covariance matrix explicitely. The overload with the measurement only uses default covariance matrix.

Parameters

measurement	The measurement in the form of an Orientation object
CovarianceMatrix	the covariance matrix

setAbsoluteOriSensorDefaultCovarianceMatrix()

void stateObservation::KineticsObserver::setAbsoluteOriSensorDefaultCovarianceMatrix

(

const Matrix3 &

covMat

)

Set the Absolute Orientation Sensor Default Covariance Matrix.

Parameters

covMat

setAbsolutePoseSensor() [1/2]

void stateObservation::KineticsObserver::setAbsolutePoseSensor

(

const Kinematics &

measurement

)

Set an Absolute Pose Sensor measurement The measurement is the kinematics namely position and orientation of the observed frame in the global frame.

The overload with the measurement only uses default covariance matrix.

Parameters

measurement

The measurement in the form of a Kinematics object

setAbsolutePoseSensor() [2/2]

void stateObservation::KineticsObserver::setAbsolutePoseSensor	(	const Kinematics &	measurement,
		const Matrix6 &	CovarianceMatrix
	)

Set an Absolute Pose Sensor measurement The measurement is the kinematics namely position and orientation of the observed frame in the global frame.

This version sets the Covariance matrix explicitely. The overload with the measurement only uses default covariance matrix.

Parameters

measurement	The measurement in the form of a Kinematics object
CovarianceMatrix	the covariance matrix

setAbsolutePoseSensorDefaultCovarianceMatrix()

void stateObservation::KineticsObserver::setAbsolutePoseSensorDefaultCovarianceMatrix

(

const Matrix6 &

covMat

)

Set the Absolute Pose Sensor Default Covariance Matrix.

Parameters

covMat

setAdditionalWrench()

void stateObservation::KineticsObserver::setAdditionalWrench	(	const Vector3 &	force,
		const Vector3 &	torque
	)

Set any Additional resultant known wrench (e.g. measured external forces and moments but no contact ones) expressed in the local estimated frame.

Set to zero if no forces or unknown

Parameters

force
torque

setCenterOfMass() [1/3]

void stateObservation::KineticsObserver::setCenterOfMass

(

const Vector3 &

com

)

Set the Center Of Mass kinematics expressed in the user frame.

The velocity and acceleration will be computed through finite differences

Parameters

com

position

setCenterOfMass() [2/3]

void stateObservation::KineticsObserver::setCenterOfMass	(	const Vector3 &	com,
		const Vector3 &	com_dot
	)

Set the Center Of Mass kinematics expressed in the user frame.

The acceleration will be computed through finite differences

Parameters

com	position
com_dot	velocity

setCenterOfMass() [3/3]

void stateObservation::KineticsObserver::setCenterOfMass	(	const Vector3 &	com,
		const Vector3 &	com_dot,
		const Vector3 &	com_dot_dot
	)

Set the Center Of Mass kinematics expressed in the user frame.

Parameters

com	position
com_dot	velocity
com_dot_dot	acceleration

setCoMAngularMomentum() [1/2]

void stateObservation::KineticsObserver::setCoMAngularMomentum

(

const Vector3 &

sigma

)

Set the Angular Momentum around the CoM expressed in the user frame.

The derivative will be computed using finite differences

Parameters

sigma

The angular momentum

setCoMAngularMomentum() [2/2]

void stateObservation::KineticsObserver::setCoMAngularMomentum	(	const Vector3 &	sigma,
		const Vector3 &	sigma_dot
	)

Set the Angular Momentum around the CoM and its derviative expressed in the user frame.

Parameters

sigma	The angular momentum
sigma_dot	The angular momentum derivative

setCoMInertiaMatrix() [1/4]

void stateObservation::KineticsObserver::setCoMInertiaMatrix

(

const Matrix3 &

I

)

Set the 3x3 inertia matrix expressed in the user frame.

The derivative will be computed using finite differences

Parameters

I	Inertia matrix
I_dot	Derivative of inertia matrix

setCoMInertiaMatrix() [2/4]

void stateObservation::KineticsObserver::setCoMInertiaMatrix	(	const Matrix3 &	I,
		const Matrix3 &	I_dot
	)

Set the 3x3 inertia matrix and its derivative expressed in the user frame.

Parameters

I	Set the inertia matrix at the CoM
I_dot	Derivative of inertia matrix

setCoMInertiaMatrix() [3/4]

void stateObservation::KineticsObserver::setCoMInertiaMatrix

(

const Vector6 &

I

)

Set the inertia matrix as a Vector6 expressed in the user frame.

The derivative will be computed using finite differences

Parameters

I	Inertia matrix as a vector containing the diagonal and the three non diagonal values concatenated

setCoMInertiaMatrix() [4/4]

void stateObservation::KineticsObserver::setCoMInertiaMatrix	(	const Vector6 &	I,
		const Vector6 &	I_dot
	)

Set the inertia matrix and its derivative as a Vector6 expressed in the user frame.

Parameters

I	Inertia matrix as a vector containing the diagonal and the three non diagonal values concatenated
I_dot	Derivative of inertia matrix expressed in the same way

setContactInitCovMatDefault()

void stateObservation::KineticsObserver::setContactInitCovMatDefault

(

const Matrix12 &

contactCovMat

)

Set the default valut for the Initial Covariance Matrix of the contact in the state.

Parameters

contactCovMat

setContactProcessCovarianceDefault()

void stateObservation::KineticsObserver::setContactProcessCovarianceDefault

(

const Matrix12 &

covMat

)

Set the default contact Process Covariance.

setContactProcessCovMat()

void stateObservation::KineticsObserver::setContactProcessCovMat	(	Index	contactNbr,
		const Matrix12 &	contactCovMat
	)

Set the Contact Process Covariance Matrix.

Parameters

contactNbr
contactCovMat	the contact number id

setContactStateCovMat()

void stateObservation::KineticsObserver::setContactStateCovMat	(	Index	contactNbr,
		const Matrix12 &	contactCovMat
	)

Set the Contact State Covariance Matrix.

Parameters

contactNbr
contactCovMat	the contact number id

setContactWrenchSensorDefaultCovarianceMatrix()

void stateObservation::KineticsObserver::setContactWrenchSensorDefaultCovarianceMatrix

(

const Matrix6 &

wrenchSensorCovMat

)

Set the Contact Wrench Sensor Default Covariance Matrix.

Parameters

wrenchSensorCovMat

the new default covariance matrix

setFiniteDifferenceStep()

virtual void stateObservation::KineticsObserver::setFiniteDifferenceStep ( const Vector &  dx )

virtual

Set the Finite Difference time step.

Parameters

dx	the timestep

setGyroBias()

void stateObservation::KineticsObserver::setGyroBias	(	const Vector3 &	,
		unsigned	numberOfIMU = 1,
		bool	resetCovariance = true
	)

Set the Gyro Bias Allows to initializa the value of the gyro bias of the IMU corresponding to the numberOfIMU.

Parameters

numberOfIMU	number id of the IMU
resetCovariance	set if the covariance of the IMU bias should be reset

setGyroBiasInitCovarianceDefault()

void stateObservation::KineticsObserver::setGyroBiasInitCovarianceDefault

(

const Matrix3 &

covMat

)

Set the Default value for Gyro Bias Init Covariance.

setGyroBiasProcessCovariance()

void stateObservation::KineticsObserver::setGyroBiasProcessCovariance	(	const Matrix3 &	covMat,
		unsigned	imuNumber
	)

Set the Gyro Bias Process Covariance.

Parameters

covMat	the new process covariance matrix
imuNumber	the number id of the IMU

setGyroBiasProcessCovarianceDefault()

void stateObservation::KineticsObserver::setGyroBiasProcessCovarianceDefault

(

const Matrix3 &

covMat

)

Set the default Gyro Bias Process Covariance.

setGyroBiasStateCovariance()

void stateObservation::KineticsObserver::setGyroBiasStateCovariance	(	const Matrix3 &	covMat,
		unsigned	imuNumber
	)

Set the Gyro Bias State Covariance.

Parameters

covMat	the nwe covariance matrix
imuNumber	the number id of the IMU

setIMU() [1/2]

Index stateObservation::KineticsObserver::setIMU	(	const Vector3 &	accelero,
		const Vector3 &	gyrometer,
		const Kinematics &	userImuKinematics,
		Index	num = -1
	)

Set the measurements of an IMU and give the Kinematic of the IMU in the user frame.

The overload that does not have the covariance matrices as an inputs uses default ones.

The IMU is located in a sensor frame. We suppose we know the kinematics of this sensor frame in the centroid's frame

Returns

the number of the IMU (useful in case there are several ones)

Parameters

accelero	measured value
gyrometer	measured gyro value
userImuKinematics	sets the kinematics of the IMU in the user frame. The best is to provide the position, the orientation, the angular and linear velocities and the linear acceleration Nevertheless if velocities or accelerations are not available they will be automatically computed through finite differences
num	the number of the IMU (useful in case there are several ones). If not set it will be generated automatically.

setIMU() [2/2]

Index stateObservation::KineticsObserver::setIMU	(	const Vector3 &	accelero,
		const Vector3 &	gyrometer,
		const Matrix3 &	acceleroCov,
		const Matrix3 &	gyroCov,
		const Kinematics &	userImuKinematics,
		Index	num = -1
	)

Provides also the associated covariance matrices

This version specifies the covariance matrices of these measurements.

Parameters

acceleroCov	The covariance matrix of the accelerometer
gyroCov	The covariance matrix of the gyrometer

setIMUDefaultCovarianceMatrix()

void stateObservation::KineticsObserver::setIMUDefaultCovarianceMatrix	(	const Matrix3 &	acceleroCov,
		const Matrix3 &	gyroCov
	)

set the default covariance matrix for IMU.

this is used to set the covariances wgen not given explicitely (see setIMU(const Vector3&,const Vector3&,const Kinematics &,int)).

Parameters

acceleroCov	The covariance matrix of the accelerometer
gyroCov	The covariance matrix of the gyrometer

setInitWorldCentroidStateVector()

void stateObservation::KineticsObserver::setInitWorldCentroidStateVector

(

const Vector &

initStateVector

)

Initializes the state vector.

Parameters

initStateVector

the initial state vector.

setKinematicsInitCovarianceDefault() [1/2]

void stateObservation::KineticsObserver::setKinematicsInitCovarianceDefault

(

const Matrix &

)

Set the Default value for Kinematics Init Covariance.

setKinematicsInitCovarianceDefault() [2/2]

void stateObservation::KineticsObserver::setKinematicsInitCovarianceDefault	(	const Matrix3 &	P_pos,
		const Matrix3 &	P_ori,
		const Matrix3 &	P_linVel,
		const Matrix3 &	P_angVel
	)

Set the Default value for Kinematics Init Covariance.

setKinematicsProcessCovariance()

void stateObservation::KineticsObserver::setKinematicsProcessCovariance

(

const Matrix12 &

)

Set the Kinematics Process Covariance.

setKinematicsProcessCovarianceDefault() [1/2]

void stateObservation::KineticsObserver::setKinematicsProcessCovarianceDefault

(

const Matrix12 &

)

Set the default Kinematics Process Covariance.

setKinematicsProcessCovarianceDefault() [2/2]

void stateObservation::KineticsObserver::setKinematicsProcessCovarianceDefault	(	const Matrix3 &	P_pos,
		const Matrix3 &	P_ori,
		const Matrix3 &	P_linVel,
		const Matrix3 &	P_angVel
	)

Set the default Kinematics Process Covariance.

setKinematicsStateCovariance()

void stateObservation::KineticsObserver::setKinematicsStateCovariance

(

const Matrix &

)

Set the Kinematics State Covariance.

setMass()

void stateObservation::KineticsObserver::setMass

(

double

)

Set the total mass of the robot. This can be changed online.

Returns

sets

setMeasurementNoise()

virtual void stateObservation::KineticsObserver::setMeasurementNoise ( NoiseBase *  )

protectedvirtual

Sets a noise which disturbs the measurements.

setProcessNoise()

virtual void stateObservation::KineticsObserver::setProcessNoise ( NoiseBase *  )

protectedvirtual

Sets a noise which disturbs the state dynamics.

setProcessNoiseCovarianceMat()

void stateObservation::KineticsObserver::setProcessNoiseCovarianceMat

(

const Matrix &

Q

)

Set the covariance matrices for the process noises.

Parameters

Q	process noise

setSamplingTime()

void stateObservation::KineticsObserver::setSamplingTime

(

double

)

Set the Sampling Time.

setStateContact()

void stateObservation::KineticsObserver::setStateContact	(	Index	index,
		Kinematics	worldContactRestPose,
		const Vector6 &	wrench,
		bool	resetCovariance = true
	)

Set the state contact kinematics and wrench.

Sets a value for the contact part of the state. Might be useful to reset this state for instance.

Parameters

index	index of the contact
worldContactRestPose	new state rest pose of the contact
wrench	new state wrench of the contact
resetCovariance	set if the associated part of the state covariance matrix should be reset

setStateCovarianceMat()

void stateObservation::KineticsObserver::setStateCovarianceMat

(

const Matrix &

P

)

Set the State Covariance Matrix This is useful in case of a setting a guess on a whole state vect9or.

Parameters

P	The covariance matrix

setStateUnmodeledWrench()

void stateObservation::KineticsObserver::setStateUnmodeledWrench	(	const Vector6 &	,
		bool	resetCovariance = true
	)

Set the State Unmodeled Wrench.

this modifies the current guess for external unmodeled Wrench. This is different from setAdditionalWrench() since it modifies a state component. This function is likely useful when initializing the estimation and reduce the convergence time

Parameters

resetCovariance

set if the covariance should be reset

setStateVector()

void stateObservation::KineticsObserver::setStateVector	(	const Vector &	newvalue,
		bool	resetCovariance = true
	)

Set a value of the state x_k provided from another source.

can be used for initialization of the estimator

Parameters

newvalue	The new value for the state vector
resetCovariance	set if the state covariance should be reset

setUnmodeledWrenchInitCovMatDefault()

void stateObservation::KineticsObserver::setUnmodeledWrenchInitCovMatDefault

(

const Matrix6 &

initCovMat

)

Set the default value for init Unmodeled Wrench covariance matrix.

Parameters

initCovMat

setUnmodeledWrenchProcessCovarianceDefault()

void stateObservation::KineticsObserver::setUnmodeledWrenchProcessCovarianceDefault

(

const Matrix6 &

covMat

)

Set the default Unmodeled Wrench Process Covariance.

setUnmodeledWrenchProcessCovMat()

void stateObservation::KineticsObserver::setUnmodeledWrenchProcessCovMat

(

const Matrix6 &

processCovMat

)

Set the Unmodeled Wrench Process Covariance Mattix.

Parameters

processCovMat

setUnmodeledWrenchStateCovMat()

void stateObservation::KineticsObserver::setUnmodeledWrenchStateCovMat

(

const Matrix6 &

newCovMat

)

Set the Unmodeled Wrench State Cov Mat.

Parameters

newCovMat

setWithAccelerationEstimation()

void stateObservation::KineticsObserver::setWithAccelerationEstimation

(

bool

b = true

)

Sets if the estimation computes also the accelerations.

This will not modify the estimated value, but just compute the modeled acceleration, which gives a model-based filtered acceleration

Parameters

b

setWithGyroBias()

void stateObservation::KineticsObserver::setWithGyroBias

(

bool

b = true

)

Set if the gyrometers bias is computed or not. This parameter is global for all the IMUs.

Parameters

b

setWithUnmodeledWrench()

void stateObservation::KineticsObserver::setWithUnmodeledWrench

(

bool

b = true

)

Set if the unmodeled and unmeasured external wrench should be estimated.

Activating this estimation will assume that the contact exists therefore, it is likely to modify the value of the estimated state. The estimation will also be slower.

Parameters

b

setWorldCentroidStateKinematics() [1/2]

void stateObservation::KineticsObserver::setWorldCentroidStateKinematics	(	const Kinematics &	kine,
		bool	resetCovariance = true
	)

Set the State Kinematics.

Sets a value for the kinematics part of the state

Parameters

localKine	are the new kinematics of the state
resetContactWrenches	set if the contact wrenches should be reset
resetCovariance	set if the covariance of the state should be reset

setWorldCentroidStateKinematics() [2/2]

void stateObservation::KineticsObserver::setWorldCentroidStateKinematics	(	const LocalKinematics &	localKine,
		bool	resetContactWrenches = true,
		bool	resetCovariance = true
	)

Set the State Kinematics.

Sets a value for the kinematics part of the state

Parameters

localKine	are the new local kinematics of the state
resetContactWrenches	set if the contact wrenches should be reset
resetCovariance	set if the covariance of the state should be reset

startNewIteration_()

void stateObservation::KineticsObserver::startNewIteration_ ( )

protected

function to call before adding any measurement detects if there is a new estimation beginning and then calls the reset of the iteration

stateDifference() [1/2]

Vector stateObservation::KineticsObserver::stateDifference ( const Vector &  stateVector1, const Vector &  stateVector2  )

inline

the difference operator for the state statevector1 ⊖ statevector2

it amounts at a time derivation betweeen state vectors with a time step of one second. Therefore the result is a tangent vector

Parameters

stateVector1	Operator 1
stateVector2	Operator 2

stateDifference() [2/2]

virtual void stateObservation::KineticsObserver::stateDifference ( const Vector &  stateVector1, const Vector &  stateVector2, Vector &  difference  )

virtual

the difference operator for the state statevector1 ⊖ statevector2 This version prevents a nwe vector allocation

it amounts at a time derivation betweeen state vectors with a time step of one second. Therefore the result is a tangent vector

Parameters

stateVector1	Operator 1
stateVector2	Operator 2
difference	The result

Reimplemented from stateObservation::StateVectorArithmetics.

stateDynamics()

virtual Vector stateObservation::KineticsObserver::stateDynamics ( const Vector &  x, const Vector &  u, TimeIndex  k  )

protectedvirtual

Applies the state-transition model to the given state vector using the given input to predict the future state.

DYNAMICAL SYSTEM IMPLEMENTATION

Parameters

x	The current state vector
u	The current input vector
k	The current time index

Returns

Vector&

Implements stateObservation::DynamicalSystemFunctorBase.

stateNaNCorrection_()

void stateObservation::KineticsObserver::stateNaNCorrection_ ( )

protected

stateSum() [1/2]

Vector stateObservation::KineticsObserver::stateSum ( const Vector &  stateVector, const Vector &  tangentVector  )

inline

the sum operator for the state vector

it amounts at a time-integration of the state vector using a tangent vector (constant for 1 second)

Parameters

stateVector	The state vector
tangentVector	The tangent Vector

stateSum() [2/2]

virtual void stateObservation::KineticsObserver::stateSum ( const Vector &  stateVector, const Vector &  tangentVector, Vector &  sum  )

virtual

the sum operator for the state vector This version does not allocate a new vector

it amounts at a time-integration of the state vector using a tangent vector (constant for 1 second)

Parameters

stateVector	The state vector
tangentVector	The tangent Vector
sum	The result of the operation

Reimplemented from stateObservation::StateVectorArithmetics.

unmodeledForceIndex()

Index stateObservation::KineticsObserver::unmodeledForceIndex ( ) const

inline

Get the unmodeled external linear force index of the state vector.

Returns

Index

unmodeledForceIndexTangent()

Index stateObservation::KineticsObserver::unmodeledForceIndexTangent ( ) const

inline

Get the unmodeled external linear force index of the tangent state vector.

Returns

Index

unmodeledTorqueIndex()

Index stateObservation::KineticsObserver::unmodeledTorqueIndex ( ) const

inline

Get the unmodeled external torque force index of the state vector.

Returns

Index

unmodeledTorqueIndexTangent()

Index stateObservation::KineticsObserver::unmodeledTorqueIndexTangent ( ) const

inline

Get the unmodeled external torque force index of the tangent state vector.

Returns

Index

unmodeledWrenchIndex()

Index stateObservation::KineticsObserver::unmodeledWrenchIndex ( ) const

inline

Get the unmodeled external wrench index of the state vector.

Returns

Index

unmodeledWrenchIndexTangent()

Index stateObservation::KineticsObserver::unmodeledWrenchIndexTangent ( ) const

inline

Get the unmodeled external wrench index of the tangent state vector.

Returns

Index

update()

const Vector& stateObservation::KineticsObserver::update

(

)

Runs the estimation.

This is the function that allows to 1- compute the estimation 2- move to the next iteration (increments time, resets the measurements, etc)

Returns

const Vector& The state vector

updateContactWithNoSensor()

void stateObservation::KineticsObserver::updateContactWithNoSensor	(	const Kinematics &	localKine,
		unsigned	contactNumber
	)

Update the contact when it is NOT equipped with wrench sensor.

Parameters

localKine	the new kinematics of the contact expressed in the centroid's frame frame the best is to provide the position, the orientation, the angular and the linear velocities. Otherwise they will be computed automatically
contactNumber	The number id of the contact

updateContactWithWrenchSensor() [1/2]

void stateObservation::KineticsObserver::updateContactWithWrenchSensor	(	const Vector6 &	wrenchMeasurement,
		const Kinematics &	localKine,
		unsigned	contactNumber
	)

Update the contact when it is equipped with wrench sensor.

Parameters

wrenchMeasurement	wrenchMeasurement is the measurment vector composed with 3D forces and 3D torques
localKine	the new kinematics of the contact expressed in the centroid's frame frame the best is to provide the position, the orientation, the angular and the linear velocities. Otherwise they will be computed automatically
contactNumber	The number id of the contact

updateContactWithWrenchSensor() [2/2]

void stateObservation::KineticsObserver::updateContactWithWrenchSensor	(	const Vector6 &	wrenchMeasurement,
		const Matrix6 &	wrenchCovMatrix,
		const Kinematics &	localKine,
		unsigned	contactNumber
	)

Update the contact when it is equipped with wrench sensor.

This version sets the Covariance matrix explicitely.

Parameters

wrenchCovMatrix	The covariance matrix of the wrench measurement
wrenchMeasurement	wrenchMeasurement is the measurment vector composed with 3D forces and 3D torques
localKine	the new kinematics of the contact expressed in the centroid's frame frame the best is to provide the position, the orientation, the angular and the linear velocities. Otherwise they will be computed automatically
contactNumber	The number id of the contact

updateGlobalKine_()

void stateObservation::KineticsObserver::updateGlobalKine_ ( )

protected

updates the global kinematics of the centroid from the local ones, that can be more interpretable

updateLocalKineAndContacts_()

void stateObservation::KineticsObserver::updateLocalKineAndContacts_ ( )

protected

update of the state kinematics worldCentroidStateKinematics_ and of the contacts pose with the newly estimated state

updateMeasurements()

void stateObservation::KineticsObserver::updateMeasurements

(

)

Updates the measurements.

Updates the measurement sensors and the associated vectors and covariance matrices

useFiniteDifferencesJacobians()

virtual void stateObservation::KineticsObserver::useFiniteDifferencesJacobians ( bool  b = true )

virtual

Define if we use dinite differences Jacobian or analytic.

Parameters

b	true means we use finite differences

Friends And Related Function Documentation

testAccelerationsJacobians

int testAccelerationsJacobians ( KineticsObserver &  ko, int  errcode, double  relativeErrorThreshold, double  threshold  )

friend

Comparison between the Jacobians of the linear and angular accelerations with respect to the state, obtained with finite differences and analyticially.

testAnalyticalAJacobianVsFD

int testAnalyticalAJacobianVsFD ( KineticsObserver &  ko, int  errcode, double  relativeErrorThreshold, double  threshold  )

friend

Comparison between the analytical Jacobian matrix A and the one obtained by finite differences. Used to test the analytical method.

Parameters

threshold

Threshold on the relative error between both Jacobians (in percentage)

testAnalyticalCJacobianVsFD

int testAnalyticalCJacobianVsFD ( KineticsObserver &  ko, int  errcode, double  relativeErrorThreshold, double  threshold  )

friend

testOrientationsJacobians

int testOrientationsJacobians ( KineticsObserver &  ko, int  errcode, double  relativeErrorThreshold, double  threshold  )

friend

Comparison between the Jacobians of orientation integration with respect to an increment vector delta, obtained with finite differences and analyticially.

Member Data Documentation

absOriSensor_

AbsoluteOriSensor stateObservation::KineticsObserver::absOriSensor_

protected

absOriSensorCovMatDefault_

Matrix3 stateObservation::KineticsObserver::absOriSensorCovMatDefault_

protected

absPoseSensor_

AbsolutePoseSensor stateObservation::KineticsObserver::absPoseSensor_

protected

absPoseSensorCovMatDefault_

Matrix6 stateObservation::KineticsObserver::absPoseSensorCovMatDefault_

protected

acceleroCovMatDefault_

Matrix3 stateObservation::KineticsObserver::acceleroCovMatDefault_

protected

acceleroVarianceDefault

const double stateObservation::KineticsObserver::acceleroVarianceDefault

static

additionalForce_

Vector3 stateObservation::KineticsObserver::additionalForce_

protected

additionalTorque_

Vector3 stateObservation::KineticsObserver::additionalTorque_

protected

angularDampingDefault

const double stateObservation::KineticsObserver::angularDampingDefault

static

angularDampingMatDefault_

Matrix3 stateObservation::KineticsObserver::angularDampingMatDefault_

protected

angularStiffnessDefault

const double stateObservation::KineticsObserver::angularStiffnessDefault

static

angularStiffnessMatDefault_

Matrix3 stateObservation::KineticsObserver::angularStiffnessMatDefault_

protected

com_

IndexedVector3 stateObservation::KineticsObserver::com_

protected

comd_

IndexedVector3 stateObservation::KineticsObserver::comd_

protected

comdd_

IndexedVector3 stateObservation::KineticsObserver::comdd_

protected

contactForceInitVarianceDefault

const double stateObservation::KineticsObserver::contactForceInitVarianceDefault

static

contactForceProcessCovMat_

Matrix3 stateObservation::KineticsObserver::contactForceProcessCovMat_

protected

contactForceProcessVarianceDefault

const double stateObservation::KineticsObserver::contactForceProcessVarianceDefault

static

contactInitCovMatDefault_

Matrix12 stateObservation::KineticsObserver::contactInitCovMatDefault_

protected

contactOrientationProcessCovMat_

Matrix3 stateObservation::KineticsObserver::contactOrientationProcessCovMat_

protected

contactOrientationProcessVarianceDefault

const double stateObservation::KineticsObserver::contactOrientationProcessVarianceDefault

static

contactPositionProcessCovMat_

Matrix3 stateObservation::KineticsObserver::contactPositionProcessCovMat_

protected

contactPositionProcessVarianceDefault

const double stateObservation::KineticsObserver::contactPositionProcessVarianceDefault

static

contactProcessCovMatDefault_

Matrix12 stateObservation::KineticsObserver::contactProcessCovMatDefault_

protected

contacts_

VectorContact stateObservation::KineticsObserver::contacts_

protected

contactTorqueInitVarianceDefault

const double stateObservation::KineticsObserver::contactTorqueInitVarianceDefault

static

contactTorqueProcessCovMat_

Matrix3 stateObservation::KineticsObserver::contactTorqueProcessCovMat_

protected

contactTorqueProcessVarianceDefault

const double stateObservation::KineticsObserver::contactTorqueProcessVarianceDefault

static

contactWrenchSensorCovMatDefault_

Matrix6 stateObservation::KineticsObserver::contactWrenchSensorCovMatDefault_

protected

currentIMUSensorNumber_

Index stateObservation::KineticsObserver::currentIMUSensorNumber_

protected

defaultdx

const double stateObservation::KineticsObserver::defaultdx

staticprotected

default derivation steps

defaultMass

const double stateObservation::KineticsObserver::defaultMass

static

dt_

double stateObservation::KineticsObserver::dt_

protected

ekf_

stateObservation::ExtendedKalmanFilter stateObservation::KineticsObserver::ekf_

protected

finiteDifferencesJacobians_

bool stateObservation::KineticsObserver::finiteDifferencesJacobians_

protected

flagsContactKine

constexpr Kinematics::Flags::Byte stateObservation::KineticsObserver::flagsContactKine

inlinestaticconstexpr

Initial value:

=
      Kinematics::Flags::position | Kinematics::Flags::orientation

flagsIMUKine

constexpr Kinematics::Flags::Byte stateObservation::KineticsObserver::flagsIMUKine

inlinestaticconstexpr

Initial value:

=
      Kinematics::Flags::position | Kinematics::Flags::orientation | Kinematics::Flags::linVel
      | Kinematics::Flags::angVel | Kinematics::Flags::linAcc | Kinematics::Flags::angAcc

flagsPoseKine

constexpr Kinematics::Flags::Byte stateObservation::KineticsObserver::flagsPoseKine

inlinestaticconstexpr

Initial value:

=
      Kinematics::Flags::position | Kinematics::Flags::orientation

flagsPosKine

constexpr Kinematics::Flags::Byte stateObservation::KineticsObserver::flagsPosKine = Kinematics::Flags::position

inlinestaticconstexpr

flagsStateKine

constexpr Kinematics::Flags::Byte stateObservation::KineticsObserver::flagsStateKine

inlinestaticconstexpr

Initial value:

=
      Kinematics::Flags::position | Kinematics::Flags::orientation | Kinematics::Flags::linVel
      | Kinematics::Flags::angVel

forceSensorVarianceDefault

const double stateObservation::KineticsObserver::forceSensorVarianceDefault

static

gyroBiasInitCovMat_

Matrix3 stateObservation::KineticsObserver::gyroBiasInitCovMat_

protected

gyroBiasInitVarianceDefault

const double stateObservation::KineticsObserver::gyroBiasInitVarianceDefault

static

gyroBiasProcessCovMat_

Matrix3 stateObservation::KineticsObserver::gyroBiasProcessCovMat_

protected

gyroBiasProcessVarianceDefault

const double stateObservation::KineticsObserver::gyroBiasProcessVarianceDefault

static

gyroCovMatDefault_

Matrix3 stateObservation::KineticsObserver::gyroCovMatDefault_

protected

gyroVarianceDefault

const double stateObservation::KineticsObserver::gyroVarianceDefault

static

I_

IndexedMatrix3 stateObservation::KineticsObserver::I_

protected

Id_

IndexedMatrix3 stateObservation::KineticsObserver::Id_

protected

imuSensors_

VectorIMU stateObservation::KineticsObserver::imuSensors_

protected

initTotalCentroidForce_

Vector3 stateObservation::KineticsObserver::initTotalCentroidForce_

protected

initTotalCentroidTorque_

Vector3 stateObservation::KineticsObserver::initTotalCentroidTorque_

protected

k_data_

TimeIndex stateObservation::KineticsObserver::k_data_

protected

k_est_

TimeIndex stateObservation::KineticsObserver::k_est_

protected

linearDampingDefault

const double stateObservation::KineticsObserver::linearDampingDefault

static

linearDampingMatDefault_

Matrix3 stateObservation::KineticsObserver::linearDampingMatDefault_

protected

linearStiffnessDefault

const double stateObservation::KineticsObserver::linearStiffnessDefault

static

linearStiffnessMatDefault_

Matrix3 stateObservation::KineticsObserver::linearStiffnessMatDefault_

protected

Default Stiffness and damping.

mass_

double stateObservation::KineticsObserver::mass_

protected

maxContacts_

unsigned stateObservation::KineticsObserver::maxContacts_

protected

maxImuNumber_

unsigned stateObservation::KineticsObserver::maxImuNumber_

protected

measurementCovMatrix_

Matrix stateObservation::KineticsObserver::measurementCovMatrix_

protected

measurementNoise_

NoiseBase* stateObservation::KineticsObserver::measurementNoise_

protected

measurementSize_

Index stateObservation::KineticsObserver::measurementSize_

protected

measurementTangentSize_

Index stateObservation::KineticsObserver::measurementTangentSize_

protected

measurementVector_

Vector stateObservation::KineticsObserver::measurementVector_

protected

nanDetected_

bool stateObservation::KineticsObserver::nanDetected_ = false

numberOfContactRealSensors_

Index stateObservation::KineticsObserver::numberOfContactRealSensors_

protected

oldWorldCentroidStateVector_

Vector stateObservation::KineticsObserver::oldWorldCentroidStateVector_

protected

opt_

struct stateObservation::KineticsObserver::Opt stateObservation::KineticsObserver::opt_

protected

orientationSensorVarianceDefault

const double stateObservation::KineticsObserver::orientationSensorVarianceDefault

static

positionSensorVarianceDefault

const double stateObservation::KineticsObserver::positionSensorVarianceDefault

static

processNoise_

NoiseBase* stateObservation::KineticsObserver::processNoise_

protected

sigma_

IndexedVector3 stateObservation::KineticsObserver::sigma_

protected

sigmad_

IndexedVector3 stateObservation::KineticsObserver::sigmad_

protected

sizeAcceleroSignal

constexpr Index stateObservation::KineticsObserver::sizeAcceleroSignal = 3

inlinestaticconstexpr

sizeAngVel

constexpr Index stateObservation::KineticsObserver::sizeAngVel = sizeOriTangent

inlinestaticconstexpr

sizeAngVelTangent

constexpr Index stateObservation::KineticsObserver::sizeAngVelTangent = sizeAngVel

inlinestaticconstexpr

sizeContact

constexpr Index stateObservation::KineticsObserver::sizeContact = sizeContactKine + sizeWrench

inlinestaticconstexpr

sizeContactKine

constexpr Index stateObservation::KineticsObserver::sizeContactKine = sizePose

inlinestaticconstexpr

sizeContactKineTangent

constexpr Index stateObservation::KineticsObserver::sizeContactKineTangent = sizePoseTangent

inlinestaticconstexpr

sizeContactTangent

constexpr Index stateObservation::KineticsObserver::sizeContactTangent = sizeContactKineTangent + sizeWrench

inlinestaticconstexpr

sizeForce

constexpr Index stateObservation::KineticsObserver::sizeForce = 3

inlinestaticconstexpr

sizeForceTangent

constexpr Index stateObservation::KineticsObserver::sizeForceTangent = sizeForce

inlinestaticconstexpr

sizeGyroBias

constexpr Index stateObservation::KineticsObserver::sizeGyroBias = sizeGyroSignal

inlinestaticconstexpr

sizeGyroBiasTangent

constexpr Index stateObservation::KineticsObserver::sizeGyroBiasTangent = sizeGyroBias

inlinestaticconstexpr

sizeGyroSignal

constexpr Index stateObservation::KineticsObserver::sizeGyroSignal = 3

inlinestaticconstexpr

sizeIMUSignal

constexpr Index stateObservation::KineticsObserver::sizeIMUSignal = sizeAcceleroSignal + sizeGyroSignal

inlinestaticconstexpr

sizeLinAccTangent

constexpr Index stateObservation::KineticsObserver::sizeLinAccTangent = sizeLinVelTangent

inlinestaticconstexpr

sizeLinVel

constexpr Index stateObservation::KineticsObserver::sizeLinVel = sizePos

inlinestaticconstexpr

sizeLinVelTangent

constexpr Index stateObservation::KineticsObserver::sizeLinVelTangent = sizeLinVel

inlinestaticconstexpr

sizeOri

constexpr Index stateObservation::KineticsObserver::sizeOri = 4

inlinestaticconstexpr

sizeOriTangent

constexpr Index stateObservation::KineticsObserver::sizeOriTangent = 3

inlinestaticconstexpr

sizePos

constexpr Index stateObservation::KineticsObserver::sizePos = 3

inlinestaticconstexpr

sizePose

constexpr Index stateObservation::KineticsObserver::sizePose = sizePos + sizeOri

inlinestaticconstexpr

sizePoseTangent

constexpr Index stateObservation::KineticsObserver::sizePoseTangent = sizePos + sizeOriTangent

inlinestaticconstexpr

sizePosTangent

constexpr Index stateObservation::KineticsObserver::sizePosTangent = 3

inlinestaticconstexpr

sizeStateBase

constexpr Index stateObservation::KineticsObserver::sizeStateBase = sizeStateKine + sizeForce + sizeTorque

inlinestaticconstexpr

sizeStateKine

constexpr Index stateObservation::KineticsObserver::sizeStateKine = sizePos + sizeOri + sizeLinVel + sizeAngVel

inlinestaticconstexpr

sizeStateKineTangent

constexpr Index stateObservation::KineticsObserver::sizeStateKineTangent = sizePos + sizeOriTangent + sizeLinVel + sizeAngVel

inlinestaticconstexpr

sizeStateTangentBase

constexpr Index stateObservation::KineticsObserver::sizeStateTangentBase = sizeStateKineTangent + sizeForce + sizeTorque

inlinestaticconstexpr

sizeTorque

constexpr Index stateObservation::KineticsObserver::sizeTorque = 3

inlinestaticconstexpr

sizeTorqueTangent

constexpr Index stateObservation::KineticsObserver::sizeTorqueTangent = sizeTorque

inlinestaticconstexpr

sizeWrench

constexpr Index stateObservation::KineticsObserver::sizeWrench = sizeForce + sizeTorque

inlinestaticconstexpr

stateAngVelInitCovMat_

Matrix3 stateObservation::KineticsObserver::stateAngVelInitCovMat_

protected

stateAngVelInitVarianceDefault

const double stateObservation::KineticsObserver::stateAngVelInitVarianceDefault

static

stateAngVelProcessCovMat_

Matrix3 stateObservation::KineticsObserver::stateAngVelProcessCovMat_

protected

stateAngVelProcessVarianceDefault

const double stateObservation::KineticsObserver::stateAngVelProcessVarianceDefault

static

stateKinematicsInitCovMat_

Matrix12 stateObservation::KineticsObserver::stateKinematicsInitCovMat_

protected

stateKinematicsProcessCovMat_

Matrix12 stateObservation::KineticsObserver::stateKinematicsProcessCovMat_

protected

stateLinVelInitCovMat_

Matrix3 stateObservation::KineticsObserver::stateLinVelInitCovMat_

protected

stateLinVelInitVarianceDefault

const double stateObservation::KineticsObserver::stateLinVelInitVarianceDefault

static

stateLinVelProcessCovMat_

Matrix3 stateObservation::KineticsObserver::stateLinVelProcessCovMat_

protected

stateLinVelProcessVarianceDefault

const double stateObservation::KineticsObserver::stateLinVelProcessVarianceDefault

static

stateOriInitCovMat_

Matrix3 stateObservation::KineticsObserver::stateOriInitCovMat_

protected

stateOriInitVarianceDefault

const double stateObservation::KineticsObserver::stateOriInitVarianceDefault

static

stateOriProcessCovMat_

Matrix3 stateObservation::KineticsObserver::stateOriProcessCovMat_

protected

stateOriProcessVarianceDefault

const double stateObservation::KineticsObserver::stateOriProcessVarianceDefault

static

statePoseInitVarianceDefault

const double stateObservation::KineticsObserver::statePoseInitVarianceDefault

static

statePoseProcessVarianceDefault

const double stateObservation::KineticsObserver::statePoseProcessVarianceDefault

static

statePosInitCovMat_

Matrix3 stateObservation::KineticsObserver::statePosInitCovMat_

protected

statePosProcessCovMat_

Matrix3 stateObservation::KineticsObserver::statePosProcessCovMat_

protected

stateSize_

Index stateObservation::KineticsObserver::stateSize_

protected

stateTangentSize_

Index stateObservation::KineticsObserver::stateTangentSize_

protected

torqueSensorVarianceDefault

const double stateObservation::KineticsObserver::torqueSensorVarianceDefault

static

unmodeledWrenchInitCovMat_

Matrix6 stateObservation::KineticsObserver::unmodeledWrenchInitCovMat_

protected

unmodeledWrenchInitVarianceDefault

const double stateObservation::KineticsObserver::unmodeledWrenchInitVarianceDefault

static

unmodeledWrenchProcessCovMat_

Matrix6 stateObservation::KineticsObserver::unmodeledWrenchProcessCovMat_

protected

unmodeledWrenchProcessVarianceDefault

const double stateObservation::KineticsObserver::unmodeledWrenchProcessVarianceDefault

static

withAccelerationEstimation_

bool stateObservation::KineticsObserver::withAccelerationEstimation_

protected

withGyroBias_

bool stateObservation::KineticsObserver::withGyroBias_

protected

withUnmodeledWrench_

bool stateObservation::KineticsObserver::withUnmodeledWrench_

protected

worldCentroidKinematics_

Kinematics stateObservation::KineticsObserver::worldCentroidKinematics_

protected

worldCentroidStateKinematics_

LocalKinematics stateObservation::KineticsObserver::worldCentroidStateKinematics_

protected

worldCentroidStateVector_

Vector stateObservation::KineticsObserver::worldCentroidStateVector_

protected

worldCentroidStateVectorDx_

Vector stateObservation::KineticsObserver::worldCentroidStateVectorDx_

protected

---

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

• include/state-observation/dynamics-estimators/kinetics-observer.hpp