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< PosVelAtt >	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.

std::shared_ptr< PosVelAtt >	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.

const PosVelAttDerivativeConstants &	getConstants () 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< PosVelAtt >	calcInertialSolutionFromMeasurementBuffer (const ImuPos &imuPos, const char *nameId)
	Calculates the inertial navigation solution.

std::shared_ptr< PosVelAtt >	lastStateAsPosVelAtt () 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

using NAV::InertialIntegrator::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

integrationFrame Integration 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]	epoch	Epoch of the measurement
[in]	dt	Time between observation and last state in [s]
[in]	deltaTime	Delta time over which the deltaVelocity and deltaTheta were measured in [s]
[in]	p_deltaVelocity	Integrated acceleration in platform frame coordinates in [m/s]
[in]	p_deltaTheta	Integrated angular rate in platform frame coordinates in [rad]
[in]	nameId	NameId 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]	epoch	Epoch of the measurement
[in]	dt	Time between observation and last state in [s]
[in]	p_acceleration	Acceleration in platform frame coordinates in [m/s^2]
[in]	p_angularRate	Angular rate in platform frame coordinates in [rad/s]
[in]	nameId	NameId 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]	state	State to set
[in]	nameId	NameId 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_deltaBiasAcceleration	Acceleration bias increment in platform frame coordinates in [m/s^2]
[in]	p_deltaBiasAngularRate	Angular 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

imuPos	IMU mounting position connecting the platform to the body frame
state__t0	State at the epoch to calculate (measurements only)
state__t1	State at the previous epoch (state + measurements)
nameId	NameId 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]	obsTime	Time of the observation
[in]	p_acceleration	Acceleration in platform frame coordinates in [m/s^2]
[in]	p_angularRate	Angular rate in platform frame coordinates in [rad/s]
[in]	imuPos	IMU platform frame position with respect to body frame
[in]	nameId	NameId 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]	obsTime	Time of the observation
[in]	deltaTime	Delta time over which the deltaVelocity and deltaTheta were measured in [s]
[in]	p_deltaVelocity	Integrated acceleration in platform frame coordinates in [m/s]
[in]	p_deltaTheta	Integrated angular rate in platform frame coordinates in [rad]
[in]	imuPos	IMU platform frame position with respect to body frame
[in]	nameId	NameId 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]	imuPos	IMU platform frame position with respect to body frame
[in]	nameId	NameId 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]	state	State to set
[in]	nameId	NameId 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_biasAcceleration	Acceleration bias in platform frame coordinates in [m/s^2]
[in]	p_biasAngularRate	Angular 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]	j	Json variable to read info from
[out]	data	Output 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]	label	Label to show. This has to be a unique id for ImGui.
[in]	integrator	Reference to the integrator to configure
[in]	preferAccelerationOverDeltaMeasurements	Wether to prefer accelerations over delta measurements
[in]	width	Width 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]	j	Json output
[in]	data	Object 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] algorithm Enum 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] frame Enum 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:

/home/runner/work/INSTINCT/INSTINCT/src/Navigation/INS/InertialIntegrator.hpp
/home/runner/work/INSTINCT/INSTINCT/src/Navigation/INS/InertialIntegrator.cpp

Data Structures

Public Types

Public Member Functions

Private Member Functions

Private Attributes

Friends

Detailed Description

Member Typedef Documentation

◆ State

Member Enumeration Documentation

◆ IntegrationAlgorithm

◆ IntegrationFrame

Constructor & Destructor Documentation

◆ InertialIntegrator() [1/2]

◆ InertialIntegrator() [2/2]

Member Function Documentation

◆ addDeltaMeasurement()

◆ addMeasurement()

◆ addState()

◆ applySensorBiasesIncrements()

◆ applyStateErrors_e()

◆ applyStateErrors_n()

◆ areAccelerationsAveragedMeasurements()

◆ calcInertialSolution() [1/2]

◆ calcInertialSolution() [2/2]

◆ calcInertialSolutionDelta()

◆ calcInertialSolutionFromMeasurementBuffer()

◆ getConstants()

◆ getIntegrationFrame()

◆ getLatestState()

◆ hasInitialPosition()

◆ lastStateAsPosVelAtt()

◆ p_calcCurrentAcceleration()

◆ p_calcCurrentAngularRate()

◆ p_getLastAccelerationBias()

◆ p_getLastAngularRateBias()

◆ reset()

◆ setBufferSizes()

◆ setInitialState()

◆ setTotalSensorBiases()

Friends And Related Symbol Documentation

◆ from_json

◆ InertialIntegratorGui

◆ to_json

◆ to_string [1/2]

◆ to_string [2/2]

Field Documentation

◆ _accelerationsAreAveragedMeasurements

◆ _integrationAlgorithm

◆ _integrationFrame

◆ _lockIntegrationFrame

◆ _p_lastBiasAcceleration

◆ _p_lastBiasAngularRate

◆ _posVelAttDerivativeConstants

◆ _states