Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 #ifndef __ODOM_ESTIMATION__
00037 #define __ODOM_ESTIMATION__
00038
00039
00040 #include <bfl/filter/extendedkalmanfilter.h>
00041 #include <bfl/wrappers/matrix/matrix_wrapper.h>
00042 #include <bfl/model/linearanalyticsystemmodel_gaussianuncertainty.h>
00043 #include <bfl/model/linearanalyticmeasurementmodel_gaussianuncertainty.h>
00044 #include <bfl/pdf/analyticconditionalgaussian.h>
00045 #include <bfl/pdf/linearanalyticconditionalgaussian.h>
00046 #include "nonlinearanalyticconditionalgaussianodo.h"
00047
00048
00049 #include <tf/tf.h>
00050
00051
00052 #include <geometry_msgs/PoseWithCovarianceStamped.h>
00053
00054
00055 #include <fstream>
00056
00057 namespace estimation
00058 {
00059
00060 class OdomEstimation
00061 {
00062 public:
00064 OdomEstimation();
00065
00067 virtual ~OdomEstimation();
00068
00078 bool update(bool odom_active, bool imu_active, bool gps_active, bool vo_active, const ros::Time& filter_time, bool& diagnostics_res);
00079
00084 void initialize(const tf::Transform& prior, const ros::Time& time);
00085
00089 bool isInitialized() {return filter_initialized_;};
00090
00094 void getEstimate(MatrixWrapper::ColumnVector& estimate);
00095
00100 void getEstimate(ros::Time time, tf::Transform& estimate);
00101
00106 void getEstimate(ros::Time time, tf::StampedTransform& estimate);
00107
00111 void getEstimate(geometry_msgs::PoseWithCovarianceStamped& estimate);
00112
00116 void addMeasurement(const tf::StampedTransform& meas);
00117
00122 void addMeasurement(const tf::StampedTransform& meas, const MatrixWrapper::SymmetricMatrix& covar);
00123
00127 void setOutputFrame(const std::string& output_frame);
00128
00132 void setBaseFootprintFrame(const std::string& base_frame);
00133
00134 private:
00136 void angleOverflowCorrect(double& a, double ref);
00137
00138
00139 void decomposeTransform(const tf::StampedTransform& trans,
00140 double& x, double& y, double&z, double&Rx, double& Ry, double& Rz);
00141 void decomposeTransform(const tf::Transform& trans,
00142 double& x, double& y, double&z, double&Rx, double& Ry, double& Rz);
00143
00144
00145
00146 BFL::AnalyticSystemModelGaussianUncertainty* sys_model_;
00147 BFL::NonLinearAnalyticConditionalGaussianOdo* sys_pdf_;
00148 BFL::LinearAnalyticConditionalGaussian* odom_meas_pdf_;
00149 BFL::LinearAnalyticMeasurementModelGaussianUncertainty* odom_meas_model_;
00150 BFL::LinearAnalyticConditionalGaussian* imu_meas_pdf_;
00151 BFL::LinearAnalyticMeasurementModelGaussianUncertainty* imu_meas_model_;
00152 BFL::LinearAnalyticConditionalGaussian* vo_meas_pdf_;
00153 BFL::LinearAnalyticMeasurementModelGaussianUncertainty* vo_meas_model_;
00154 BFL::LinearAnalyticConditionalGaussian* gps_meas_pdf_;
00155 BFL::LinearAnalyticMeasurementModelGaussianUncertainty* gps_meas_model_;
00156 BFL::Gaussian* prior_;
00157 BFL::ExtendedKalmanFilter* filter_;
00158 MatrixWrapper::SymmetricMatrix odom_covariance_, imu_covariance_, vo_covariance_, gps_covariance_;
00159
00160
00161 MatrixWrapper::ColumnVector vel_desi_, filter_estimate_old_vec_;
00162 tf::Transform filter_estimate_old_;
00163 tf::StampedTransform odom_meas_, odom_meas_old_, imu_meas_, imu_meas_old_, vo_meas_, vo_meas_old_, gps_meas_, gps_meas_old_;
00164 ros::Time filter_time_old_;
00165 bool filter_initialized_, odom_initialized_, imu_initialized_, vo_initialized_, gps_initialized_;
00166
00167
00168 double diagnostics_odom_rot_rel_, diagnostics_imu_rot_rel_;
00169
00170
00171 tf::Transformer transformer_;
00172
00173 std::string output_frame_;
00174 std::string base_footprint_frame_;
00175
00176 };
00177
00178 };
00179
00180 #endif