Go to the documentation of this file.
12 #ifndef ZMPTRACKINGGAINESTIMATOR_HPP
13 #define ZMPTRACKINGGAINESTIMATOR_HPP
15 #include <state-observation/api.h>
30 constexpr
static double defaultDt_ = 0.005;
31 constexpr
static double defaultGain_ = 1;
35 constexpr
static double defaultGainDriftSecond = 0.002;
38 constexpr
static double defaultZmpProcessErrorStd = 0.005;
41 constexpr
static double defaultZmpMeasurementErrorStd = 0.005;
45 constexpr
static double defaultGainMinimum = .01;
48 constexpr
static double defaultZmpUncertainty = 0.005;
51 constexpr
static double defaultGainUncertainty = 20;
70 const Vector2 & zmpMeasureErrorStd = Vector2::Constant(defaultZmpMeasurementErrorStd),
71 const Vector3 & gainDriftPerSecond = Vector3::Constant(defaultGainDriftSecond),
72 const Vector2 & zmpProcessErrorStd = Vector2::Constant(defaultZmpProcessErrorStd),
73 double minimumGain = defaultGainMinimum,
74 const Vector2 & initZMP = Vector2::Zero(),
75 const Vector3 & initGain = Vector3::Zero(),
76 const Vector2 & initZMPUncertainty = Vector2::Constant(defaultZmpUncertainty),
77 const Vector3 & initGainUncertainty = Vector3::Constant(defaultGainUncertainty));
88 void resetWithMeasurements(
const Vector2 & initZMP = Vector2::Zero(),
89 const Vector3 & initGain = Vector3::Zero(),
90 const Matrix2 & yaw = Matrix2::Identity(),
91 const Vector2 & initZMPUncertainty = Vector2::Constant(defaultZmpUncertainty),
92 const Vector3 & initGainUncertainty = Vector3::Constant(defaultGainUncertainty));
106 const Vector2 & initZMPUncertainty = Vector2::Constant(defaultZmpUncertainty),
107 const Vector3 & initGainUncertainty = Vector3::Constant(defaultGainUncertainty))
109 resetWithMeasurements(initZMP, initGain,
Rotation2D(yaw).toRotationMatrix(), initZMPUncertainty,
110 initGainUncertainty);
127 const Vector2 & initZMPUncertainty = Vector2::Constant(defaultZmpUncertainty),
128 const Vector3 & initGainUncertainty = Vector3::Constant(defaultGainUncertainty))
131 initGainUncertainty);
140 void setSamplingTime(
double dt);
146 void setGain(
const Vector3 & gain);
157 void setGainDriftPerSecond(
const Vector3 &);
162 void setMinimumGain(
const double & minGain);
167 void setZMPProcesError(
const Vector2 &);
171 void setZmpMeasureErrorStd(
const Vector2 &);
195 setInputs(zmpErr, zmp,
Rotation2D(yaw).toRotationMatrix());
203 void setInputs(
const Vector2 & zmpErr,
const Vector2 & zmp,
const Matrix2 & R = Matrix2::Identity());
217 return (previousOrientation_ * getEstimatedLocalGain().triangularView<Eigen::Upper>()
218 * previousOrientation_.transpose())
219 .selfadjointView<Eigen::Upper>();
225 Matrix2 getEstimatedLocalGain()
const;
272 return Vector2(v.array().square()).asDiagonal();
278 return Vector3(v.array().square()).asDiagonal();
283 R_ = Vec2ToSqDiag_(zmpMeasureErrorstd_);
289 Q_.topLeftCorner<2, 2>() = Vec2ToSqDiag_(zmpProcessErrorStd_);
290 Q_.bottomRightCorner<3, 3>() = Vec3ToSqDiag_(gainDriftPerSecondStd_);
295 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Matrix5 Q_
process noise
Definition: zmp-tracking-gain-estimator.hpp:265
static Matrix3 Vec3ToSqDiag_(const Vector3 &v)
builds a diagonal out of the square valued of the Vec3
Definition: zmp-tracking-gain-estimator.hpp:276
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.
Definition: zmp-tracking-gain-estimator.hpp:124
void setProcessCovariance(const Qmatrix &Q)
Definition: kalman-filter-base.hpp:136
const LinearKalmanFilter & getFilter() const
Get the Kalman Filter object This can be used to run specific Advanced Kalman filter related funcions...
Definition: zmp-tracking-gain-estimator.hpp:237
Eigen::Matrix4d Matrix4
4x4 Scalar Matrix
Definition: definitions.hpp:115
void setInputs(const Vector2 &zmpErr, const Vector2 &zmp, double yaw)
Set the Inputs of the estimator.
Definition: zmp-tracking-gain-estimator.hpp:193
Matrix4 A_
Definition: zmp-tracking-gain-estimator.hpp:257
void updateQ_()
Definition: zmp-tracking-gain-estimator.hpp:287
Vector3 gainDriftPerSecondStd_
Definition: zmp-tracking-gain-estimator.hpp:251
Eigen::Matrix< double, 2, 1 > Vector2
2d Vector
Definition: definitions.hpp:82
Eigen::Matrix< double, 5, 5 > Matrix5
5x5 Scalar Matrix
Definition: definitions.hpp:118
double minimumGain_
Definition: zmp-tracking-gain-estimator.hpp:248
Eigen::Matrix< double, 2, 5 > Matrix25
Definition: zmp-tracking-gain-estimator.hpp:243
Eigen::Matrix< double, 2, 3 > Matrix23
Definition: zmp-tracking-gain-estimator.hpp:244
LinearKalmanFilter & getFilter()
Get the Kalman Filter object This can be used to run specific Advanced Kalman filter related funcions...
Definition: zmp-tracking-gain-estimator.hpp:230
Definition: zmp-tracking-gain-estimator.hpp:27
The class of a Linear Kalman filter.
Definition: linear-kalman-filter.hpp:47
Matrix2 R_
measurement noise
Definition: zmp-tracking-gain-estimator.hpp:262
void setMeasurementCovariance(const Rmatrix &R)
Definition: kalman-filter-base.hpp:119
double rotationMatrixToYawAxisAgnostic(const Matrix3 &rotation)
take 3x3 matrix represeting a rotation and gives a corresponding angle around upward vertical axis
Implements integrators for the kinematics, in terms or rotations and translations.
Eigen::Matrix2d Matrix2
2D scalar Matrix
Definition: definitions.hpp:106
Defines the class of a Linear Kalman filter.
Eigen::Matrix3d Matrix3
3x3 Scalar Matrix
Definition: definitions.hpp:109
~ZmpTrackingGainEstimator()
Destroy the Lipm Dcm Bias Estimator object.
Definition: zmp-tracking-gain-estimator.hpp:135
double dt_
Definition: zmp-tracking-gain-estimator.hpp:246
LinearKalmanFilter filter_
Definition: zmp-tracking-gain-estimator.hpp:256
static Matrix2 Vec2ToSqDiag_(const Vector2 &v)
builds a diagonal out of the square valued of the Vec2
Definition: zmp-tracking-gain-estimator.hpp:270
Vector2 zmpMeasureErrorstd_
Definition: zmp-tracking-gain-estimator.hpp:250
Gathers many kinds of algorithms.
Vector2 zmpProcessErrorStd_
Definition: zmp-tracking-gain-estimator.hpp:252
Eigen::Vector3d Vector3
3D vector
Definition: definitions.hpp:85
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.
Definition: zmp-tracking-gain-estimator.hpp:103
Matrix2 getEstimatedGain() const
Get the Unbiased DCM filtered by the estimator.
Definition: zmp-tracking-gain-estimator.hpp:214
Eigen::Rotation2D< double > Rotation2D
2D rotations
Definition: definitions.hpp:136
Definition: bidim-elastic-inv-pendulum-dyn-sys.hpp:20
Matrix25 C_
The B matrix is zero.
Definition: zmp-tracking-gain-estimator.hpp:259
Matrix2 yaw_
Definition: zmp-tracking-gain-estimator.hpp:254
void updateR_()
Definition: zmp-tracking-gain-estimator.hpp:281
Matrix2 previousOrientation_
Definition: zmp-tracking-gain-estimator.hpp:267
void setInputs(const Vector2 &zmpErr, const Vector2 &zmp, const Matrix3 &orientation)
Set the Inputs of the estimator.
Definition: zmp-tracking-gain-estimator.hpp:182