The core class of the EKF Does propagation of state and covariance but also applying measurements and managing states and measurements in lists sorted by time stamp.

#include <msf_core.h>

Public Types
enum	{ nErrorStatesAtCompileTime = EKFState_T::nErrorStatesAtCompileTime, nStatesAtCompileTime = EKFState_T::nStatesAtCompileTime }
typedef EKFState_T::StateDefinition_T	StateDefinition_T
typedef EKFState_T::StateSequence_T	StateSequence_T
typedef Eigen::Matrix< double, nErrorStatesAtCompileTime, 1 >	ErrorState
	The error state type.
typedef Eigen::Matrix< double, nErrorStatesAtCompileTime, nErrorStatesAtCompileTime >	ErrorStateCov
	The error state covariance type.
typedef msf_core::SortedContainer < EKFState_T >	StateBuffer_T
	The type of the state buffer containing all the states.
typedef msf_core::SortedContainer < typename msf_core::MSF_MeasurementBase < EKFState_T >, typename msf_core::MSF_InvalidMeasurement < EKFState_T > >	measurementBufferT
	The type of the measurement buffer containing all the measurements.

Public Member Functions
void	addMeasurement (shared_ptr< MSF_MeasurementBase< EKFState_T > > measurement)
	Add a sensor measurement or an init measurement to the internal queue and apply it to the state.
void	init (shared_ptr< MSF_MeasurementBase< EKFState_T > > measurement)
	Initializes the filter with the values of the given measurement, other init values from other sensors can be passed in as "measurement" using the initMeasurement structs.
shared_ptr< EKFState_T >	getClosestState (double tstamp)
	Finds the closest state to the requested time in the internal state.
void	getAccumF_SC (const shared_ptr< EKFState_T > &state_old, const shared_ptr< EKFState_T > &state_new, Eigen::Matrix< double, nErrorStatesAtCompileTime, nErrorStatesAtCompileTime > &F)
	Returns the accumulated dynamic matrix between two states.
shared_ptr < msf_core::MSF_MeasurementBase < EKFState_T > >	getPreviousMeasurement (double time, int sensorID)
	Returns previous measurement of the same type.
shared_ptr< EKFState_T >	getStateAtTime (double tstamp)
	Finds the state at the requested time in the internal state.
void	predictProcessCovariance (shared_ptr< EKFState_T > &state_old, shared_ptr< EKFState_T > &state_new)
	Propagates the error state covariance.
void	propagateState (shared_ptr< EKFState_T > &state_old, shared_ptr< EKFState_T > &state_new)
	Propagates the state with given dt.
void	CleanUpBuffers ()
	Delete very old states and measurements from the buffers to free memory.
void	setPCore (Eigen::Matrix< double, EKFState_T::nErrorStatesAtCompileTime, EKFState_T::nErrorStatesAtCompileTime > &P)
	sets the covariance matrix of the core states to simulated values.
	MSF_Core (const MSF_SensorManager< EKFState_T > &usercalc)
	Ctor takes a pointer to an object which does the user defined calculations and provides interfaces for initialization etc.
	~MSF_Core ()
const MSF_SensorManager < EKFState_T > &	usercalc () const

Public Attributes
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

Private Types
enum	{ indexOfStateWithoutTemporalDrift }
	Get the index of the best state having no temporal drift at compile time. More...
typedef msf_tmp::getEnumStateType < StateSequence_T, indexOfStateWithoutTemporalDrift > ::value	nonDriftingStateType
	Returns void type for invalid types.

Private Member Functions
bool	applyCorrection (shared_ptr< EKFState_T > &delaystate, ErrorState &correction, double fuzzythres=0.1)
	Applies the correction.
void	propPToState (shared_ptr< EKFState_T > &state)
	Propagate covariance to a given state in time.
void	process_imu (const msf_core::Vector3 &linear_acceleration, const msf_core::Vector3 &angular_velocity, const double &msg_stamp, size_t msg_seq)
	This function gets called on incoming imu messages and then performs the state prediction internally. Only use this OR stateCallback by remapping the topics accordingly.
void	process_extstate (const msf_core::Vector3 &linear_acceleration, const msf_core::Vector3 &angular_velocity, const msf_core::Vector3 &p, const msf_core::Vector3 &v, const msf_core::Quaternion &q, bool is_already_propagated, const double &msg_stamp, size_t msg_seq)
	External state propagation:
void	propagatePOneStep ()
	Propagates P by one step to distribute processing load.
void	handlePendingMeasurements ()
	Checks the queue of measurements to be applied in the future.

Private Attributes
StateBuffer_T	stateBuffer_
	EKF buffer containing pretty much all info needed at time t. Sorted by t.
measurementBufferT	MeasurementBuffer_
	EKF Measurements and init values sorted by t asc.
std::queue< shared_ptr < MSF_MeasurementBase < EKFState_T > > >	queueFutureMeasurements_
	Buffer for measurements to apply in future.
double	time_P_propagated
	Last time stamp where we have a valid propagation.
StateBuffer_T::iterator_T	it_last_IMU
	Last time stamp where we have a valid state.
Eigen::Matrix< double, 3, 1 >	g_
	Gravity vector.
bool	initialized_
	Is the filter initialized, so that we can propagate the state?
bool	predictionMade_
	Is there a state prediction, so we can apply measurements?
bool	isfuzzyState_
	Was the filter pushed to fuzzy state by a measurement?
CheckFuzzyTracking< EKFState_T, nonDriftingStateType >	fuzzyTracker_
	Watch dog to determine fuzzy tracking by observing non temporal drifting.
const MSF_SensorManager < EKFState_T > &	usercalc_
	A class which provides methods for customization of several calculations.

Friends
class	MSF_MeasurementBase< EKFState_T >
class	IMUHandler< EKFState_T >

Member Typedef Documentation

template<typename EKFState_T>

typedef Eigen::Matrix<double, nErrorStatesAtCompileTime, 1> msf_core::MSF_Core< EKFState_T >::ErrorState

template<typename EKFState_T>

typedef Eigen::Matrix<double, nErrorStatesAtCompileTime, nErrorStatesAtCompileTime> msf_core::MSF_Core< EKFState_T >::ErrorStateCov

template<typename EKFState_T>

typedef msf_core::SortedContainer< typename msf_core::MSF_MeasurementBase<EKFState_T>, typename msf_core::MSF_InvalidMeasurement<EKFState_T> > msf_core::MSF_Core< EKFState_T >::measurementBufferT

template<typename EKFState_T>

typedef msf_tmp::getEnumStateType<StateSequence_T, indexOfStateWithoutTemporalDrift>::value msf_core::MSF_Core< EKFState_T >::nonDriftingStateType

private

template<typename EKFState_T>

typedef msf_core::SortedContainer<EKFState_T> msf_core::MSF_Core< EKFState_T >::StateBuffer_T

template<typename EKFState_T>

typedef EKFState_T::StateDefinition_T msf_core::MSF_Core< EKFState_T >::StateDefinition_T

template<typename EKFState_T>

typedef EKFState_T::StateSequence_T msf_core::MSF_Core< EKFState_T >::StateSequence_T

Member Enumeration Documentation

template<typename EKFState_T>

anonymous enum

Enumerator:

nErrorStatesAtCompileTime	Error state length.
nStatesAtCompileTime	Complete state length.

template<typename EKFState_T>

anonymous enum

private

Enumerator:

indexOfStateWithoutTemporalDrift

Constructor & Destructor Documentation

template<typename EKFState_T >

msf_core::MSF_Core< EKFState_T >::MSF_Core ( const MSF_SensorManager< EKFState_T > & usercalc )

Parameters

usercalc The class providing the user defined calculations DO ABSOLUTELY NOT USE THIS REFERENCE INSIDE THIS CTOR!!

template<typename EKFState_T >

msf_core::MSF_Core< EKFState_T >::~MSF_Core ( )

Member Function Documentation

template<typename EKFState_T >

void msf_core::MSF_Core< EKFState_T >::addMeasurement ( shared_ptr< MSF_MeasurementBase< EKFState_T > > measurement )

Parameters

Measurement the measurement to add to the internal measurement queue.

template<typename EKFState_T >

bool msf_core::MSF_Core< EKFState_T >::applyCorrection	(	shared_ptr< EKFState_T > &	delaystate,
		ErrorState &	correction,
		double	fuzzythres = `0.1`
	)

private

Parameters

delaystate	The state to apply the correction on.
correction	The correction vector.
fuzzythres	The error of the non temporal drifting state allowed before fuzzy tracking will be triggered.

template<typename EKFState_T >

void msf_core::MSF_Core< EKFState_T >::CleanUpBuffers ( )

template<typename EKFState_T>

void msf_core::MSF_Core< EKFState_T >::getAccumF_SC	(	const shared_ptr< EKFState_T > &	state_old,
		const shared_ptr< EKFState_T > &	state_new,
		Eigen::Matrix< double, nErrorStatesAtCompileTime, nErrorStatesAtCompileTime > &	F
	)

template<typename EKFState_T >

shared_ptr< EKFState_T > msf_core::MSF_Core< EKFState_T >::getClosestState ( double tstamp )

Parameters

tstamp The time stamp to find the closest state to.

template<typename EKFState_T >

shared_ptr< msf_core::MSF_MeasurementBase< EKFState_T > > msf_core::MSF_Core< EKFState_T >::getPreviousMeasurement	(	double	time,
		int	sensorID
	)

template<typename EKFState_T >

shared_ptr< EKFState_T > msf_core::MSF_Core< EKFState_T >::getStateAtTime ( double tstamp )

Parameters

tstamp The time stamp to find the state to.

template<typename EKFState_T >

void msf_core::MSF_Core< EKFState_T >::handlePendingMeasurements ( )

private

template<typename EKFState_T >

void msf_core::MSF_Core< EKFState_T >::init ( shared_ptr< MSF_MeasurementBase< EKFState_T > > measurement )

Parameters

Measurement a measurement containing initial values for the state

template<typename EKFState_T >

void msf_core::MSF_Core< EKFState_T >::predictProcessCovariance	(	shared_ptr< EKFState_T > &	state_old,
		shared_ptr< EKFState_T > &	state_new
	)

Parameters

state_old	The state to propagate the covariance from.
state_new	The state to propagate the covariance to.

template<typename EKFState_T >

void msf_core::MSF_Core< EKFState_T >::process_extstate	(	const msf_core::Vector3 &	linear_acceleration,
		const msf_core::Vector3 &	angular_velocity,
		const msf_core::Vector3 &	p,
		const msf_core::Vector3 &	v,
		const msf_core::Quaternion &	q,
		bool	is_already_propagated,
		const double &	msg_stamp,
		size_t	msg_seq
	)

private

This function gets called when state prediction is performed externally, e.g. by asctec_mav_framework. Msg has to be the latest predicted state. Only use this OR imuCallback by remapping the topics accordingly.

Parameters

msg	The state message from the external propagation.

See Also: {imuCallback}

template<typename EKFState_T >

void msf_core::MSF_Core< EKFState_T >::process_imu	(	const msf_core::Vector3 &	linear_acceleration,
		const msf_core::Vector3 &	angular_velocity,
		const double &	msg_stamp,
		size_t	msg_seq
	)

private

Parameters

msg	The imu ros message.

See Also: {stateCallback}

template<typename EKFState_T >

void msf_core::MSF_Core< EKFState_T >::propagatePOneStep ( )

private

template<typename EKFState_T >

void msf_core::MSF_Core< EKFState_T >::propagateState	(	shared_ptr< EKFState_T > &	state_old,
		shared_ptr< EKFState_T > &	state_new
	)

Parameters

state_old	The state to propagate from.
state_new	The state to propagate to.

template<typename EKFState_T >

void msf_core::MSF_Core< EKFState_T >::propPToState ( shared_ptr< EKFState_T > & state )

private

Parameters

State the state to propagate to from the last propagated time.

template<typename EKFState_T >

void msf_core::MSF_Core< EKFState_T >::setPCore ( Eigen::Matrix< double, EKFState_T::nErrorStatesAtCompileTime, EKFState_T::nErrorStatesAtCompileTime > & P )

Parameters

P	the error state covariance Matrix to fill.

template<typename EKFState_T >

const MSF_SensorManager< EKFState_T > & msf_core::MSF_Core< EKFState_T >::usercalc ( ) const

Friends And Related Function Documentation

template<typename EKFState_T>

friend class IMUHandler< EKFState_T >

friend

template<typename EKFState_T>

friend class MSF_MeasurementBase< EKFState_T >

friend

Member Data Documentation

template<typename EKFState_T>

msf_core::MSF_Core< EKFState_T >::EIGEN_MAKE_ALIGNED_OPERATOR_NEW

template<typename EKFState_T>

CheckFuzzyTracking<EKFState_T, nonDriftingStateType> msf_core::MSF_Core< EKFState_T >::fuzzyTracker_

private

template<typename EKFState_T>

Eigen::Matrix<double, 3, 1> msf_core::MSF_Core< EKFState_T >::g_

private

template<typename EKFState_T>

bool msf_core::MSF_Core< EKFState_T >::initialized_

private

template<typename EKFState_T>

bool msf_core::MSF_Core< EKFState_T >::isfuzzyState_

private

template<typename EKFState_T>

StateBuffer_T::iterator_T msf_core::MSF_Core< EKFState_T >::it_last_IMU

private

template<typename EKFState_T>

measurementBufferT msf_core::MSF_Core< EKFState_T >::MeasurementBuffer_

private

template<typename EKFState_T>

bool msf_core::MSF_Core< EKFState_T >::predictionMade_

private

template<typename EKFState_T>

std::queue<shared_ptr<MSF_MeasurementBase<EKFState_T> > > msf_core::MSF_Core< EKFState_T >::queueFutureMeasurements_

private

template<typename EKFState_T>

StateBuffer_T msf_core::MSF_Core< EKFState_T >::stateBuffer_

private

template<typename EKFState_T>

double msf_core::MSF_Core< EKFState_T >::time_P_propagated

private

template<typename EKFState_T>

const MSF_SensorManager<EKFState_T>& msf_core::MSF_Core< EKFState_T >::usercalc_

private

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

msf_core/include/msf_core/msf_core.h
msf_core/include/msf_core/implementation/msf_core.hpp

Public Types

Public Member Functions

Public Attributes

Private Types

Private Member Functions

Private Attributes

Friends

Member Typedef Documentation

Member Enumeration Documentation

Constructor & Destructor Documentation

Member Function Documentation

Friends And Related Function Documentation

Member Data Documentation