Go to the documentation of this file.
13 #ifndef FLEXBILITYESTMATOR_MODELBASEEKFFLEXIBILITYESTIMATOR_IMU_H
14 #define FLEXBILITYESTMATOR_MODELBASEEKFFLEXIBILITYESTIMATOR_IMU_H
16 #include <state-observation/api.h>
24 namespace flexibilityEstimation
39 private boost::noncopyable
56 void setContactsNumber(
unsigned i);
60 return functor_.getContactsNumber();
70 return functor_.computeAccelerations(getFlexibilityVector(), getInput());
73 void setContactModel(
unsigned nb);
76 virtual void setMeasurement(
const Vector & y);
79 virtual void setProcessNoiseCovariance(
const Matrix & Q);
82 virtual void setMeasurementNoiseCovariance(
const Matrix & R);
85 virtual Matrix getProcessNoiseCovariance()
const;
88 virtual Matrix getMeasurementNoiseCovariance()
const;
90 virtual Vector getMomentaDotFromForces();
91 virtual Vector getMomentaDotFromKinematics();
92 virtual Vector getForcesAndMoments();
99 for(
Index i = 0; i < ekf_.getStateSize(); ++i) Pvec(i) = P(i, i);
107 virtual void setFlexibilityGuess(
const Matrix & x);
110 virtual Matrix4 getFlexibility();
113 virtual const Vector & getFlexibilityVector();
126 virtual Index getMeasurementSize()
const;
128 virtual Index getStateSize()
const;
130 virtual Index getInputSize()
const;
133 virtual void setWithForcesMeasurements(
bool);
135 bool getWithForcesMeasurements();
137 virtual void setWithAbsolutePos(
bool);
139 void setWithUnmodeledForces(
bool b);
143 return withUnmodeledForces_;
148 return withAbsolutePos_;
151 virtual void setWithComBias(
bool b);
158 virtual void setUnmodeledForceVariance(
double d);
159 virtual void setUnmodeledForceProcessVariance(
double d);
163 virtual void setForceVariance(
double d);
164 virtual void setForceVariance(
const Matrix3 & C);
166 virtual void setAbsolutePosVariance(
double d);
169 virtual void setSamplingPeriod(
double);
172 void setOn(
bool & b);
174 virtual void setKfe(
const Matrix3 &
m);
175 virtual void setKfv(
const Matrix3 &
m);
176 virtual void setKte(
const Matrix3 &
m);
177 virtual void setKtv(
const Matrix3 &
m);
179 virtual void setKfeRopes(
const Matrix3 &
m);
180 virtual void setKfvRopes(
const Matrix3 &
m);
181 virtual void setKteRopes(
const Matrix3 &
m);
182 virtual void setKtvRopes(
const Matrix3 &
m);
189 virtual Matrix getKfe()
const;
190 virtual Matrix getKfv()
const;
191 virtual Matrix getKte()
const;
192 virtual Matrix getKtv()
const;
195 virtual void resetCovarianceMatrices();
196 virtual void resetStateCovarianceMatrix();
198 virtual void setRobotMass(
double m);
201 return functor_.getRobotMass();
221 return limitTorques_;
234 static Matrix getDefaultQ();
236 static Matrix6 getDefaultRIMU();
238 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
241 virtual void updateMeasurementCovarianceMatrix_();
251 static const Index measurementSizeBase_ = 12;
284 #endif // FLEXBILITYESTMATOR_MODELBASEEKFFLEXIBILITYESTIMATOR_IMU_H
Eigen::VectorXd Vector
Dynamic sized scalar vector.
Definition: definitions.hpp:76
double unmodeledForceVariance_
Definition: model-base-ekf-flex-estimator-imu.hpp:259
stateObservation::Matrix CA
Definition: model-base-ekf-flex-estimator-imu.hpp:276
void setLimitOn(const bool &b)
Definition: model-base-ekf-flex-estimator-imu.hpp:224
Eigen::Matrix4d Matrix4
4x4 Scalar Matrix
Definition: definitions.hpp:115
Vector3 limitForces_
Definition: model-base-ekf-flex-estimator-imu.hpp:270
Eigen::Matrix< double, 6, 6 > Matrix6
6x6 Scalar Matrix
Definition: definitions.hpp:121
double dt_
Definition: model-base-ekf-flex-estimator-imu.hpp:256
void setTorquesLimit(const Vector3 &v)
Definition: model-base-ekf-flex-estimator-imu.hpp:204
bool limitOn_
Definition: model-base-ekf-flex-estimator-imu.hpp:271
Matrix R_
Definition: model-base-ekf-flex-estimator-imu.hpp:247
stateObservation::Vector getStateCovariance() const
Definition: model-base-ekf-flex-estimator-imu.hpp:95
void setForcesLimit(const Vector3 &v)
Definition: model-base-ekf-flex-estimator-imu.hpp:209
bool on_
Definition: model-base-ekf-flex-estimator-imu.hpp:257
This class is the base class of the flexibility estimators that use an extended Kalman Filter....
Definition: ekf-flexibility-estimator-base.hpp:35
IMUElasticLocalFrameDynamicalSystem getFunctor()
Definition: model-base-ekf-flex-estimator-imu.hpp:63
double absPosVariance_
Definition: model-base-ekf-flex-estimator-imu.hpp:261
const Index stateSize_
Definition: model-base-ekf-flex-estimator-imu.hpp:249
This class describes the dynamics of a robot's flexibility this dynamics with elastic forces to bring...
Definition: imu-elastic-local-frame-dynamical-system.hpp:34
virtual void setPe(const stateObservation::Vector3 &Pe)
Definition: model-base-ekf-flex-estimator-imu.hpp:184
unsigned getContactsNumber()
Definition: model-base-ekf-flex-estimator-imu.hpp:58
Eigen::MatrixXd Matrix
Dynamic sized Matrix.
Definition: definitions.hpp:100
bool withAbsolutePos_
Definition: model-base-ekf-flex-estimator-imu.hpp:265
virtual bool getLimitOn() const
Definition: model-base-ekf-flex-estimator-imu.hpp:229
Matrix forceVariance_
Definition: model-base-ekf-flex-estimator-imu.hpp:260
virtual stateObservation::Vector computeAccelerations()
Definition: model-base-ekf-flex-estimator-imu.hpp:68
bool withComBias_
Definition: model-base-ekf-flex-estimator-imu.hpp:266
Eigen::Matrix3d Matrix3
3x3 Scalar Matrix
Definition: definitions.hpp:109
This class implements the flexibility estimation of a robot with the hypothesis that the contact posi...
Definition: model-base-ekf-flex-estimator-imu.hpp:38
constexpr double m
mass of the robot
Definition: hrp2.hpp:36
virtual bool getWithComBias()
Definition: model-base-ekf-flex-estimator-imu.hpp:153
virtual stateObservation::Matrix getCMatrix()
Definition: model-base-ekf-flex-estimator-imu.hpp:121
bool useFTSensors_
Definition: model-base-ekf-flex-estimator-imu.hpp:263
Eigen::Index Index
Definition: definitions.hpp:138
Definition: model-base-ekf-flex-estimator-imu.hpp:273
stateObservation::Matrix O
Definition: model-base-ekf-flex-estimator-imu.hpp:275
bool withUnmodeledForces_
Definition: model-base-ekf-flex-estimator-imu.hpp:267
Vector3 limitTorques_
Definition: model-base-ekf-flex-estimator-imu.hpp:269
virtual stateObservation::Matrix getAMatrix()
Definition: model-base-ekf-flex-estimator-imu.hpp:116
Vector x_
Definition: model-base-ekf-flex-estimator-imu.hpp:245
virtual stateObservation::Vector3 getForcesLimit() const
Definition: model-base-ekf-flex-estimator-imu.hpp:214
Eigen::Vector3d Vector3
3D vector
Definition: definitions.hpp:85
virtual double getRobotMass() const
Definition: model-base-ekf-flex-estimator-imu.hpp:199
Definition: bidim-elastic-inv-pendulum-dyn-sys.hpp:20
Declare the class of the flexibility estimation using the extended Kalman Filter.
bool getWithUnmodeledForces()
Definition: model-base-ekf-flex-estimator-imu.hpp:141
bool getWithAbsolutePos()
Definition: model-base-ekf-flex-estimator-imu.hpp:146
virtual stateObservation::Vector3 getTorquesLimit() const
Definition: model-base-ekf-flex-estimator-imu.hpp:219
IMUElasticLocalFrameDynamicalSystem functor_
Definition: model-base-ekf-flex-estimator-imu.hpp:243
Index inputSize_
Definition: model-base-ekf-flex-estimator-imu.hpp:254