Public Member Functions | Private Member Functions | Private Attributes | List of all members
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 (geometry_msgs::PoseWithCovarianceStamped &estimate)
 
void getEstimate (MatrixWrapper::ColumnVector &estimate)
 
void getEstimate (ros::Time time, tf::StampedTransform &estimate)
 
void getEstimate (ros::Time time, tf::Transform &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::NonLinearAnalyticConditionalGaussianOdosys_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 91 of file odom_estimation.h.

Constructor & Destructor Documentation

◆ OdomEstimation()

estimation::OdomEstimation::OdomEstimation ( )

constructor

Definition at line 49 of file odom_estimation.cpp.

◆ ~OdomEstimation()

estimation::OdomEstimation::~OdomEstimation ( )
virtual

destructor

Definition at line 112 of file odom_estimation.cpp.

Member Function Documentation

◆ addMeasurement() [1/2]

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

Add a sensor measurement to the measurement buffer

Parameters
measthe measurement to add

Definition at line 322 of file odom_estimation.cpp.

◆ addMeasurement() [2/2]

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

Add a sensor measurement to the measurement buffer

Parameters
measthe measurement to add
covarthe 6x6 covariance matrix of this measurement, as defined in the PoseWithCovariance message

Definition at line 332 of file odom_estimation.cpp.

◆ angleOverflowCorrect()

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

correct for angle overflow

Definition at line 403 of file odom_estimation.cpp.

◆ decomposeTransform() [1/2]

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.

◆ decomposeTransform() [2/2]

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.

◆ getEstimate() [1/4]

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

get the filter posterior

Parameters
estimatethe filter posterior as a pose with covariance

Definition at line 380 of file odom_estimation.cpp.

◆ getEstimate() [2/4]

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

get the filter posterior

Parameters
estimatethe filter posterior as a columnvector

Definition at line 352 of file odom_estimation.cpp.

◆ getEstimate() [3/4]

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

get the filter posterior

Parameters
timethe time of the filter posterior
estimatethe filter posterior as a stamped tf transform

Definition at line 370 of file odom_estimation.cpp.

◆ getEstimate() [4/4]

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

get the filter posterior

Parameters
timethe time of the filter posterior
estimatethe filter posterior as a tf transform

Definition at line 358 of file odom_estimation.cpp.

◆ initialize()

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

initialize the extended Kalman filter

Parameters
priorthe prior robot pose
timethe initial time of the ekf

Definition at line 129 of file odom_estimation.cpp.

◆ isInitialized()

bool estimation::OdomEstimation::isInitialized ( )
inline

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

Definition at line 151 of file odom_estimation.h.

◆ setBaseFootprintFrame()

void estimation::OdomEstimation::setBaseFootprintFrame ( const std::string &  base_frame)

set the base_footprint frame of the robot used by tf

Parameters
base_framethe 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.

◆ setOutputFrame()

void estimation::OdomEstimation::setOutputFrame ( const std::string &  output_frame)

set the output frame used by tf

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

Definition at line 427 of file odom_estimation.cpp.

◆ update()

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_activespecifies if the odometry sensor is active or not
imu_activespecifies if the imu sensor is active or not
gps_activespecifies if the gps sensor is active or not
vo_activespecifies if the vo sensor is active or not
filter_timeupdate the ekf up to this time
diagnostics_resreturns 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

◆ base_footprint_frame_

std::string estimation::OdomEstimation::base_footprint_frame_
private

Definition at line 236 of file odom_estimation.h.

◆ diagnostics_imu_rot_rel_

double estimation::OdomEstimation::diagnostics_imu_rot_rel_
private

Definition at line 230 of file odom_estimation.h.

◆ diagnostics_odom_rot_rel_

double estimation::OdomEstimation::diagnostics_odom_rot_rel_
private

Definition at line 230 of file odom_estimation.h.

◆ filter_

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

Definition at line 219 of file odom_estimation.h.

◆ filter_estimate_old_

tf::Transform estimation::OdomEstimation::filter_estimate_old_
private

Definition at line 224 of file odom_estimation.h.

◆ filter_estimate_old_vec_

MatrixWrapper::ColumnVector estimation::OdomEstimation::filter_estimate_old_vec_
private

Definition at line 223 of file odom_estimation.h.

◆ filter_initialized_

bool estimation::OdomEstimation::filter_initialized_
private

Definition at line 227 of file odom_estimation.h.

◆ filter_time_old_

ros::Time estimation::OdomEstimation::filter_time_old_
private

Definition at line 226 of file odom_estimation.h.

◆ gps_covariance_

MatrixWrapper::SymmetricMatrix estimation::OdomEstimation::gps_covariance_
private

Definition at line 220 of file odom_estimation.h.

◆ gps_initialized_

bool estimation::OdomEstimation::gps_initialized_
private

Definition at line 227 of file odom_estimation.h.

◆ gps_meas_

tf::StampedTransform estimation::OdomEstimation::gps_meas_
private

Definition at line 225 of file odom_estimation.h.

◆ gps_meas_model_

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

Definition at line 217 of file odom_estimation.h.

◆ gps_meas_old_

tf::StampedTransform estimation::OdomEstimation::gps_meas_old_
private

Definition at line 225 of file odom_estimation.h.

◆ gps_meas_pdf_

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

Definition at line 216 of file odom_estimation.h.

◆ imu_covariance_

MatrixWrapper::SymmetricMatrix estimation::OdomEstimation::imu_covariance_
private

Definition at line 220 of file odom_estimation.h.

◆ imu_initialized_

bool estimation::OdomEstimation::imu_initialized_
private

Definition at line 227 of file odom_estimation.h.

◆ imu_meas_

tf::StampedTransform estimation::OdomEstimation::imu_meas_
private

Definition at line 225 of file odom_estimation.h.

◆ imu_meas_model_

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

Definition at line 213 of file odom_estimation.h.

◆ imu_meas_old_

tf::StampedTransform estimation::OdomEstimation::imu_meas_old_
private

Definition at line 225 of file odom_estimation.h.

◆ imu_meas_pdf_

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

Definition at line 212 of file odom_estimation.h.

◆ odom_covariance_

MatrixWrapper::SymmetricMatrix estimation::OdomEstimation::odom_covariance_
private

Definition at line 220 of file odom_estimation.h.

◆ odom_initialized_

bool estimation::OdomEstimation::odom_initialized_
private

Definition at line 227 of file odom_estimation.h.

◆ odom_meas_

tf::StampedTransform estimation::OdomEstimation::odom_meas_
private

Definition at line 225 of file odom_estimation.h.

◆ odom_meas_model_

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

Definition at line 211 of file odom_estimation.h.

◆ odom_meas_old_

tf::StampedTransform estimation::OdomEstimation::odom_meas_old_
private

Definition at line 225 of file odom_estimation.h.

◆ odom_meas_pdf_

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

Definition at line 210 of file odom_estimation.h.

◆ output_frame_

std::string estimation::OdomEstimation::output_frame_
private

Definition at line 235 of file odom_estimation.h.

◆ prior_

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

Definition at line 218 of file odom_estimation.h.

◆ sys_model_

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

Definition at line 208 of file odom_estimation.h.

◆ sys_pdf_

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

Definition at line 209 of file odom_estimation.h.

◆ transformer_

tf::Transformer estimation::OdomEstimation::transformer_
private

Definition at line 233 of file odom_estimation.h.

◆ vel_desi_

MatrixWrapper::ColumnVector estimation::OdomEstimation::vel_desi_
private

Definition at line 223 of file odom_estimation.h.

◆ vo_covariance_

MatrixWrapper::SymmetricMatrix estimation::OdomEstimation::vo_covariance_
private

Definition at line 220 of file odom_estimation.h.

◆ vo_initialized_

bool estimation::OdomEstimation::vo_initialized_
private

Definition at line 227 of file odom_estimation.h.

◆ vo_meas_

tf::StampedTransform estimation::OdomEstimation::vo_meas_
private

Definition at line 225 of file odom_estimation.h.

◆ vo_meas_model_

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

Definition at line 215 of file odom_estimation.h.

◆ vo_meas_old_

tf::StampedTransform estimation::OdomEstimation::vo_meas_old_
private

Definition at line 225 of file odom_estimation.h.

◆ vo_meas_pdf_

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

Definition at line 214 of file odom_estimation.h.


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


robot_pose_ekf
Author(s): Wim Meeussen
autogenerated on Wed Mar 2 2022 00:50:47