odom_estimation.h
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 * 
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 * 
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 * 
00011 *     notice, this list of conditions and the following disclaimer.
00012 *   * Redistributions in binary form must reproduce the above
00013 *     copyright notice, this list of conditions and the following
00014 *     disclaimer in the documentation and/or other materials provided
00015 *     with the distribution.
00016 *   * Neither the name of the Willow Garage nor the names of its
00017 *     contributors may be used to endorse or promote products derived
00018 *     from this software without specific prior written permission.
00019 * 
00020 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031 *  POSSIBILITY OF SUCH DAMAGE.
00032 *********************************************************************/
00033 
00034 /* Author: Wim Meeussen */
00035 
00036 #ifndef __ODOM_ESTIMATION__
00037 #define __ODOM_ESTIMATION__
00038 
00039 // bayesian filtering
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 // TF
00049 #include <tf/tf.h>
00050 
00051 // msgs
00052 #include <geometry_msgs/PoseWithCovarianceStamped.h>
00053 
00054 // log files
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   // decompose Transform into x,y,z,Rx,Ry,Rz
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   // pdf / model / filter
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   // vars
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   // diagnostics
00168   double diagnostics_odom_rot_rel_, diagnostics_imu_rot_rel_;
00169 
00170   // tf transformer
00171   tf::Transformer transformer_;
00172 
00173   std::string output_frame_;
00174   std::string base_footprint_frame_;
00175 
00176 }; // class
00177 
00178 }; // namespace
00179 
00180 #endif


robot_pose_ekf
Author(s): Wim Meeussen, contradict@gmail.com
autogenerated on Thu Aug 27 2015 14:07:38