The CoreLogic class represents the high-level logic for the operation of the filter. More...

#include </home/runner/work/mars_lib/mars_lib/source/mars/include/mars/core_logic.h>

Collaboration diagram for mars::CoreLogic:

Public Member Functions
	CoreLogic (std::shared_ptr< CoreState > core_states)
	CoreLogic. More...

	CoreLogic ()=default

int	Initialize (const Eigen::Vector3d &p_wi_init, const Eigen::Quaterniond &q_wi_init)
	Initialize the filter with information available in the prior init buffer. More...

CoreStateMatrix	GenerateStateTransitionBlock (const int &first_transition_idx, const int &last_transition_idx)
	GenerateStateTransitionBlock Returns the state transition block between 'first_transition_idx' and 'last_transition_idx'. More...

Eigen::MatrixXd	PropagateSensorCrossCov (const Eigen::MatrixXd &sensor_cov, const CoreStateMatrix &core_cov, const CoreStateMatrix &state_transition)
	PropagateSensorCrossCov. More...

bool	PerformSensorUpdate (std::shared_ptr< SensorAbsClass > sensor, const Time &timestamp, BufferEntryType *sensor_data)
	PerformSensorUpdate Returns new state with corrected state and updated covariance. More...

bool	PerformSensorUpdate (std::shared_ptr< SensorAbsClass > sensor, const Time &timestamp, BufferEntryType sensor_data, bool added_interm_state)

void	PerformCoreStatePropagation (std::shared_ptr< SensorAbsClass > sensor, const Time &timestamp, const BufferEntryType &prior_state_entry, BufferEntryType *sensor_entry)
	PerformCoreStatePropagation Propagates the core state and returns the new state entry. More...

void	ReworkBufferStartingAtIndex (const int &index)
	ReworkBufferStartingAtIndex Reprocesses the buffer after an out of order update, starting at given 'idx'. More...

bool	ProcessMeasurement (std::shared_ptr< SensorAbsClass > sensor, const Time &timestamp, const BufferDataType &data)
	ProcessMeasurement Processes the sensor input. More...

Public Attributes
EIGEN_MAKE_ALIGNED_OPERATOR_NEW std::shared_ptr< CoreState >	core_states_ { nullptr }

Buffer	buffer_
	Holds a pointer to the core_states. More...

Buffer	buffer_prior_core_init_
	Main buffer of the filter. More...

SensorManager	sensor_manager_
	Buffer that holds measurements prior initialization. More...

bool	core_is_initialized_ { false }

bool	core_init_warn_once_ { false }

bool	verbose_ { false }

bool	verbose_out_of_order_ { false }
	Increased output of information. More...

bool	discard_ooo_prop_meas_ { false }
	Increased output of information for delayed measurements. More...

bool	add_interm_buffer_entries_ { false }
	Discard out of order propagation sensor measurements. More...

Detailed Description

The CoreLogic class represents the high-level logic for the operation of the filter.

Constructor & Destructor Documentation

◆ CoreLogic() [1/2]

mars::CoreLogic::CoreLogic ( std::shared_ptr< CoreState > core_states )

CoreLogic.

Determines if intermediate entries before a sensor update are stored to the buffer

Parameters

core_states Core state type used for updates and propagation

◆ CoreLogic() [2/2]

mars::CoreLogic::CoreLogic ( )

default

Member Function Documentation

◆ Initialize()

int mars::CoreLogic::Initialize	(	const Eigen::Vector3d &	p_wi_init,
		const Eigen::Quaterniond &	q_wi_init
	)

Initialize the filter with information available in the prior init buffer.

Uses the latest propagation sensor to initialize the core state and writes the init state to the main buffer.

Note: At leased one IMU measurement must exist

◆ GenerateStateTransitionBlock()

CoreStateMatrix mars::CoreLogic::GenerateStateTransitionBlock	(	const int &	first_transition_idx,
		const int &	last_transition_idx
	)

GenerateStateTransitionBlock Returns the state transition block between 'first_transition_idx' and 'last_transition_idx'.

◆ PropagateSensorCrossCov()

Eigen::MatrixXd mars::CoreLogic::PropagateSensorCrossCov	(	const Eigen::MatrixXd &	sensor_cov,
		const CoreStateMatrix &	core_cov,
		const CoreStateMatrix &	state_transition
	)

PropagateSensorCrossCov.

◆ PerformSensorUpdate() [1/2]

bool mars::CoreLogic::PerformSensorUpdate	(	std::shared_ptr< SensorAbsClass >	sensor,
		const Time &	timestamp,
		BufferEntryType *	sensor_data
	)

PerformSensorUpdate Returns new state with corrected state and updated covariance.

◆ PerformSensorUpdate() [2/2]

bool mars::CoreLogic::PerformSensorUpdate	(	std::shared_ptr< SensorAbsClass >	sensor,
		const Time &	timestamp,
		BufferEntryType *	sensor_data,
		bool *	added_interm_state
	)

◆ PerformCoreStatePropagation()

void mars::CoreLogic::PerformCoreStatePropagation	(	std::shared_ptr< SensorAbsClass >	sensor,
		const Time &	timestamp,
		const BufferEntryType &	prior_state_entry,
		BufferEntryType *	sensor_entry
	)

PerformCoreStatePropagation Propagates the core state and returns the new state entry.

We know that the current sensor is the input for the propagation. All that is needed is the past core state and the input for the system determined by the propagation sensor handle. The core_state propagation function needs to be able to handle the data structure of the propagation sensor.

◆ ReworkBufferStartingAtIndex()

void mars::CoreLogic::ReworkBufferStartingAtIndex ( const int & index )

ReworkBufferStartingAtIndex Reprocesses the buffer after an out of order update, starting at given 'idx'.

Note: It is important that the max buffer size is only enforced after all states of the current repropagation are handles. This ensures that the buffer index is valid during the repropagation. Overflowing old entries are removed after the repropagation.; The update segment of this method is similar to ProcessMeasurement. However, this method handles buffer indexes directly and knows exactly where to insert a new state. Thus, this methode can use 'insert state at idx' and not 'add state sorted' which increases performance.

◆ ProcessMeasurement()

bool mars::CoreLogic::ProcessMeasurement	(	std::shared_ptr< SensorAbsClass >	sensor,
		const Time &	timestamp,
		const BufferDataType &	data
	)

ProcessMeasurement Processes the sensor input.

Parameters

sensor	Pointer to the sensor instance associated with the sensor data
timestamp	Timestamp associated with the sensor data
data	Sensor data to process

This is the primary function that controls the sensor updates. The function determines filter operations based on the sensor type (propagation or regular sensor).

The function generates a measurement handle decides if the measurement should be used or if it is too old. A check for out of order measurements is performed, and the measurement is either processed depending on the sensor handle either as a propagation sensor or a regular sensor. If the measurement was out of order; the buffer is reprocessed starting at the index of the out of order measurement.

Returns: True if the processing of the measurement was successful

Member Data Documentation

◆ core_states_

EIGEN_MAKE_ALIGNED_OPERATOR_NEW std::shared_ptr<CoreState> mars::CoreLogic::core_states_ { nullptr }

◆ buffer_

Buffer mars::CoreLogic::buffer_

Holds a pointer to the core_states.

◆ buffer_prior_core_init_

Buffer mars::CoreLogic::buffer_prior_core_init_

Main buffer of the filter.

◆ sensor_manager_

SensorManager mars::CoreLogic::sensor_manager_

Buffer that holds measurements prior initialization.

◆ core_is_initialized_

bool mars::CoreLogic::core_is_initialized_ { false }

◆ core_init_warn_once_

bool mars::CoreLogic::core_init_warn_once_ { false }

core_is_initialized_ = true if the core state was initialized, false otherwise

◆ verbose_

bool mars::CoreLogic::verbose_ { false }

◆ verbose_out_of_order_

bool mars::CoreLogic::verbose_out_of_order_ { false }

Increased output of information.

◆ discard_ooo_prop_meas_

bool mars::CoreLogic::discard_ooo_prop_meas_ { false }

Increased output of information for delayed measurements.

◆ add_interm_buffer_entries_

bool mars::CoreLogic::add_interm_buffer_entries_ { false }

Discard out of order propagation sensor measurements.

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

core_logic.h

Public Member Functions

Public Attributes