lipm-dcm-estimator.hpp

Go to the documentation of this file.

#ifndef LIPMDCMBIASESTIMATOR_HPP
#define LIPMDCMBIASESTIMATOR_HPP
 
#include <state-observation/api.h>
#include <state-observation/observer/linear-kalman-filter.hpp>
#include <state-observation/tools/miscellaneous-algorithms.hpp>
#include <state-observation/tools/rigid-body-kinematics.hpp>
 
namespace stateObservation
{

 
class STATE_OBSERVATION_DLLAPI LipmDcmEstimator
{
private:
  constexpr static double defaultDt_ = 0.005;
  constexpr static double defaultOmega_ = tools::sqrt(cst::gravityConstant);
 
public:
  constexpr static double defaultBiasDriftSecond = 0.002;

  constexpr static double defaultZmpErrorStd = 0.005;
  constexpr static double defaultDcmErrorStd = 0.01;

  constexpr static double defaultDCMUncertainty = 0.01;
  constexpr static double defaultBiasUncertainty = 0.01;

  constexpr static double defaultBiasLimit = -1;

  LipmDcmEstimator(double dt = defaultDt_,
                   double omega_0 = defaultOmega_,
                   double biasDriftPerSecondStd = defaultBiasDriftSecond,
                   double dcmMeasureErrorStd = defaultDcmErrorStd,
                   double zmpMeasureErrorStd = defaultZmpErrorStd,
                   const Vector2 & biasLimit = Vector2::Constant(defaultBiasLimit),
                   const Vector2 & initZMP = Vector2::Zero(),
                   const Vector2 & initDcm = Vector2::Zero(),
                   const Vector2 & initBias = Vector2::Zero(),
                   const Vector2 & initDcmUncertainty = Vector2::Constant(defaultDCMUncertainty),
                   const Vector2 & initBiasUncertainty = Vector2::Constant(defaultBiasUncertainty));

  void resetWithMeasurements(const Vector2 & measuredDcm,
                             const Vector2 & measuredZMP,
                             const Matrix2 & yaw = Matrix2::Identity(),
                             bool measurementIsWithBias = true,
                             const Vector2 & initBias = Vector2::Constant(0),
                             const Vector2 & initBiasuncertainty = Vector2::Constant(defaultBiasUncertainty));

  inline 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))
  {
    resetWithMeasurements(measuredDcm, measuredZMP, Rotation2D(yaw).toRotationMatrix(), measurementIsWithBias, initBias,
                          initBiasuncertainty);
  }

  inline 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))
  {
    resetWithMeasurements(measuredDcm, measuredZMP, kine::rotationMatrixToYawAxisAgnostic(rotation),
                          measurementIsWithBias, initBias, initBiasuncertainty);
  }

  ~LipmDcmEstimator();

  void setLipmNaturalFrequency(double omega_0);

  inline double getLipmNaturalFrequency() const
  {
    return omega0_;
  }

  void setUnbiasedCoMOffset(const Vector2 & gamma);

  Vector2 getUnbiasedCoMOffset() const
  {
    return gamma_;
  }

  void setZMPCoef(double kappa);

  double getZMPCoef() const
  {
    return kappa_;
  }

  void setSamplingTime(double dt);

  void setBias(const Vector2 & bias);

  void setBias(const Vector2 & bias, const Vector2 & uncertainty);

  void setBiasDriftPerSecond(double driftPerSecond);

  void setBiasLimit(const Vector2 & biasLimit);

  void setUnbiasedDCM(const Vector2 & dcm);

  void setUnbiasedDCM(const Vector2 & dcm, const Vector2 & uncertainty);

  void setZmpMeasureErrorStd(double);

  void setDcmMeasureErrorStd(double);

  inline void setInputs(const Vector2 & dcm,
                        const Vector2 & zmp,
                        const Matrix3 & orientation,
                        const Vector2 & CoMOffset_gamma = Vector2::Zero(),
                        const double ZMPCoef_kappa = 1)
  {
    setInputs(dcm, zmp, kine::rotationMatrixToYawAxisAgnostic(orientation), CoMOffset_gamma, ZMPCoef_kappa);
  }

  inline void setInputs(const Vector2 & dcm,
                        const Vector2 & zmp,
                        double yaw,
                        const Vector2 & CoMOffset_gamma = Vector2::Zero(),
                        const double ZMPCoef_kappa = 1)
  {
    setInputs(dcm, zmp, Rotation2D(yaw).toRotationMatrix(), CoMOffset_gamma, ZMPCoef_kappa);
  }

  void setInputs(const Vector2 & dcm,
                 const Vector2 & zmp,
                 const Matrix2 & R = Matrix2::Identity(),
                 const Vector2 & CoMOffset_gamma = Vector2::Zero(),
                 const double ZMPCoef_kappa = 1);

  void update();

  Vector2 getUnbiasedDCM() const;

  Vector2 getBias() const;

  inline Vector2 getLocalBias() const
  {
    return previousOrientation_.transpose() * getBias();
  }

  inline LinearKalmanFilter & getFilter()
  {
    return filter_;
  }

  inline const LinearKalmanFilter & getFilter() const
  {
    return filter_;
  }
 
protected:
  typedef Eigen::Matrix<double, 4, 2> Matrix42;
  typedef Eigen::Matrix<double, 2, 4> Matrix24;

  void updateMatricesABQ_();
 
  double omega0_;
  Vector2 gamma_ = Vector2::Zero();
  double kappa_ = 1;
  double dt_;
  double biasDriftStd_;
  double zmpErrorStd_;
 
  bool needUpdateMatrices_ = true;
 
  Vector2 previousZmp_;
 
  Vector2 biasLimit_;
 
  LinearKalmanFilter filter_;
  Matrix4 A_;
  Matrix4 B_;
  Matrix24 C_;
  Matrix2 R_;
  Matrix4 Q_;
 
  Matrix2 previousOrientation_;

  inline static Matrix2 Vec2ToSqDiag_(const Vector2 & v)
  {
    return Vector2(v.array().square()).asDiagonal();
  }

  inline static Matrix2 dblToDiag_(const double & d)
  {
    return Vector2::Constant(d).asDiagonal();
  }

  inline static Matrix2 dblToSqDiag_(const double & d)
  {
    return dblToDiag_(d * d);
  }
 
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
 
} // namespace stateObservation
 
#endif