imu-dynamical-system.hpp

Go to the documentation of this file.

 
#ifndef IMU_DYNAMICAL_SYSTEM_HPP
#define IMU_DYNAMICAL_SYSTEM_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/sensors-simulation/accelerometer-gyrometer.hpp>
#include <state-observation/tools/rigid-body-kinematics.hpp>
 
namespace stateObservation
{

class STATE_OBSERVATION_DLLAPI IMUDynamicalSystem : public DynamicalSystemFunctorBase
{
public:
  IMUDynamicalSystem(bool withGyroBias = false);

  virtual ~IMUDynamicalSystem();

  virtual Vector stateDynamics(const Vector & x, const Vector & u, TimeIndex k);

  virtual Vector measureDynamics(const Vector & x, const Vector & u, TimeIndex k);

  virtual void setProcessNoise(NoiseBase *);
  virtual void resetProcessNoise();
  virtual NoiseBase * getProcessNoise() const;

  virtual void setMeasurementNoise(NoiseBase *);
  virtual void resetMeasurementNoise();
  virtual NoiseBase * getMeasurementNoise() const;

  virtual void setSamplingPeriod(double dt);

  virtual Index getStateSize() const;
  virtual Index getInputSize() const;
  virtual Index getMeasurementSize() const;

  void setWithGyroBias(bool);
 
  void updatestatesize()
  {
    if(withGyroBias_)
    {
      statesize_ = stateSizeBase_ + 3;
    }
    else
    {
      statesize_ = stateSizeBase_;
    }
  }
 
  struct indexes : public kine::indexes<kine::rotationVector>
  {
    static const Index gyroBias = 18;
  };
 
protected:
  AccelerometerGyrometer sensor_;
 
  NoiseBase * processNoise_;
 
  double dt_;
 
  Vector3Unaligned orientationVector_;
  QuaternionUnaligned quaternion_;
 
  Quaternion computeQuaternion_(const Vector3 & x);
 
  static const Index stateSizeBase_ = 18; 
  Index statesize_;
  static const Index inputSize_ = 6;
  static const Index measurementSize_ = 6;
 
  bool withGyroBias_;

  static constexpr double one_ = 0.9999;
 
private:
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace stateObservation
#endif // IMU-DYNAMICAL-SYSTEM_HPP