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   {
00058     // create SYSTEM MODEL
00059     ColumnVector sysNoise_Mu(6);  sysNoise_Mu = 0;
00060     SymmetricMatrix sysNoise_Cov(6); sysNoise_Cov = 0;
00061     for (unsigned int i=1; i<=6; i++) sysNoise_Cov(i,i) = pow(1000.0,2);
00062     Gaussian system_Uncertainty(sysNoise_Mu, sysNoise_Cov);
00063     sys_pdf_   = new NonLinearAnalyticConditionalGaussianOdo(system_Uncertainty);
00064     sys_model_ = new AnalyticSystemModelGaussianUncertainty(sys_pdf_);
00065 
00066     // create MEASUREMENT MODEL ODOM
00067     ColumnVector measNoiseOdom_Mu(6);  measNoiseOdom_Mu = 0;
00068     SymmetricMatrix measNoiseOdom_Cov(6);  measNoiseOdom_Cov = 0;
00069     for (unsigned int i=1; i<=6; i++) measNoiseOdom_Cov(i,i) = 1;
00070     Gaussian measurement_Uncertainty_Odom(measNoiseOdom_Mu, measNoiseOdom_Cov);
00071     Matrix Hodom(6,6);  Hodom = 0;
00072     Hodom(1,1) = 1;    Hodom(2,2) = 1;    Hodom(6,6) = 1;
00073     odom_meas_pdf_   = new LinearAnalyticConditionalGaussian(Hodom, measurement_Uncertainty_Odom);
00074     odom_meas_model_ = new LinearAnalyticMeasurementModelGaussianUncertainty(odom_meas_pdf_);
00075 
00076     // create MEASUREMENT MODEL IMU
00077     ColumnVector measNoiseImu_Mu(3);  measNoiseImu_Mu = 0;
00078     SymmetricMatrix measNoiseImu_Cov(3);  measNoiseImu_Cov = 0;
00079     for (unsigned int i=1; i<=3; i++) measNoiseImu_Cov(i,i) = 1;
00080     Gaussian measurement_Uncertainty_Imu(measNoiseImu_Mu, measNoiseImu_Cov);
00081     Matrix Himu(3,6);  Himu = 0;
00082     Himu(1,4) = 1;    Himu(2,5) = 1;    Himu(3,6) = 1;
00083     imu_meas_pdf_   = new LinearAnalyticConditionalGaussian(Himu, measurement_Uncertainty_Imu);
00084     imu_meas_model_ = new LinearAnalyticMeasurementModelGaussianUncertainty(imu_meas_pdf_);
00085 
00086     // create MEASUREMENT MODEL VO
00087     ColumnVector measNoiseVo_Mu(6);  measNoiseVo_Mu = 0;
00088     SymmetricMatrix measNoiseVo_Cov(6);  measNoiseVo_Cov = 0;
00089     for (unsigned int i=1; i<=6; i++) measNoiseVo_Cov(i,i) = 1;
00090     Gaussian measurement_Uncertainty_Vo(measNoiseVo_Mu, measNoiseVo_Cov);
00091     Matrix Hvo(6,6);  Hvo = 0;
00092     Hvo(1,1) = 1;    Hvo(2,2) = 1;    Hvo(3,3) = 1;    Hvo(4,4) = 1;    Hvo(5,5) = 1;    Hvo(6,6) = 1;
00093     vo_meas_pdf_   = new LinearAnalyticConditionalGaussian(Hvo, measurement_Uncertainty_Vo);
00094     vo_meas_model_ = new LinearAnalyticMeasurementModelGaussianUncertainty(vo_meas_pdf_);
00095 
00096 
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, "odom_combined", "base_footprint"));
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","wheelodom", filter_time)){
00189         ROS_ERROR("filter time older than odom message buffer");
00190         return false;
00191       }
00192       transformer_.lookupTransform("wheelodom", "base_footprint", 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","imu", filter_time)){
00222         ROS_ERROR("filter time older than imu message buffer");
00223         return false;
00224       }
00225       transformer_.lookupTransform("imu", "base_footprint", filter_time, imu_meas_);
00226       if (imu_initialized_){
00227         // convert absolute imu yaw measurement to relative imu yaw measurement 
00228         Transform imu_rel_frame;
00229         if (gps_active)
00230                 imu_rel_frame = imu_meas_;
00231         else
00232                 imu_rel_frame =  filter_estimate_old_ * imu_meas_old_.inverse() * imu_meas_;
00233 
00234 
00235         ColumnVector imu_rel(3); double tmp;
00236         decomposeTransform(imu_rel_frame, tmp, tmp, tmp, tmp, tmp, imu_rel(3));
00237         decomposeTransform(imu_meas_,     tmp, tmp, tmp, imu_rel(1), imu_rel(2), tmp);
00238         angleOverflowCorrect(imu_rel(3), filter_estimate_old_vec_(6));
00239         diagnostics_imu_rot_rel_ = imu_rel(3);
00240         // update filter
00241         imu_meas_pdf_->AdditiveNoiseSigmaSet(imu_covariance_ * pow(dt,2));
00242         filter_->Update(imu_meas_model_,  imu_rel);
00243       }
00244       else{
00245         imu_initialized_ = true;
00246         diagnostics_imu_rot_rel_ = 0;
00247       }
00248       imu_meas_old_ = imu_meas_; 
00249     }
00250     // sensor not active
00251     else imu_initialized_ = false;
00252     
00253     
00254     
00255     // process vo measurement
00256     // ----------------------
00257     if (vo_active){
00258       if (!transformer_.canTransform("base_footprint","vo", filter_time)){
00259         ROS_ERROR("filter time older than vo message buffer");
00260         return false;
00261       }
00262       transformer_.lookupTransform("vo", "base_footprint", filter_time, vo_meas_);
00263       if (vo_initialized_){
00264         // convert absolute vo measurements to relative vo measurements
00265         Transform vo_rel_frame =  filter_estimate_old_ * vo_meas_old_.inverse() * vo_meas_;
00266         ColumnVector vo_rel(6);
00267         decomposeTransform(vo_rel_frame, vo_rel(1),  vo_rel(2), vo_rel(3), vo_rel(4), vo_rel(5), vo_rel(6));
00268         angleOverflowCorrect(vo_rel(6), filter_estimate_old_vec_(6));
00269         // update filter
00270         vo_meas_pdf_->AdditiveNoiseSigmaSet(vo_covariance_ * pow(dt,2));
00271         filter_->Update(vo_meas_model_,  vo_rel);
00272       }
00273       else vo_initialized_ = true;
00274       vo_meas_old_ = vo_meas_;
00275     }
00276     // sensor not active
00277     else vo_initialized_ = false;
00278   
00279 
00280 
00281 // process gps measurement
00282     // ----------------------
00283     if (gps_active){
00284       if (!transformer_.canTransform("base_footprint","gps", filter_time)){
00285         ROS_ERROR("filter time older than gps message buffer");
00286         return false;
00287       }
00288       transformer_.lookupTransform("gps", "base_footprint", filter_time, gps_meas_);
00289       if (gps_initialized_){
00290         gps_meas_pdf_->AdditiveNoiseSigmaSet(gps_covariance_ * pow(dt,2));
00291         ColumnVector gps_vec(3);
00292         double tmp;
00293         //Take gps as an absolute measurement, do not convert to relative measurement
00294         decomposeTransform(gps_meas_, gps_vec(1), gps_vec(2), gps_vec(3), tmp, tmp, tmp);
00295         filter_->Update(gps_meas_model_,  gps_vec);
00296       }
00297       else {
00298         gps_initialized_ = true;
00299         gps_meas_old_ = gps_meas_;
00300       }
00301     }
00302     // sensor not active
00303     else gps_initialized_ = false;
00304 
00305   
00306     
00307     // remember last estimate
00308     filter_estimate_old_vec_ = filter_->PostGet()->ExpectedValueGet();
00309     tf::Quaternion q;
00310     q.setRPY(filter_estimate_old_vec_(4), filter_estimate_old_vec_(5), filter_estimate_old_vec_(6));
00311     filter_estimate_old_ = Transform(q,
00312                                      Vector3(filter_estimate_old_vec_(1), filter_estimate_old_vec_(2), filter_estimate_old_vec_(3)));
00313     filter_time_old_ = filter_time;
00314     addMeasurement(StampedTransform(filter_estimate_old_, filter_time, "odom_combined", "base_footprint"));
00315 
00316     // diagnostics
00317     diagnostics_res = true;
00318     if (odom_active && imu_active){
00319       double diagnostics = fabs(diagnostics_odom_rot_rel_ - diagnostics_imu_rot_rel_)/dt;
00320       if (diagnostics > 0.3 && dt > 0.01){
00321         diagnostics_res = false;
00322       }
00323     }
00324 
00325     return true;
00326 };
00327 
00328   void OdomEstimation::addMeasurement(const StampedTransform& meas)
00329   {
00330     ROS_DEBUG("AddMeasurement from %s to %s:  (%f, %f, %f)  (%f, %f, %f, %f)",
00331               meas.frame_id_.c_str(), meas.child_frame_id_.c_str(),
00332               meas.getOrigin().x(), meas.getOrigin().y(), meas.getOrigin().z(),
00333               meas.getRotation().x(),  meas.getRotation().y(), 
00334               meas.getRotation().z(), meas.getRotation().w());
00335     transformer_.setTransform( meas );
00336   }
00337 
00338   void OdomEstimation::addMeasurement(const StampedTransform& meas, const MatrixWrapper::SymmetricMatrix& covar)
00339   {
00340     // check covariance
00341     for (unsigned int i=0; i<covar.rows(); i++){
00342       if (covar(i+1,i+1) == 0){
00343         ROS_ERROR("Covariance specified for measurement on topic %s is zero", meas.child_frame_id_.c_str());
00344         return;
00345       }
00346     }
00347     // add measurements
00348     addMeasurement(meas);
00349     if (meas.child_frame_id_ == "wheelodom") odom_covariance_ = covar;
00350     else if (meas.child_frame_id_ == "imu")  imu_covariance_  = covar;
00351     else if (meas.child_frame_id_ == "vo")   vo_covariance_   = covar;
00352     else if (meas.child_frame_id_ == "gps")  gps_covariance_  = covar;
00353     else ROS_ERROR("Adding a measurement for an unknown sensor %s", meas.child_frame_id_.c_str());
00354   };
00355 
00356 
00357   // get latest filter posterior as vector
00358   void OdomEstimation::getEstimate(ColumnVector& estimate)
00359   {
00360     estimate = filter_estimate_old_vec_;
00361   };
00362 
00363   // get filter posterior at time 'time' as Transform
00364   void OdomEstimation::getEstimate(Time time, Transform& estimate)
00365   {
00366     StampedTransform tmp;
00367     if (!transformer_.canTransform("base_footprint","odom_combined", time)){
00368       ROS_ERROR("Cannot get transform at time %f", time.toSec());
00369       return;
00370     }
00371     transformer_.lookupTransform("odom_combined", "base_footprint", time, tmp);
00372     estimate = tmp;
00373   };
00374 
00375   // get filter posterior at time 'time' as Stamped Transform
00376   void OdomEstimation::getEstimate(Time time, StampedTransform& estimate)
00377   {
00378     if (!transformer_.canTransform("odom_combined", "base_footprint", time)){
00379       ROS_ERROR("Cannot get transform at time %f", time.toSec());
00380       return;
00381     }
00382     transformer_.lookupTransform("odom_combined", "base_footprint", time, estimate);
00383   };
00384 
00385   // get most recent filter posterior as PoseWithCovarianceStamped
00386   void OdomEstimation::getEstimate(geometry_msgs::PoseWithCovarianceStamped& estimate)
00387   {
00388     // pose
00389     StampedTransform tmp;
00390     if (!transformer_.canTransform("odom_combined", "base_footprint", ros::Time())){
00391       ROS_ERROR("Cannot get transform at time %f", 0.0);
00392       return;
00393     }
00394     transformer_.lookupTransform("odom_combined", "base_footprint", ros::Time(), tmp);
00395     poseTFToMsg(tmp, estimate.pose.pose);
00396 
00397     // header
00398     estimate.header.stamp = tmp.stamp_;
00399     estimate.header.frame_id = "odom";
00400 
00401     // covariance
00402     SymmetricMatrix covar =  filter_->PostGet()->CovarianceGet();
00403     for (unsigned int i=0; i<6; i++)
00404       for (unsigned int j=0; j<6; j++)
00405         estimate.pose.covariance[6*i+j] = covar(i+1,j+1);
00406   };
00407 
00408   // correct for angle overflow
00409   void OdomEstimation::angleOverflowCorrect(double& a, double ref)
00410   {
00411     while ((a-ref) >  M_PI) a -= 2*M_PI;
00412     while ((a-ref) < -M_PI) a += 2*M_PI;
00413   };
00414 
00415   // decompose Transform into x,y,z,Rx,Ry,Rz
00416   void OdomEstimation::decomposeTransform(const StampedTransform& trans, 
00417                                            double& x, double& y, double&z, double&Rx, double& Ry, double& Rz){
00418     x = trans.getOrigin().x();   
00419     y = trans.getOrigin().y(); 
00420     z = trans.getOrigin().z(); 
00421     trans.getBasis().getEulerYPR(Rz, Ry, Rx);
00422   };
00423 
00424   // decompose Transform into x,y,z,Rx,Ry,Rz
00425   void OdomEstimation::decomposeTransform(const Transform& trans, 
00426                                            double& x, double& y, double&z, double&Rx, double& Ry, double& Rz){
00427     x = trans.getOrigin().x();   
00428     y = trans.getOrigin().y(); 
00429     z = trans.getOrigin().z(); 
00430     trans.getBasis().getEulerYPR(Rz, Ry, Rx);
00431   };
00432 
00433 }; // namespace


robot_pose_ekf
Author(s): Wim Meeussen
autogenerated on Sat Dec 28 2013 17:14:32