zmp-tracking-gain-estimator.hpp

#ifndef ZMPTRACKINGGAINESTIMATOR_HPP
#define ZMPTRACKINGGAINESTIMATOR_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 ZmpTrackingGainEstimator
{
private:
  constexpr static double defaultDt_ = 0.005;
  constexpr static double defaultGain_ = 1;
 
public:
  constexpr static double defaultGainDriftSecond = 0.002;

  constexpr static double defaultZmpProcessErrorStd = 0.005;

  constexpr static double defaultZmpMeasurementErrorStd = 0.005;

  constexpr static double defaultGainMinimum = .01;

  constexpr static double defaultZmpUncertainty = 0.005;

  constexpr static double defaultGainUncertainty = 20;

  ZmpTrackingGainEstimator(double dt = defaultDt_,
                           const Vector2 & zmpMeasureErrorStd = Vector2::Constant(defaultZmpMeasurementErrorStd),
                           const Vector3 & gainDriftPerSecond = Vector3::Constant(defaultGainDriftSecond),
                           const Vector2 & zmpProcessErrorStd = Vector2::Constant(defaultZmpProcessErrorStd),
                           double minimumGain = defaultGainMinimum,
                           const Vector2 & initZMP = Vector2::Zero(),
                           const Vector3 & initGain = Vector3::Zero(),
                           const Vector2 & initZMPUncertainty = Vector2::Constant(defaultZmpUncertainty),
                           const Vector3 & initGainUncertainty = Vector3::Constant(defaultGainUncertainty));

  void resetWithMeasurements(const Vector2 & initZMP = Vector2::Zero(),
                             const Vector3 & initGain = Vector3::Zero(),
                             const Matrix2 & yaw = Matrix2::Identity(),
                             const Vector2 & initZMPUncertainty = Vector2::Constant(defaultZmpUncertainty),
                             const Vector3 & initGainUncertainty = Vector3::Constant(defaultGainUncertainty));

  inline void resetWithMeasurements(const Vector2 & initZMP,
                                    const Vector3 & initGain,
                                    double yaw,
                                    const Vector2 & initZMPUncertainty = Vector2::Constant(defaultZmpUncertainty),
                                    const Vector3 & initGainUncertainty = Vector3::Constant(defaultGainUncertainty))
  {
    resetWithMeasurements(initZMP, initGain, Rotation2D(yaw).toRotationMatrix(), initZMPUncertainty,
                          initGainUncertainty);
  }

  inline void resetWithMeasurements(const Vector2 & initZMP,
                                    const Vector3 & initGain,
                                    const Matrix3 & rotation,
                                    const Vector2 & initZMPUncertainty = Vector2::Constant(defaultZmpUncertainty),
                                    const Vector3 & initGainUncertainty = Vector3::Constant(defaultGainUncertainty))
  {
    resetWithMeasurements(initZMP, initGain, kine::rotationMatrixToYawAxisAgnostic(rotation), initZMPUncertainty,
                          initGainUncertainty);
  }

  ~ZmpTrackingGainEstimator() {}

  void setSamplingTime(double dt);

  void setGain(const Vector3 & gain);

  void setGain(const Vector3 & gain, const Vector3 & uncertainty);

  void setGainDriftPerSecond(const Vector3 &);

  void setMinimumGain(const double & minGain);

  void setZMPProcesError(const Vector2 &);

  void setZmpMeasureErrorStd(const Vector2 &);

  inline void setInputs(const Vector2 & zmpErr, const Vector2 & zmp, const Matrix3 & orientation)
  {
    setInputs(zmpErr, zmp, kine::rotationMatrixToYawAxisAgnostic(orientation));
  }

  inline void setInputs(const Vector2 & zmpErr, const Vector2 & zmp, double yaw)
  {
    setInputs(zmpErr, zmp, Rotation2D(yaw).toRotationMatrix());
  }

  void setInputs(const Vector2 & zmpErr, const Vector2 & zmp, const Matrix2 & R = Matrix2::Identity());

  void update();

  inline Matrix2 getEstimatedGain() const
  {
    return (previousOrientation_ * getEstimatedLocalGain().triangularView<Eigen::Upper>()
            * previousOrientation_.transpose())
        .selfadjointView<Eigen::Upper>();
  }

  Matrix2 getEstimatedLocalGain() const;

  inline LinearKalmanFilter & getFilter()
  {
    return filter_;
  }

  inline const LinearKalmanFilter & getFilter() const
  {
    return filter_;
  }
 
protected:
  typedef Eigen::Matrix<double, 2, 5> Matrix25;
  typedef Eigen::Matrix<double, 2, 3> Matrix23;
 
  double dt_;
 
  double minimumGain_;
 
  Vector2 zmpMeasureErrorstd_;
  Vector3 gainDriftPerSecondStd_;
  Vector2 zmpProcessErrorStd_;
 
  Matrix2 yaw_;
 
  LinearKalmanFilter filter_;
  Matrix4 A_;
  Matrix25 C_;

  Matrix2 R_;

  Matrix5 Q_;
 
  Matrix2 previousOrientation_;

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

  inline static Matrix3 Vec3ToSqDiag_(const Vector3 & v)
  {
    return Vector3(v.array().square()).asDiagonal();
  }
 
  inline void updateR_()
  {
    R_ = Vec2ToSqDiag_(zmpMeasureErrorstd_);
    filter_.setMeasurementCovariance(R_);
  }
 
  inline void updateQ_()
  {
    Q_.topLeftCorner<2, 2>() = Vec2ToSqDiag_(zmpProcessErrorStd_);
    Q_.bottomRightCorner<3, 3>() = Vec3ToSqDiag_(gainDriftPerSecondStd_);
    filter_.setProcessCovariance(Q_);
  }
 
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
 
} // namespace stateObservation
 
#endif