Collaboration diagram for stateObservation::ZmpTrackingGainEstimator:

[legend]

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.
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.
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.
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.
	~ZmpTrackingGainEstimator ()
	Destroy the Lipm Dcm Bias Estimator object.
void	setSamplingTime (double dt)
	Set the Sampling Time.
void	setGain (const Vector3 &gain)
	Set the Gain from a guess.
void	setGain (const Vector3 &gain, const Vector3 &uncertainty)
	Set the Gain from a guess.
void	setGainDriftPerSecond (const Vector3 &)
	Set the Gain Drift Per Second.
void	setMinimumGain (const double &minGain)
	Set the Gain Limit.
void	setZMPProcesError (const Vector2 &)
	Set the covariance of the zmp process linear dynamics error.
void	setZmpMeasureErrorStd (const Vector2 &)
	Set the Zmp Measurement Error Stamdard deviation.
void	setInputs (const Vector2 &zmpErr, const Vector2 &zmp, const Matrix3 &orientation)
	Set the Inputs of the estimator.
void	setInputs (const Vector2 &zmpErr, const Vector2 &zmp, double yaw)
	Set the Inputs of the estimator.
void	setInputs (const Vector2 &zmpErr, const Vector2 &zmp, const Matrix2 &R=Matrix2::Identity())
	Set the Inputs of the estimator.
void	update ()
	Runs the estimation. Needs to be called every timestep.
Matrix2	getEstimatedGain () const
	Get the Unbiased DCM filtered by the estimator.
Matrix2	getEstimatedLocalGain () const
	Get the estimated Bias expressed in the local frame of the robot.
LinearKalmanFilter &	getFilter ()
	Get the Kalman Filter object This can be used to run specific Advanced Kalman filter related funcions.
const LinearKalmanFilter &	getFilter () const
	Get the Kalman Filter object This can be used to run specific Advanced Kalman filter related funcions.

Static Public Attributes
static constexpr double	defaultGainDriftSecond = 0.002
	default expected drift of the bias every second
static constexpr double	defaultZmpProcessErrorStd = 0.005
	default error in the process linear dynamics of the zmp (in meters)
static constexpr double	defaultZmpMeasurementErrorStd = 0.005
	default error in the measurements of the zmp
static constexpr double	defaultGainMinimum = .01
static constexpr double	defaultZmpUncertainty = 0.005
	default valu for the initial ZMP uncertainty. It should be quite low since this is supposed to be measured
static constexpr double	defaultGainUncertainty = 20
	default value for the uncertainty of the Gain

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
static Matrix3	Vec3ToSqDiag_ (const Vector3 &v)
	builds a diagonal out of the square valued of the Vec3

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.
Matrix2	R_
	measurement noise
Matrix5	Q_
	process noise
Matrix2	previousOrientation_

Detailed Description

Definition at line 27 of file zmp-tracking-gain-estimator.hpp.

Member Typedef Documentation

◆ Matrix25

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

protected

Definition at line 243 of file zmp-tracking-gain-estimator.hpp.

◆ Matrix23

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

protected

Definition at line 244 of file zmp-tracking-gain-estimator.hpp.

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.

Definition at line 135 of file zmp-tracking-gain-estimator.hpp.

Member Function Documentation

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

◆ 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)

Definition at line 103 of file zmp-tracking-gain-estimator.hpp.

◆ resetWithMeasurements() [3/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)

Definition at line 124 of file zmp-tracking-gain-estimator.hpp.

◆ setSamplingTime()

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

Set the Sampling Time.

Parameters

dt	sampling time

◆ 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

◆ setMinimumGain()

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

Set the Gain Limit.

Parameters

minGain the minimal value of the gain

◆ 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

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

Definition at line 182 of file zmp-tracking-gain-estimator.hpp.

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

Definition at line 193 of file zmp-tracking-gain-estimator.hpp.

◆ setInputs() [3/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

◆ update()

void stateObservation::ZmpTrackingGainEstimator::update ( )

Runs the estimation. Needs to be called every timestep.

Returns: Vector2

◆ 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();

Definition at line 214 of file zmp-tracking-gain-estimator.hpp.

◆ 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&

Definition at line 230 of file zmp-tracking-gain-estimator.hpp.

◆ 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

Definition at line 237 of file zmp-tracking-gain-estimator.hpp.

◆ Vec2ToSqDiag_()

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

inlinestaticprotected

builds a diagonal out of the square valued of the Vec2

Definition at line 270 of file zmp-tracking-gain-estimator.hpp.

◆ Vec3ToSqDiag_()

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

inlinestaticprotected

builds a diagonal out of the square valued of the Vec3

Definition at line 276 of file zmp-tracking-gain-estimator.hpp.

◆ updateR_()

void stateObservation::ZmpTrackingGainEstimator::updateR_ ( )

inlineprotected

Definition at line 281 of file zmp-tracking-gain-estimator.hpp.

◆ updateQ_()

void stateObservation::ZmpTrackingGainEstimator::updateQ_ ( )

inlineprotected

Definition at line 287 of file zmp-tracking-gain-estimator.hpp.

Member Data Documentation

◆ defaultGainDriftSecond

double stateObservation::ZmpTrackingGainEstimator::defaultGainDriftSecond = 0.002

staticconstexpr

default expected drift of the bias every second

Definition at line 35 of file zmp-tracking-gain-estimator.hpp.

◆ defaultZmpProcessErrorStd

double stateObservation::ZmpTrackingGainEstimator::defaultZmpProcessErrorStd = 0.005

staticconstexpr

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

Definition at line 38 of file zmp-tracking-gain-estimator.hpp.

◆ defaultZmpMeasurementErrorStd

double stateObservation::ZmpTrackingGainEstimator::defaultZmpMeasurementErrorStd = 0.005

staticconstexpr

default error in the measurements of the zmp

Definition at line 41 of file zmp-tracking-gain-estimator.hpp.

◆ defaultGainMinimum

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

Definition at line 45 of file zmp-tracking-gain-estimator.hpp.

◆ defaultZmpUncertainty

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

Definition at line 48 of file zmp-tracking-gain-estimator.hpp.

◆ defaultGainUncertainty

double stateObservation::ZmpTrackingGainEstimator::defaultGainUncertainty = 20

staticconstexpr

default value for the uncertainty of the Gain

Definition at line 51 of file zmp-tracking-gain-estimator.hpp.

◆ dt_

double stateObservation::ZmpTrackingGainEstimator::dt_

protected

Definition at line 246 of file zmp-tracking-gain-estimator.hpp.

◆ minimumGain_

double stateObservation::ZmpTrackingGainEstimator::minimumGain_

protected

Definition at line 248 of file zmp-tracking-gain-estimator.hpp.

◆ zmpMeasureErrorstd_

Vector2 stateObservation::ZmpTrackingGainEstimator::zmpMeasureErrorstd_

protected

Definition at line 250 of file zmp-tracking-gain-estimator.hpp.

◆ gainDriftPerSecondStd_

Vector3 stateObservation::ZmpTrackingGainEstimator::gainDriftPerSecondStd_

protected

Definition at line 251 of file zmp-tracking-gain-estimator.hpp.

◆ zmpProcessErrorStd_

Vector2 stateObservation::ZmpTrackingGainEstimator::zmpProcessErrorStd_

protected

Definition at line 252 of file zmp-tracking-gain-estimator.hpp.

◆ yaw_

Matrix2 stateObservation::ZmpTrackingGainEstimator::yaw_

protected

Definition at line 254 of file zmp-tracking-gain-estimator.hpp.

◆ filter_

LinearKalmanFilter stateObservation::ZmpTrackingGainEstimator::filter_

protected

Definition at line 256 of file zmp-tracking-gain-estimator.hpp.

◆ A_

Matrix4 stateObservation::ZmpTrackingGainEstimator::A_

protected

Definition at line 257 of file zmp-tracking-gain-estimator.hpp.

◆ C_

Matrix25 stateObservation::ZmpTrackingGainEstimator::C_

protected

The B matrix is zero.

Definition at line 259 of file zmp-tracking-gain-estimator.hpp.

◆ R_

Matrix2 stateObservation::ZmpTrackingGainEstimator::R_

protected

measurement noise

Definition at line 262 of file zmp-tracking-gain-estimator.hpp.

◆ Q_

Matrix5 stateObservation::ZmpTrackingGainEstimator::Q_

protected

process noise

Definition at line 265 of file zmp-tracking-gain-estimator.hpp.

◆ previousOrientation_

Matrix2 stateObservation::ZmpTrackingGainEstimator::previousOrientation_

protected

Definition at line 267 of file zmp-tracking-gain-estimator.hpp.

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

/build/source/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

Detailed Description

Member Typedef Documentation

◆ Matrix25

◆ Matrix23

Constructor & Destructor Documentation

◆ ZmpTrackingGainEstimator()

◆ ~ZmpTrackingGainEstimator()

Member Function Documentation

◆ resetWithMeasurements() [1/3]

◆ resetWithMeasurements() [2/3]

◆ resetWithMeasurements() [3/3]

◆ setSamplingTime()

◆ setGain() [1/2]

◆ setGain() [2/2]

◆ setGainDriftPerSecond()

◆ setMinimumGain()

◆ setZMPProcesError()

◆ setInputs() [1/3]

◆ setInputs() [2/3]

◆ setInputs() [3/3]

◆ update()

◆ getEstimatedGain()

◆ getEstimatedLocalGain()

◆ getFilter() [1/2]

◆ getFilter() [2/2]

◆ Vec2ToSqDiag_()

◆ Vec3ToSqDiag_()

◆ updateR_()

◆ updateQ_()

Member Data Documentation

◆ defaultGainDriftSecond

◆ defaultZmpProcessErrorStd

◆ defaultZmpMeasurementErrorStd

◆ defaultGainMinimum

◆ defaultZmpUncertainty

◆ defaultGainUncertainty

◆ dt_

◆ minimumGain_

◆ zmpMeasureErrorstd_

◆ gainDriftPerSecondStd_

◆ zmpProcessErrorStd_

◆ yaw_

◆ filter_

◆ A_

◆ C_

◆ R_

◆ Q_

◆ previousOrientation_