stateObservation::IMUDynamicalSystem Class Reference

The class is an implementation of the dynamical system defined by an inertial measurement unit (IMU) fixed on a rigid body. The state is the position velocity and acceleration and the orientaion and rotation velocity and acceleration. The sensors are the accelerometer and the gyrometer. More...

#include <state-observation/dynamical-system/imu-dynamical-system.hpp>

Inheritance diagram for stateObservation::IMUDynamicalSystem:
Collaboration diagram for stateObservation::IMUDynamicalSystem:

Classes

struct  indexes
 

Public Member Functions

 IMUDynamicalSystem (bool withGyroBias=false)
 The constructor. More...
 
virtual ~IMUDynamicalSystem ()
 The virtual destructor. More...
 
virtual Vector stateDynamics (const Vector &x, const Vector &u, TimeIndex k)
 Description of the state dynamics. More...
 
virtual Vector measureDynamics (const Vector &x, const Vector &u, TimeIndex k)
 Description of the sensor's dynamics. More...
 
virtual void setProcessNoise (NoiseBase *)
 Sets a noise which disturbs the state dynamics. More...
 
virtual void resetProcessNoise ()
 Removes the process noise. More...
 
virtual NoiseBasegetProcessNoise () const
 Gets the process noise. More...
 
virtual void setMeasurementNoise (NoiseBase *)
 Sets a noise which disturbs the measurements. More...
 
virtual void resetMeasurementNoise ()
 Removes the measurement noise. More...
 
virtual NoiseBasegetMeasurementNoise () const
 Gets a pointer on the measurement noise. More...
 
virtual void setSamplingPeriod (double dt)
 Set the period of the time discretization. More...
 
virtual Index getStateSize () const
 Gets the state size. More...
 
virtual Index getInputSize () const
 Gets the input size. More...
 
virtual Index getMeasurementSize () const
 Gets the measurement size. More...
 
void setWithGyroBias (bool)
 Set whether we use Gyro Bias. More...
 
void updatestatesize ()
 
- Public Member Functions inherited from stateObservation::DynamicalSystemFunctorBase
 DynamicalSystemFunctorBase ()
 
virtual ~DynamicalSystemFunctorBase ()
 
virtual void reset ()
 
virtual bool checkStateVector (const Vector &)
 Gives a boolean answer on whether or not the vector is correctly sized to be a state vector. More...
 
virtual bool checkInputvector (const Vector &)
 Gives a boolean answer on whether or not the vector is correctly sized to be an input vector. More...
 

Protected Member Functions

Quaternion computeQuaternion_ (const Vector3 &x)
 
- Protected Member Functions inherited from stateObservation::DynamicalSystemFunctorBase
void assertStateVector_ (const Vector &v)
 
void assertInputVector_ (const Vector &v)
 

Protected Attributes

AccelerometerGyrometer sensor_
 
NoiseBaseprocessNoise_
 
double dt_
 
Vector3Unaligned orientationVector_
 
QuaternionUnaligned quaternion_
 
Index statesize_
 the state size may be bigger if the bias is considered More...
 
bool withGyroBias_
 

Static Protected Attributes

static const Index stateSizeBase_ = 18
 
static const Index inputSize_ = 6
 
static const Index measurementSize_ = 6
 
static constexpr double one_ = 0.9999
 the factor that approximate the "one" to avoid drifting of unobservable values More...
 

Detailed Description

The class is an implementation of the dynamical system defined by an inertial measurement unit (IMU) fixed on a rigid body. The state is the position velocity and acceleration and the orientaion and rotation velocity and acceleration. The sensors are the accelerometer and the gyrometer.

Constructor & Destructor Documentation

◆ IMUDynamicalSystem()

stateObservation::IMUDynamicalSystem::IMUDynamicalSystem ( bool  withGyroBias = false)

The constructor.

◆ ~IMUDynamicalSystem()

virtual stateObservation::IMUDynamicalSystem::~IMUDynamicalSystem ( )
virtual

The virtual destructor.

Member Function Documentation

◆ computeQuaternion_()

Quaternion stateObservation::IMUDynamicalSystem::computeQuaternion_ ( const Vector3 x)
protected

◆ getInputSize()

virtual Index stateObservation::IMUDynamicalSystem::getInputSize ( ) const
virtual

Gets the input size.

Implements stateObservation::DynamicalSystemFunctorBase.

◆ getMeasurementNoise()

virtual NoiseBase* stateObservation::IMUDynamicalSystem::getMeasurementNoise ( ) const
virtual

Gets a pointer on the measurement noise.

◆ getMeasurementSize()

virtual Index stateObservation::IMUDynamicalSystem::getMeasurementSize ( ) const
virtual

Gets the measurement size.

Implements stateObservation::DynamicalSystemFunctorBase.

◆ getProcessNoise()

virtual NoiseBase* stateObservation::IMUDynamicalSystem::getProcessNoise ( ) const
virtual

Gets the process noise.

◆ getStateSize()

virtual Index stateObservation::IMUDynamicalSystem::getStateSize ( ) const
virtual

Gets the state size.

Implements stateObservation::DynamicalSystemFunctorBase.

◆ measureDynamics()

virtual Vector stateObservation::IMUDynamicalSystem::measureDynamics ( const Vector x,
const Vector u,
TimeIndex  k 
)
virtual

Description of the sensor's dynamics.

Implements stateObservation::DynamicalSystemFunctorBase.

◆ resetMeasurementNoise()

virtual void stateObservation::IMUDynamicalSystem::resetMeasurementNoise ( )
virtual

Removes the measurement noise.

◆ resetProcessNoise()

virtual void stateObservation::IMUDynamicalSystem::resetProcessNoise ( )
virtual

Removes the process noise.

◆ setMeasurementNoise()

virtual void stateObservation::IMUDynamicalSystem::setMeasurementNoise ( NoiseBase )
virtual

Sets a noise which disturbs the measurements.

◆ setProcessNoise()

virtual void stateObservation::IMUDynamicalSystem::setProcessNoise ( NoiseBase )
virtual

Sets a noise which disturbs the state dynamics.

◆ setSamplingPeriod()

virtual void stateObservation::IMUDynamicalSystem::setSamplingPeriod ( double  dt)
virtual

Set the period of the time discretization.

◆ setWithGyroBias()

void stateObservation::IMUDynamicalSystem::setWithGyroBias ( bool  )

Set whether we use Gyro Bias.

◆ stateDynamics()

virtual Vector stateObservation::IMUDynamicalSystem::stateDynamics ( const Vector x,
const Vector u,
TimeIndex  k 
)
virtual

Description of the state dynamics.

Implements stateObservation::DynamicalSystemFunctorBase.

◆ updatestatesize()

void stateObservation::IMUDynamicalSystem::updatestatesize ( )
inline

Member Data Documentation

◆ dt_

double stateObservation::IMUDynamicalSystem::dt_
protected

◆ inputSize_

const Index stateObservation::IMUDynamicalSystem::inputSize_ = 6
staticprotected

◆ measurementSize_

const Index stateObservation::IMUDynamicalSystem::measurementSize_ = 6
staticprotected

◆ one_

constexpr double stateObservation::IMUDynamicalSystem::one_ = 0.9999
staticconstexprprotected

the factor that approximate the "one" to avoid drifting of unobservable values

◆ orientationVector_

Vector3Unaligned stateObservation::IMUDynamicalSystem::orientationVector_
protected

◆ processNoise_

NoiseBase* stateObservation::IMUDynamicalSystem::processNoise_
protected

◆ quaternion_

QuaternionUnaligned stateObservation::IMUDynamicalSystem::quaternion_
protected

◆ sensor_

AccelerometerGyrometer stateObservation::IMUDynamicalSystem::sensor_
protected

◆ statesize_

Index stateObservation::IMUDynamicalSystem::statesize_
protected

the state size may be bigger if the bias is considered

◆ stateSizeBase_

const Index stateObservation::IMUDynamicalSystem::stateSizeBase_ = 18
staticprotected

◆ withGyroBias_

bool stateObservation::IMUDynamicalSystem::withGyroBias_
protected

The documentation for this class was generated from the following file: