unidim-lipm-dcm-estimator.hpp

Go to the documentation of this file.

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

 
class STATE_OBSERVATION_DLLAPI UnidimLipmDcmEstimator
{
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;

  UnidimLipmDcmEstimator(double dt = defaultDt_,
                         double omega_0 = defaultOmega_,
                         double biasDriftPerSecondStd = defaultBiasDriftSecond,
                         double initDcm = 0,
                         double initZMP = 0,
                         double initBias = 0,
                         double dcmMeasureErrorStd = defaultDcmErrorStd,
                         double zmpMeasureErrorStd = defaultZmpErrorStd,
                         double initDcmUncertainty = defaultDCMUncertainty,
                         double initBiasUncertainty = defaultBiasUncertainty);

  void resetWithInputs(double measuredDcm,
                       double measuredZMP,
                       bool measurementIsWithBias = true,
                       double biasDriftPerSecondStd = defaultBiasDriftSecond,
                       double dcmMeasureErrorStd = defaultDcmErrorStd,
                       double zmpMeasureErrorStd = defaultZmpErrorStd,
                       double initBias = 0,
                       double initBiasuncertainty = defaultBiasUncertainty);

  ~UnidimLipmDcmEstimator() {}

  void setLipmNaturalFrequency(double omega_0);

  void setSamplingTime(double dt);

  void setBias(double bias);

  void setBias(double bias, double uncertainty);

  void setBiasDriftPerSecond(double driftPerSecond);

  void setUnbiasedDCM(double dcm);

  void setUnbiasedDCM(double dcm, double uncertainty);

  void setZmpMeasureErrorStd(double);

  void setDcmMeasureErrorStd(double);

  void setInputs(double dcm, double zmp);

  inline Vector2 update()
  {
    return filter_.getEstimatedState(filter_.getMeasurementTime());
  }

  double getUnbiasedDCM() const;

  double getBias() const;

  inline LinearKalmanFilter & getFilter()
  {
    return filter_;
  }

  inline const LinearKalmanFilter & getFilter() const
  {
    return filter_;
  }
 
private:
  void updateMatricesABQ_();
 
  double omega0_;
  double dt_;
  double biasDriftStd_;
  double zmpErrorStd_;
 
  double previousZmp_;
 
  LinearKalmanFilter filter_;
  Matrix2 A_;
  Vector2 B_;
  Vector2 C_;
  Matrix1 R_;

  Matrix2 Q_;
 
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
 
} // namespace stateObservation
 
#endif