#include <SSF_Core.h>

Public Types
typedef Eigen::Matrix< double, N_STATE, 1 >	ErrorState
typedef Eigen::Matrix< double, N_STATE, N_STATE >	ErrorStateCov
Public Member Functions
template<class H_type , class Res_type , class R_type >
bool	applyMeasurement (unsigned char idx_delaystate, const Eigen::MatrixBase< H_type > &H_delayed, const Eigen::MatrixBase< Res_type > &res_delayed, const Eigen::MatrixBase< R_type > &R_delayed, double fuzzythres=0.1)
	main update routine called by a given sensor
unsigned char	getClosestState (State *timestate, ros::Time tstamp, double delay=0.00)
	retreive all state information at time t. Used to build H, residual and noise matrix by update sensors
bool	getStateAtIdx (State *timestate, unsigned char idx)
	get all state information at a given index in the ringbuffer
void	initialize (const Eigen::Matrix< double, 3, 1 > &p, const Eigen::Matrix< double, 3, 1 > &v, const Eigen::Quaternion< double > &q, const Eigen::Matrix< double, 3, 1 > &b_w, const Eigen::Matrix< double, 3, 1 > &b_a, const double &L, const Eigen::Quaternion< double > &q_wv, const Eigen::Matrix< double, N_STATE, N_STATE > &P, const Eigen::Matrix< double, 3, 1 > &w_m, const Eigen::Matrix< double, 3, 1 > &a_m, const Eigen::Matrix< double, 3, 1 > &g, const Eigen::Quaternion< double > &q_ci, const Eigen::Matrix< double, 3, 1 > &p_ci)
	big init routine
template<class T >
void	registerCallback (void(T::cb_func)(ssf_core::SSF_CoreConfig &config, uint32_t level), T p_obj)
	registers dynamic reconfigure callbacks
	SSF_Core ()
	~SSF_Core ()
Private Types
enum	{ NO_UP, GOOD_UP, FUZZY_UP }
typedef boost::function< void(ssf_core::SSF_CoreConfig &config, uint32_t level)>	CallbackType
Private Member Functions
bool	applyCorrection (unsigned char idx_delaystate, const ErrorState &res_delayed, double fuzzythres=0.1)
	applies the correction
void	Config (ssf_core::SSF_CoreConfig &config, uint32_t level)
	gets called by dynamic reconfigure and calls all registered callbacks in callbacks_
void	DynConfig (ssf_core::SSF_CoreConfig &config, uint32_t level)
	handles the dynamic reconfigure for ssf_core
double	getMedian (const Eigen::Matrix< double, nBuff_, 1 > &data)
	computes the median of a given vector
void	imuCallback (const sensor_msgs::ImuConstPtr &msg)
	internal state propagation
void	predictProcessCovariance (const double dt)
	propagets the error state covariance
void	propagateState (const double dt)
	propagates the state with given dt
void	propPToIdx (unsigned char idx)
	propagate covariance to a given index in the ringbuffer
void	stateCallback (const sensor_fusion_comm::ExtEkfConstPtr &msg)
	external state propagation
Private Attributes
std::vector< CallbackType >	callbacks_
ssf_core::SSF_CoreConfig	config_
	dynamic reconfigure config
Eigen::Matrix< double, N_STATE, 1 >	correction_
	correction from EKF update
bool	data_playback_
	enables internal state predictions for log replay
Eigen::Matrix< double, N_STATE, N_STATE >	Fd_
	discrete state propagation matrix
Eigen::Matrix< double, 3, 1 >	g_
	gravity vector
sensor_fusion_comm::ExtEkf	hl_state_buf_
	buffer to store external propagation data
unsigned char	idx_P_
	pointer to state buffer at P latest propagated
unsigned char	idx_state_
	pointer to state buffer at most recent state
unsigned char	idx_time_
	pointer to state buffer at a specific time
bool	initialized_
sensor_fusion_comm::ExtEkf	msgCorrect_
geometry_msgs::PoseWithCovarianceStamped	msgPose_
sensor_fusion_comm::ExtState	msgPoseCtrl_
sensor_fusion_comm::DoubleArrayStamped	msgState_
bool	predictionMade_
ros::Publisher	pubCorrect_
	publishes corrections for external state propagation
ros::Publisher	pubPose_
	publishes 6DoF pose output
ros::Publisher	pubPoseCrtl_
	publishes 6DoF pose including velocity output
ros::Publisher	pubState_
	publishes all states of the filter
Eigen::Matrix< double, nBuff_, 4 >	qbuff_
Eigen::Matrix< double, N_STATE, N_STATE >	Qd_
	discrete propagation noise matrix
int	qvw_inittimer_
	vision-world drift watch dog to determine fuzzy tracking
Eigen::Matrix< double, 3, 3 >	R_CI_
	Rot Camera->IMU.
Eigen::Matrix< double, 3, 3 >	R_IW_
	Rot IMU->World.
Eigen::Matrix< double, 3, 3 >	R_WV_
	Rot World->Vision.
ReconfigureServer *	reconfServer_
State	StateBuffer_ [N_STATE_BUFFER]
	state variables
ros::Subscriber	subImu_
	subscriber to IMU readings
ros::Subscriber	subState_
	subscriber to external state propagation
Static Private Attributes
static const int	nBuff_ = 30
	buffer size for median q_vw
static const int	nFullState_ = 28
	complete state
static const int	nMaxCorr_ = 50
	number of IMU measurements buffered for time correction actions
static const int	QualityThres_ = 1e3

Detailed Description

Definition at line 59 of file SSF_Core.h.

Member Typedef Documentation

typedef boost::function<void(ssf_core::SSF_CoreConfig& config, uint32_t level)> ssf_core::SSF_Core::CallbackType [private]

Definition at line 149 of file SSF_Core.h.

typedef Eigen::Matrix<double, N_STATE, 1> ssf_core::SSF_Core::ErrorState

Definition at line 63 of file SSF_Core.h.

typedef Eigen::Matrix<double, N_STATE, N_STATE> ssf_core::SSF_Core::ErrorStateCov

Definition at line 64 of file SSF_Core.h.

Member Enumeration Documentation

anonymous enum [private]

Enumerator:

NO_UP
GOOD_UP
FUZZY_UP

Definition at line 125 of file SSF_Core.h.

Constructor & Destructor Documentation

ssf_core::SSF_Core::SSF_Core ( )

ros stuff

Definition at line 39 of file SSF_Core.cpp.

ssf_core::SSF_Core::~SSF_Core ( )

Definition at line 70 of file SSF_Core.cpp.

Member Function Documentation

bool ssf_core::SSF_Core::applyCorrection	(	unsigned char	idx_delaystate,
		const ErrorState &	res_delayed,
		double	fuzzythres = `0.1`
	)		`[private]`

applies the correction

Definition at line 502 of file SSF_Core.cpp.

template<class H_type , class Res_type , class R_type >

bool ssf_core::SSF_Core::applyMeasurement	(	unsigned char	idx_delaystate,
		const Eigen::MatrixBase< H_type > &	H_delayed,
		const Eigen::MatrixBase< Res_type > &	res_delayed,
		const Eigen::MatrixBase< R_type > &	R_delayed,
		double	fuzzythres = `0.1`
	)		`[inline]`

main update routine called by a given sensor

Definition at line 196 of file SSF_Core.h.

void ssf_core::SSF_Core::Config	(	ssf_core::SSF_CoreConfig &	config,
		uint32_t	level
	)		`[private]`

gets called by dynamic reconfigure and calls all registered callbacks in callbacks_

Definition at line 664 of file SSF_Core.cpp.

void ssf_core::SSF_Core::DynConfig	(	ssf_core::SSF_CoreConfig &	config,
		uint32_t	level
	)		`[private]`

handles the dynamic reconfigure for ssf_core

Definition at line 670 of file SSF_Core.cpp.

unsigned char ssf_core::SSF_Core::getClosestState	(	State *	timestate,
		ros::Time	tstamp,
		double	delay = `0.00`
	)

retreive all state information at time t. Used to build H, residual and noise matrix by update sensors

Definition at line 460 of file SSF_Core.cpp.

double ssf_core::SSF_Core::getMedian ( const Eigen::Matrix< double, nBuff_, 1 > & data ) [private]

computes the median of a given vector

Definition at line 675 of file SSF_Core.cpp.

bool ssf_core::SSF_Core::getStateAtIdx	(	State *	timestate,
		unsigned char	idx
	)

get all state information at a given index in the ringbuffer

Definition at line 447 of file SSF_Core.cpp.

void ssf_core::SSF_Core::imuCallback ( const sensor_msgs::ImuConstPtr & msg ) [private]

internal state propagation

This function gets called on incoming imu messages an then performs the state prediction internally. Only use this OR stateCallback by remapping the topics accordingly.

See also:: {stateCallback}

Definition at line 185 of file SSF_Core.cpp.

void ssf_core::SSF_Core::initialize	(	const Eigen::Matrix< double, 3, 1 > &	p,
		const Eigen::Matrix< double, 3, 1 > &	v,
		const Eigen::Quaternion< double > &	q,
		const Eigen::Matrix< double, 3, 1 > &	b_w,
		const Eigen::Matrix< double, 3, 1 > &	b_a,
		const double &	L,
		const Eigen::Quaternion< double > &	q_wv,
		const Eigen::Matrix< double, N_STATE, N_STATE > &	P,
		const Eigen::Matrix< double, 3, 1 > &	w_m,
		const Eigen::Matrix< double, 3, 1 > &	a_m,
		const Eigen::Matrix< double, 3, 1 > &	g,
		const Eigen::Quaternion< double > &	q_ci,
		const Eigen::Matrix< double, 3, 1 > &	p_ci
	)

big init routine

Definition at line 75 of file SSF_Core.cpp.

void ssf_core::SSF_Core::predictProcessCovariance ( const double dt ) [private]

propagets the error state covariance

Definition at line 382 of file SSF_Core.cpp.

void ssf_core::SSF_Core::propagateState ( const double dt ) [private]

propagates the state with given dt

Definition at line 321 of file SSF_Core.cpp.

void ssf_core::SSF_Core::propPToIdx ( unsigned char idx ) [private]

propagate covariance to a given index in the ringbuffer

Definition at line 494 of file SSF_Core.cpp.

template<class T >

void ssf_core::SSF_Core::registerCallback	(	void(T::*)(ssf_core::SSF_CoreConfig &config, uint32_t level)	cb_func,
		T *	p_obj
	)		`[inline]`

registers dynamic reconfigure callbacks

Definition at line 229 of file SSF_Core.h.

void ssf_core::SSF_Core::stateCallback ( const sensor_fusion_comm::ExtEkfConstPtr & msg ) [private]

external state propagation

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.

See also:: {imuCallback}

Definition at line 237 of file SSF_Core.cpp.

Member Data Documentation

std::vector<CallbackType> ssf_core::SSF_Core::callbacks_ [private]

Definition at line 150 of file SSF_Core.h.

ssf_core::SSF_CoreConfig ssf_core::SSF_Core::config_ [private]

dynamic reconfigure config

Definition at line 108 of file SSF_Core.h.

Eigen::Matrix<double, N_STATE, 1> ssf_core::SSF_Core::correction_ [private]

correction from EKF update

Definition at line 105 of file SSF_Core.h.

bool ssf_core::SSF_Core::data_playback_ [private]

enables internal state predictions for log replay

used to determine if internal states get overwritten by the external state prediction (online) or internal state prediction is performed for log replay, when the external prediction is not available.

Definition at line 123 of file SSF_Core.h.

Eigen::Matrix<double, N_STATE, N_STATE> ssf_core::SSF_Core::Fd_ [private]

discrete state propagation matrix

Definition at line 89 of file SSF_Core.h.

Eigen::Matrix<double, 3, 1> ssf_core::SSF_Core::g_ [private]

gravity vector

Definition at line 98 of file SSF_Core.h.

sensor_fusion_comm::ExtEkf ssf_core::SSF_Core::hl_state_buf_ [private]

buffer to store external propagation data

Definition at line 145 of file SSF_Core.h.

unsigned char ssf_core::SSF_Core::idx_P_ [private]

pointer to state buffer at P latest propagated

Definition at line 95 of file SSF_Core.h.

unsigned char ssf_core::SSF_Core::idx_state_ [private]

pointer to state buffer at most recent state

Definition at line 94 of file SSF_Core.h.

unsigned char ssf_core::SSF_Core::idx_time_ [private]

pointer to state buffer at a specific time

Definition at line 96 of file SSF_Core.h.

bool ssf_core::SSF_Core::initialized_ [private]

Definition at line 114 of file SSF_Core.h.

sensor_fusion_comm::ExtEkf ssf_core::SSF_Core::msgCorrect_ [private]

Definition at line 140 of file SSF_Core.h.

geometry_msgs::PoseWithCovarianceStamped ssf_core::SSF_Core::msgPose_ [private]

Definition at line 134 of file SSF_Core.h.

sensor_fusion_comm::ExtState ssf_core::SSF_Core::msgPoseCtrl_ [private]

Definition at line 137 of file SSF_Core.h.

sensor_fusion_comm::DoubleArrayStamped ssf_core::SSF_Core::msgState_ [private]

Definition at line 131 of file SSF_Core.h.

const int ssf_core::SSF_Core::nBuff_ = 30 [static, private]

buffer size for median q_vw

Definition at line 85 of file SSF_Core.h.

const int ssf_core::SSF_Core::nFullState_ = 28 [static, private]

complete state

Definition at line 84 of file SSF_Core.h.

const int ssf_core::SSF_Core::nMaxCorr_ = 50 [static, private]

number of IMU measurements buffered for time correction actions

Definition at line 86 of file SSF_Core.h.

bool ssf_core::SSF_Core::predictionMade_ [private]

Definition at line 115 of file SSF_Core.h.

ros::Publisher ssf_core::SSF_Core::pubCorrect_ [private]

publishes corrections for external state propagation

Definition at line 139 of file SSF_Core.h.

ros::Publisher ssf_core::SSF_Core::pubPose_ [private]

publishes 6DoF pose output

Definition at line 133 of file SSF_Core.h.

ros::Publisher ssf_core::SSF_Core::pubPoseCrtl_ [private]

publishes 6DoF pose including velocity output

Definition at line 136 of file SSF_Core.h.

ros::Publisher ssf_core::SSF_Core::pubState_ [private]

publishes all states of the filter

Definition at line 130 of file SSF_Core.h.

Eigen::Matrix<double, nBuff_, 4> ssf_core::SSF_Core::qbuff_ [private]

Definition at line 102 of file SSF_Core.h.

Eigen::Matrix<double, N_STATE, N_STATE> ssf_core::SSF_Core::Qd_ [private]

discrete propagation noise matrix

Definition at line 90 of file SSF_Core.h.

const int ssf_core::SSF_Core::QualityThres_ = 1e3 [static, private]

Definition at line 87 of file SSF_Core.h.

int ssf_core::SSF_Core::qvw_inittimer_ [private]

vision-world drift watch dog to determine fuzzy tracking

Definition at line 101 of file SSF_Core.h.

Eigen::Matrix<double, 3, 3> ssf_core::SSF_Core::R_CI_ [private]

Rot Camera->IMU.

Definition at line 111 of file SSF_Core.h.

Eigen::Matrix<double, 3, 3> ssf_core::SSF_Core::R_IW_ [private]

Rot IMU->World.

Definition at line 110 of file SSF_Core.h.

Eigen::Matrix<double, 3, 3> ssf_core::SSF_Core::R_WV_ [private]

Rot World->Vision.

Definition at line 112 of file SSF_Core.h.

ReconfigureServer* ssf_core::SSF_Core::reconfServer_ [private]

Definition at line 148 of file SSF_Core.h.

State ssf_core::SSF_Core::StateBuffer_[N_STATE_BUFFER] [private]

state variables

EKF ringbuffer containing pretty much all info needed at time t

Definition at line 93 of file SSF_Core.h.

ros::Subscriber ssf_core::SSF_Core::subImu_ [private]

subscriber to IMU readings

Definition at line 143 of file SSF_Core.h.

ros::Subscriber ssf_core::SSF_Core::subState_ [private]

subscriber to external state propagation

Definition at line 142 of file SSF_Core.h.

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

Public Types

Public Member Functions

Private Types

Private Member Functions