msf_core::GenericState_T< StateSeq_T, StateDef_T > Struct Template Reference

The state vector containing all the state variables for this EKF configuration.

#include <msf_state.h>

Public Types
enum	{   nStateVarsAtCompileTime, nErrorStatesAtCompileTime, nStatesAtCompileTime, nCoreStatesAtCompileTime,   nPropagatedCoreStatesAtCompileTime, nPropagatedCoreErrorStatesAtCompileTime }
typedef StateSeq_T	StateSequence_T
	The state vector defining the state variables of this EKF.
typedef StateDef_T	StateDefinition_T
typedef Eigen::Matrix< double, nErrorStatesAtCompileTime, nErrorStatesAtCompileTime >	P_type
	Type of the error state.
typedef P_type	F_type
typedef P_type	Q_type

Public Member Functions
	GenericState_T ()
void	correct (const Eigen::Matrix< double, nErrorStatesAtCompileTime, 1 > &correction)
	Apply the correction vector to all state vars.
template<int INDEX>
msf_tmp::StripReference < typename boost::fusion::result_of::at_c < StateSequence_T, INDEX > ::type >::result_t::Q_T &	getQBlock ()
	Returns the Q-block of the state at position INDEX in the state list, not allowed for core states.
template<int INDEX>
const msf_tmp::StripReference < typename boost::fusion::result_of::at_c < StateSequence_T, INDEX > ::type >::result_t::Q_T &	getQBlock () const
	Returns the Q-block of the state at position INDEX in the state list, also possible for core states, since const.
void	reset (msf_core::StateVisitor< GenericState_T< StateSequence_T, StateDefinition_T > > *usercalc=nullptr)
	Reset the state 3D vectors: 0; quaternion: unit quaternion; scale: 1; time:0; Error covariance: zeros.
void	getPoseCovariance (geometry_msgs::PoseWithCovariance::_covariance_type &cov)
	Write the covariance corresponding to position and attitude to cov.
void	toPoseMsg (geometry_msgs::PoseWithCovarianceStamped &pose)
	Assembles a PoseWithCovarianceStamped message from the state.
void	toExtStateMsg (sensor_fusion_comm::ExtState &state)
	Assemble an ExtState message from the state.
void	toFullStateMsg (sensor_fusion_comm::DoubleArrayStamped &state)
	Assembles a DoubleArrayStamped message from the state.
void	toCoreStateMsg (sensor_fusion_comm::DoubleArrayStamped &state)
	Assembles a DoubleArrayStamped message from the state.
Eigen::Matrix< double, nCoreStatesAtCompileTime, 1 >	toEigenVector ()
	Returns all values as an eigen vector.
void	calculateIndicesInErrorState (std::vector< std::tuple< int, int, int > > &vec)
std::string	print ()
	Returns a string describing the state.
bool	checkStateForNumeric ()
	Returns whether the state is sane. No NaN no inf.
template<int INDEX>
const msf_tmp::StripReference < typename boost::fusion::result_of::at_c < StateSequence_T, INDEX > ::type >::result_t::value_t &	get () const
	Returns the state at position INDEX in the state list, const version.
template<int INDEX>
msf_tmp::AddConstReference < typename boost::fusion::result_of::at_c < StateSequence_T, INDEX > ::type >::result_t	getStateVar () const
	Returns the stateVar at position INDEX in the state list, const version.
template<int INDEX>
void	set (const typename msf_tmp::StripConstReference< typename boost::fusion::result_of::at_c< StateSequence_T, INDEX >::type >::result_t::value_t &newvalue)
	Sets state at position INDEX in the state list, fails for core states at compile time.
template<int INDEX>
void	clearCrossCov ()
template<int INDEX>
void	set (const typename msf_tmp::StripConstReference< typename boost::fusion::result_of::at_c< stateVector_T, INDEX >::type >::result_t::value_t &newvalue)

Public Attributes
StateSequence_T	statevars
	The actual state variables.
Eigen::Matrix< double, 3, 1 >	w_m
	Angular velocity from IMU.
Eigen::Matrix< double, 3, 1 >	a_m
	Linear acceleration from IMU.
double	time
	Time of this state estimate.
P_type	P
	Error state covariance.
F_type	Fd
	Discrete state propagation matrix.
Q_type	Qd
	Discrete propagation noise matrix.

Private Member Functions
template<int INDEX>
boost::fusion::result_of::at_c < StateSequence_T, INDEX > ::type	getStateVar ()
	Returns the stateVar at position INDEX in the state list. Non const version only for msf_core use. You must not make these functions public. Instead const_cast the state object to const to use the overload.
template<int INDEX>
msf_tmp::StripReference < typename boost::fusion::result_of::at_c < StateSequence_T, INDEX > ::type >::result_t::value_t &	get ()
	Returns the state at position INDEX in the state list, non const version. You must not make these functions public. Instead const_cast the state object to const to use the overload.

Friends
class	msf_core::MSF_Core< GenericState_T< StateSequence_T, StateDefinition_T > >
struct	msf_core::copyNonPropagationStates< GenericState_T >
class	msf_core::MSF_InitMeasurement< GenericState_T< StateSequence_T, StateDefinition_T > >

Member Typedef Documentation

template<typename StateSeq_T, typename StateDef_T>

typedef P_type msf_core::GenericState_T< StateSeq_T, StateDef_T >::F_type

template<typename StateSeq_T, typename StateDef_T>

typedef Eigen::Matrix<double, nErrorStatesAtCompileTime, nErrorStatesAtCompileTime> msf_core::GenericState_T< StateSeq_T, StateDef_T >::P_type

template<typename StateSeq_T, typename StateDef_T>

typedef P_type msf_core::GenericState_T< StateSeq_T, StateDef_T >::Q_type

template<typename StateSeq_T, typename StateDef_T>

typedef StateDef_T msf_core::GenericState_T< StateSeq_T, StateDef_T >::StateDefinition_T

template<typename StateSeq_T, typename StateDef_T>

typedef StateSeq_T msf_core::GenericState_T< StateSeq_T, StateDef_T >::StateSequence_T

The enums of the state variables.

Member Enumeration Documentation

template<typename StateSeq_T, typename StateDef_T>

anonymous enum

Enumerator:

nStateVarsAtCompileTime	N state vars.
nErrorStatesAtCompileTime	N error states.
nStatesAtCompileTime	N total states.
nCoreStatesAtCompileTime	N total core states.
nPropagatedCoreStatesAtCompileTime	N total core states with propagation.
nPropagatedCoreErrorStatesAtCompileTime	N total error states with propagation.

Constructor & Destructor Documentation

template<typename StateSeq_T, typename StateDef_T>

msf_core::GenericState_T< StateSeq_T, StateDef_T >::GenericState_T ( )

inline

Member Function Documentation

template<typename stateVector_T , typename StateDefinition_T >

void msf_core::GenericState_T< stateVector_T, StateDefinition_T >::calculateIndicesInErrorState

(

std::vector< std::tuple< int, int, int > > &

vec

)

Returns a vector of int pairs with enum index in errorstate and numblocks.

template<typename stateVector_T , typename StateDefinition_T >

bool msf_core::GenericState_T< stateVector_T, StateDefinition_T >::checkStateForNumeric

(

)

template<typename stateVector_T , typename StateDefinition_T >

template<int INDEX>

void msf_core::GenericState_T< stateVector_T, StateDefinition_T >::clearCrossCov ( )

inline

template<typename stateVector_T , typename StateDefinition_T >

void msf_core::GenericState_T< stateVector_T, StateDefinition_T >::correct ( const Eigen::Matrix< double, nErrorStatesAtCompileTime, 1 > &  correction )

inline

template<typename stateVector_T , typename StateDefinition_T >

template<int INDEX>

msf_tmp::StripReference< typename boost::fusion::result_of::at_c< stateVector_T, INDEX >::type >::result_t::value_t & msf_core::GenericState_T< stateVector_T, StateDefinition_T >::get ( )

inlineprivate

template<typename stateVector_T , typename StateDefinition_T >

template<int INDEX>

const msf_tmp::StripReference< typename boost::fusion::result_of::at_c< stateVector_T, INDEX >::type >::result_t::value_t & msf_core::GenericState_T< stateVector_T, StateDefinition_T >::get ( ) const

inline

template<typename stateVector_T , typename StateDefinition_T >

void msf_core::GenericState_T< stateVector_T, StateDefinition_T >::getPoseCovariance

(

geometry_msgs::PoseWithCovariance::_covariance_type &

cov

)

Writes the covariance corresponding to position and attitude to cov.

template<typename stateVector_T , typename StateDefinition_T >

template<int INDEX>

msf_tmp::StripReference< typename boost::fusion::result_of::at_c< stateVector_T, INDEX >::type >::result_t::Q_T & msf_core::GenericState_T< stateVector_T, StateDefinition_T >::getQBlock ( )

inline

template<typename stateVector_T , typename StateDefinition_T >

template<int INDEX>

const msf_tmp::StripReference< typename boost::fusion::result_of::at_c< stateVector_T, INDEX >::type >::result_t::Q_T & msf_core::GenericState_T< stateVector_T, StateDefinition_T >::getQBlock ( ) const

inline

template<typename stateVector_T , typename StateDefinition_T >

template<int INDEX>

boost::fusion::result_of::at_c< stateVector_T, INDEX >::type msf_core::GenericState_T< stateVector_T, StateDefinition_T >::getStateVar ( )

inlineprivate

template<typename stateVector_T , typename StateDefinition_T >

template<int INDEX>

msf_tmp::AddConstReference< typename boost::fusion::result_of::at_c< stateVector_T, INDEX >::type >::result_t msf_core::GenericState_T< stateVector_T, StateDefinition_T >::getStateVar ( ) const

inline

template<typename stateVector_T , typename StateDefinition_T >

std::string msf_core::GenericState_T< stateVector_T, StateDefinition_T >::print

(

)

template<typename StateSeq_T, typename StateDef_T>

void msf_core::GenericState_T< stateVector_T, StateDefinition_T >::reset

(

msf_core::StateVisitor< GenericState_T< StateSequence_T, StateDefinition_T > > *

usercalc = nullptr

)

Resets the state.

3D vectors: 0; quaternion: unit quaternion; scale: 1; time:0; Error covariance: zeros

template<typename StateSeq_T, typename StateDef_T>

template<int INDEX>

void msf_core::GenericState_T< StateSeq_T, StateDef_T >::set ( const typename msf_tmp::StripConstReference< typename boost::fusion::result_of::at_c< stateVector_T, INDEX >::type >::result_t::value_t &  newvalue )

inline

template<typename StateSeq_T, typename StateDef_T>

template<int INDEX>

void msf_core::GenericState_T< StateSeq_T, StateDef_T >::set ( const typename msf_tmp::StripConstReference< typename boost::fusion::result_of::at_c< StateSequence_T, INDEX >::type >::result_t::value_t &  newvalue )

inline

template<typename stateVector_T , typename StateDefinition_T >

void msf_core::GenericState_T< stateVector_T, StateDefinition_T >::toCoreStateMsg

(

sensor_fusion_comm::DoubleArrayStamped &

state

)

Note

It does not set the header

it does not set the header

template<typename stateVector_T , typename StateDefinition_T >

Eigen::Matrix< double, GenericState_T< stateVector_T, StateDefinition_T >::nCoreStatesAtCompileTime, 1 > msf_core::GenericState_T< stateVector_T, StateDefinition_T >::toEigenVector

(

)

template<typename stateVector_T , typename StateDefinition_T >

void msf_core::GenericState_T< stateVector_T, StateDefinition_T >::toExtStateMsg

(

sensor_fusion_comm::ExtState &

state

)

Assembles an ExtState message from the state.

Note

It does not set the header.

it does not set the header

template<typename stateVector_T , typename StateDefinition_T >

void msf_core::GenericState_T< stateVector_T, StateDefinition_T >::toFullStateMsg

(

sensor_fusion_comm::DoubleArrayStamped &

state

)

it does not set the header

template<typename stateVector_T , typename StateDefinition_T >

void msf_core::GenericState_T< stateVector_T, StateDefinition_T >::toPoseMsg

(

geometry_msgs::PoseWithCovarianceStamped &

pose

)

Note

It does not set the header.

it does not set the header

Friends And Related Function Documentation

template<typename StateSeq_T, typename StateDef_T>

friend struct msf_core::copyNonPropagationStates< GenericState_T >

friend

template<typename StateSeq_T, typename StateDef_T>

friend class msf_core::MSF_Core< GenericState_T< StateSequence_T, StateDefinition_T > >

friend

template<typename StateSeq_T, typename StateDef_T>

friend class msf_core::MSF_InitMeasurement< GenericState_T< StateSequence_T, StateDefinition_T > >

friend

Member Data Documentation

template<typename StateSeq_T, typename StateDef_T>

Eigen::Matrix<double, 3, 1> msf_core::GenericState_T< StateSeq_T, StateDef_T >::a_m

template<typename StateSeq_T, typename StateDef_T>

F_type msf_core::GenericState_T< StateSeq_T, StateDef_T >::Fd

template<typename StateSeq_T, typename StateDef_T>

P_type msf_core::GenericState_T< StateSeq_T, StateDef_T >::P

template<typename StateSeq_T, typename StateDef_T>

Q_type msf_core::GenericState_T< StateSeq_T, StateDef_T >::Qd

template<typename StateSeq_T, typename StateDef_T>

StateSequence_T msf_core::GenericState_T< StateSeq_T, StateDef_T >::statevars

template<typename StateSeq_T, typename StateDef_T>

double msf_core::GenericState_T< StateSeq_T, StateDef_T >::time

template<typename StateSeq_T, typename StateDef_T>

Eigen::Matrix<double, 3, 1> msf_core::GenericState_T< StateSeq_T, StateDef_T >::w_m

---

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

• msf_core/include/msf_core/msf_state.h
• msf_core/include/msf_core/implementation/msf_state_.hpp