kinetics-observer.hpp

Go to the documentation of this file.

 
#ifndef KINETICSOBSERVER_HPP
#define KINETICSOBSERVER_HPP
 
#include <set>
 
#include <boost/utility.hpp>
 
#include <state-observation/api.h>
#include <state-observation/dynamical-system/dynamical-system-functor-base.hpp>
#include <state-observation/noise/noise-base.hpp>
#include <state-observation/observer/extended-kalman-filter.hpp>
#include <state-observation/sensors-simulation/accelerometer-gyrometer.hpp>
#include <state-observation/tools/definitions.hpp>
#include <state-observation/tools/rigid-body-kinematics.hpp>
#include <state-observation/tools/state-vector-arithmetics.hpp>
 
namespace stateObservation
{
 
 
 
class STATE_OBSERVATION_DLLAPI KineticsObserver : protected DynamicalSystemFunctorBase, protected StateVectorArithmetics
{
public:
  typedef kine::Kinematics Kinematics;
  typedef kine::LocalKinematics LocalKinematics;
  typedef kine::Orientation Orientation;
 
  // ////////////////////////////////////////////////////////////
  // ///////////////////////////////////////////////////////////
 
  KineticsObserver(unsigned maxContacts = 4, unsigned maxNumberOfIMU = 1);
 
  virtual ~KineticsObserver();
 
 
  // ////////////////////////////////////////////////////////////
  // ///////////////////////////////////////////////////////////
 
 
  double getSamplingTime() const;
 
  void setSamplingTime(double);
 
  void setWithUnmodeledWrench(bool b = true);
 
  void setWithAccelerationEstimation(bool b = true);
 
  bool getWithAccelerationEstimation() const;
 
  void setWithGyroBias(bool b = true);
 
  void setMass(double);
 
  double getMass() const;
 
  const IndexedMatrix3 & getInertiaMatrix() const;
 
  const IndexedMatrix3 & getInertiaMatrixDot() const;
 
  const IndexedVector3 & getAngularMomentum() const;
 
  const IndexedVector3 & getAngularMomentumDot() const;
 
  const IndexedVector3 & getCenterOfMass() const;
 
  const IndexedVector3 & getCenterOfMassDot() const;
 
  const IndexedVector3 & getCenterOfMassDotDot() const;
 
  Vector6 getAdditionalWrench() const;
 
 
  // ///////////////////////////////////////////////////////////
 
 
  Index setIMU(const Vector3 & accelero,
               const Vector3 & gyrometer,
               const Kinematics & userImuKinematics,
               Index num = -1);
 
  Index setIMU(const Vector3 & accelero,
               const Vector3 & gyrometer,
               const Matrix3 & acceleroCov,
               const Matrix3 & gyroCov,
               const Kinematics & userImuKinematics,
               Index num = -1);
 
  void setIMUDefaultCovarianceMatrix(const Matrix3 & acceleroCov, const Matrix3 & gyroCov);
 
  void setAbsolutePoseSensor(const Kinematics & measurement);
 
  void setAbsolutePoseSensor(const Kinematics & measurement, const Matrix6 & CovarianceMatrix);
 
  void setAbsolutePoseSensorDefaultCovarianceMatrix(const Matrix6 & covMat);
 
  void setAbsoluteOriSensor(const Orientation & measurement);
 
  void setAbsoluteOriSensor(const Orientation & measurement, const Matrix3 & CovarianceMatrix);
 
  void setAbsoluteOriSensorDefaultCovarianceMatrix(const Matrix3 & covMat);
 
 
  // ///////////////////////////////////////////////////////////
  // ///////////////////////////////////////////////////////////
 
  Index addContact(const Kinematics & pose,
                   const Matrix12 & initialCovarianceMatrix,
                   const Matrix12 & processCovarianceMatrix,
                   Index contactNumber = -1,
                   const Matrix3 & linearStiffness = Matrix3::Constant(-1),
                   const Matrix3 & linearDamping = Matrix3::Constant(-1),
                   const Matrix3 & angularStiffness = Matrix3::Constant(-1),
                   const Matrix3 & angularDamping = Matrix3::Constant(-1));
 
  Index addContact(const Kinematics & pose,
                   Index contactNumber = -1,
                   const Matrix3 & linearStiffness = Matrix3::Constant(-1),
                   const Matrix3 & linearDamping = Matrix3::Constant(-1),
                   const Matrix3 & angularStiffness = Matrix3::Constant(-1),
                   const Matrix3 & angularDamping = Matrix3::Constant(-1));
 
  void removeContact(Index contactnbr);
 
  void clearContacts();
 
  Index getNumberOfSetContacts() const;
 
  std::vector<Index> getListOfContacts() const;
 
 
  // ///////////////////////////////////////////////////////////
  // //////////////////////////////////////////////////////////
 
  void updateContactWithNoSensor(const Kinematics & localKine, unsigned contactNumber);
 
  void updateContactWithWrenchSensor(const Vector6 & wrenchMeasurement,
                                     const Kinematics & localKine,
                                     unsigned contactNumber);
 
  void updateContactWithWrenchSensor(const Vector6 & wrenchMeasurement,
                                     const Matrix6 & wrenchCovMatrix,
                                     const Kinematics & localKine,
                                     unsigned contactNumber);
 
  void setContactWrenchSensorDefaultCovarianceMatrix(const Matrix6 & wrenchSensorCovMat);
 
 
  // /////////////////////////////////////////////
 
  void setCenterOfMass(const Vector3 & com, const Vector3 & com_dot, const Vector3 & com_dot_dot);
 
  void setCenterOfMass(const Vector3 & com, const Vector3 & com_dot);
 
  void setCenterOfMass(const Vector3 & com);
 
  void setCoMInertiaMatrix(const Matrix3 & I, const Matrix3 & I_dot);
 
  void setCoMInertiaMatrix(const Matrix3 & I);
 
  void setCoMInertiaMatrix(const Vector6 & I, const Vector6 & I_dot);
 
  void setCoMInertiaMatrix(const Vector6 & I);
 
  void setCoMAngularMomentum(const Vector3 & sigma, const Vector3 & sigma_dot);
 
  void setCoMAngularMomentum(const Vector3 & sigma);
 
  void setAdditionalWrench(const Vector3 & force, const Vector3 & torque);
 
 
  // ///////////////////////////////////////////////////////////
 
  void updateMeasurements();
 
  const Vector & update();
 
 
  void convertWrenchFromUserToCentroid(const Vector3 & forceUserFrame,
                                       const Vector3 & momentUserFrame,
                                       Vector3 & forceCentroidFrame,
                                       Vector3 & momentCentroidFrame);
 
  LocalKinematics getLocalCentroidKinematics() const;
 
  Kinematics getGlobalCentroidKinematics() const;
 
  LocalKinematics estimateAccelerations();
 
  LocalKinematics getLocalKinematicsOf(const Kinematics & userBodyKine);
 
  Kinematics getGlobalKinematicsOf(const Kinematics & userBodyKin) const;
 
 
  Vector6 getContactWrench(Index contactNbr) const;
 
  Kinematics getContactPosition(Index contactNbr) const;
 
  Vector6 getUnmodeledWrench() const;
 
  // /////////////////////////////////////////////////////////
 
  void setWorldCentroidStateKinematics(const LocalKinematics & localKine,
                                       bool resetContactWrenches = true,
                                       bool resetCovariance = true);
 
  void setWorldCentroidStateKinematics(const Kinematics & kine, bool resetCovariance = true);
 
  void setStateContact(Index index,
                       Kinematics worldContactRestPose,
                       const Vector6 & wrench,
                       bool resetCovariance = true);
 
  // TODO
  // void setVelocityGuess(const Kinematics)
 
  void setGyroBias(const Vector3 &, unsigned numberOfIMU = 1, bool resetCovariance = true);
 
  void setStateUnmodeledWrench(const Vector6 &, bool resetCovariance = true);
 
 
  // /////////////////////////////////////////////////////////////
  // /////////////////////////////////////////////////////////////
 
  void resetSensorsDefaultCovMats();
 
  void resetInputs();
 
  // /////////////////////////////////////////////////////////////
  // /////////////////////////////////////////////////////////////
 
  void setKinematicsInitCovarianceDefault(const Matrix &);
  void setKinematicsInitCovarianceDefault(const Matrix3 & P_pos,
                                          const Matrix3 & P_ori,
                                          const Matrix3 & P_linVel,
                                          const Matrix3 & P_angVel);
 
  void setGyroBiasInitCovarianceDefault(const Matrix3 & covMat);
 
  void setUnmodeledWrenchInitCovMatDefault(const Matrix6 & initCovMat);
 
  void setContactInitCovMatDefault(const Matrix12 & contactCovMat);
 
  void setKinematicsStateCovariance(const Matrix &);
 
  void setGyroBiasStateCovariance(const Matrix3 & covMat, unsigned imuNumber);
 
  void setUnmodeledWrenchStateCovMat(const Matrix6 & newCovMat);
 
  void setContactStateCovMat(Index contactNbr, const Matrix12 & contactCovMat);
 
  void setKinematicsProcessCovarianceDefault(const Matrix12 &);
 
  void setKinematicsProcessCovarianceDefault(const Matrix3 & P_pos,
                                             const Matrix3 & P_ori,
                                             const Matrix3 & P_linVel,
                                             const Matrix3 & P_angVel);
 
  void setGyroBiasProcessCovarianceDefault(const Matrix3 & covMat);
 
  void setUnmodeledWrenchProcessCovarianceDefault(const Matrix6 & covMat);
 
  void setContactProcessCovarianceDefault(const Matrix12 & covMat);
 
  void setKinematicsProcessCovariance(const Matrix12 &);
 
  void setGyroBiasProcessCovariance(const Matrix3 & covMat, unsigned imuNumber);
 
  void setUnmodeledWrenchProcessCovMat(const Matrix6 & processCovMat);
 
  void setContactProcessCovMat(Index contactNbr, const Matrix12 & contactCovMat);
 
  void resetStateCovarianceMat();
  void resetStateKinematicsCovMat();
  void resetStateGyroBiasCovMat(Index i);
  void resetStateUnmodeledWrenchCovMat();
  void resetStateContactsCovMat();
  void resetStateContactCovMat(Index contactNbr);
 
  void resetProcessCovarianceMat();
  void resetProcessKinematicsCovMat();
  void resetProcessGyroBiasCovMat(Index i);
  void resetProcessUnmodeledWrenchCovMat();
  void resetProcessContactsCovMat();
  void resetProcessContactCovMat(Index contactNbr);
 
  // /////////////////////////////////////////////////////////////
  // /////////////////////////////////////////////////////////////
 
 
  Index getStateSize() const;
 
  Index getStateTangentSize() const;
 
  Index getMeasurementSize() const;
 
  Matrix getStateCovarianceMat() const;
 
  void setStateCovarianceMat(const Matrix & P);
 
  void setProcessNoiseCovarianceMat(const Matrix & Q);
 
 
  const Vector & getCurrentStateVector() const;
 
  TimeIndex getStateVectorTimeIndex() const;
 
  void setInitWorldCentroidStateVector(const Vector & initStateVector);
 
  void setStateVector(const Vector & newvalue, bool resetCovariance = true);
 
  Vector getMeasurementVector();
 
 
  inline Index kineIndex() const;
 
  inline Index posIndex() const;
 
  inline Index oriIndex() const;
 
  inline Index linVelIndex() const;
 
  inline Index angVelIndex() const;
 
  inline Index gyroBiasIndex(Index IMUNumber) const;
 
  inline Index unmodeledWrenchIndex() const;
 
  inline Index unmodeledForceIndex() const;
 
  inline Index unmodeledTorqueIndex() const;
 
  inline Index contactsIndex() const;
 
  inline Index contactIndex(Index contactNbr) const;
 
  inline Index contactKineIndex(Index contactNbr) const;
 
  inline Index contactPosIndex(Index contactNbr) const;
 
  inline Index contactOriIndex(Index contactNbr) const;
 
  inline Index contactForceIndex(Index contactNbr) const;
 
  inline Index contactTorqueIndex(Index contactNbr) const;
 
  inline Index contactWrenchIndex(Index contactNbr) const;
 
  // ///////////////////////////////////////////////////////////
 
  inline Index kineIndexTangent() const;
 
  inline Index posIndexTangent() const;
 
  inline Index oriIndexTangent() const;
 
  inline Index linVelIndexTangent() const;
 
  inline Index angVelIndexTangent() const;
 
  inline Index gyroBiasIndexTangent(Index IMUNumber) const;
 
  inline Index unmodeledWrenchIndexTangent() const;
 
  inline Index unmodeledForceIndexTangent() const;
 
  inline Index unmodeledTorqueIndexTangent() const;
 
  inline Index contactsIndexTangent() const;
 
  inline Index contactIndexTangent(Index contactNbr) const;
 
  inline Index contactKineIndexTangent(Index contactNbr) const;
 
  inline Index contactPosIndexTangent(Index contactNbr) const;
 
  inline Index contactOriIndexTangent(Index contactNbr) const;
 
  inline Index contactForceIndexTangent(Index contactNbr) const;
 
  inline Index contactTorqueIndexTangent(Index contactNbr) const;
 
  inline Index contactWrenchIndexTangent(Index contactNbr) const;
 
 
  // /////////////////////////////////////////////////////////////
  // /////////////////////////////////////////////////////////////
 
  const ExtendedKalmanFilter & getEKF() const;
 
  ExtendedKalmanFilter & getEKF();
 
protected:
  struct Sensor
  {
    Sensor(Index signalSize) : measIndex(-1), measIndexTangent(-1), size(signalSize), time(0) {}
    ~Sensor() {}
    Index measIndex;
    Index measIndexTangent;
    Index size;
    TimeIndex time;
 
    inline Vector extractFromVector(const Vector & v)
    {
      return v.segment(size, measIndex);
    }
  };
 
  struct IMU : public Sensor
  {
    ~IMU() {}
    IMU() : Sensor(sizeIMUSignal) {}
 
    Kinematics userImuKinematics; // the kinematics of the IMU in the user's frame
    LocalKinematics centroidImuKinematics; // the kinematics of the IMU in the IMU's frame
    Vector6 acceleroGyro;
    Matrix3 covMatrixAccelero;
    Matrix3 covMatrixGyro;
 
    Index stateIndex;
    Index stateIndexTangent;
 
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  };
 
  typedef std::vector<IMU, Eigen::aligned_allocator<IMU>> VectorIMU;
  typedef VectorIMU::iterator VectorIMUIterator;
  typedef VectorIMU::const_iterator VectorIMUConstIterator;
 
  struct Contact : public Sensor
  {
    Contact() : Sensor(sizeWrench), isSet(false), withRealSensor(false), stateIndex(-1), stateIndexTangent(-1)
    {
      worldRestPose.angVel = worldRestPose.linVel = Vector3::Zero();
    }
    ~Contact() {}
 
    Kinematics worldRestPose; // the rest pose of the contact in the world frame
 
    Vector6 wrenchMeasurement; 
 
    Kinematics userContactKine; 
    Kinematics centroidContactKine; 
    CheckedMatrix6 sensorCovMatrix; 
 
    Matrix3 linearStiffness; 
    Matrix3 linearDamping; 
    Matrix3 angularStiffness; 
    Matrix3 angularDamping; 
 
 
    bool isSet;
    bool withRealSensor;
    Index stateIndex;
    Index stateIndexTangent;
 
    static const Kinematics::Flags::Byte contactKineFlags = 
        Kinematics::Flags::position | Kinematics::Flags::orientation | Kinematics::Flags::linVel
        | Kinematics::Flags::angVel;
 
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  };
 
  typedef std::vector<Contact, Eigen::aligned_allocator<Contact>> VectorContact;
  typedef VectorContact::iterator VectorContactIterator;
  typedef VectorContact::const_iterator VectorContactConstIterator;
 
  struct AbsolutePoseSensor : public Sensor
  {
    AbsolutePoseSensor() : Sensor(sizePose) {}
 
    Kinematics pose;
    static const Kinematics::Flags::Byte poseFlags = Kinematics::Flags::position | Kinematics::Flags::orientation;
    CheckedMatrix6 covMatrix;
  };
 
  struct AbsoluteOriSensor : public Sensor
  {
    AbsoluteOriSensor() : Sensor(sizePose) {}
 
    Orientation ori;
    CheckedMatrix3 covMatrix;
  };
 
protected:
  virtual Vector stateDynamics(const Vector & x, const Vector & u, TimeIndex k);
 
  virtual Vector measureDynamics(const Vector & x, const Vector & u, TimeIndex k);
 
  void addUnmodeledAndContactWrench_(const Vector & centroidStateVector, Vector3 & force, Vector3 & torque);
 
  void addUnmodeledWrench_(const Vector & centroidStateVector, Vector3 & force, Vector3 & torque);
 
  void addContactWrench_(const Kinematics & centroidContactKine,
                         const Vector3 & centroidContactForce,
                         const Vector3 & centroidContactTorque,
                         Vector3 & totalCentroidForce,
                         Vector3 & totalCentroidTorque);
 
 
  void computeLocalAccelerations_(LocalKinematics & localStateKine,
                                  const Vector3 & totalForceLocal,
                                  const Vector3 & totalMomentLocal,
                                  Vector3 & linAcc,
                                  Vector3 & angAcc);
 
  void computeContactForce_(VectorContactIterator i,
                            LocalKinematics & worldCentroidStateKinematics,
                            Kinematics & worldRestContactPose,
                            Vector3 & contactForce,
                            Vector3 & contactTorque);
 
  void computeContactForces_(LocalKinematics & worldCentroidStateKinematics,
                             Vector3 & contactForce,
                             Vector3 & contactTorque);
 
  virtual void setProcessNoise(NoiseBase *);
 
  virtual void resetProcessNoise();
  virtual NoiseBase * getProcessNoise() const;
 
  virtual void setMeasurementNoise(NoiseBase *);
  virtual void resetMeasurementNoise();
  virtual NoiseBase * getMeasurementNoise() const;
 
  virtual Index getInputSize() const;
 
public:
  Vector6 getCentroidContactWrench(Index numContact) const;
 
  Kinematics getCentroidContactInputPose(Index numContact) const;
 
  Kinematics getWorldContactPoseFromCentroid(Index numContact) const;
 
  Kinematics getContactStateRestKinematics(Index numContact) const;
 
  Kinematics getUserContactInputPose(Index numContact) const;
 
  Index getIMUMeasIndexByNum(Index num) const;
 
  Index getContactMeasIndexByNum(Index num) const;
 
  bool getContactIsSetByNum(Index num) const;
 
 
 
  inline Vector stateSum(const Vector & stateVector, const Vector & tangentVector);
 
  virtual void stateSum(const Vector & stateVector, const Vector & tangentVector, Vector & sum);
 
  inline Vector stateDifference(const Vector & stateVector1, const Vector & stateVector2);
 
  virtual void stateDifference(const Vector & stateVector1, const Vector & stateVector2, Vector & difference);
 
  virtual void measurementDifference(const Vector & measureVector1, const Vector & measureVector2, Vector & difference);
 
  virtual void useFiniteDifferencesJacobians(bool b = true);
 
  virtual void setFiniteDifferenceStep(const Vector & dx);
 
  virtual Matrix computeAMatrix();
 
  virtual Matrix computeCMatrix();
 
  void computeLocalAccelerations(const Vector & x, Vector & acceleration);
 
  friend int testAccelerationsJacobians(KineticsObserver & ko,
                                        int errcode,
                                        double relativeErrorThreshold,
                                        double threshold); // declared out of namespace state-observation
 
  friend int testAnalyticalAJacobianVsFD(KineticsObserver & ko,
                                         int errcode,
                                         double relativeErrorThreshold,
                                         double threshold); // declared out of namespace state-observation
 
  friend int testAnalyticalCJacobianVsFD(KineticsObserver & ko,
                                         int errcode,
                                         double relativeErrorThreshold,
                                         double threshold); // declared out of namespace state-observation
 
  friend int testOrientationsJacobians(KineticsObserver & ko,
                                       int errcode,
                                       double relativeErrorThreshold,
                                       double threshold); // declared out of namespace state-observation
 
protected:
  void stateNaNCorrection_();
 
  void updateLocalKineAndContacts_();
 
  void updateGlobalKine_();
 
protected:
  unsigned maxContacts_;
  unsigned maxImuNumber_;
 
  AbsolutePoseSensor absPoseSensor_;
  AbsoluteOriSensor absOriSensor_;
  VectorContact contacts_;
  VectorIMU imuSensors_;
 
  Index stateSize_;
  Index stateTangentSize_;
  Index measurementSize_;
  Index measurementTangentSize_;
 
  Vector worldCentroidStateVector_;
  Vector worldCentroidStateVectorDx_;
  Vector oldWorldCentroidStateVector_;
 
  LocalKinematics worldCentroidStateKinematics_;
  Kinematics worldCentroidKinematics_;
 
  Vector3 additionalForce_;
  Vector3 additionalTorque_;
 
  Vector3 initTotalCentroidForce_; // Initial total force used in the state prediction
  Vector3 initTotalCentroidTorque_; // Initial total torque used in the state prediction
 
  Vector measurementVector_;
  Matrix measurementCovMatrix_;
 
  stateObservation::ExtendedKalmanFilter ekf_;
  bool finiteDifferencesJacobians_;
  bool withGyroBias_;
  bool withUnmodeledWrench_;
  bool withAccelerationEstimation_;
 
  IndexedVector3 com_, comd_, comdd_;
  IndexedVector3 sigma_, sigmad_;
  IndexedMatrix3 I_, Id_;
 
  TimeIndex k_est_; // time index of the last estimation
  TimeIndex k_data_; // time index of the current measurements
 
  double mass_;
 
  double dt_;
 
  NoiseBase * processNoise_;
  NoiseBase * measurementNoise_;
 
  Index numberOfContactRealSensors_;
  Index currentIMUSensorNumber_;
 
  void startNewIteration_();
 
 
  void convertUserToCentroidFrame_(const Kinematics & userKine, Kinematics & centroidKine, TimeIndex k_data);
 
 
  Kinematics convertUserToCentroidFrame_(const Kinematics & userKine, TimeIndex k_data);
 
 
  inline Index gyroBiasIndex(VectorIMUConstIterator i) const;
  inline Index gyroBiasIndexTangent(VectorIMUConstIterator i) const;
 
  inline Index contactIndex(VectorContactConstIterator i) const;
  inline Index contactKineIndex(VectorContactConstIterator i) const;
  inline Index contactPosIndex(VectorContactConstIterator i) const;
  inline Index contactOriIndex(VectorContactConstIterator i) const;
  inline Index contactForceIndex(VectorContactConstIterator i) const;
  inline Index contactTorqueIndex(VectorContactConstIterator i) const;
  inline Index contactWrenchIndex(VectorContactConstIterator i) const;
 
  inline Index contactIndexTangent(VectorContactConstIterator i) const;
  inline Index contactKineIndexTangent(VectorContactConstIterator i) const;
  inline Index contactPosIndexTangent(VectorContactConstIterator i) const;
  inline Index contactOriIndexTangent(VectorContactConstIterator i) const;
  inline Index contactForceIndexTangent(VectorContactConstIterator i) const;
  inline Index contactTorqueIndexTangent(VectorContactConstIterator i) const;
  inline Index contactWrenchIndexTangent(VectorContactConstIterator i) const;
 
public: 
  inline static constexpr Index sizeAcceleroSignal = 3;
  inline static constexpr Index sizeGyroSignal = 3;
  inline static constexpr Index sizeIMUSignal = sizeAcceleroSignal + sizeGyroSignal;
 
  inline static constexpr Index sizePos = 3;
  inline static constexpr Index sizePosTangent = 3;
  inline static constexpr Index sizeOri = 4;
  inline static constexpr Index sizeOriTangent = 3;
  inline static constexpr Index sizeLinVel = sizePos;
  inline static constexpr Index sizeLinVelTangent = sizeLinVel;
  inline static constexpr Index sizeLinAccTangent = sizeLinVelTangent;
  inline static constexpr Index sizeAngVel = sizeOriTangent;
  inline static constexpr Index sizeAngVelTangent = sizeAngVel;
  inline static constexpr Index sizeGyroBias = sizeGyroSignal;
  inline static constexpr Index sizeGyroBiasTangent = sizeGyroBias;
 
  inline static constexpr Index sizeForce = 3;
  inline static constexpr Index sizeForceTangent = sizeForce;
  inline static constexpr Index sizeTorque = 3;
  inline static constexpr Index sizeTorqueTangent = sizeTorque;
 
  inline static constexpr Index sizeWrench = sizeForce + sizeTorque;
 
  inline static constexpr Index sizeStateKine = sizePos + sizeOri + sizeLinVel + sizeAngVel;
  inline static constexpr Index sizeStateBase = sizeStateKine + sizeForce + sizeTorque;
  inline static constexpr Index sizeStateKineTangent = sizePos + sizeOriTangent + sizeLinVel + sizeAngVel;
  inline static constexpr Index sizeStateTangentBase = sizeStateKineTangent + sizeForce + sizeTorque;
 
  inline static constexpr Index sizePose = sizePos + sizeOri;
  inline static constexpr Index sizePoseTangent = sizePos + sizeOriTangent;
 
  inline static constexpr Index sizeContactKine = sizePose;
  inline static constexpr Index sizeContactKineTangent = sizePoseTangent;
 
  inline static constexpr Index sizeContact = sizeContactKine + sizeWrench;
  inline static constexpr Index sizeContactTangent = sizeContactKineTangent + sizeWrench;
 
  inline static constexpr Kinematics::Flags::Byte flagsStateKine =
      Kinematics::Flags::position | Kinematics::Flags::orientation | Kinematics::Flags::linVel
      | Kinematics::Flags::angVel;
 
  inline static constexpr Kinematics::Flags::Byte flagsContactKine =
      Kinematics::Flags::position | Kinematics::Flags::orientation;
 
  inline static constexpr Kinematics::Flags::Byte flagsPoseKine =
      Kinematics::Flags::position | Kinematics::Flags::orientation;
 
  inline static constexpr Kinematics::Flags::Byte flagsPosKine = Kinematics::Flags::position;
 
  inline static constexpr Kinematics::Flags::Byte flagsIMUKine =
      Kinematics::Flags::position | Kinematics::Flags::orientation | Kinematics::Flags::linVel
      | Kinematics::Flags::angVel | Kinematics::Flags::linAcc | Kinematics::Flags::angAcc;
 
  static const double defaultMass;
 
  static const double statePoseInitVarianceDefault;
  static const double stateOriInitVarianceDefault;
  static const double stateLinVelInitVarianceDefault;
  static const double stateAngVelInitVarianceDefault;
  static const double gyroBiasInitVarianceDefault;
  static const double unmodeledWrenchInitVarianceDefault;
  static const double contactForceInitVarianceDefault;
  static const double contactTorqueInitVarianceDefault;
 
  static const double statePoseProcessVarianceDefault;
  static const double stateOriProcessVarianceDefault;
  static const double stateLinVelProcessVarianceDefault;
  static const double stateAngVelProcessVarianceDefault;
  static const double gyroBiasProcessVarianceDefault;
  static const double unmodeledWrenchProcessVarianceDefault;
  static const double contactPositionProcessVarianceDefault;
  static const double contactOrientationProcessVarianceDefault;
  static const double contactForceProcessVarianceDefault;
  static const double contactTorqueProcessVarianceDefault;
 
  static const double acceleroVarianceDefault;
  static const double gyroVarianceDefault;
  static const double forceSensorVarianceDefault;
  static const double torqueSensorVarianceDefault;
  static const double positionSensorVarianceDefault;
  static const double orientationSensorVarianceDefault;
 
  static const double linearStiffnessDefault;
  static const double angularStiffnessDefault;
  static const double linearDampingDefault;
  static const double angularDampingDefault;
 
 
  bool nanDetected_ = false;
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
protected:
  Matrix3 linearStiffnessMatDefault_;
  Matrix3 angularStiffnessMatDefault_;
  Matrix3 linearDampingMatDefault_;
  Matrix3 angularDampingMatDefault_;
 
  Matrix3 acceleroCovMatDefault_;
  Matrix3 gyroCovMatDefault_;
  Matrix6 contactWrenchSensorCovMatDefault_;
  Matrix6 absPoseSensorCovMatDefault_;
  Matrix3 absOriSensorCovMatDefault_;
 
  Matrix3 statePosInitCovMat_;
  Matrix3 stateOriInitCovMat_;
  Matrix3 stateLinVelInitCovMat_;
  Matrix3 stateAngVelInitCovMat_;
  Matrix3 gyroBiasInitCovMat_;
  Matrix6 unmodeledWrenchInitCovMat_;
  Matrix12 contactInitCovMatDefault_;
 
  Matrix3 statePosProcessCovMat_;
  Matrix3 stateOriProcessCovMat_;
  Matrix3 stateLinVelProcessCovMat_;
  Matrix3 stateAngVelProcessCovMat_;
  Matrix3 gyroBiasProcessCovMat_;
  Matrix6 unmodeledWrenchProcessCovMat_;
  Matrix3 contactPositionProcessCovMat_;
  Matrix3 contactOrientationProcessCovMat_;
  Matrix3 contactForceProcessCovMat_;
  Matrix3 contactTorqueProcessCovMat_;
  Matrix12 contactProcessCovMatDefault_;
 
  Matrix12 stateKinematicsInitCovMat_;
  Matrix12 stateKinematicsProcessCovMat_;
 
  static const double defaultdx;
 
  struct Opt
  {
    Opt() : locKine(locKine1), ori(locKine.orientation), ori1(locKine1.orientation), ori2(locKine2.orientation) {}
 
    LocalKinematics locKine1, locKine2;
    LocalKinematics & locKine;
    Orientation & ori;
    Orientation & ori1;
    Orientation & ori2;
  } opt_;
 
private:
  Index setIMU(const Vector3 & accelero,
               const Vector3 & gyrometer,
               const Kinematics & userImuKinematics,
               Index num,
               const Matrix3 * acceleroCov,
               const Matrix3 * gyroCov);
};
 
#include <state-observation/dynamics-estimators/kinetics-observer.hxx>
 
} // namespace stateObservation
 
#endif