estimation::OdomEstimation Class Reference

#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 &estimate)
void	getEstimate (ros::Time time, tf::StampedTransform &estimate)
void	getEstimate (geometry_msgs::PoseWithCovarianceStamped &estimate)
void	initialize (const tf::Transform &prior, const ros::Time &time)
bool	isInitialized ()
	OdomEstimation ()
	constructor More...
void	setBaseFootprintFrame (const std::string &base_frame)
void	setOutputFrame (const std::string &output_frame)
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 More...

Private Member Functions
void	angleOverflowCorrect (double &a, double ref)
	correct for angle overflow More...
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
std::string	base_footprint_frame_
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_
std::string	output_frame_
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 322 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 332 of file odom_estimation.cpp.

void estimation::OdomEstimation::angleOverflowCorrect ( double &  a, double  ref  )

private

correct for angle overflow

Definition at line 403 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 410 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 419 of file odom_estimation.cpp.

void estimation::OdomEstimation::getEstimate

(

MatrixWrapper::ColumnVector &

estimate

)

get the filter posterior

Parameters

estimate

the filter posterior as a columnvector

Definition at line 352 of file odom_estimation.cpp.

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

get the filter posterior

Parameters

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

Definition at line 358 of file odom_estimation.cpp.

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

get the filter posterior

Parameters

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

Definition at line 370 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 380 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.

void estimation::OdomEstimation::setBaseFootprintFrame

(

const std::string &

base_frame

)

set the base_footprint frame of the robot used by tf

Parameters

base_frame

the desired base frame from which to transform when publishing the combined odometry frame (e.g., base_footprint)

Definition at line 431 of file odom_estimation.cpp.

void estimation::OdomEstimation::setOutputFrame

(

const std::string &

output_frame

)

set the output frame used by tf

Parameters

output_frame

the desired output frame published on /tf (e.g., odom_combined)

Definition at line 427 of file odom_estimation.cpp.

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

std::string estimation::OdomEstimation::base_footprint_frame_

private

Definition at line 174 of file odom_estimation.h.

double estimation::OdomEstimation::diagnostics_imu_rot_rel_

private

Definition at line 168 of file odom_estimation.h.

double estimation::OdomEstimation::diagnostics_odom_rot_rel_

private

Definition at line 168 of file odom_estimation.h.

BFL::ExtendedKalmanFilter* estimation::OdomEstimation::filter_

private

Definition at line 157 of file odom_estimation.h.

tf::Transform estimation::OdomEstimation::filter_estimate_old_

private

Definition at line 162 of file odom_estimation.h.

MatrixWrapper::ColumnVector estimation::OdomEstimation::filter_estimate_old_vec_

private

Definition at line 161 of file odom_estimation.h.

bool estimation::OdomEstimation::filter_initialized_

private

Definition at line 165 of file odom_estimation.h.

ros::Time estimation::OdomEstimation::filter_time_old_

private

Definition at line 164 of file odom_estimation.h.

MatrixWrapper::SymmetricMatrix estimation::OdomEstimation::gps_covariance_

private

Definition at line 158 of file odom_estimation.h.

bool estimation::OdomEstimation::gps_initialized_

private

Definition at line 165 of file odom_estimation.h.

tf::StampedTransform estimation::OdomEstimation::gps_meas_

private

Definition at line 163 of file odom_estimation.h.

BFL::LinearAnalyticMeasurementModelGaussianUncertainty* estimation::OdomEstimation::gps_meas_model_

private

Definition at line 155 of file odom_estimation.h.

tf::StampedTransform estimation::OdomEstimation::gps_meas_old_

private

Definition at line 163 of file odom_estimation.h.

BFL::LinearAnalyticConditionalGaussian* estimation::OdomEstimation::gps_meas_pdf_

private

Definition at line 154 of file odom_estimation.h.

MatrixWrapper::SymmetricMatrix estimation::OdomEstimation::imu_covariance_

private

Definition at line 158 of file odom_estimation.h.

bool estimation::OdomEstimation::imu_initialized_

private

Definition at line 165 of file odom_estimation.h.

tf::StampedTransform estimation::OdomEstimation::imu_meas_

private

Definition at line 163 of file odom_estimation.h.

BFL::LinearAnalyticMeasurementModelGaussianUncertainty* estimation::OdomEstimation::imu_meas_model_

private

Definition at line 151 of file odom_estimation.h.

tf::StampedTransform estimation::OdomEstimation::imu_meas_old_

private

Definition at line 163 of file odom_estimation.h.

BFL::LinearAnalyticConditionalGaussian* estimation::OdomEstimation::imu_meas_pdf_

private

Definition at line 150 of file odom_estimation.h.

MatrixWrapper::SymmetricMatrix estimation::OdomEstimation::odom_covariance_

private

Definition at line 158 of file odom_estimation.h.

bool estimation::OdomEstimation::odom_initialized_

private

Definition at line 165 of file odom_estimation.h.

tf::StampedTransform estimation::OdomEstimation::odom_meas_

private

Definition at line 163 of file odom_estimation.h.

BFL::LinearAnalyticMeasurementModelGaussianUncertainty* estimation::OdomEstimation::odom_meas_model_

private

Definition at line 149 of file odom_estimation.h.

tf::StampedTransform estimation::OdomEstimation::odom_meas_old_

private

Definition at line 163 of file odom_estimation.h.

BFL::LinearAnalyticConditionalGaussian* estimation::OdomEstimation::odom_meas_pdf_

private

Definition at line 148 of file odom_estimation.h.

std::string estimation::OdomEstimation::output_frame_

private

Definition at line 173 of file odom_estimation.h.

BFL::Gaussian* estimation::OdomEstimation::prior_

private

Definition at line 156 of file odom_estimation.h.

BFL::AnalyticSystemModelGaussianUncertainty* estimation::OdomEstimation::sys_model_

private

Definition at line 146 of file odom_estimation.h.

BFL::NonLinearAnalyticConditionalGaussianOdo* estimation::OdomEstimation::sys_pdf_

private

Definition at line 147 of file odom_estimation.h.

tf::Transformer estimation::OdomEstimation::transformer_

private

Definition at line 171 of file odom_estimation.h.

MatrixWrapper::ColumnVector estimation::OdomEstimation::vel_desi_

private

Definition at line 161 of file odom_estimation.h.

MatrixWrapper::SymmetricMatrix estimation::OdomEstimation::vo_covariance_

private

Definition at line 158 of file odom_estimation.h.

bool estimation::OdomEstimation::vo_initialized_

private

Definition at line 165 of file odom_estimation.h.

tf::StampedTransform estimation::OdomEstimation::vo_meas_

private

Definition at line 163 of file odom_estimation.h.

BFL::LinearAnalyticMeasurementModelGaussianUncertainty* estimation::OdomEstimation::vo_meas_model_

private

Definition at line 153 of file odom_estimation.h.

tf::StampedTransform estimation::OdomEstimation::vo_meas_old_

private

Definition at line 163 of file odom_estimation.h.

BFL::LinearAnalyticConditionalGaussian* estimation::OdomEstimation::vo_meas_pdf_

private

Definition at line 152 of file odom_estimation.h.

---

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

• odom_estimation.h
• odom_estimation.cpp