Go to the documentation of this file.
12 #ifndef LIPMDCMBIASESTIMATOR_HPP
13 #define LIPMDCMBIASESTIMATOR_HPP
15 #include <state-observation/api.h>
74 constexpr
static double defaultDt_ = 0.005;
79 constexpr
static double defaultBiasDriftSecond = 0.002;
82 constexpr
static double defaultZmpErrorStd = 0.005;
83 constexpr
static double defaultDcmErrorStd = 0.01;
86 constexpr
static double defaultDCMUncertainty = 0.01;
87 constexpr
static double defaultBiasUncertainty = 0.01;
90 constexpr
static double defaultBiasLimit = -1;
109 double omega_0 = defaultOmega_,
110 double biasDriftPerSecondStd = defaultBiasDriftSecond,
111 double dcmMeasureErrorStd = defaultDcmErrorStd,
112 double zmpMeasureErrorStd = defaultZmpErrorStd,
113 const Vector2 & biasLimit = Vector2::Constant(defaultBiasLimit),
114 const Vector2 & initZMP = Vector2::Zero(),
115 const Vector2 & initDcm = Vector2::Zero(),
116 const Vector2 & initBias = Vector2::Zero(),
117 const Vector2 & initDcmUncertainty = Vector2::Constant(defaultDCMUncertainty),
118 const Vector2 & initBiasUncertainty = Vector2::Constant(defaultBiasUncertainty));
131 void resetWithMeasurements(
const Vector2 & measuredDcm,
133 const Matrix2 & yaw = Matrix2::Identity(),
134 bool measurementIsWithBias =
true,
135 const Vector2 & initBias = Vector2::Constant(0),
136 const Vector2 & initBiasuncertainty = Vector2::Constant(defaultBiasUncertainty));
152 bool measurementIsWithBias =
true,
153 const Vector2 & initBias = Vector2::Constant(0),
154 const Vector2 & initBiasuncertainty = Vector2::Constant(defaultBiasUncertainty))
156 resetWithMeasurements(measuredDcm, measuredZMP,
Rotation2D(yaw).toRotationMatrix(), measurementIsWithBias, initBias,
157 initBiasuncertainty);
175 bool measurementIsWithBias =
true,
176 const Vector2 & initBias = Vector2::Constant(0),
177 const Vector2 & initBiasuncertainty = Vector2::Constant(defaultBiasUncertainty))
180 measurementIsWithBias, initBias, initBiasuncertainty);
189 void setLipmNaturalFrequency(
double omega_0);
205 void setUnbiasedCoMOffset(
const Vector2 & gamma);
218 void setZMPCoef(
double kappa);
230 void setSamplingTime(
double dt);
235 void setBias(
const Vector2 & bias);
245 void setBiasDriftPerSecond(
double driftPerSecond);
251 void setBiasLimit(
const Vector2 & biasLimit);
256 void setUnbiasedDCM(
const Vector2 & dcm);
261 void setUnbiasedDCM(
const Vector2 & dcm,
const Vector2 & uncertainty);
265 void setZmpMeasureErrorStd(
double);
269 void setDcmMeasureErrorStd(
double);
284 const Vector2 & CoMOffset_gamma = Vector2::Zero(),
285 const double ZMPCoef_kappa = 1)
301 const Vector2 & CoMOffset_gamma = Vector2::Zero(),
302 const double ZMPCoef_kappa = 1)
304 setInputs(dcm, zmp,
Rotation2D(yaw).toRotationMatrix(), CoMOffset_gamma, ZMPCoef_kappa);
314 void setInputs(
const Vector2 & dcm,
316 const Matrix2 & R = Matrix2::Identity(),
317 const Vector2 & CoMOffset_gamma = Vector2::Zero(),
318 const double ZMPCoef_kappa = 1);
329 Vector2 getUnbiasedDCM()
const;
341 return previousOrientation_.transpose() * getBias();
364 void updateMatricesABQ_();
373 bool needUpdateMatrices_ =
true;
394 return Vector2(v.array().square()).asDiagonal();
400 return Vector2::Constant(d).asDiagonal();
406 return dblToDiag_(d * d);
410 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Vector2 getUnbiasedCoMOffset() const
Get the ubiased offset on the CoM dynamics.
Definition: lipm-dcm-estimator.hpp:211
Matrix2 R_
measurement noise
Definition: lipm-dcm-estimator.hpp:385
double getZMPCoef() const
get the ZMP Coefficient
Definition: lipm-dcm-estimator.hpp:222
Eigen::Matrix4d Matrix4
4x4 Scalar Matrix
Definition: definitions.hpp:115
Matrix24 C_
this needs to be transposed
Definition: lipm-dcm-estimator.hpp:383
double omega0_
Definition: lipm-dcm-estimator.hpp:366
LinearKalmanFilter filter_
Definition: lipm-dcm-estimator.hpp:379
LinearKalmanFilter & getFilter()
Get the Kalman Filter object This can be used to run specific Advanced Kalman filter related funcions...
Definition: lipm-dcm-estimator.hpp:347
const LinearKalmanFilter & getFilter() const
Get the Kalman Filter object This can be used to run specific Advanced Kalman filter related funcions...
Definition: lipm-dcm-estimator.hpp:354
Eigen::Matrix< double, 2, 1 > Vector2
2d Vector
Definition: definitions.hpp:82
double biasDriftStd_
Definition: lipm-dcm-estimator.hpp:370
Matrix4 Q_
process noise
Definition: lipm-dcm-estimator.hpp:387
Eigen::Matrix< double, 2, 4 > Matrix24
Definition: lipm-dcm-estimator.hpp:361
Eigen::Matrix< double, 4, 2 > Matrix42
Definition: lipm-dcm-estimator.hpp:360
The class of a Linear Kalman filter.
Definition: linear-kalman-filter.hpp:47
static Matrix2 Vec2ToSqDiag_(const Vector2 &v)
builds a diagonal out of the square valued of the Vec2
Definition: lipm-dcm-estimator.hpp:392
void setInputs(const Vector2 &dcm, const Vector2 &zmp, const Matrix3 &orientation, const Vector2 &CoMOffset_gamma=Vector2::Zero(), const double ZMPCoef_kappa=1)
Set the Inputs of the estimator.
Definition: lipm-dcm-estimator.hpp:281
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.
double dt_
Definition: lipm-dcm-estimator.hpp:369
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
static Matrix2 dblToDiag_(const double &d)
builds a constant 2x2 diagonal from a double
Definition: lipm-dcm-estimator.hpp:398
void setInputs(const Vector2 &dcm, const Vector2 &zmp, double yaw, const Vector2 &CoMOffset_gamma=Vector2::Zero(), const double ZMPCoef_kappa=1)
Set the Inputs of the estimator.
Definition: lipm-dcm-estimator.hpp:298
static Matrix2 dblToSqDiag_(const double &d)
builds a constant 2x2 diagonal from a square of a double
Definition: lipm-dcm-estimator.hpp:404
Matrix2 previousOrientation_
Definition: lipm-dcm-estimator.hpp:389
Vector2 biasLimit_
Definition: lipm-dcm-estimator.hpp:377
Vector2 getLocalBias() const
Get the estimated Bias expressed in the local frame of the robot.
Definition: lipm-dcm-estimator.hpp:339
Filtering of divergent component of motion (DCM) and estimation of a bias betweeen the DCM and the co...
Definition: lipm-dcm-estimator.hpp:71
constexpr double gravityConstant
Definition: definitions.hpp:663
double getLipmNaturalFrequency() const
Get the Lipm Natural Frequency.
Definition: lipm-dcm-estimator.hpp:194
Gathers many kinds of algorithms.
void resetWithMeasurements(const Vector2 &measuredDcm, const Vector2 &measuredZMP, const Matrix3 &rotation, bool measurementIsWithBias=true, const Vector2 &initBias=Vector2::Constant(0), const Vector2 &initBiasuncertainty=Vector2::Constant(defaultBiasUncertainty))
Resets the estimator with first measurements.
Definition: lipm-dcm-estimator.hpp:172
Eigen::Rotation2D< double > Rotation2D
2D rotations
Definition: definitions.hpp:136
double zmpErrorStd_
Definition: lipm-dcm-estimator.hpp:371
Definition: bidim-elastic-inv-pendulum-dyn-sys.hpp:20
Matrix4 B_
Definition: lipm-dcm-estimator.hpp:381
Vector2 previousZmp_
Definition: lipm-dcm-estimator.hpp:375
void resetWithMeasurements(const Vector2 &measuredDcm, const Vector2 &measuredZMP, double yaw, bool measurementIsWithBias=true, const Vector2 &initBias=Vector2::Constant(0), const Vector2 &initBiasuncertainty=Vector2::Constant(defaultBiasUncertainty))
Resets the estimator with first measurements.
Definition: lipm-dcm-estimator.hpp:149
Matrix4 A_
Definition: lipm-dcm-estimator.hpp:380