#include <odom_estimation.h>

Public Member Functions
void	addMeasurement (const tf::StampedTransform &meas)
void	addMeasurement (const tf::StampedTransform &meas, const MatrixWrapper::SymmetricMatrix &covar)
void	getEstimate (MatrixWrapper::ColumnVector &estimate)
void	getEstimate (ros::Time time, tf::Transform &estiamte)
void	getEstimate (ros::Time time, tf::StampedTransform &estiamte)
void	getEstimate (geometry_msgs::PoseWithCovarianceStamped &estimate)
void	initialize (const tf::Transform &prior, const ros::Time &time)
bool	isInitialized ()
	OdomEstimation ()
	constructor
bool	update (bool odom_active, bool imu_active, bool gps_active, bool vo_active, const ros::Time &filter_time, bool &diagnostics_res)
virtual	~OdomEstimation ()
	destructor
Private Member Functions
void	angleOverflowCorrect (double &a, double ref)
	correct for angle overflow
void	decomposeTransform (const tf::StampedTransform &trans, double &x, double &y, double &z, double &Rx, double &Ry, double &Rz)
void	decomposeTransform (const tf::Transform &trans, double &x, double &y, double &z, double &Rx, double &Ry, double &Rz)
Private Attributes
double	diagnostics_imu_rot_rel_
double	diagnostics_odom_rot_rel_
BFL::ExtendedKalmanFilter *	filter_
tf::Transform	filter_estimate_old_
MatrixWrapper::ColumnVector	filter_estimate_old_vec_
bool	filter_initialized_
ros::Time	filter_time_old_
MatrixWrapper::SymmetricMatrix	gps_covariance_
bool	gps_initialized_
tf::StampedTransform	gps_meas_
BFL::LinearAnalyticMeasurementModelGaussianUncertainty *	gps_meas_model_
tf::StampedTransform	gps_meas_old_
BFL::LinearAnalyticConditionalGaussian *	gps_meas_pdf_
MatrixWrapper::SymmetricMatrix	imu_covariance_
bool	imu_initialized_
tf::StampedTransform	imu_meas_
BFL::LinearAnalyticMeasurementModelGaussianUncertainty *	imu_meas_model_
tf::StampedTransform	imu_meas_old_
BFL::LinearAnalyticConditionalGaussian *	imu_meas_pdf_
MatrixWrapper::SymmetricMatrix	odom_covariance_
bool	odom_initialized_
tf::StampedTransform	odom_meas_
BFL::LinearAnalyticMeasurementModelGaussianUncertainty *	odom_meas_model_
tf::StampedTransform	odom_meas_old_
BFL::LinearAnalyticConditionalGaussian *	odom_meas_pdf_
BFL::Gaussian *	prior_
BFL::AnalyticSystemModelGaussianUncertainty *	sys_model_
BFL::NonLinearAnalyticConditionalGaussianOdo *	sys_pdf_
tf::Transformer	transformer_
MatrixWrapper::ColumnVector	vel_desi_
MatrixWrapper::SymmetricMatrix	vo_covariance_
bool	vo_initialized_
tf::StampedTransform	vo_meas_
BFL::LinearAnalyticMeasurementModelGaussianUncertainty *	vo_meas_model_
tf::StampedTransform	vo_meas_old_
BFL::LinearAnalyticConditionalGaussian *	vo_meas_pdf_

Detailed Description

Definition at line 60 of file odom_estimation.h.

Constructor & Destructor Documentation

estimation::OdomEstimation::OdomEstimation ( )

constructor

Definition at line 49 of file odom_estimation.cpp.

estimation::OdomEstimation::~OdomEstimation ( ) [virtual]

destructor

Definition at line 112 of file odom_estimation.cpp.

Member Function Documentation

void estimation::OdomEstimation::addMeasurement ( const tf::StampedTransform & meas )

Add a sensor measurement to the measurement buffer

Parameters:

meas	the measurement to add

Definition at line 328 of file odom_estimation.cpp.

void estimation::OdomEstimation::addMeasurement	(	const tf::StampedTransform &	meas,
		const MatrixWrapper::SymmetricMatrix &	covar
	)

Add a sensor measurement to the measurement buffer

Parameters:

meas	the measurement to add
covar	the 6x6 covariance matrix of this measurement, as defined in the PoseWithCovariance message

Definition at line 338 of file odom_estimation.cpp.

void estimation::OdomEstimation::angleOverflowCorrect	(	double &	a,
		double	ref
	)		`[private]`

correct for angle overflow

Definition at line 409 of file odom_estimation.cpp.

void estimation::OdomEstimation::decomposeTransform	(	const tf::StampedTransform &	trans,
		double &	x,
		double &	y,
		double &	z,
		double &	Rx,
		double &	Ry,
		double &	Rz
	)		`[private]`

Definition at line 416 of file odom_estimation.cpp.

void estimation::OdomEstimation::decomposeTransform	(	const tf::Transform &	trans,
		double &	x,
		double &	y,
		double &	z,
		double &	Rx,
		double &	Ry,
		double &	Rz
	)		`[private]`

Definition at line 425 of file odom_estimation.cpp.

void estimation::OdomEstimation::getEstimate ( MatrixWrapper::ColumnVector & estimate )

get the filter posterior

Parameters:

estimate the filter posterior as a columnvector

void estimation::OdomEstimation::getEstimate	(	ros::Time	time,
		tf::Transform &	estiamte
	)

get the filter posterior

Parameters:

time	the time of the filter posterior
estimate	the filter posterior as a tf transform

Definition at line 364 of file odom_estimation.cpp.

void estimation::OdomEstimation::getEstimate	(	ros::Time	time,
		tf::StampedTransform &	estiamte
	)

get the filter posterior

Parameters:

time	the time of the filter posterior
estimate	the filter posterior as a stamped tf transform

Definition at line 376 of file odom_estimation.cpp.

void estimation::OdomEstimation::getEstimate ( geometry_msgs::PoseWithCovarianceStamped & estimate )

get the filter posterior

Parameters:

estimate the filter posterior as a pose with covariance

Definition at line 386 of file odom_estimation.cpp.

void estimation::OdomEstimation::initialize	(	const tf::Transform &	prior,
		const ros::Time &	time
	)

initialize the extended Kalman filter

Parameters:

prior	the prior robot pose
time	the initial time of the ekf

Definition at line 129 of file odom_estimation.cpp.

bool estimation::OdomEstimation::isInitialized ( ) [inline]

check if the filter is initialized returns true if the ekf has been initialized already

Definition at line 89 of file odom_estimation.h.

bool estimation::OdomEstimation::update	(	bool	odom_active,
		bool	imu_active,
		bool	gps_active,
		bool	vo_active,
		const ros::Time &	filter_time,
		bool &	diagnostics_res
	)

update the extended Kalman filter

Parameters:

odom_active	specifies if the odometry sensor is active or not
imu_active	specifies if the imu sensor is active or not
gps_active	specifies if the gps sensor is active or not
vo_active	specifies if the vo sensor is active or not
filter_time	update the ekf up to this time
diagnostics_res	returns false if the diagnostics found that the sensor measurements are inconsistent returns true on successfull update

Definition at line 159 of file odom_estimation.cpp.

Member Data Documentation

double estimation::OdomEstimation::diagnostics_imu_rot_rel_ [private]

Definition at line 158 of file odom_estimation.h.

double estimation::OdomEstimation::diagnostics_odom_rot_rel_ [private]

Definition at line 158 of file odom_estimation.h.

BFL::ExtendedKalmanFilter* estimation::OdomEstimation::filter_ [private]

Definition at line 147 of file odom_estimation.h.

tf::Transform estimation::OdomEstimation::filter_estimate_old_ [private]

Definition at line 152 of file odom_estimation.h.

MatrixWrapper::ColumnVector estimation::OdomEstimation::filter_estimate_old_vec_ [private]

Definition at line 151 of file odom_estimation.h.

bool estimation::OdomEstimation::filter_initialized_ [private]

Definition at line 155 of file odom_estimation.h.

ros::Time estimation::OdomEstimation::filter_time_old_ [private]

Definition at line 154 of file odom_estimation.h.

MatrixWrapper::SymmetricMatrix estimation::OdomEstimation::gps_covariance_ [private]

Definition at line 148 of file odom_estimation.h.

bool estimation::OdomEstimation::gps_initialized_ [private]

Definition at line 155 of file odom_estimation.h.

tf::StampedTransform estimation::OdomEstimation::gps_meas_ [private]

Definition at line 153 of file odom_estimation.h.

BFL::LinearAnalyticMeasurementModelGaussianUncertainty* estimation::OdomEstimation::gps_meas_model_ [private]

Definition at line 145 of file odom_estimation.h.

tf::StampedTransform estimation::OdomEstimation::gps_meas_old_ [private]

Definition at line 153 of file odom_estimation.h.

BFL::LinearAnalyticConditionalGaussian* estimation::OdomEstimation::gps_meas_pdf_ [private]

Definition at line 144 of file odom_estimation.h.

MatrixWrapper::SymmetricMatrix estimation::OdomEstimation::imu_covariance_ [private]

Definition at line 148 of file odom_estimation.h.

bool estimation::OdomEstimation::imu_initialized_ [private]

Definition at line 155 of file odom_estimation.h.

tf::StampedTransform estimation::OdomEstimation::imu_meas_ [private]

Definition at line 153 of file odom_estimation.h.

BFL::LinearAnalyticMeasurementModelGaussianUncertainty* estimation::OdomEstimation::imu_meas_model_ [private]

Definition at line 141 of file odom_estimation.h.

tf::StampedTransform estimation::OdomEstimation::imu_meas_old_ [private]

Definition at line 153 of file odom_estimation.h.

BFL::LinearAnalyticConditionalGaussian* estimation::OdomEstimation::imu_meas_pdf_ [private]

Definition at line 140 of file odom_estimation.h.

MatrixWrapper::SymmetricMatrix estimation::OdomEstimation::odom_covariance_ [private]

Definition at line 148 of file odom_estimation.h.

bool estimation::OdomEstimation::odom_initialized_ [private]

Definition at line 155 of file odom_estimation.h.

tf::StampedTransform estimation::OdomEstimation::odom_meas_ [private]

Definition at line 153 of file odom_estimation.h.

BFL::LinearAnalyticMeasurementModelGaussianUncertainty* estimation::OdomEstimation::odom_meas_model_ [private]

Definition at line 139 of file odom_estimation.h.

tf::StampedTransform estimation::OdomEstimation::odom_meas_old_ [private]

Definition at line 153 of file odom_estimation.h.

BFL::LinearAnalyticConditionalGaussian* estimation::OdomEstimation::odom_meas_pdf_ [private]

Definition at line 138 of file odom_estimation.h.

BFL::Gaussian* estimation::OdomEstimation::prior_ [private]

Definition at line 146 of file odom_estimation.h.

BFL::AnalyticSystemModelGaussianUncertainty* estimation::OdomEstimation::sys_model_ [private]

Definition at line 136 of file odom_estimation.h.

BFL::NonLinearAnalyticConditionalGaussianOdo* estimation::OdomEstimation::sys_pdf_ [private]

Definition at line 137 of file odom_estimation.h.

tf::Transformer estimation::OdomEstimation::transformer_ [private]

Definition at line 161 of file odom_estimation.h.

MatrixWrapper::ColumnVector estimation::OdomEstimation::vel_desi_ [private]

Definition at line 151 of file odom_estimation.h.

MatrixWrapper::SymmetricMatrix estimation::OdomEstimation::vo_covariance_ [private]

Definition at line 148 of file odom_estimation.h.

bool estimation::OdomEstimation::vo_initialized_ [private]

Definition at line 155 of file odom_estimation.h.

tf::StampedTransform estimation::OdomEstimation::vo_meas_ [private]

Definition at line 153 of file odom_estimation.h.

BFL::LinearAnalyticMeasurementModelGaussianUncertainty* estimation::OdomEstimation::vo_meas_model_ [private]

Definition at line 143 of file odom_estimation.h.

tf::StampedTransform estimation::OdomEstimation::vo_meas_old_ [private]

Definition at line 153 of file odom_estimation.h.

BFL::LinearAnalyticConditionalGaussian* estimation::OdomEstimation::vo_meas_pdf_ [private]

Definition at line 142 of file odom_estimation.h.

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

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation