#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_ |
Definition at line 60 of file odom_estimation.h.
constructor
Definition at line 49 of file odom_estimation.cpp.
estimation::OdomEstimation::~OdomEstimation | ( | ) | [virtual] |
destructor
Definition at line 112 of file odom_estimation.cpp.
void estimation::OdomEstimation::addMeasurement | ( | const tf::StampedTransform & | meas | ) |
Add a sensor measurement to the measurement buffer
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
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
estimate | the filter posterior as a columnvector |
void estimation::OdomEstimation::getEstimate | ( | ros::Time | time, |
tf::Transform & | estiamte | ||
) |
get the filter posterior
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
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
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
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
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.
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.
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.
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.
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.
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.
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.
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.
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.
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.
Definition at line 137 of file odom_estimation.h.
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.
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.
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.