odom_estimation.cpp
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 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 * 
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Wim Meeussen */
00036 
00037 #include <robot_pose_ekf/odom_estimation.h>
00038 
00039 using namespace MatrixWrapper;
00040 using namespace BFL;
00041 using namespace tf;
00042 using namespace std;
00043 using namespace ros;
00044 
00045 
00046 namespace estimation
00047 {
00048   // constructor
00049   OdomEstimation::OdomEstimation():
00050     prior_(NULL),
00051     filter_(NULL),
00052     filter_initialized_(false),
00053     odom_initialized_(false),
00054     imu_initialized_(false),
00055     vo_initialized_(false),
00056     gps_initialized_(false),
00057     output_frame_(std::string("odom_combined")),
00058     base_footprint_frame_(std::string("base_footprint"))
00059   {
00060     // create SYSTEM MODEL
00061     ColumnVector sysNoise_Mu(6);  sysNoise_Mu = 0;
00062     SymmetricMatrix sysNoise_Cov(6); sysNoise_Cov = 0;
00063     for (unsigned int i=1; i<=6; i++) sysNoise_Cov(i,i) = pow(1000.0,2);
00064     Gaussian system_Uncertainty(sysNoise_Mu, sysNoise_Cov);
00065     sys_pdf_   = new NonLinearAnalyticConditionalGaussianOdo(system_Uncertainty);
00066     sys_model_ = new AnalyticSystemModelGaussianUncertainty(sys_pdf_);
00067 
00068     // create MEASUREMENT MODEL ODOM
00069     ColumnVector measNoiseOdom_Mu(6);  measNoiseOdom_Mu = 0;
00070     SymmetricMatrix measNoiseOdom_Cov(6);  measNoiseOdom_Cov = 0;
00071     for (unsigned int i=1; i<=6; i++) measNoiseOdom_Cov(i,i) = 1;
00072     Gaussian measurement_Uncertainty_Odom(measNoiseOdom_Mu, measNoiseOdom_Cov);
00073     Matrix Hodom(6,6);  Hodom = 0;
00074     Hodom(1,1) = 1;    Hodom(2,2) = 1;    Hodom(6,6) = 1;
00075     odom_meas_pdf_   = new LinearAnalyticConditionalGaussian(Hodom, measurement_Uncertainty_Odom);
00076     odom_meas_model_ = new LinearAnalyticMeasurementModelGaussianUncertainty(odom_meas_pdf_);
00077 
00078     // create MEASUREMENT MODEL IMU
00079     ColumnVector measNoiseImu_Mu(3);  measNoiseImu_Mu = 0;
00080     SymmetricMatrix measNoiseImu_Cov(3);  measNoiseImu_Cov = 0;
00081     for (unsigned int i=1; i<=3; i++) measNoiseImu_Cov(i,i) = 1;
00082     Gaussian measurement_Uncertainty_Imu(measNoiseImu_Mu, measNoiseImu_Cov);
00083     Matrix Himu(3,6);  Himu = 0;
00084     Himu(1,4) = 1;    Himu(2,5) = 1;    Himu(3,6) = 1;
00085     imu_meas_pdf_   = new LinearAnalyticConditionalGaussian(Himu, measurement_Uncertainty_Imu);
00086     imu_meas_model_ = new LinearAnalyticMeasurementModelGaussianUncertainty(imu_meas_pdf_);
00087 
00088     // create MEASUREMENT MODEL VO
00089     ColumnVector measNoiseVo_Mu(6);  measNoiseVo_Mu = 0;
00090     SymmetricMatrix measNoiseVo_Cov(6);  measNoiseVo_Cov = 0;
00091     for (unsigned int i=1; i<=6; i++) measNoiseVo_Cov(i,i) = 1;
00092     Gaussian measurement_Uncertainty_Vo(measNoiseVo_Mu, measNoiseVo_Cov);
00093     Matrix Hvo(6,6);  Hvo = 0;
00094     Hvo(1,1) = 1;    Hvo(2,2) = 1;    Hvo(3,3) = 1;    Hvo(4,4) = 1;    Hvo(5,5) = 1;    Hvo(6,6) = 1;
00095     vo_meas_pdf_   = new LinearAnalyticConditionalGaussian(Hvo, measurement_Uncertainty_Vo);
00096     vo_meas_model_ = new LinearAnalyticMeasurementModelGaussianUncertainty(vo_meas_pdf_);
00097 
00098     // create MEASUREMENT MODEL GPS
00099     ColumnVector measNoiseGps_Mu(3);  measNoiseGps_Mu = 0;
00100     SymmetricMatrix measNoiseGps_Cov(3);  measNoiseGps_Cov = 0;
00101     for (unsigned int i=1; i<=3; i++) measNoiseGps_Cov(i,i) = 1;
00102     Gaussian measurement_Uncertainty_GPS(measNoiseGps_Mu, measNoiseGps_Cov);
00103     Matrix Hgps(3,6);  Hgps = 0;
00104     Hgps(1,1) = 1;    Hgps(2,2) = 1;    Hgps(3,3) = 1;    
00105     gps_meas_pdf_   = new LinearAnalyticConditionalGaussian(Hgps, measurement_Uncertainty_GPS);
00106     gps_meas_model_ = new LinearAnalyticMeasurementModelGaussianUncertainty(gps_meas_pdf_);
00107   };
00108 
00109 
00110 
00111   // destructor
00112   OdomEstimation::~OdomEstimation(){
00113     if (filter_) delete filter_;
00114     if (prior_)  delete prior_;
00115     delete odom_meas_model_;
00116     delete odom_meas_pdf_;
00117     delete imu_meas_model_;
00118     delete imu_meas_pdf_;
00119     delete vo_meas_model_;
00120     delete vo_meas_pdf_;
00121     delete gps_meas_model_;
00122     delete gps_meas_pdf_;
00123     delete sys_pdf_;
00124     delete sys_model_;
00125   };
00126 
00127 
00128   // initialize prior density of filter 
00129   void OdomEstimation::initialize(const Transform& prior, const Time& time)
00130   {
00131     // set prior of filter
00132     ColumnVector prior_Mu(6); 
00133     decomposeTransform(prior, prior_Mu(1), prior_Mu(2), prior_Mu(3), prior_Mu(4), prior_Mu(5), prior_Mu(6));
00134     SymmetricMatrix prior_Cov(6); 
00135     for (unsigned int i=1; i<=6; i++) {
00136       for (unsigned int j=1; j<=6; j++){
00137         if (i==j)  prior_Cov(i,j) = pow(0.001,2);
00138         else prior_Cov(i,j) = 0;
00139       }
00140     }
00141     prior_  = new Gaussian(prior_Mu,prior_Cov);
00142     filter_ = new ExtendedKalmanFilter(prior_);
00143 
00144     // remember prior
00145     addMeasurement(StampedTransform(prior, time, output_frame_, base_footprint_frame_));
00146     filter_estimate_old_vec_ = prior_Mu;
00147     filter_estimate_old_ = prior;
00148     filter_time_old_     = time;
00149 
00150     // filter initialized
00151     filter_initialized_ = true;
00152   }
00153 
00154 
00155 
00156 
00157 
00158   // update filter
00159   bool OdomEstimation::update(bool odom_active, bool imu_active, bool gps_active, bool vo_active, const Time&  filter_time, bool& diagnostics_res)
00160   {
00161     // only update filter when it is initialized
00162     if (!filter_initialized_){
00163       ROS_INFO("Cannot update filter when filter was not initialized first.");
00164       return false;
00165     }
00166 
00167     // only update filter for time later than current filter time
00168     double dt = (filter_time - filter_time_old_).toSec();
00169     if (dt == 0) return false;
00170     if (dt <  0){
00171       ROS_INFO("Will not update robot pose with time %f sec in the past.", dt);
00172       return false;
00173     }
00174     ROS_DEBUG("Update filter at time %f with dt %f", filter_time.toSec(), dt);
00175 
00176 
00177     // system update filter
00178     // --------------------
00179     // for now only add system noise
00180     ColumnVector vel_desi(2); vel_desi = 0;
00181     filter_->Update(sys_model_, vel_desi);
00182 
00183     
00184     // process odom measurement
00185     // ------------------------
00186     ROS_DEBUG("Process odom meas");
00187     if (odom_active){
00188       if (!transformer_.canTransform(base_footprint_frame_,"wheelodom", filter_time)){
00189         ROS_ERROR("filter time older than odom message buffer");
00190         return false;
00191       }
00192       transformer_.lookupTransform("wheelodom", base_footprint_frame_, filter_time, odom_meas_);
00193       if (odom_initialized_){
00194         // convert absolute odom measurements to relative odom measurements in horizontal plane
00195         Transform odom_rel_frame =  Transform(tf::createQuaternionFromYaw(filter_estimate_old_vec_(6)), 
00196                                               filter_estimate_old_.getOrigin()) * odom_meas_old_.inverse() * odom_meas_;
00197         ColumnVector odom_rel(6); 
00198         decomposeTransform(odom_rel_frame, odom_rel(1), odom_rel(2), odom_rel(3), odom_rel(4), odom_rel(5), odom_rel(6));
00199         angleOverflowCorrect(odom_rel(6), filter_estimate_old_vec_(6));
00200         // update filter
00201         odom_meas_pdf_->AdditiveNoiseSigmaSet(odom_covariance_ * pow(dt,2));
00202 
00203         ROS_DEBUG("Update filter with odom measurement %f %f %f %f %f %f", 
00204                   odom_rel(1), odom_rel(2), odom_rel(3), odom_rel(4), odom_rel(5), odom_rel(6));
00205         filter_->Update(odom_meas_model_, odom_rel);
00206         diagnostics_odom_rot_rel_ = odom_rel(6);
00207       }
00208       else{
00209         odom_initialized_ = true;
00210         diagnostics_odom_rot_rel_ = 0;
00211       }
00212       odom_meas_old_ = odom_meas_;
00213     }
00214     // sensor not active
00215     else odom_initialized_ = false;
00216 
00217     
00218     // process imu measurement
00219     // -----------------------
00220     if (imu_active){
00221       if (!transformer_.canTransform(base_footprint_frame_,"imu", filter_time)){
00222         ROS_ERROR("filter time older than imu message buffer");
00223         return false;
00224       }
00225       transformer_.lookupTransform("imu", base_footprint_frame_, filter_time, imu_meas_);
00226       if (imu_initialized_){
00227         // convert absolute imu yaw measurement to relative imu yaw measurement 
00228         Transform imu_rel_frame =  filter_estimate_old_ * imu_meas_old_.inverse() * imu_meas_;
00229         ColumnVector imu_rel(3); double tmp;
00230         decomposeTransform(imu_rel_frame, tmp, tmp, tmp, tmp, tmp, imu_rel(3));
00231         decomposeTransform(imu_meas_,     tmp, tmp, tmp, imu_rel(1), imu_rel(2), tmp);
00232         angleOverflowCorrect(imu_rel(3), filter_estimate_old_vec_(6));
00233         diagnostics_imu_rot_rel_ = imu_rel(3);
00234         // update filter
00235         imu_meas_pdf_->AdditiveNoiseSigmaSet(imu_covariance_ * pow(dt,2));
00236         filter_->Update(imu_meas_model_,  imu_rel);
00237       }
00238       else{
00239         imu_initialized_ = true;
00240         diagnostics_imu_rot_rel_ = 0;
00241       }
00242       imu_meas_old_ = imu_meas_; 
00243     }
00244     // sensor not active
00245     else imu_initialized_ = false;
00246     
00247     
00248     
00249     // process vo measurement
00250     // ----------------------
00251     if (vo_active){
00252       if (!transformer_.canTransform(base_footprint_frame_,"vo", filter_time)){
00253         ROS_ERROR("filter time older than vo message buffer");
00254         return false;
00255       }
00256       transformer_.lookupTransform("vo", base_footprint_frame_, filter_time, vo_meas_);
00257       if (vo_initialized_){
00258         // convert absolute vo measurements to relative vo measurements
00259         Transform vo_rel_frame =  filter_estimate_old_ * vo_meas_old_.inverse() * vo_meas_;
00260         ColumnVector vo_rel(6);
00261         decomposeTransform(vo_rel_frame, vo_rel(1),  vo_rel(2), vo_rel(3), vo_rel(4), vo_rel(5), vo_rel(6));
00262         angleOverflowCorrect(vo_rel(6), filter_estimate_old_vec_(6));
00263         // update filter
00264         vo_meas_pdf_->AdditiveNoiseSigmaSet(vo_covariance_ * pow(dt,2));
00265         filter_->Update(vo_meas_model_,  vo_rel);
00266       }
00267       else vo_initialized_ = true;
00268       vo_meas_old_ = vo_meas_;
00269     }
00270     // sensor not active
00271     else vo_initialized_ = false;
00272   
00273 
00274 
00275     // process gps measurement
00276     // ----------------------
00277     if (gps_active){
00278       if (!transformer_.canTransform(base_footprint_frame_,"gps", filter_time)){
00279         ROS_ERROR("filter time older than gps message buffer");
00280         return false;
00281       }
00282       transformer_.lookupTransform("gps", base_footprint_frame_, filter_time, gps_meas_);
00283       if (gps_initialized_){
00284         gps_meas_pdf_->AdditiveNoiseSigmaSet(gps_covariance_ * pow(dt,2));
00285         ColumnVector gps_vec(3);
00286         double tmp;
00287         //Take gps as an absolute measurement, do not convert to relative measurement
00288         decomposeTransform(gps_meas_, gps_vec(1), gps_vec(2), gps_vec(3), tmp, tmp, tmp);
00289         filter_->Update(gps_meas_model_,  gps_vec);
00290       }
00291       else {
00292         gps_initialized_ = true;
00293         gps_meas_old_ = gps_meas_;
00294       }
00295     }
00296     // sensor not active
00297     else gps_initialized_ = false;
00298 
00299   
00300     
00301     // remember last estimate
00302     filter_estimate_old_vec_ = filter_->PostGet()->ExpectedValueGet();
00303     tf::Quaternion q;
00304     q.setRPY(filter_estimate_old_vec_(4), filter_estimate_old_vec_(5), filter_estimate_old_vec_(6));
00305     filter_estimate_old_ = Transform(q,
00306                                      Vector3(filter_estimate_old_vec_(1), filter_estimate_old_vec_(2), filter_estimate_old_vec_(3)));
00307     filter_time_old_ = filter_time;
00308     addMeasurement(StampedTransform(filter_estimate_old_, filter_time, output_frame_, base_footprint_frame_));
00309 
00310     // diagnostics
00311     diagnostics_res = true;
00312     if (odom_active && imu_active){
00313       double diagnostics = fabs(diagnostics_odom_rot_rel_ - diagnostics_imu_rot_rel_)/dt;
00314       if (diagnostics > 0.3 && dt > 0.01){
00315         diagnostics_res = false;
00316       }
00317     }
00318 
00319     return true;
00320   };
00321 
00322   void OdomEstimation::addMeasurement(const StampedTransform& meas)
00323   {
00324     ROS_DEBUG("AddMeasurement from %s to %s:  (%f, %f, %f)  (%f, %f, %f, %f)",
00325               meas.frame_id_.c_str(), meas.child_frame_id_.c_str(),
00326               meas.getOrigin().x(), meas.getOrigin().y(), meas.getOrigin().z(),
00327               meas.getRotation().x(),  meas.getRotation().y(), 
00328               meas.getRotation().z(), meas.getRotation().w());
00329     transformer_.setTransform( meas );
00330   }
00331 
00332   void OdomEstimation::addMeasurement(const StampedTransform& meas, const MatrixWrapper::SymmetricMatrix& covar)
00333   {
00334     // check covariance
00335     for (unsigned int i=0; i<covar.rows(); i++){
00336       if (covar(i+1,i+1) == 0){
00337         ROS_ERROR("Covariance specified for measurement on topic %s is zero", meas.child_frame_id_.c_str());
00338         return;
00339       }
00340     }
00341     // add measurements
00342     addMeasurement(meas);
00343     if (meas.child_frame_id_ == "wheelodom") odom_covariance_ = covar;
00344     else if (meas.child_frame_id_ == "imu")  imu_covariance_  = covar;
00345     else if (meas.child_frame_id_ == "vo")   vo_covariance_   = covar;
00346     else if (meas.child_frame_id_ == "gps")  gps_covariance_  = covar;
00347     else ROS_ERROR("Adding a measurement for an unknown sensor %s", meas.child_frame_id_.c_str());
00348   };
00349 
00350 
00351   // get latest filter posterior as vector
00352   void OdomEstimation::getEstimate(MatrixWrapper::ColumnVector& estimate)
00353   {
00354     estimate = filter_estimate_old_vec_;
00355   };
00356 
00357   // get filter posterior at time 'time' as Transform
00358   void OdomEstimation::getEstimate(Time time, Transform& estimate)
00359   {
00360     StampedTransform tmp;
00361     if (!transformer_.canTransform(base_footprint_frame_,output_frame_, time)){
00362       ROS_ERROR("Cannot get transform at time %f", time.toSec());
00363       return;
00364     }
00365     transformer_.lookupTransform(output_frame_, base_footprint_frame_, time, tmp);
00366     estimate = tmp;
00367   };
00368 
00369   // get filter posterior at time 'time' as Stamped Transform
00370   void OdomEstimation::getEstimate(Time time, StampedTransform& estimate)
00371   {
00372     if (!transformer_.canTransform(output_frame_, base_footprint_frame_, time)){
00373       ROS_ERROR("Cannot get transform at time %f", time.toSec());
00374       return;
00375     }
00376     transformer_.lookupTransform(output_frame_, base_footprint_frame_, time, estimate);
00377   };
00378 
00379   // get most recent filter posterior as PoseWithCovarianceStamped
00380   void OdomEstimation::getEstimate(geometry_msgs::PoseWithCovarianceStamped& estimate)
00381   {
00382     // pose
00383     StampedTransform tmp;
00384     if (!transformer_.canTransform(output_frame_, base_footprint_frame_, ros::Time())){
00385       ROS_ERROR("Cannot get transform at time %f", 0.0);
00386       return;
00387     }
00388     transformer_.lookupTransform(output_frame_, base_footprint_frame_, ros::Time(), tmp);
00389     poseTFToMsg(tmp, estimate.pose.pose);
00390 
00391     // header
00392     estimate.header.stamp = tmp.stamp_;
00393     estimate.header.frame_id = output_frame_;
00394 
00395     // covariance
00396     SymmetricMatrix covar =  filter_->PostGet()->CovarianceGet();
00397     for (unsigned int i=0; i<6; i++)
00398       for (unsigned int j=0; j<6; j++)
00399         estimate.pose.covariance[6*i+j] = covar(i+1,j+1);
00400   };
00401 
00402   // correct for angle overflow
00403   void OdomEstimation::angleOverflowCorrect(double& a, double ref)
00404   {
00405     while ((a-ref) >  M_PI) a -= 2*M_PI;
00406     while ((a-ref) < -M_PI) a += 2*M_PI;
00407   };
00408 
00409   // decompose Transform into x,y,z,Rx,Ry,Rz
00410   void OdomEstimation::decomposeTransform(const StampedTransform& trans, 
00411                                            double& x, double& y, double&z, double&Rx, double& Ry, double& Rz){
00412     x = trans.getOrigin().x();   
00413     y = trans.getOrigin().y(); 
00414     z = trans.getOrigin().z(); 
00415     trans.getBasis().getEulerYPR(Rz, Ry, Rx);
00416   };
00417 
00418   // decompose Transform into x,y,z,Rx,Ry,Rz
00419   void OdomEstimation::decomposeTransform(const Transform& trans, 
00420                                            double& x, double& y, double&z, double&Rx, double& Ry, double& Rz){
00421     x = trans.getOrigin().x();   
00422     y = trans.getOrigin().y(); 
00423     z = trans.getOrigin().z(); 
00424     trans.getBasis().getEulerYPR(Rz, Ry, Rx);
00425   };
00426 
00427   void OdomEstimation::setOutputFrame(const std::string& output_frame){
00428         output_frame_ = output_frame;
00429   };
00430 
00431   void OdomEstimation::setBaseFootprintFrame(const std::string& base_frame){
00432         base_footprint_frame_ = base_frame;
00433   };
00434 
00435 }; // namespace


robot_pose_ekf
Author(s): Wim Meeussen, contradict@gmail.com
autogenerated on Sun Mar 3 2019 03:46:08