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 <filter/extendedkalmanfilter.h>
00041 #include <wrappers/matrix/matrix_wrapper.h>
00042 #include <model/linearanalyticsystemmodel_gaussianuncertainty.h>
00043 #include <model/linearanalyticmeasurementmodel_gaussianuncertainty.h>
00044 #include <pdf/analyticconditionalgaussian.h>
00045 #include <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& estiamte);
00101
00106 void getEstimate(ros::Time time, tf::StampedTransform& estiamte);
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
00124 private:
00126 void angleOverflowCorrect(double& a, double ref);
00127
00128
00129 void decomposeTransform(const tf::StampedTransform& trans,
00130 double& x, double& y, double&z, double&Rx, double& Ry, double& Rz);
00131 void decomposeTransform(const tf::Transform& trans,
00132 double& x, double& y, double&z, double&Rx, double& Ry, double& Rz);
00133
00134
00135
00136 BFL::AnalyticSystemModelGaussianUncertainty* sys_model_;
00137 BFL::NonLinearAnalyticConditionalGaussianOdo* sys_pdf_;
00138 BFL::LinearAnalyticConditionalGaussian* odom_meas_pdf_;
00139 BFL::LinearAnalyticMeasurementModelGaussianUncertainty* odom_meas_model_;
00140 BFL::LinearAnalyticConditionalGaussian* imu_meas_pdf_;
00141 BFL::LinearAnalyticMeasurementModelGaussianUncertainty* imu_meas_model_;
00142 BFL::LinearAnalyticConditionalGaussian* vo_meas_pdf_;
00143 BFL::LinearAnalyticMeasurementModelGaussianUncertainty* vo_meas_model_;
00144 BFL::LinearAnalyticConditionalGaussian* gps_meas_pdf_;
00145 BFL::LinearAnalyticMeasurementModelGaussianUncertainty* gps_meas_model_;
00146 BFL::Gaussian* prior_;
00147 BFL::ExtendedKalmanFilter* filter_;
00148 MatrixWrapper::SymmetricMatrix odom_covariance_, imu_covariance_, vo_covariance_, gps_covariance_;
00149
00150
00151 MatrixWrapper::ColumnVector vel_desi_, filter_estimate_old_vec_;
00152 tf::Transform filter_estimate_old_;
00153 tf::StampedTransform odom_meas_, odom_meas_old_, imu_meas_, imu_meas_old_, vo_meas_, vo_meas_old_, gps_meas_, gps_meas_old_;
00154 ros::Time filter_time_old_;
00155 bool filter_initialized_, odom_initialized_, imu_initialized_, vo_initialized_, gps_initialized_;
00156
00157
00158 double diagnostics_odom_rot_rel_, diagnostics_imu_rot_rel_;
00159
00160
00161 tf::Transformer transformer_;
00162
00163 };
00164
00165 };
00166
00167 #endif