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