imu-magnetometer-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-magnetometer.hpp>
#include <state-observation/tools/rigid-body-kinematics.hpp>
 
namespace stateObservation
{

class STATE_OBSERVATION_DLLAPI IMUMagnetometerDynamicalSystem : public DynamicalSystemFunctorBase
{
public:
  IMUMagnetometerDynamicalSystem();

  virtual ~IMUMagnetometerDynamicalSystem();

  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;
 
protected:
  typedef kine::indexes<kine::rotationVector> indexes;
 
  AccelerometerGyrometerMagnetometer sensor_;
 
  NoiseBase * processNoise_;
 
  double dt_;
 
  Vector3Unaligned orientationVector_;
  QuaternionUnaligned quaternion_;
 
  Quaternion computeQuaternion_(const Vector3 & x);
 
  static const Index stateSize_ = 18;
  static const Index inputSize_ = 0;
  static const Index measurementSize_ = 9;
 
private:
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace stateObservation
#endif // IMU-DYNAMICAL-SYSTEM_HPP