imu-mltpctive-dynamical-system.hpp

 
#ifndef IMU_MULTIPLICATIVE_DYNAMICAL_SYSTEM_HPP
#define IMU_MULTIPLICATIVE_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>
#include <state-observation/tools/state-vector-arithmetics.hpp>
 
namespace stateObservation
{

class STATE_OBSERVATION_DLLAPI IMUMltpctiveDynamicalSystem : public DynamicalSystemFunctorBase,
                                                             public StateVectorArithmetics
{
public:
  IMUMltpctiveDynamicalSystem();

  virtual ~IMUMltpctiveDynamicalSystem();

  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 Matrix getAMatrix(const Vector & xh);
  virtual Matrix getCMatrix(const Vector & xp);

  virtual Index getStateSize() const;
  virtual Index getInputSize() const;
  virtual Index getMeasurementSize() const;
 
  void stateSum(const Vector & stateVector, const Vector & tangentVector, Vector & sum);
 
  void stateDifference(const Vector & stateVector1, const Vector & stateVector2, Vector & difference);
 
protected:
  static const Index stateSize_ = 19;
  static const Index stateTangentSize_ = 18;
  static const Index inputSize_ = 6;
  static const Index measurementSize_ = 6;
  typedef kine::indexes<kine::quaternion> indexes;
  typedef kine::indexes<kine::rotationVector> indexesTangent;
 
  struct opt
  {
    Matrix3 jRR, jRv;
 
    Vector3 deltaR;
 
    Matrix AJacobian;
    Matrix CJacobian;
 
    Matrix3 Rt;
 
    opt(int stateSize, int measurementSize) : AJacobian(stateSize, stateSize), CJacobian(measurementSize, stateSize)
    {
      AJacobian.setZero();
      AJacobian.block<3, 3>(indexesTangent::pos, indexesTangent::pos).setIdentity();
      AJacobian.block<6, 6>(indexesTangent::linVel, indexesTangent::linVel).setIdentity();
 
      CJacobian.setZero();
    }
  } opt_;
 
  AccelerometerGyrometer sensor_;
 
  NoiseBase * processNoise_;
 
  double dt_;
 
private:
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace stateObservation
#endif // IMU_MULTIPLICATIVE_DYNAMICAL_SYSTEM_HPP