#include <state-observation/dynamics-estimators/zmp-tracking-gain-estimator.hpp>

Collaboration diagram for stateObservation::ZmpTrackingGainEstimator:

Public Member Functions
	ZmpTrackingGainEstimator (double dt=defaultDt_, const Vector2 &zmpMeasureErrorStd=Vector2::Constant(defaultZmpMeasurementErrorStd), const Vector3 &gainDriftPerSecond=Vector3::Constant(defaultGainDriftSecond), const Vector2 &zmpProcessErrorStd=Vector2::Constant(defaultZmpProcessErrorStd), double minimumGain=defaultGainMinimum, const Vector2 &initZMP=Vector2::Zero(), const Vector3 &initGain=Vector3::Zero(), const Vector2 &initZMPUncertainty=Vector2::Constant(defaultZmpUncertainty), const Vector3 &initGainUncertainty=Vector3::Constant(defaultGainUncertainty))
	Construct a new ZMP Gain Estimator object. More...

void	resetWithMeasurements (const Vector2 &initZMP=Vector2::Zero(), const Vector3 &initGain=Vector3::Zero(), const Matrix2 &yaw=Matrix2::Identity(), const Vector2 &initZMPUncertainty=Vector2::Constant(defaultZmpUncertainty), const Vector3 &initGainUncertainty=Vector3::Constant(defaultGainUncertainty))
	Resets the estimator. More...

void	resetWithMeasurements (const Vector2 &initZMP, const Vector3 &initGain, double yaw, const Vector2 &initZMPUncertainty=Vector2::Constant(defaultZmpUncertainty), const Vector3 &initGainUncertainty=Vector3::Constant(defaultGainUncertainty))
	Resets the estimator. More...

void	resetWithMeasurements (const Vector2 &initZMP, const Vector3 &initGain, const Matrix3 &rotation, const Vector2 &initZMPUncertainty=Vector2::Constant(defaultZmpUncertainty), const Vector3 &initGainUncertainty=Vector3::Constant(defaultGainUncertainty))
	Resets the estimator with first measurements. More...

	~ZmpTrackingGainEstimator ()
	Destroy the Lipm Dcm Bias Estimator object. More...

void	setSamplingTime (double dt)
	Set the Sampling Time. More...

void	setGain (const Vector3 &gain)
	Set the Gain from a guess. More...

void	setGain (const Vector3 &gain, const Vector3 &uncertainty)
	Set the Gain from a guess. More...

void	setGainDriftPerSecond (const Vector3 &)
	Set the Gain Drift Per Second. More...

void	setMinimumGain (const double &minGain)
	Set the Gain Limit. More...

void	setZMPProcesError (const Vector2 &)
	Set the covariance of the zmp process linear dynamics error. More...

void	setZmpMeasureErrorStd (const Vector2 &)
	Set the Zmp Measurement Error Stamdard deviation. More...

void	setInputs (const Vector2 &zmpErr, const Vector2 &zmp, const Matrix3 &orientation)
	Set the Inputs of the estimator. More...

void	setInputs (const Vector2 &zmpErr, const Vector2 &zmp, double yaw)
	Set the Inputs of the estimator. More...

void	setInputs (const Vector2 &zmpErr, const Vector2 &zmp, const Matrix2 &R=Matrix2::Identity())
	Set the Inputs of the estimator. More...

void	update ()
	Runs the estimation. Needs to be called every timestep. More...

Matrix2	getEstimatedGain () const
	Get the Unbiased DCM filtered by the estimator. More...

Matrix2	getEstimatedLocalGain () const
	Get the estimated Bias expressed in the local frame of the robot. More...

LinearKalmanFilter &	getFilter ()
	Get the Kalman Filter object This can be used to run specific Advanced Kalman filter related funcions. More...

const LinearKalmanFilter &	getFilter () const
	Get the Kalman Filter object This can be used to run specific Advanced Kalman filter related funcions. More...

Static Public Attributes
constexpr static double	defaultGainDriftSecond = 0.002
	default expected drift of the bias every second More...

constexpr static double	defaultZmpProcessErrorStd = 0.005
	default error in the process linear dynamics of the zmp (in meters) More...

constexpr static double	defaultZmpMeasurementErrorStd = 0.005
	default error in the measurements of the zmp More...

constexpr static double	defaultGainMinimum = .01

constexpr static double	defaultZmpUncertainty = 0.005
	default valu for the initial ZMP uncertainty. It should be quite low since this is supposed to be measured More...

constexpr static double	defaultGainUncertainty = 20
	default value for the uncertainty of the Gain More...

Protected Types
typedef Eigen::Matrix< double, 2, 5 >	Matrix25

typedef Eigen::Matrix< double, 2, 3 >	Matrix23

Protected Member Functions
void	updateR_ ()

void	updateQ_ ()

Static Protected Member Functions
static Matrix2	Vec2ToSqDiag_ (const Vector2 &v)
	builds a diagonal out of the square valued of the Vec2 More...

static Matrix3	Vec3ToSqDiag_ (const Vector3 &v)
	builds a diagonal out of the square valued of the Vec3 More...

Protected Attributes
double	dt_

double	minimumGain_

Vector2	zmpMeasureErrorstd_

Vector3	gainDriftPerSecondStd_

Vector2	zmpProcessErrorStd_

Matrix2	yaw_

LinearKalmanFilter	filter_

Matrix4	A_

Matrix25	C_
	The B matrix is zero. More...

Matrix2	R_
	measurement noise More...

Matrix5	Q_
	process noise More...

Matrix2	previousOrientation_

Member Typedef Documentation

◆ Matrix23

typedef Eigen::Matrix<double, 2, 3> stateObservation::ZmpTrackingGainEstimator::Matrix23

protected

◆ Matrix25

typedef Eigen::Matrix<double, 2, 5> stateObservation::ZmpTrackingGainEstimator::Matrix25

protected

Constructor & Destructor Documentation

◆ ZmpTrackingGainEstimator()

stateObservation::ZmpTrackingGainEstimator::ZmpTrackingGainEstimator	(	double	dt = `defaultDt_`,
		const Vector2 &	zmpMeasureErrorStd = `Vector2::Constant(defaultZmpMeasurementErrorStd)`,
		const Vector3 &	gainDriftPerSecond = `Vector3::Constant(defaultGainDriftSecond)`,
		const Vector2 &	zmpProcessErrorStd = `Vector2::Constant(defaultZmpProcessErrorStd)`,
		double	minimumGain = `defaultGainMinimum`,
		const Vector2 &	initZMP = `Vector2::Zero()`,
		const Vector3 &	initGain = `Vector3::Zero()`,
		const Vector2 &	initZMPUncertainty = `Vector2::Constant(defaultZmpUncertainty)`,
		const Vector3 &	initGainUncertainty = `Vector3::Constant(defaultGainUncertainty)`
	)

Construct a new ZMP Gain Estimator object.

Parameters

dt	the sampling time in seconds
zmpMeasureErrorStd	the standard deviation of the zmp estimation error (m)
gainDriftPerSecond	the standard deviation of the gain drift (1/s). The components of this vetor represent respectively the diagonal components then the nondiagonal one
zmpProcessErrorStd	the standard deviation of the process linear dynamics of the zmp (m)
minimumGain	minimum, the gain has to be a positive definite matrix with the smallest eigenvalue bigger than this value
initZMP	the initial value of the ZMP (m)
initGain	the initial value of the Gain (Vector3 :two diagonals first then the nondiagonal term)
biasLimit	the X and Y (expressed in local frame) largest accepted absolute values of the bias (zero means no limit)
initZMPUncertainty	the uncertainty in the ZMP initial value in meters
initGainUncertainty	the uncertainty in the Gain initial value in (1/s)

◆ ~ZmpTrackingGainEstimator()

stateObservation::ZmpTrackingGainEstimator::~ZmpTrackingGainEstimator ( )

inline

Destroy the Lipm Dcm Bias Estimator object.

Member Function Documentation

◆ getEstimatedGain()

Matrix2 stateObservation::ZmpTrackingGainEstimator::getEstimatedGain ( ) const

inline

Get the Unbiased DCM filtered by the estimator.

@detailt This is the recommended output to take

Returns: double

the meaning is previousOrientation_ * getLocalGain() * previousOrientation_.transpose();

◆ getEstimatedLocalGain()

Matrix2 stateObservation::ZmpTrackingGainEstimator::getEstimatedLocalGain ( ) const

Get the estimated Bias expressed in the local frame of the robot.

Returns: double

◆ getFilter() [1/2]

LinearKalmanFilter& stateObservation::ZmpTrackingGainEstimator::getFilter ( )

inline

Get the Kalman Filter object This can be used to run specific Advanced Kalman filter related funcions.

Returns: LinearKalmanFilter&

◆ getFilter() [2/2]

const LinearKalmanFilter& stateObservation::ZmpTrackingGainEstimator::getFilter ( ) const

inline

Get the Kalman Filter object This can be used to run specific Advanced Kalman filter related funcions.

Returns: LinearKalmanFilter& const version

◆ resetWithMeasurements() [1/3]

void stateObservation::ZmpTrackingGainEstimator::resetWithMeasurements	(	const Vector2 &	initZMP,
		const Vector3 &	initGain,
		const Matrix3 &	rotation,
		const Vector2 &	initZMPUncertainty = `Vector2::Constant(defaultZmpUncertainty)`,
		const Vector3 &	initGainUncertainty = `Vector3::Constant(defaultGainUncertainty)`
	)

inline

Resets the estimator with first measurements.

Use this when initializing with an available DCM (biased / or not) measurement

Parameters

initZMP	the initial value of the ZMP (m)
initGain	the initial value of the Gain (Vector3 :two diagonals first then the nondiagonal term)
rotation	the 3d orientation from which the initial yaw angle will be extracted using the angle agnostic approach. This orientation is from local to global. i.e. zmp_global == orientation * zmp_local
zmpMeasureErrorStd	the standard deviation of the zmp estimation error (m)
initZMPUncertainty	the uncertainty in the ZMP initial value in meters
initGainUncertainty	the uncertainty in the Gain initial value in (1/s)

◆ resetWithMeasurements() [2/3]

void stateObservation::ZmpTrackingGainEstimator::resetWithMeasurements	(	const Vector2 &	initZMP,
		const Vector3 &	initGain,
		double	yaw,
		const Vector2 &	initZMPUncertainty = `Vector2::Constant(defaultZmpUncertainty)`,
		const Vector3 &	initGainUncertainty = `Vector3::Constant(defaultGainUncertainty)`
	)

inline

Resets the estimator.

Parameters

initZMP	the initial value of the ZMP (m)
initGain	the initial value of the Gain (Vector3 :two diagonals first then the nondiagonal term)
yaw	the yaw angle
zmpMeasureErrorStd	the standard deviation of the zmp estimation error (m)
initZMPUncertainty	the uncertainty in the ZMP initial value in meters
initGainUncertainty	the uncertainty in the Gain initial value in (1/s)

◆ resetWithMeasurements() [3/3]

void stateObservation::ZmpTrackingGainEstimator::resetWithMeasurements	(	const Vector2 &	initZMP = `Vector2::Zero()`,
		const Vector3 &	initGain = `Vector3::Zero()`,
		const Matrix2 &	yaw = `Matrix2::Identity()`,
		const Vector2 &	initZMPUncertainty = `Vector2::Constant(defaultZmpUncertainty)`,
		const Vector3 &	initGainUncertainty = `Vector3::Constant(defaultGainUncertainty)`
	)

Resets the estimator.

Parameters

initZMP	the initial value of the ZMP (m)
initGain	the initial value of the Gain (Vector3 :two diagonals first then the nondiagonal term)
yaw	the yaw expressed as a 2d matrix (a rotation in the X Y plane)
zmpMeasureErrorStd	the standard deviation of the zmp estimation error (m)
initZMPUncertainty	the uncertainty in the ZMP initial value in meters
initGainUncertainty	the uncertainty in the Gain initial value in (1/s)

◆ setGain() [1/2]

void stateObservation::ZmpTrackingGainEstimator::setGain ( const Vector3 & gain )

Set the Gain from a guess.

Parameters

gain	guess in the world frame. The two first components are the diagonal ones and the last one is the non diagonal scalar

◆ setGain() [2/2]

void stateObservation::ZmpTrackingGainEstimator::setGain	(	const Vector3 &	gain,
		const Vector3 &	uncertainty
	)

Set the Gain from a guess.

Parameters

gain	guess in the world frame. The two first components are the diagonal ones and the last one is the non diagonal scalar
the	uncertainty you have in this guess in meters

◆ setGainDriftPerSecond()

void stateObservation::ZmpTrackingGainEstimator::setGainDriftPerSecond ( const Vector3 & )

Set the Gain Drift Per Second.

Parameters

driftPerSecond the expectation of the drift of the gain per second. The components of this vetor represent respectively the diagonal components then the nondiagonal one

◆ setInputs() [1/3]

void stateObservation::ZmpTrackingGainEstimator::setInputs	(	const Vector2 &	zmpErr,
		const Vector2 &	zmp,
		const Matrix2 &	R = `Matrix2::Identity()`
	)

Set the Inputs of the estimator.

Parameters

zmperr	measurement of the DCM in the world frame
zmp	mesaurement of the ZMP in the world frame
R	the 2x2 Matrix'representing the yaw angle i.e. bias_global == R * bias*local

◆ setInputs() [2/3]

void stateObservation::ZmpTrackingGainEstimator::setInputs	(	const Vector2 &	zmpErr,
		const Vector2 &	zmp,
		const Matrix3 &	orientation
	)

inline

Set the Inputs of the estimator.

The yaw will be extracted from the orientation using the axis agnostic approach.

Parameters

zmperr	measurement of zmp error (zmp-zmp^{ref})
zmp	mesaurement of the ZMP in the world frame
orientation	the 3d orientation from which the yaw will be extracted. This orientation is from local to global. i.e. bias_global == orientation * bias*local

◆ setInputs() [3/3]

void stateObservation::ZmpTrackingGainEstimator::setInputs	(	const Vector2 &	zmpErr,
		const Vector2 &	zmp,
		double	yaw
	)

inline

Set the Inputs of the estimator.

Parameters

zmperr	measurement of zmp error (zmp-zmp^{ref})
zmp	mesaurement of the ZMP in the world frame
yaw	is the yaw angle to be used. This orientation is from local to global. i.e. bias_global == R * bias*local

◆ setMinimumGain()

void stateObservation::ZmpTrackingGainEstimator::setMinimumGain ( const double & minGain )

Set the Gain Limit.

Parameters

minGain the minimal value of the gain

◆ setSamplingTime()

void stateObservation::ZmpTrackingGainEstimator::setSamplingTime ( double dt )

Set the Sampling Time.

Parameters

dt	sampling time

◆ setZmpMeasureErrorStd()

void stateObservation::ZmpTrackingGainEstimator::setZmpMeasureErrorStd ( const Vector2 & )

Set the Zmp Measurement Error Stamdard deviation.

◆ setZMPProcesError()

void stateObservation::ZmpTrackingGainEstimator::setZMPProcesError ( const Vector2 & )

Set the covariance of the zmp process linear dynamics error.

Parameters

processErrorStd the standard deviation of the process dynamcis error

◆ update()

void stateObservation::ZmpTrackingGainEstimator::update ( )

Runs the estimation. Needs to be called every timestep.

Returns: Vector2

◆ updateQ_()

void stateObservation::ZmpTrackingGainEstimator::updateQ_ ( )

inlineprotected

◆ updateR_()

void stateObservation::ZmpTrackingGainEstimator::updateR_ ( )

inlineprotected

◆ Vec2ToSqDiag_()

static Matrix2 stateObservation::ZmpTrackingGainEstimator::Vec2ToSqDiag_ ( const Vector2 & v )

inlinestaticprotected

builds a diagonal out of the square valued of the Vec2

◆ Vec3ToSqDiag_()

static Matrix3 stateObservation::ZmpTrackingGainEstimator::Vec3ToSqDiag_ ( const Vector3 & v )

inlinestaticprotected

builds a diagonal out of the square valued of the Vec3

Member Data Documentation

◆ A_

Matrix4 stateObservation::ZmpTrackingGainEstimator::A_

protected

◆ C_

Matrix25 stateObservation::ZmpTrackingGainEstimator::C_

protected

The B matrix is zero.

◆ defaultGainDriftSecond

constexpr static double stateObservation::ZmpTrackingGainEstimator::defaultGainDriftSecond = 0.002

staticconstexpr

default expected drift of the bias every second

◆ defaultGainMinimum

constexpr static double stateObservation::ZmpTrackingGainEstimator::defaultGainMinimum = .01

staticconstexpr

default value for gain minimum, the gain has to be a positive definite matrix with the smallest eigenvalue bigger than this value

◆ defaultGainUncertainty

constexpr static double stateObservation::ZmpTrackingGainEstimator::defaultGainUncertainty = 20

staticconstexpr

default value for the uncertainty of the Gain

◆ defaultZmpMeasurementErrorStd

constexpr static double stateObservation::ZmpTrackingGainEstimator::defaultZmpMeasurementErrorStd = 0.005

staticconstexpr

default error in the measurements of the zmp

◆ defaultZmpProcessErrorStd

constexpr static double stateObservation::ZmpTrackingGainEstimator::defaultZmpProcessErrorStd = 0.005

staticconstexpr

default error in the process linear dynamics of the zmp (in meters)

◆ defaultZmpUncertainty

constexpr static double stateObservation::ZmpTrackingGainEstimator::defaultZmpUncertainty = 0.005

staticconstexpr

default valu for the initial ZMP uncertainty. It should be quite low since this is supposed to be measured

◆ dt_

double stateObservation::ZmpTrackingGainEstimator::dt_

protected

◆ filter_

LinearKalmanFilter stateObservation::ZmpTrackingGainEstimator::filter_

protected

◆ gainDriftPerSecondStd_

Vector3 stateObservation::ZmpTrackingGainEstimator::gainDriftPerSecondStd_

protected

◆ minimumGain_

double stateObservation::ZmpTrackingGainEstimator::minimumGain_

protected

◆ previousOrientation_

Matrix2 stateObservation::ZmpTrackingGainEstimator::previousOrientation_

protected

◆ Q_

Matrix5 stateObservation::ZmpTrackingGainEstimator::Q_

protected

process noise

◆ R_

Matrix2 stateObservation::ZmpTrackingGainEstimator::R_

protected

measurement noise

◆ yaw_

Matrix2 stateObservation::ZmpTrackingGainEstimator::yaw_

protected

◆ zmpMeasureErrorstd_

Vector2 stateObservation::ZmpTrackingGainEstimator::zmpMeasureErrorstd_

protected

◆ zmpProcessErrorStd_

Vector2 stateObservation::ZmpTrackingGainEstimator::zmpProcessErrorStd_

protected

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

include/state-observation/dynamics-estimators/zmp-tracking-gain-estimator.hpp

Public Member Functions

Static Public Attributes

Protected Types

Protected Member Functions

Static Protected Member Functions

Protected Attributes

Member Typedef Documentation

◆ Matrix23

◆ Matrix25

Constructor & Destructor Documentation

◆ ZmpTrackingGainEstimator()

◆ ~ZmpTrackingGainEstimator()

Member Function Documentation

◆ getEstimatedGain()

◆ getEstimatedLocalGain()

◆ getFilter() [1/2]

◆ getFilter() [2/2]

◆ resetWithMeasurements() [1/3]

◆ resetWithMeasurements() [2/3]

◆ resetWithMeasurements() [3/3]

◆ setGain() [1/2]

◆ setGain() [2/2]

◆ setGainDriftPerSecond()

◆ setInputs() [1/3]

◆ setInputs() [2/3]

◆ setInputs() [3/3]

◆ setMinimumGain()

◆ setSamplingTime()

◆ setZmpMeasureErrorStd()

◆ setZMPProcesError()

◆ update()

◆ updateQ_()

◆ updateR_()

◆ Vec2ToSqDiag_()

◆ Vec3ToSqDiag_()

Member Data Documentation

◆ A_

◆ C_

◆ defaultGainDriftSecond

◆ defaultGainMinimum

◆ defaultGainUncertainty

◆ defaultZmpMeasurementErrorStd

◆ defaultZmpProcessErrorStd

◆ defaultZmpUncertainty

◆ dt_

◆ filter_

◆ gainDriftPerSecondStd_

◆ minimumGain_

◆ previousOrientation_

◆ Q_

◆ R_

◆ yaw_

◆ zmpMeasureErrorstd_

◆ zmpProcessErrorStd_