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 <kinetics-observer.hpp>

Inheritance diagram for stateObservation::KineticsObserver:

Collaboration diagram for stateObservation::KineticsObserver:

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

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.
virtual	~KineticsObserver ()
	Destroy the Kinetics Observer.
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.
void	setSamplingTime (double)
	Set the Sampling Time.
void	setWithUnmodeledWrench (bool b=true)
	Set if the unmodeled and unmeasured external wrench should be estimated.
void	setWithAccelerationEstimation (bool b=true)
	Sets if the estimation computes also the accelerations.
bool	getWithAccelerationEstimation () const
	Returns if the estimation computes also the accelerations.
void	setWithGyroBias (bool b=true)
	Set if the gyrometers bias is computed or not. This parameter is global for all the IMUs.
void	setMass (double)
	Set the total mass of the robot. This can be changed online.
double	getMass () const
	Returns the mass of the robot.
const IndexedMatrix3 &	getInertiaMatrix () const
	Returns the global inertia matrix of the robot at the center of mass.
const IndexedMatrix3 &	getInertiaMatrixDot () const
	Returns the derivative of the global inertia matrix of the robot at the center of mass.
const IndexedVector3 &	getAngularMomentum () const
	Returns the angular momentum of the robot at the center of mass.
const IndexedVector3 &	getAngularMomentumDot () const
	Returns the derivative of the angular momentum of the robot at the center of mass.
const IndexedVector3 &	getCenterOfMass () const
	Returns the position of the CoM of the robot in the user frame.
const IndexedVector3 &	getCenterOfMassDot () const
	Returns the linear velocity of the CoM of the robot in the user frame.
const IndexedVector3 &	getCenterOfMassDotDot () const
	Returns the linear acceleration of the CoM of the robot in the user frame.
Vector6	getAdditionalWrench () const
	Returns the input additional wrench, expressed in the centroid frame.
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.
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
void	setIMUDefaultCovarianceMatrix (const Matrix3 &acceleroCov, const Matrix3 &gyroCov)
	set the default covariance matrix for IMU.
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.
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.
void	setAbsolutePoseSensorDefaultCovarianceMatrix (const Matrix6 &covMat)
	Set the Absolute Pose Sensor Default Covariance Matrix.
void	setAbsoluteOriSensor (const Orientation &measurement)
	Set an Absolute Orientation Sensor measurement The measurement is the orientation of the observed frame in the global frame.
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.
void	setAbsoluteOriSensorDefaultCovarianceMatrix (const Matrix3 &covMat)
	Set the Absolute Orientation Sensor Default Covariance Matrix.
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.
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).
void	removeContact (Index contactnbr)
	Remove a contact.
void	clearContacts ()
	remove all the contacts
Index	getNumberOfSetContacts () const
	Get the Current Number Of Contacts.
std::vector< Index >	getListOfContacts () const
	Get the List Of Contact ids.
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.
void	updateContactWithWrenchSensor (const Vector6 &wrenchMeasurement, const Kinematics &localKine, unsigned contactNumber)
	Update the contact when it is equipped with wrench sensor.
void	updateContactWithWrenchSensor (const Vector6 &wrenchMeasurement, const Matrix6 &wrenchCovMatrix, const Kinematics &localKine, unsigned contactNumber)
	Update the contact when it is equipped with wrench sensor.
void	setContactWrenchSensorDefaultCovarianceMatrix (const Matrix6 &wrenchSensorCovMat)
	Set the Contact Wrench Sensor Default Covariance Matrix.
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.
void	setCenterOfMass (const Vector3 &com, const Vector3 &com_dot)
	Set the Center Of Mass kinematics expressed in the user frame.
void	setCenterOfMass (const Vector3 &com)
	Set the Center Of Mass kinematics expressed in the user frame.
void	setCoMInertiaMatrix (const Matrix3 &I, const Matrix3 &I_dot)
	Set the 3x3 inertia matrix and its derivative expressed in the user frame.
void	setCoMInertiaMatrix (const Matrix3 &I)
	Set the 3x3 inertia matrix expressed in the user frame.
void	setCoMInertiaMatrix (const Vector6 &I, const Vector6 &I_dot)
	Set the inertia matrix and its derivative as a Vector6 expressed in the user frame.
void	setCoMInertiaMatrix (const Vector6 &I)
	Set the inertia matrix as a Vector6 expressed in the user frame.
void	setCoMAngularMomentum (const Vector3 &sigma, const Vector3 &sigma_dot)
	Set the Angular Momentum around the CoM and its derviative expressed in the user frame.
void	setCoMAngularMomentum (const Vector3 &sigma)
	Set the Angular Momentum around the CoM expressed in the user frame.
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.
Running and getting the estimations
/////////////////////////////////////////////////////////
void	updateMeasurements ()
	Updates the measurements.
const Vector &	update ()
	Runs the estimation.
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.
LocalKinematics	getLocalCentroidKinematics () const
	Get the estimated local Kinematics of the centroid frame in the world frame (local, which means expressed in the centroid frame).
Kinematics	getGlobalCentroidKinematics () const
	Get the estimated Kinematics of the centroid frame in the world frame.
LocalKinematics	estimateAccelerations ()
	gets the Kinematics that include the linear and angular accelerations.
LocalKinematics	getLocalKinematicsOf (const Kinematics &userBodyKine)
	Get the local kinematics of a given frame (in the user frame) in the centroid frame.
Kinematics	getGlobalKinematicsOf (const Kinematics &userBodyKin) const
	Get the global kinematics of a given frame (in the user frame) in the centroid frame.
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.
Kinematics	getContactPosition (Index contactNbr) const
	Get the Contact 6D pose n in the global frame.
Vector6	getUnmodeledWrench () const
	Get the Unmodeled External Wrench (requires setWithUnmodeledWrench() to true before to update()).
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.
void	setWorldCentroidStateKinematics (const Kinematics &kine, bool resetCovariance=true)
	Set the State Kinematics.
void	setStateContact (Index index, Kinematics worldContactRestPose, const Vector6 &wrench, bool resetCovariance=true)
	Set the state contact kinematics and wrench.
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.
void	setStateUnmodeledWrench (const Vector6 &, bool resetCovariance=true)
	Set the State Unmodeled Wrench.
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.
void	resetInputs ()
	reset all the sensor inputs and provided contact information but keeps the contacts themselves
Covariance matrices operations
void	setKinematicsInitCovarianceDefault (const Matrix &)
	Set the Default value for Kinematics Init Covariance.
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.
void	setGyroBiasInitCovarianceDefault (const Matrix3 &covMat)
	Set the Default value for Gyro Bias Init Covariance.
void	setUnmodeledWrenchInitCovMatDefault (const Matrix6 &initCovMat)
	Set the default value for init Unmodeled Wrench covariance matrix.
void	setContactInitCovMatDefault (const Matrix12 &contactCovMat)
	Set the default valut for the Initial Covariance Matrix of the contact in the state.
void	setKinematicsStateCovariance (const Matrix &)
	Set the Kinematics State Covariance.
void	setGyroBiasStateCovariance (const Matrix3 &covMat, unsigned imuNumber)
	Set the Gyro Bias State Covariance.
void	setUnmodeledWrenchStateCovMat (const Matrix6 &newCovMat)
	Set the Unmodeled Wrench State Cov Mat.
void	setContactStateCovMat (Index contactNbr, const Matrix12 &contactCovMat)
	Set the Contact State Covariance Matrix.
void	setKinematicsProcessCovarianceDefault (const Matrix12 &)
	Set the default Kinematics Process Covariance.
void	setKinematicsProcessCovarianceDefault (const Matrix3 &P_pos, const Matrix3 &P_ori, const Matrix3 &P_linVel, const Matrix3 &P_angVel)
	Set the default Kinematics Process Covariance.
void	setGyroBiasProcessCovarianceDefault (const Matrix3 &covMat)
	Set the default Gyro Bias Process Covariance.
void	setUnmodeledWrenchProcessCovarianceDefault (const Matrix6 &covMat)
	Set the default Unmodeled Wrench Process Covariance.
void	setContactProcessCovarianceDefault (const Matrix12 &covMat)
	Set the default contact Process Covariance.
void	setKinematicsProcessCovariance (const Matrix12 &)
	Set the Kinematics Process Covariance.
void	setGyroBiasProcessCovariance (const Matrix3 &covMat, unsigned imuNumber)
	Set the Gyro Bias Process Covariance.
void	setUnmodeledWrenchProcessCovMat (const Matrix6 &processCovMat)
	Set the Unmodeled Wrench Process Covariance Mattix.
void	setContactProcessCovMat (Index contactNbr, const Matrix12 &contactCovMat)
	Set the Contact Process Covariance Matrix.
void	resetStateCovarianceMat ()
	Resets the covariance matrices to their original values.
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.
Index	getStateTangentSize () const
	Get the State Vector Tangent Size.
Index	getMeasurementSize () const
	Get the Measurement vector Size.
Matrix	getStateCovarianceMat () const
	Get the State Covariance matrix.
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.
void	setProcessNoiseCovarianceMat (const Matrix &Q)
	Set the covariance matrices for the process noises.
const Vector &	getCurrentStateVector () const
	Gets the current value of the state estimation in the form of a state vector \(\hat{x_{k}}\).
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.
void	setInitWorldCentroidStateVector (const Vector &initStateVector)
	Initializes the state vector.
void	setStateVector (const Vector &newvalue, bool resetCovariance=true)
	Set a value of the state x_k provided from another source.
Vector	getMeasurementVector ()
	Get the Measurement Vector.
Index	kineIndex () const
	Get the kinematics index of the state vector.
Index	posIndex () const
	Get the position index of the state vector.
Index	oriIndex () const
	Get the orientation index of the state vector.
Index	linVelIndex () const
	Get the linear velocity index of the state vector.
Index	angVelIndex () const
	Get the angular velocity index of the state vector.
Index	gyroBiasIndex (Index IMUNumber) const
	Get the gyro bias index of the state vector.
Index	unmodeledWrenchIndex () const
	Get the unmodeled external wrench index of the state vector.
Index	unmodeledForceIndex () const
	Get the unmodeled external linear force index of the state vector.
Index	unmodeledTorqueIndex () const
	Get the unmodeled external torque force index of the state vector.
Index	contactsIndex () const
	Get the index for the contact segment in the state vector.
Index	contactIndex (Index contactNbr) const
	Get the index of a specific contact in the sate vector.
Index	contactKineIndex (Index contactNbr) const
	Get the index of the kinematics of a specific contact in the sate vector.
Index	contactPosIndex (Index contactNbr) const
	Get the index of the position of a specific contact in the sate vector.
Index	contactOriIndex (Index contactNbr) const
	Get the index of the orientation of a specific contact in the sate vector.
Index	contactForceIndex (Index contactNbr) const
	Get the index of the linear force of a specific contact in the sate vector.
Index	contactTorqueIndex (Index contactNbr) const
	Get the index of the toraue of a specific contact in the sate vector.
Index	contactWrenchIndex (Index contactNbr) const
	Get the index of the wrench of a specific contact in the sate vector.
Index	kineIndexTangent () const
	Get the kinematics index of the tangent state vector.
Index	posIndexTangent () const
	Get the position index of the tangent state vector.
Index	oriIndexTangent () const
	Get the orientation index of the tangent state vector.
Index	linVelIndexTangent () const
	Get the linear velocity index of the tangent state vector.
Index	angVelIndexTangent () const
	Get the angular velocity index of the tangent state vector.
Index	gyroBiasIndexTangent (Index IMUNumber) const
	Get the gyro bias index of the tangent state vector.
Index	unmodeledWrenchIndexTangent () const
	Get the unmodeled external wrench index of the tangent state vector.
Index	unmodeledForceIndexTangent () const
	Get the unmodeled external linear force index of the tangent state vector.
Index	unmodeledTorqueIndexTangent () const
	Get the unmodeled external torque force index of the tangent state vector.
Index	contactsIndexTangent () const
	Get the index for the contact segment in the tangent state vector.
Index	contactIndexTangent (Index contactNbr) const
	Get the index of a specific contact in the tangent sate vector.
Index	contactKineIndexTangent (Index contactNbr) const
	Get the index of the kinematics of a specific contact in the tangent sate vector.
Index	contactPosIndexTangent (Index contactNbr) const
	Get the index of the position of a specific contact in the tangent sate vector.
Index	contactOriIndexTangent (Index contactNbr) const
	Get the index of the orientation of a specific contact in the tangent sate vector.
Index	contactForceIndexTangent (Index contactNbr) const
	Get the index of the linear force of a specific contact in the tangent sate vector.
Index	contactTorqueIndexTangent (Index contactNbr) const
	Get the index of the toraue of a specific contact in the tangent sate vector.
Index	contactWrenchIndexTangent (Index contactNbr) const
	Get the index of the wrench of a specific contact in the sate tangent vector.
Getters for the extended Kalman filter (advanced use)
const ExtendedKalmanFilter &	getEKF () const
	Gets a const reference on the extended Kalman filter.
ExtendedKalmanFilter &	getEKF ()
Vector6	getCentroidContactWrench (Index numContact) const
	Returns the wrench exerted at the contact, expressed in the frame of the centroid.
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).
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.
Kinematics	getContactStateRestKinematics (Index numContact) const
	Returns the estimated rest pose of the contact in the world frame.
Kinematics	getUserContactInputPose (Index numContact) const
	Returns the pose of the contact in the user frame, given as an input when updating the contact.
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.
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.
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.
void	addUnmodeledAndContactWrench_ (const Vector &centroidStateVector, Vector3 &force, Vector3 &torque)
	Adds the unmodeled and contact wrenches from the state to the given wrench.
void	addUnmodeledWrench_ (const Vector &centroidStateVector, Vector3 &force, Vector3 &torque)
	Adds the unmodeled wrench from the state to the given wrench.
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
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.
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.
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.
virtual void	setProcessNoise (NoiseBase *)
	Sets a noise which disturbs the state dynamics.
virtual void	resetProcessNoise ()
	Removes the process noise.
virtual NoiseBase *	getProcessNoise () const
	Gets the process noise.
virtual void	setMeasurementNoise (NoiseBase *)
	Sets a noise which disturbs the measurements.
virtual void	resetMeasurementNoise ()
	Removes the measurement noise.
virtual NoiseBase *	getMeasurementNoise () const
	Gets a pointer on the measurement noise.
virtual Index	getInputSize () const
	Gets the input size.
void	stateNaNCorrection_ ()
void	updateLocalKineAndContacts_ ()
	update of the state kinematics worldCentroidStateKinematics_ and of the contacts pose with the newly estimated state
void	updateGlobalKine_ ()
	updates the global kinematics of the centroid from the local ones, that can be more interpretable
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.
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.
Index	gyroBiasIndex (VectorIMUConstIterator i) const
	Getters for the indexes of the state Vector using private types.
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.
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)
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.
virtual bool	checkInputvector (const Vector &)
	Gives a boolean answer on whether or not the vector is correctly sized to be an input vector.

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

State vector representation arithmetics and derivation (advanced use)
Vector	stateSum (const Vector &stateVector, const Vector &tangentVector)
	the sum operator for the state vector
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
Vector	stateDifference (const Vector &stateVector1, const Vector &stateVector2)
	the difference operator for the state statevector1 ⊖ statevector2
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
virtual void	measurementDifference (const Vector &measureVector1, const Vector &measureVector2, Vector &difference)
	the difference operator for the measurement statevector1 ⊖ statevector2
virtual void	useFiniteDifferencesJacobians (bool b=true)
	Define if we use dinite differences Jacobian or analytic.
virtual void	setFiniteDifferenceStep (const Vector &dx)
	Set the Finite Difference time step.
virtual Matrix	computeAMatrix ()
virtual Matrix	computeCMatrix ()
void	computeLocalAccelerations (const Vector &x, Vector &acceleration)
	computes the local acceleration from the given state vector
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.
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.
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.

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.

Definition at line 41 of file kinetics-observer.hpp.

Member Typedef Documentation

Kinematics

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

Definition at line 44 of file kinetics-observer.hpp.

LocalKinematics

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

Definition at line 45 of file kinetics-observer.hpp.

Orientation

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

Definition at line 46 of file kinetics-observer.hpp.

VectorIMU

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

protected

Definition at line 1031 of file kinetics-observer.hpp.

VectorIMUIterator

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

protected

Definition at line 1032 of file kinetics-observer.hpp.

VectorIMUConstIterator

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

protected

Definition at line 1033 of file kinetics-observer.hpp.

VectorContact

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

protected

Definition at line 1073 of file kinetics-observer.hpp.

VectorContactIterator

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

protected

Definition at line 1074 of file kinetics-observer.hpp.

VectorContactConstIterator

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

protected

Definition at line 1075 of file kinetics-observer.hpp.

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

Member Function Documentation

getSamplingTime()

double stateObservation::KineticsObserver::getSamplingTime

(

)

const

Get the Sampling Time.

Returns

const double &

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

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

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.

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

setMass()

void stateObservation::KineticsObserver::setMass

(

double

)

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

Returns

sets

getMass()

double stateObservation::KineticsObserver::getMass

(

)

const

Returns the mass of the robot.

Returns

the mass of the robot.

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.

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.

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.

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

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

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

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)

removeContact()

void stateObservation::KineticsObserver::removeContact

(

Index

contactnbr

)

Remove a contact.

Parameters

contactnbr

the number of the contact to remove

getNumberOfSetContacts()

Index stateObservation::KineticsObserver::getNumberOfSetContacts

(

)

const

Get the Current Number Of Contacts.

Returns

Index The current number of contacts

getListOfContacts()

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

(

)

const

Get the List Of Contact ids.

Returns

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

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

setContactWrenchSensorDefaultCovarianceMatrix()

void stateObservation::KineticsObserver::setContactWrenchSensorDefaultCovarianceMatrix

(

const Matrix6 &

wrenchSensorCovMat

)

Set the Contact Wrench Sensor Default Covariance Matrix.

Parameters

wrenchSensorCovMat

the new default covariance matrix

setCenterOfMass() [1/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

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

)

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

The velocity and acceleration will be computed through finite differences

Parameters

com

position

setCoMInertiaMatrix() [1/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() [2/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() [3/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

setCoMInertiaMatrix() [4/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

setCoMAngularMomentum() [1/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

setCoMAngularMomentum() [2/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

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

updateMeasurements()

void stateObservation::KineticsObserver::updateMeasurements

(

)

Updates the measurements.

Updates the measurement sensors and the associated vectors and covariance matrices

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

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.

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

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

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

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

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

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

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

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

setWorldCentroidStateKinematics() [1/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

setWorldCentroidStateKinematics() [2/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

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

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

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

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

setUnmodeledWrenchInitCovMatDefault()

void stateObservation::KineticsObserver::setUnmodeledWrenchInitCovMatDefault

(

const Matrix6 &

initCovMat

)

Set the default value for init Unmodeled Wrench covariance matrix.

Parameters

initCovMat

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

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

setUnmodeledWrenchStateCovMat()

void stateObservation::KineticsObserver::setUnmodeledWrenchStateCovMat

(

const Matrix6 &

newCovMat

)

Set the Unmodeled Wrench State Cov Mat.

Parameters

newCovMat

setContactStateCovMat()

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

Set the Contact State Covariance Matrix.

Parameters

contactNbr
contactCovMat	the contact number id

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

setUnmodeledWrenchProcessCovMat()

void stateObservation::KineticsObserver::setUnmodeledWrenchProcessCovMat

(

const Matrix6 &

processCovMat

)

Set the Unmodeled Wrench Process Covariance Mattix.

Parameters

processCovMat

setContactProcessCovMat()

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

Set the Contact Process Covariance Matrix.

Parameters

contactNbr
contactCovMat	the contact number id

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

getMeasurementSize()

Index stateObservation::KineticsObserver::getMeasurementSize ( ) const

virtual

Get the Measurement vector Size.

Returns

Index

Implements stateObservation::DynamicalSystemFunctorBase.

getStateCovarianceMat()

Matrix stateObservation::KineticsObserver::getStateCovarianceMat

(

)

const

Get the State Covariance matrix.

Returns

Matrix

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

setProcessNoiseCovarianceMat()

void stateObservation::KineticsObserver::setProcessNoiseCovarianceMat

(

const Matrix &

Q

)

Set the covariance matrices for the process noises.

Parameters

Q	process noise

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&

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

setInitWorldCentroidStateVector()

void stateObservation::KineticsObserver::setInitWorldCentroidStateVector

(

const Vector &

initStateVector

)

Initializes the state vector.

Parameters

initStateVector

the initial state vector.

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

getMeasurementVector()

Vector stateObservation::KineticsObserver::getMeasurementVector

(

)

Get the Measurement Vector.

Returns

Vector

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

posIndex()

Index stateObservation::KineticsObserver::posIndex ( ) const

inline

Get the position index of the state vector.

Returns

Index

oriIndex()

Index stateObservation::KineticsObserver::oriIndex ( ) const

inline

Get the orientation index of the state vector.

Returns

Index

linVelIndex()

Index stateObservation::KineticsObserver::linVelIndex ( ) const

inline

Get the linear velocity index of the state vector.

Returns

Index

angVelIndex()

Index stateObservation::KineticsObserver::angVelIndex ( ) const

inline

Get the angular velocity index of the state vector.

Returns

Index

gyroBiasIndex()

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

inline

Get the gyro bias index of the state vector.

Returns

Index

unmodeledWrenchIndex()

Index stateObservation::KineticsObserver::unmodeledWrenchIndex ( ) const

inline

Get the unmodeled external wrench index of the state vector.

Returns

Index

unmodeledForceIndex()

Index stateObservation::KineticsObserver::unmodeledForceIndex ( ) const

inline

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

Returns

Index

unmodeledTorqueIndex()

Index stateObservation::KineticsObserver::unmodeledTorqueIndex ( ) const

inline

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

Returns

Index

contactsIndex()

Index stateObservation::KineticsObserver::contactsIndex ( ) const

inline

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

Returns

Index

contactIndex()

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

contactKineIndex()

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

contactPosIndex()

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

contactOriIndex()

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

contactForceIndex()

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

contactTorqueIndex()

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

contactWrenchIndex()

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

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

posIndexTangent()

Index stateObservation::KineticsObserver::posIndexTangent ( ) const

inline

Get the position index of the tangent state vector.

Returns

Index

oriIndexTangent()

Index stateObservation::KineticsObserver::oriIndexTangent ( ) const

inline

Get the orientation index of the tangent state vector.

Returns

Index

linVelIndexTangent()

Index stateObservation::KineticsObserver::linVelIndexTangent ( ) const

inline

Get the linear velocity index of the tangent state vector.

Returns

Index

angVelIndexTangent()

Index stateObservation::KineticsObserver::angVelIndexTangent ( ) const

inline

Get the angular velocity index of the tangent state vector.

Returns

Index

gyroBiasIndexTangent()

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

inline

Get the gyro bias index of the tangent state vector.

Returns

Index

unmodeledWrenchIndexTangent()

Index stateObservation::KineticsObserver::unmodeledWrenchIndexTangent ( ) const

inline

Get the unmodeled external wrench index of the tangent 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

unmodeledTorqueIndexTangent()

Index stateObservation::KineticsObserver::unmodeledTorqueIndexTangent ( ) const

inline

Get the unmodeled external torque force index of the tangent 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

contactIndexTangent()

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

contactKineIndexTangent()

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

contactPosIndexTangent()

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

contactOriIndexTangent()

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

contactForceIndexTangent()

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

contactTorqueIndexTangent()

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

contactWrenchIndexTangent()

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

getEKF()

ExtendedKalmanFilter & stateObservation::KineticsObserver::getEKF

(

)

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

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.

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.

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.

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

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.

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

getInputSize()

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

protectedvirtual

Gets the input size.

Implements stateObservation::DynamicalSystemFunctorBase.

getCentroidContactWrench()

Vector6 stateObservation::KineticsObserver::getCentroidContactWrench

(

Index

numContact

)

const

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

Returns

Vector6

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

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

getContactStateRestKinematics()

Kinematics stateObservation::KineticsObserver::getContactStateRestKinematics

(

Index

numContact

)

const

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

Returns

Kinematics

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

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

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.

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.

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.

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

setFiniteDifferenceStep()

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

virtual

Set the Finite Difference time step.

Parameters

dx	the timestep

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

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.

Friends And Related Symbol Documentation

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)

Member Data Documentation

maxContacts_

unsigned stateObservation::KineticsObserver::maxContacts_

protected

Definition at line 1325 of file kinetics-observer.hpp.

maxImuNumber_

unsigned stateObservation::KineticsObserver::maxImuNumber_

protected

Definition at line 1326 of file kinetics-observer.hpp.

absPoseSensor_

AbsolutePoseSensor stateObservation::KineticsObserver::absPoseSensor_

protected

Definition at line 1328 of file kinetics-observer.hpp.

absOriSensor_

AbsoluteOriSensor stateObservation::KineticsObserver::absOriSensor_

protected

Definition at line 1329 of file kinetics-observer.hpp.

contacts_

VectorContact stateObservation::KineticsObserver::contacts_

protected

Definition at line 1330 of file kinetics-observer.hpp.

imuSensors_

VectorIMU stateObservation::KineticsObserver::imuSensors_

protected

Definition at line 1331 of file kinetics-observer.hpp.

stateSize_

Index stateObservation::KineticsObserver::stateSize_

protected

Definition at line 1333 of file kinetics-observer.hpp.

stateTangentSize_

Index stateObservation::KineticsObserver::stateTangentSize_

protected

Definition at line 1334 of file kinetics-observer.hpp.

measurementSize_

Index stateObservation::KineticsObserver::measurementSize_

protected

Definition at line 1335 of file kinetics-observer.hpp.

measurementTangentSize_

Index stateObservation::KineticsObserver::measurementTangentSize_

protected

Definition at line 1336 of file kinetics-observer.hpp.

worldCentroidStateVector_

Vector stateObservation::KineticsObserver::worldCentroidStateVector_

protected

Definition at line 1338 of file kinetics-observer.hpp.

worldCentroidStateVectorDx_

Vector stateObservation::KineticsObserver::worldCentroidStateVectorDx_

protected

Definition at line 1339 of file kinetics-observer.hpp.

oldWorldCentroidStateVector_

Vector stateObservation::KineticsObserver::oldWorldCentroidStateVector_

protected

Definition at line 1340 of file kinetics-observer.hpp.

worldCentroidStateKinematics_

LocalKinematics stateObservation::KineticsObserver::worldCentroidStateKinematics_

protected

Definition at line 1342 of file kinetics-observer.hpp.

worldCentroidKinematics_

Kinematics stateObservation::KineticsObserver::worldCentroidKinematics_

protected

Definition at line 1343 of file kinetics-observer.hpp.

additionalForce_

Vector3 stateObservation::KineticsObserver::additionalForce_

protected

Definition at line 1345 of file kinetics-observer.hpp.

additionalTorque_

Vector3 stateObservation::KineticsObserver::additionalTorque_

protected

Definition at line 1346 of file kinetics-observer.hpp.

initTotalCentroidForce_

Vector3 stateObservation::KineticsObserver::initTotalCentroidForce_

protected

Definition at line 1348 of file kinetics-observer.hpp.

initTotalCentroidTorque_

Vector3 stateObservation::KineticsObserver::initTotalCentroidTorque_

protected

Definition at line 1349 of file kinetics-observer.hpp.

measurementVector_

Vector stateObservation::KineticsObserver::measurementVector_

protected

Definition at line 1351 of file kinetics-observer.hpp.

measurementCovMatrix_

Matrix stateObservation::KineticsObserver::measurementCovMatrix_

protected

Definition at line 1352 of file kinetics-observer.hpp.

ekf_

stateObservation::ExtendedKalmanFilter stateObservation::KineticsObserver::ekf_

protected

Definition at line 1354 of file kinetics-observer.hpp.

finiteDifferencesJacobians_

bool stateObservation::KineticsObserver::finiteDifferencesJacobians_

protected

Definition at line 1355 of file kinetics-observer.hpp.

withGyroBias_

bool stateObservation::KineticsObserver::withGyroBias_

protected

Definition at line 1356 of file kinetics-observer.hpp.

withUnmodeledWrench_

bool stateObservation::KineticsObserver::withUnmodeledWrench_

protected

Definition at line 1357 of file kinetics-observer.hpp.

withAccelerationEstimation_

bool stateObservation::KineticsObserver::withAccelerationEstimation_

protected

Definition at line 1358 of file kinetics-observer.hpp.

com_

IndexedVector3 stateObservation::KineticsObserver::com_

protected

Definition at line 1360 of file kinetics-observer.hpp.

comd_

IndexedVector3 stateObservation::KineticsObserver::comd_

protected

Definition at line 1360 of file kinetics-observer.hpp.

comdd_

IndexedVector3 stateObservation::KineticsObserver::comdd_

protected

Definition at line 1360 of file kinetics-observer.hpp.

sigma_

IndexedVector3 stateObservation::KineticsObserver::sigma_

protected

Definition at line 1361 of file kinetics-observer.hpp.

sigmad_

IndexedVector3 stateObservation::KineticsObserver::sigmad_

protected

Definition at line 1361 of file kinetics-observer.hpp.

I_

IndexedMatrix3 stateObservation::KineticsObserver::I_

protected

Definition at line 1362 of file kinetics-observer.hpp.

Id_

IndexedMatrix3 stateObservation::KineticsObserver::Id_

protected

Definition at line 1362 of file kinetics-observer.hpp.

k_est_

TimeIndex stateObservation::KineticsObserver::k_est_

protected

Definition at line 1364 of file kinetics-observer.hpp.

k_data_

TimeIndex stateObservation::KineticsObserver::k_data_

protected

Definition at line 1365 of file kinetics-observer.hpp.

mass_

double stateObservation::KineticsObserver::mass_

protected

Definition at line 1367 of file kinetics-observer.hpp.

dt_

double stateObservation::KineticsObserver::dt_

protected

Definition at line 1369 of file kinetics-observer.hpp.

processNoise_

NoiseBase* stateObservation::KineticsObserver::processNoise_

protected

Definition at line 1371 of file kinetics-observer.hpp.

measurementNoise_

NoiseBase* stateObservation::KineticsObserver::measurementNoise_

protected

Definition at line 1372 of file kinetics-observer.hpp.

numberOfContactRealSensors_

Index stateObservation::KineticsObserver::numberOfContactRealSensors_

protected

Definition at line 1374 of file kinetics-observer.hpp.

currentIMUSensorNumber_

Index stateObservation::KineticsObserver::currentIMUSensorNumber_

protected

Definition at line 1375 of file kinetics-observer.hpp.

sizeAcceleroSignal

Index stateObservation::KineticsObserver::sizeAcceleroSignal = 3

inlinestaticconstexpr

Definition at line 1421 of file kinetics-observer.hpp.

sizeGyroSignal

Index stateObservation::KineticsObserver::sizeGyroSignal = 3

inlinestaticconstexpr

Definition at line 1422 of file kinetics-observer.hpp.

sizeIMUSignal

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

inlinestaticconstexpr

Definition at line 1423 of file kinetics-observer.hpp.

sizePos

Index stateObservation::KineticsObserver::sizePos = 3

inlinestaticconstexpr

Definition at line 1425 of file kinetics-observer.hpp.

sizePosTangent

Index stateObservation::KineticsObserver::sizePosTangent = 3

inlinestaticconstexpr

Definition at line 1426 of file kinetics-observer.hpp.

sizeOri

Index stateObservation::KineticsObserver::sizeOri = 4

inlinestaticconstexpr

Definition at line 1427 of file kinetics-observer.hpp.

sizeOriTangent

Index stateObservation::KineticsObserver::sizeOriTangent = 3

inlinestaticconstexpr

Definition at line 1428 of file kinetics-observer.hpp.

sizeLinVel

Index stateObservation::KineticsObserver::sizeLinVel = sizePos

inlinestaticconstexpr

Definition at line 1429 of file kinetics-observer.hpp.

sizeLinVelTangent

Index stateObservation::KineticsObserver::sizeLinVelTangent = sizeLinVel

inlinestaticconstexpr

Definition at line 1430 of file kinetics-observer.hpp.

sizeLinAccTangent

Index stateObservation::KineticsObserver::sizeLinAccTangent = sizeLinVelTangent

inlinestaticconstexpr

Definition at line 1431 of file kinetics-observer.hpp.

sizeAngVel

Index stateObservation::KineticsObserver::sizeAngVel = sizeOriTangent

inlinestaticconstexpr

Definition at line 1432 of file kinetics-observer.hpp.

sizeAngVelTangent

Index stateObservation::KineticsObserver::sizeAngVelTangent = sizeAngVel

inlinestaticconstexpr

Definition at line 1433 of file kinetics-observer.hpp.

sizeGyroBias

Index stateObservation::KineticsObserver::sizeGyroBias = sizeGyroSignal

inlinestaticconstexpr

Definition at line 1434 of file kinetics-observer.hpp.

sizeGyroBiasTangent

Index stateObservation::KineticsObserver::sizeGyroBiasTangent = sizeGyroBias

inlinestaticconstexpr

Definition at line 1435 of file kinetics-observer.hpp.

sizeForce

Index stateObservation::KineticsObserver::sizeForce = 3

inlinestaticconstexpr

Definition at line 1437 of file kinetics-observer.hpp.

sizeForceTangent

Index stateObservation::KineticsObserver::sizeForceTangent = sizeForce

inlinestaticconstexpr

Definition at line 1438 of file kinetics-observer.hpp.

sizeTorque

Index stateObservation::KineticsObserver::sizeTorque = 3

inlinestaticconstexpr

Definition at line 1439 of file kinetics-observer.hpp.

sizeTorqueTangent

Index stateObservation::KineticsObserver::sizeTorqueTangent = sizeTorque

inlinestaticconstexpr

Definition at line 1440 of file kinetics-observer.hpp.

sizeWrench

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

inlinestaticconstexpr

Definition at line 1442 of file kinetics-observer.hpp.

sizeStateKine

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

inlinestaticconstexpr

Definition at line 1444 of file kinetics-observer.hpp.

sizeStateBase

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

inlinestaticconstexpr

Definition at line 1445 of file kinetics-observer.hpp.

sizeStateKineTangent

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

inlinestaticconstexpr

Definition at line 1446 of file kinetics-observer.hpp.

sizeStateTangentBase

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

inlinestaticconstexpr

Definition at line 1447 of file kinetics-observer.hpp.

sizePose

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

inlinestaticconstexpr

Definition at line 1449 of file kinetics-observer.hpp.

sizePoseTangent

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

inlinestaticconstexpr

Definition at line 1450 of file kinetics-observer.hpp.

sizeContactKine

Index stateObservation::KineticsObserver::sizeContactKine = sizePose

inlinestaticconstexpr

Definition at line 1452 of file kinetics-observer.hpp.

sizeContactKineTangent

Index stateObservation::KineticsObserver::sizeContactKineTangent = sizePoseTangent

inlinestaticconstexpr

Definition at line 1453 of file kinetics-observer.hpp.

sizeContact

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

inlinestaticconstexpr

Definition at line 1455 of file kinetics-observer.hpp.

sizeContactTangent

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

inlinestaticconstexpr

Definition at line 1456 of file kinetics-observer.hpp.

flagsStateKine

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

inlinestaticconstexpr

Initial value:

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

Definition at line 1458 of file kinetics-observer.hpp.

flagsContactKine

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

inlinestaticconstexpr

Initial value:

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

Definition at line 1462 of file kinetics-observer.hpp.

flagsPoseKine

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

inlinestaticconstexpr

Initial value:

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

Definition at line 1465 of file kinetics-observer.hpp.

flagsPosKine

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

inlinestaticconstexpr

Definition at line 1468 of file kinetics-observer.hpp.

flagsIMUKine

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

Definition at line 1470 of file kinetics-observer.hpp.

defaultMass

const double stateObservation::KineticsObserver::defaultMass

static

Definition at line 1475 of file kinetics-observer.hpp.

statePoseInitVarianceDefault

const double stateObservation::KineticsObserver::statePoseInitVarianceDefault

static

Definition at line 1477 of file kinetics-observer.hpp.

stateOriInitVarianceDefault

const double stateObservation::KineticsObserver::stateOriInitVarianceDefault

static

Definition at line 1478 of file kinetics-observer.hpp.

stateLinVelInitVarianceDefault

const double stateObservation::KineticsObserver::stateLinVelInitVarianceDefault

static

Definition at line 1479 of file kinetics-observer.hpp.

stateAngVelInitVarianceDefault

const double stateObservation::KineticsObserver::stateAngVelInitVarianceDefault

static

Definition at line 1480 of file kinetics-observer.hpp.

gyroBiasInitVarianceDefault

const double stateObservation::KineticsObserver::gyroBiasInitVarianceDefault

static

Definition at line 1481 of file kinetics-observer.hpp.

unmodeledWrenchInitVarianceDefault

const double stateObservation::KineticsObserver::unmodeledWrenchInitVarianceDefault

static

Definition at line 1482 of file kinetics-observer.hpp.

contactForceInitVarianceDefault

const double stateObservation::KineticsObserver::contactForceInitVarianceDefault

static

Definition at line 1483 of file kinetics-observer.hpp.

contactTorqueInitVarianceDefault

const double stateObservation::KineticsObserver::contactTorqueInitVarianceDefault

static

Definition at line 1484 of file kinetics-observer.hpp.

statePoseProcessVarianceDefault

const double stateObservation::KineticsObserver::statePoseProcessVarianceDefault

static

Definition at line 1486 of file kinetics-observer.hpp.

stateOriProcessVarianceDefault

const double stateObservation::KineticsObserver::stateOriProcessVarianceDefault

static

Definition at line 1487 of file kinetics-observer.hpp.

stateLinVelProcessVarianceDefault

const double stateObservation::KineticsObserver::stateLinVelProcessVarianceDefault

static

Definition at line 1488 of file kinetics-observer.hpp.

stateAngVelProcessVarianceDefault

const double stateObservation::KineticsObserver::stateAngVelProcessVarianceDefault

static

Definition at line 1489 of file kinetics-observer.hpp.

gyroBiasProcessVarianceDefault

const double stateObservation::KineticsObserver::gyroBiasProcessVarianceDefault

static

Definition at line 1490 of file kinetics-observer.hpp.

unmodeledWrenchProcessVarianceDefault

const double stateObservation::KineticsObserver::unmodeledWrenchProcessVarianceDefault

static

Definition at line 1491 of file kinetics-observer.hpp.

contactPositionProcessVarianceDefault

const double stateObservation::KineticsObserver::contactPositionProcessVarianceDefault

static

Definition at line 1492 of file kinetics-observer.hpp.

contactOrientationProcessVarianceDefault

const double stateObservation::KineticsObserver::contactOrientationProcessVarianceDefault

static

Definition at line 1493 of file kinetics-observer.hpp.

contactForceProcessVarianceDefault

const double stateObservation::KineticsObserver::contactForceProcessVarianceDefault

static

Definition at line 1494 of file kinetics-observer.hpp.

contactTorqueProcessVarianceDefault

const double stateObservation::KineticsObserver::contactTorqueProcessVarianceDefault

static

Definition at line 1495 of file kinetics-observer.hpp.

acceleroVarianceDefault

const double stateObservation::KineticsObserver::acceleroVarianceDefault

static

Definition at line 1497 of file kinetics-observer.hpp.

gyroVarianceDefault

const double stateObservation::KineticsObserver::gyroVarianceDefault

static

Definition at line 1498 of file kinetics-observer.hpp.

forceSensorVarianceDefault

const double stateObservation::KineticsObserver::forceSensorVarianceDefault

static

Definition at line 1499 of file kinetics-observer.hpp.

torqueSensorVarianceDefault

const double stateObservation::KineticsObserver::torqueSensorVarianceDefault

static

Definition at line 1500 of file kinetics-observer.hpp.

positionSensorVarianceDefault

const double stateObservation::KineticsObserver::positionSensorVarianceDefault

static

Definition at line 1501 of file kinetics-observer.hpp.

orientationSensorVarianceDefault

const double stateObservation::KineticsObserver::orientationSensorVarianceDefault

static

Definition at line 1502 of file kinetics-observer.hpp.

linearStiffnessDefault

const double stateObservation::KineticsObserver::linearStiffnessDefault

static

Definition at line 1504 of file kinetics-observer.hpp.

angularStiffnessDefault

const double stateObservation::KineticsObserver::angularStiffnessDefault

static

Definition at line 1505 of file kinetics-observer.hpp.

linearDampingDefault

const double stateObservation::KineticsObserver::linearDampingDefault

static

Definition at line 1506 of file kinetics-observer.hpp.

angularDampingDefault

const double stateObservation::KineticsObserver::angularDampingDefault

static

Definition at line 1507 of file kinetics-observer.hpp.

nanDetected_

bool stateObservation::KineticsObserver::nanDetected_ = false

Definition at line 1511 of file kinetics-observer.hpp.

linearStiffnessMatDefault_

Matrix3 stateObservation::KineticsObserver::linearStiffnessMatDefault_

protected

Default Stiffness and damping.

Definition at line 1516 of file kinetics-observer.hpp.

angularStiffnessMatDefault_

Matrix3 stateObservation::KineticsObserver::angularStiffnessMatDefault_

protected

Definition at line 1517 of file kinetics-observer.hpp.

linearDampingMatDefault_

Matrix3 stateObservation::KineticsObserver::linearDampingMatDefault_

protected

Definition at line 1518 of file kinetics-observer.hpp.

angularDampingMatDefault_

Matrix3 stateObservation::KineticsObserver::angularDampingMatDefault_

protected

Definition at line 1519 of file kinetics-observer.hpp.

acceleroCovMatDefault_

Matrix3 stateObservation::KineticsObserver::acceleroCovMatDefault_

protected

Definition at line 1522 of file kinetics-observer.hpp.

gyroCovMatDefault_

Matrix3 stateObservation::KineticsObserver::gyroCovMatDefault_

protected

Definition at line 1523 of file kinetics-observer.hpp.

contactWrenchSensorCovMatDefault_

Matrix6 stateObservation::KineticsObserver::contactWrenchSensorCovMatDefault_

protected

Definition at line 1524 of file kinetics-observer.hpp.

absPoseSensorCovMatDefault_

Matrix6 stateObservation::KineticsObserver::absPoseSensorCovMatDefault_

protected

Definition at line 1525 of file kinetics-observer.hpp.

absOriSensorCovMatDefault_

Matrix3 stateObservation::KineticsObserver::absOriSensorCovMatDefault_

protected

Definition at line 1526 of file kinetics-observer.hpp.

statePosInitCovMat_

Matrix3 stateObservation::KineticsObserver::statePosInitCovMat_

protected

Definition at line 1528 of file kinetics-observer.hpp.

stateOriInitCovMat_

Matrix3 stateObservation::KineticsObserver::stateOriInitCovMat_

protected

Definition at line 1529 of file kinetics-observer.hpp.

stateLinVelInitCovMat_

Matrix3 stateObservation::KineticsObserver::stateLinVelInitCovMat_

protected

Definition at line 1530 of file kinetics-observer.hpp.

stateAngVelInitCovMat_

Matrix3 stateObservation::KineticsObserver::stateAngVelInitCovMat_

protected

Definition at line 1531 of file kinetics-observer.hpp.

gyroBiasInitCovMat_

Matrix3 stateObservation::KineticsObserver::gyroBiasInitCovMat_

protected

Definition at line 1532 of file kinetics-observer.hpp.

unmodeledWrenchInitCovMat_

Matrix6 stateObservation::KineticsObserver::unmodeledWrenchInitCovMat_

protected

Definition at line 1533 of file kinetics-observer.hpp.

contactInitCovMatDefault_

Matrix12 stateObservation::KineticsObserver::contactInitCovMatDefault_

protected

Definition at line 1534 of file kinetics-observer.hpp.

statePosProcessCovMat_

Matrix3 stateObservation::KineticsObserver::statePosProcessCovMat_

protected

Definition at line 1536 of file kinetics-observer.hpp.

stateOriProcessCovMat_

Matrix3 stateObservation::KineticsObserver::stateOriProcessCovMat_

protected

Definition at line 1537 of file kinetics-observer.hpp.

stateLinVelProcessCovMat_

Matrix3 stateObservation::KineticsObserver::stateLinVelProcessCovMat_

protected

Definition at line 1538 of file kinetics-observer.hpp.

stateAngVelProcessCovMat_

Matrix3 stateObservation::KineticsObserver::stateAngVelProcessCovMat_

protected

Definition at line 1539 of file kinetics-observer.hpp.

gyroBiasProcessCovMat_

Matrix3 stateObservation::KineticsObserver::gyroBiasProcessCovMat_

protected

Definition at line 1540 of file kinetics-observer.hpp.

unmodeledWrenchProcessCovMat_

Matrix6 stateObservation::KineticsObserver::unmodeledWrenchProcessCovMat_

protected

Definition at line 1541 of file kinetics-observer.hpp.

contactPositionProcessCovMat_

Matrix3 stateObservation::KineticsObserver::contactPositionProcessCovMat_

protected

Definition at line 1542 of file kinetics-observer.hpp.

contactOrientationProcessCovMat_

Matrix3 stateObservation::KineticsObserver::contactOrientationProcessCovMat_

protected

Definition at line 1543 of file kinetics-observer.hpp.

contactForceProcessCovMat_

Matrix3 stateObservation::KineticsObserver::contactForceProcessCovMat_

protected

Definition at line 1544 of file kinetics-observer.hpp.

contactTorqueProcessCovMat_

Matrix3 stateObservation::KineticsObserver::contactTorqueProcessCovMat_

protected

Definition at line 1545 of file kinetics-observer.hpp.

contactProcessCovMatDefault_

Matrix12 stateObservation::KineticsObserver::contactProcessCovMatDefault_

protected

Definition at line 1546 of file kinetics-observer.hpp.

stateKinematicsInitCovMat_

Matrix12 stateObservation::KineticsObserver::stateKinematicsInitCovMat_

protected

Definition at line 1548 of file kinetics-observer.hpp.

stateKinematicsProcessCovMat_

Matrix12 stateObservation::KineticsObserver::stateKinematicsProcessCovMat_

protected

Definition at line 1549 of file kinetics-observer.hpp.

defaultdx

const double stateObservation::KineticsObserver::defaultdx

staticprotected

default derivation steps

Definition at line 1552 of file kinetics-observer.hpp.

---

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

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