0.4.1
Loading...
Searching...
No Matches
NAV::InertialIntegrator Class Reference

Inertial Measurement Integrator. More...

Data Structures

struct  GenericState
 Inertial state and measurements. More...
 
struct  Measurement
 Inertial Measurement. More...
 

Public Types

enum class  IntegrationAlgorithm : uint8_t {
  SingleStepRungeKutta1 ,
  SingleStepRungeKutta2 ,
  SingleStepHeun2 ,
  SingleStepRungeKutta3 ,
  SingleStepHeun3 ,
  SingleStepRungeKutta4 ,
  MultiStepRK3 ,
  MultiStepRK4 ,
  COUNT
}
 Available Integration Algorithms. More...
 
enum class  IntegrationFrame : uint8_t {
  ECEF ,
  NED
}
 Available Integration Frames. More...
 
using State
 Inertial state and measurements.
 

Public Member Functions

void addState (const PosVelAtt &state, const char *nameId)
 Pushes the state to the list of states.
 
void applySensorBiasesIncrements (const Eigen::Vector3d &p_deltaBiasAcceleration, const Eigen::Vector3d &p_deltaBiasAngularRate)
 Sets the sensor biases increment.
 
void applyStateErrors_e (const Eigen::Vector3d &e_positionError, const Eigen::Vector3d &e_velocityError, const Eigen::Vector3d &e_attitudeError_b)
 Apply the errors to the latest state.
 
void applyStateErrors_n (const Eigen::Vector3d &lla_positionError, const Eigen::Vector3d &n_velocityError, const Eigen::Vector3d &n_attitudeError_b)
 Apply the errors to the latest state.
 
bool areAccelerationsAveragedMeasurements () const
 Wether the measurements are accumulated values over the last epoch. (always true when using delta measurements, so GUI has no effect)
 
template<typename T>
Eigen::Vector< T, 10 > calcInertialSolution (const ImuPos &imuPos, const GenericState< T > &state__t0, const GenericState< T > &state__t1, const char *nameId) const
 Calculates the inertial solution going from state__t1 to state__t0 given that measurements are available for both states.
 
std::shared_ptr< PosVelAttcalcInertialSolution (const InsTime &obsTime, const Eigen::Vector3d &p_acceleration, const Eigen::Vector3d &p_angularRate, const ImuPos &imuPos, const char *nameId)
 Calculates the inertial navigation solution.
 
std::shared_ptr< PosVelAttcalcInertialSolutionDelta (const InsTime &obsTime, double deltaTime, const Eigen::Vector3d &p_deltaVelocity, const Eigen::Vector3d &p_deltaTheta, const ImuPos &imuPos, const char *nameId)
 Calculates the inertial navigation solution.
 
const PosVelAttDerivativeConstantsgetConstants () const
 Returns the selected compensation models.
 
IntegrationFrame getIntegrationFrame () const
 Returns the selected integration frame.
 
std::optional< std::reference_wrapper< const State > > getLatestState () const
 Get the latest state if it exists.
 
bool hasInitialPosition () const
 Checks if an initial position is set.
 
 InertialIntegrator ()=default
 Default Constructor.
 
 InertialIntegrator (IntegrationFrame integrationFrame)
 Constructor.
 
std::optional< Eigen::Vector3d > p_calcCurrentAcceleration () const
 Calculate the current acceleration, if measurements area available.
 
std::optional< Eigen::Vector3d > p_calcCurrentAngularRate () const
 Calculate the current angular rate, if measurements area available.
 
const Eigen::Vector3d & p_getLastAccelerationBias () const
 Return the last acceleration bias in platform frame coordinates in [m/s^2].
 
const Eigen::Vector3d & p_getLastAngularRateBias () const
 Return the last angular rate bias in platform frame coordinates in [rad/s].
 
void reset ()
 Clears all internal data.
 
void setInitialState (const PosVelAtt &state, const char *nameId)
 Sets the initial state.
 
void setTotalSensorBiases (const Eigen::Vector3d &p_biasAcceleration, const Eigen::Vector3d &p_biasAngularRate)
 Sets the sensor biases total values.
 

Private Member Functions

void addDeltaMeasurement (const InsTime &epoch, double dt, double deltaTime, const Eigen::Vector3d &p_deltaVelocity, const Eigen::Vector3d &p_deltaTheta, const char *nameId)
 Adds the measurement to the tracking buffer.
 
void addMeasurement (const InsTime &epoch, double dt, const Eigen::Vector3d &p_acceleration, const Eigen::Vector3d &p_angularRate, const char *nameId)
 Adds the measurement to the tracking buffer.
 
std::shared_ptr< PosVelAttcalcInertialSolutionFromMeasurementBuffer (const ImuPos &imuPos, const char *nameId)
 Calculates the inertial navigation solution.
 
std::shared_ptr< PosVelAttlastStateAsPosVelAtt () const
 Returns the last state as PosVelAtt.
 
void setBufferSizes ()
 Resizes the measurement and state buffers depending on the integration algorithm.
 

Private Attributes

bool _accelerationsAreAveragedMeasurements
 If true, then the measurements are accumulated values over the last epoch. (always true when using delta measurements, so GUI has no effect)
 
IntegrationAlgorithm _integrationAlgorithm
 Integration algorithm used for the update.
 
IntegrationFrame _integrationFrame
 Frame to integrate the observations in.
 
bool _lockIntegrationFrame
 Wether to lock the integration frame.
 
Eigen::Vector3d _p_lastBiasAcceleration
 Initial values for the acceleration bias [m/s^2].
 
Eigen::Vector3d _p_lastBiasAngularRate
 Initial values for the angular rate bias [rad/s].
 
PosVelAttDerivativeConstants _posVelAttDerivativeConstants
 Settings for the models to use.
 
ScrollingBuffer< State_states
 List of states. Length depends on algorithm used.
 

Friends

void from_json (const json &j, InertialIntegrator &data)
 Read info from a json object.
 
bool InertialIntegratorGui (const char *label, InertialIntegrator &integrator, bool &preferAccelerationOverDeltaMeasurements, float width=250.0F)
 Shows a GUI for advanced configuration of the InertialIntegrator.
 
void to_json (json &j, const InertialIntegrator &data)
 Write info to a json object.
 
const char * to_string (InertialIntegrator::IntegrationAlgorithm algorithm)
 Converts the enum to a string.
 
const char * to_string (InertialIntegrator::IntegrationFrame frame)
 Converts the enum to a string.
 

Detailed Description

Inertial Measurement Integrator.

Definition at line 39 of file InertialIntegrator.hpp.

Member Typedef Documentation

◆ State

Inertial state and measurements.

Definition at line 95 of file InertialIntegrator.hpp.

Member Enumeration Documentation

◆ IntegrationAlgorithm

enum class NAV::InertialIntegrator::IntegrationAlgorithm : uint8_t
strong

Available Integration Algorithms.

Enumerator
SingleStepRungeKutta1 

Runge-Kutta 1st order (explicit) / (Forward) Euler method.

SingleStepRungeKutta2 

Runge-Kutta 2nd order (explicit) / Explicit midpoint method.

SingleStepHeun2 

Heun's method (2nd order) (explicit)

SingleStepRungeKutta3 

Runge-Kutta 3rd order (explicit) / Simpson's rule.

SingleStepHeun3 

Heun's method (3nd order) (explicit)

SingleStepRungeKutta4 

Runge-Kutta 4th order (explicit)

MultiStepRK3 

Multistep Runge-Kutta 3rd order (explicit) / Simpson's rule (taking 2 old epochs into account)

MultiStepRK4 

Multistep Runge-Kutta 4th order (explicit) (taking 2 old epochs into account)

COUNT 

Amount of available integration algorithms.

Definition at line 43 of file InertialIntegrator.hpp.

◆ IntegrationFrame

enum class NAV::InertialIntegrator::IntegrationFrame : uint8_t
strong

Available Integration Frames.

Enumerator
ECEF 

Earth-Centered Earth-Fixed frame.

NED 

Local North-East-Down frame.

Definition at line 57 of file InertialIntegrator.hpp.

Constructor & Destructor Documentation

◆ InertialIntegrator() [1/2]

NAV::InertialIntegrator::InertialIntegrator ( )
default

Default Constructor.

◆ InertialIntegrator() [2/2]

NAV::InertialIntegrator::InertialIntegrator ( IntegrationFrame integrationFrame)
explicit

Constructor.

Parameters
integrationFrameIntegration frame to lock to

Definition at line 26 of file InertialIntegrator.cpp.

Member Function Documentation

◆ addDeltaMeasurement()

void NAV::InertialIntegrator::addDeltaMeasurement ( const InsTime & epoch,
double dt,
double deltaTime,
const Eigen::Vector3d & p_deltaVelocity,
const Eigen::Vector3d & p_deltaTheta,
const char * nameId )
private

Adds the measurement to the tracking buffer.

Parameters
[in]epochEpoch of the measurement
[in]dtTime between observation and last state in [s]
[in]deltaTimeDelta time over which the deltaVelocity and deltaTheta were measured in [s]
[in]p_deltaVelocityIntegrated acceleration in platform frame coordinates in [m/s]
[in]p_deltaThetaIntegrated angular rate in platform frame coordinates in [rad]
[in]nameIdNameId of the calling node for logging

Definition at line 196 of file InertialIntegrator.cpp.

◆ addMeasurement()

void NAV::InertialIntegrator::addMeasurement ( const InsTime & epoch,
double dt,
const Eigen::Vector3d & p_acceleration,
const Eigen::Vector3d & p_angularRate,
const char * nameId )
private

Adds the measurement to the tracking buffer.

Parameters
[in]epochEpoch of the measurement
[in]dtTime between observation and last state in [s]
[in]p_accelerationAcceleration in platform frame coordinates in [m/s^2]
[in]p_angularRateAngular rate in platform frame coordinates in [rad/s]
[in]nameIdNameId of the calling node for logging

Definition at line 162 of file InertialIntegrator.cpp.

◆ addState()

void NAV::InertialIntegrator::addState ( const PosVelAtt & state,
const char * nameId )

Pushes the state to the list of states.

Parameters
[in]stateState to set
[in]nameIdNameId of the calling node for logging

Definition at line 48 of file InertialIntegrator.cpp.

◆ applySensorBiasesIncrements()

void NAV::InertialIntegrator::applySensorBiasesIncrements ( const Eigen::Vector3d & p_deltaBiasAcceleration,
const Eigen::Vector3d & p_deltaBiasAngularRate )

Sets the sensor biases increment.

Parameters
[in]p_deltaBiasAccelerationAcceleration bias increment in platform frame coordinates in [m/s^2]
[in]p_deltaBiasAngularRateAngular rate bias increment in platform frame coordinates in [rad/s]

Definition at line 70 of file InertialIntegrator.cpp.

◆ applyStateErrors_e()

void NAV::InertialIntegrator::applyStateErrors_e ( const Eigen::Vector3d & e_positionError,
const Eigen::Vector3d & e_velocityError,
const Eigen::Vector3d & e_attitudeError_b )

Apply the errors to the latest state.

Parameters
[in]e_positionErrorδr_e The position error in e frame coordinates in [m]
[in]e_velocityErrorδ𝐯_e The velocity error in e frame coordinates in [m/s]
[in]e_attitudeError_bδ𝛙_eb_e The attitude error in e frame coordinates in [rad]

Definition at line 105 of file InertialIntegrator.cpp.

◆ applyStateErrors_n()

void NAV::InertialIntegrator::applyStateErrors_n ( const Eigen::Vector3d & lla_positionError,
const Eigen::Vector3d & n_velocityError,
const Eigen::Vector3d & n_attitudeError_b )

Apply the errors to the latest state.

Parameters
[in]lla_positionErrorδ𝐩_n = [δ𝜙 δλ δ𝘩] The position error (latitude, longitude, altitude) in [rad, rad, m]
[in]n_velocityErrorδ𝐯_n The velocity error in n frame coordinates in [m/s]
[in]n_attitudeError_bδ𝛙_nb_n The attitude error in n frame coordinates in [rad]

Definition at line 76 of file InertialIntegrator.cpp.

◆ areAccelerationsAveragedMeasurements()

bool NAV::InertialIntegrator::areAccelerationsAveragedMeasurements ( ) const
nodiscard

Wether the measurements are accumulated values over the last epoch. (always true when using delta measurements, so GUI has no effect)

Definition at line 143 of file InertialIntegrator.cpp.

◆ calcInertialSolution() [1/2]

template<typename T>
Eigen::Vector< T, 10 > NAV::InertialIntegrator::calcInertialSolution ( const ImuPos & imuPos,
const GenericState< T > & state__t0,
const GenericState< T > & state__t1,
const char * nameId ) const
inlinenodiscard

Calculates the inertial solution going from state__t1 to state__t0 given that measurements are available for both states.

Parameters
imuPosIMU mounting position connecting the platform to the body frame
state__t0State at the epoch to calculate (measurements only)
state__t1State at the previous epoch (state + measurements)
nameIdNameId of the calling node for logging
Returns
Position, velocity and attitude from the integration step

Definition at line 200 of file InertialIntegrator.hpp.

◆ calcInertialSolution() [2/2]

std::shared_ptr< PosVelAtt > NAV::InertialIntegrator::calcInertialSolution ( const InsTime & obsTime,
const Eigen::Vector3d & p_acceleration,
const Eigen::Vector3d & p_angularRate,
const ImuPos & imuPos,
const char * nameId )

Calculates the inertial navigation solution.

Parameters
[in]obsTimeTime of the observation
[in]p_accelerationAcceleration in platform frame coordinates in [m/s^2]
[in]p_angularRateAngular rate in platform frame coordinates in [rad/s]
[in]imuPosIMU platform frame position with respect to body frame
[in]nameIdNameId of the calling node for logging
Returns
The new state at the observation time

Definition at line 237 of file InertialIntegrator.cpp.

◆ calcInertialSolutionDelta()

std::shared_ptr< PosVelAtt > NAV::InertialIntegrator::calcInertialSolutionDelta ( const InsTime & obsTime,
double deltaTime,
const Eigen::Vector3d & p_deltaVelocity,
const Eigen::Vector3d & p_deltaTheta,
const ImuPos & imuPos,
const char * nameId )

Calculates the inertial navigation solution.

Parameters
[in]obsTimeTime of the observation
[in]deltaTimeDelta time over which the deltaVelocity and deltaTheta were measured in [s]
[in]p_deltaVelocityIntegrated acceleration in platform frame coordinates in [m/s]
[in]p_deltaThetaIntegrated angular rate in platform frame coordinates in [rad]
[in]imuPosIMU platform frame position with respect to body frame
[in]nameIdNameId of the calling node for logging
Returns
The new state at the observation time

Definition at line 255 of file InertialIntegrator.cpp.

◆ calcInertialSolutionFromMeasurementBuffer()

std::shared_ptr< PosVelAtt > NAV::InertialIntegrator::calcInertialSolutionFromMeasurementBuffer ( const ImuPos & imuPos,
const char * nameId )
private

Calculates the inertial navigation solution.

Parameters
[in]imuPosIMU platform frame position with respect to body frame
[in]nameIdNameId of the calling node for logging
Returns
The new state at the observation time

Definition at line 273 of file InertialIntegrator.cpp.

◆ getConstants()

const PosVelAttDerivativeConstants & NAV::InertialIntegrator::getConstants ( ) const
nodiscard

Returns the selected compensation models.

Definition at line 138 of file InertialIntegrator.cpp.

◆ getIntegrationFrame()

InertialIntegrator::IntegrationFrame NAV::InertialIntegrator::getIntegrationFrame ( ) const
nodiscard

Returns the selected integration frame.

Definition at line 133 of file InertialIntegrator.cpp.

◆ getLatestState()

std::optional< std::reference_wrapper< const InertialIntegrator::State > > NAV::InertialIntegrator::getLatestState ( ) const
nodiscard

Get the latest state if it exists.

Definition at line 117 of file InertialIntegrator.cpp.

◆ hasInitialPosition()

bool NAV::InertialIntegrator::hasInitialPosition ( ) const
nodiscard

Checks if an initial position is set.

Definition at line 37 of file InertialIntegrator.cpp.

◆ lastStateAsPosVelAtt()

std::shared_ptr< PosVelAtt > NAV::InertialIntegrator::lastStateAsPosVelAtt ( ) const
nodiscardprivate

Returns the last state as PosVelAtt.

Definition at line 218 of file InertialIntegrator.cpp.

◆ p_calcCurrentAcceleration()

std::optional< Eigen::Vector3d > NAV::InertialIntegrator::p_calcCurrentAcceleration ( ) const
nodiscard

Calculate the current acceleration, if measurements area available.

Definition at line 148 of file InertialIntegrator.cpp.

◆ p_calcCurrentAngularRate()

std::optional< Eigen::Vector3d > NAV::InertialIntegrator::p_calcCurrentAngularRate ( ) const
nodiscard

Calculate the current angular rate, if measurements area available.

Definition at line 155 of file InertialIntegrator.cpp.

◆ p_getLastAccelerationBias()

const Eigen::Vector3d & NAV::InertialIntegrator::p_getLastAccelerationBias ( ) const
nodiscard

Return the last acceleration bias in platform frame coordinates in [m/s^2].

Definition at line 123 of file InertialIntegrator.cpp.

◆ p_getLastAngularRateBias()

const Eigen::Vector3d & NAV::InertialIntegrator::p_getLastAngularRateBias ( ) const
nodiscard

Return the last angular rate bias in platform frame coordinates in [rad/s].

Definition at line 128 of file InertialIntegrator.cpp.

◆ reset()

void NAV::InertialIntegrator::reset ( )

Clears all internal data.

Definition at line 29 of file InertialIntegrator.cpp.

◆ setBufferSizes()

void NAV::InertialIntegrator::setBufferSizes ( )
private

Resizes the measurement and state buffers depending on the integration algorithm.

Definition at line 312 of file InertialIntegrator.cpp.

◆ setInitialState()

void NAV::InertialIntegrator::setInitialState ( const PosVelAtt & state,
const char * nameId )

Sets the initial state.

Parameters
[in]stateState to set
[in]nameIdNameId of the calling node for logging

Definition at line 42 of file InertialIntegrator.cpp.

◆ setTotalSensorBiases()

void NAV::InertialIntegrator::setTotalSensorBiases ( const Eigen::Vector3d & p_biasAcceleration,
const Eigen::Vector3d & p_biasAngularRate )

Sets the sensor biases total values.

Parameters
[in]p_biasAccelerationAcceleration bias in platform frame coordinates in [m/s^2]
[in]p_biasAngularRateAngular rate bias in platform frame coordinates in [rad/s]

Definition at line 64 of file InertialIntegrator.cpp.

Friends And Related Symbol Documentation

◆ from_json

void from_json ( const json & j,
InertialIntegrator & data )
friend

Read info from a json object.

Parameters
[in]jJson variable to read info from
[out]dataOutput object

Definition at line 444 of file InertialIntegrator.cpp.

◆ InertialIntegratorGui

bool InertialIntegratorGui ( const char * label,
InertialIntegrator & integrator,
bool & preferAccelerationOverDeltaMeasurements,
float width = 250.0F )
friend

Shows a GUI for advanced configuration of the InertialIntegrator.

Parameters
[in]labelLabel to show. This has to be a unique id for ImGui.
[in]integratorReference to the integrator to configure
[in]preferAccelerationOverDeltaMeasurementsWether to prefer accelerations over delta measurements
[in]widthWidth of the widget

Definition at line 333 of file InertialIntegrator.cpp.

◆ to_json

void to_json ( json & j,
const InertialIntegrator & data )
friend

Write info to a json object.

Parameters
[out]jJson output
[in]dataObject to read info from

Definition at line 433 of file InertialIntegrator.cpp.

◆ to_string [1/2]

const char * to_string ( InertialIntegrator::IntegrationAlgorithm algorithm)
friend

Converts the enum to a string.

Parameters
[in]algorithmEnum value to convert into text
Returns
String representation of the enum

Definition at line 457 of file InertialIntegrator.cpp.

◆ to_string [2/2]

const char * to_string ( InertialIntegrator::IntegrationFrame frame)
friend

Converts the enum to a string.

Parameters
[in]frameEnum value to convert into text
Returns
String representation of the enum

Definition at line 483 of file InertialIntegrator.cpp.

Field Documentation

◆ _accelerationsAreAveragedMeasurements

bool NAV::InertialIntegrator::_accelerationsAreAveragedMeasurements
private

If true, then the measurements are accumulated values over the last epoch. (always true when using delta measurements, so GUI has no effect)

Definition at line 490 of file InertialIntegrator.hpp.

◆ _integrationAlgorithm

IntegrationAlgorithm NAV::InertialIntegrator::_integrationAlgorithm
private

Integration algorithm used for the update.

Definition at line 487 of file InertialIntegrator.hpp.

◆ _integrationFrame

IntegrationFrame NAV::InertialIntegrator::_integrationFrame
private

Frame to integrate the observations in.

Definition at line 481 of file InertialIntegrator.hpp.

◆ _lockIntegrationFrame

bool NAV::InertialIntegrator::_lockIntegrationFrame
private

Wether to lock the integration frame.

Definition at line 484 of file InertialIntegrator.hpp.

◆ _p_lastBiasAcceleration

Eigen::Vector3d NAV::InertialIntegrator::_p_lastBiasAcceleration
private

Initial values for the acceleration bias [m/s^2].

Definition at line 475 of file InertialIntegrator.hpp.

◆ _p_lastBiasAngularRate

Eigen::Vector3d NAV::InertialIntegrator::_p_lastBiasAngularRate
private

Initial values for the angular rate bias [rad/s].

Definition at line 476 of file InertialIntegrator.hpp.

◆ _posVelAttDerivativeConstants

PosVelAttDerivativeConstants NAV::InertialIntegrator::_posVelAttDerivativeConstants
private

Settings for the models to use.

Definition at line 495 of file InertialIntegrator.hpp.

◆ _states

ScrollingBuffer<State> NAV::InertialIntegrator::_states
private

List of states. Length depends on algorithm used.

Definition at line 473 of file InertialIntegrator.hpp.


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