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