robot_pose_ekf: robot_pose_ekf: odom_estimation.cpp Source 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   {
00057     // create SYSTEM MODEL
00058     ColumnVector sysNoise_Mu(6);  sysNoise_Mu = 0;
00059     SymmetricMatrix sysNoise_Cov(6); sysNoise_Cov = 0;
00060     for (unsigned int i=1; i<=6; i++) sysNoise_Cov(i,i) = pow(1000.0,2);
00061     Gaussian system_Uncertainty(sysNoise_Mu, sysNoise_Cov);
00062     sys_pdf_   = new NonLinearAnalyticConditionalGaussianOdo(system_Uncertainty);
00063     sys_model_ = new AnalyticSystemModelGaussianUncertainty(sys_pdf_);
00064 
00065     // create MEASUREMENT MODEL ODOM
00066     ColumnVector measNoiseOdom_Mu(6);  measNoiseOdom_Mu = 0;
00067     SymmetricMatrix measNoiseOdom_Cov(6);  measNoiseOdom_Cov = 0;
00068     for (unsigned int i=1; i<=6; i++) measNoiseOdom_Cov(i,i) = 1;
00069     Gaussian measurement_Uncertainty_Odom(measNoiseOdom_Mu, measNoiseOdom_Cov);
00070     Matrix Hodom(6,6);  Hodom = 0;
00071     Hodom(1,1) = 1;    Hodom(2,2) = 1;    Hodom(6,6) = 1;
00072     odom_meas_pdf_   = new LinearAnalyticConditionalGaussian(Hodom, measurement_Uncertainty_Odom);
00073     odom_meas_model_ = new LinearAnalyticMeasurementModelGaussianUncertainty(odom_meas_pdf_);
00074 
00075     // create MEASUREMENT MODEL IMU
00076     ColumnVector measNoiseImu_Mu(3);  measNoiseImu_Mu = 0;
00077     SymmetricMatrix measNoiseImu_Cov(3);  measNoiseImu_Cov = 0;
00078     for (unsigned int i=1; i<=3; i++) measNoiseImu_Cov(i,i) = 1;
00079     Gaussian measurement_Uncertainty_Imu(measNoiseImu_Mu, measNoiseImu_Cov);
00080     Matrix Himu(3,6);  Himu = 0;
00081     Himu(1,4) = 1;    Himu(2,5) = 1;    Himu(3,6) = 1;
00082     imu_meas_pdf_   = new LinearAnalyticConditionalGaussian(Himu, measurement_Uncertainty_Imu);
00083     imu_meas_model_ = new LinearAnalyticMeasurementModelGaussianUncertainty(imu_meas_pdf_);
00084 
00085     // create MEASUREMENT MODEL VO
00086     ColumnVector measNoiseVo_Mu(6);  measNoiseVo_Mu = 0;
00087     SymmetricMatrix measNoiseVo_Cov(6);  measNoiseVo_Cov = 0;
00088     for (unsigned int i=1; i<=6; i++) measNoiseVo_Cov(i,i) = 1;
00089     Gaussian measurement_Uncertainty_Vo(measNoiseVo_Mu, measNoiseVo_Cov);
00090     Matrix Hvo(6,6);  Hvo = 0;
00091     Hvo(1,1) = 1;    Hvo(2,2) = 1;    Hvo(3,3) = 1;    Hvo(4,4) = 1;    Hvo(5,5) = 1;    Hvo(6,6) = 1;
00092     vo_meas_pdf_   = new LinearAnalyticConditionalGaussian(Hvo, measurement_Uncertainty_Vo);
00093     vo_meas_model_ = new LinearAnalyticMeasurementModelGaussianUncertainty(vo_meas_pdf_);
00094   };
00095 
00096 
00097 
00098   // destructor
00099   OdomEstimation::~OdomEstimation(){
00100     if (filter_) delete filter_;
00101     if (prior_)  delete prior_;
00102     delete odom_meas_model_;
00103     delete odom_meas_pdf_;
00104     delete imu_meas_model_;
00105     delete imu_meas_pdf_;
00106     delete vo_meas_model_;
00107     delete vo_meas_pdf_;
00108     delete sys_pdf_;
00109     delete sys_model_;
00110   };
00111 
00112 
00113   // initialize prior density of filter 
00114   void OdomEstimation::initialize(const Transform& prior, const Time& time)
00115   {
00116     // set prior of filter
00117     ColumnVector prior_Mu(6); 
00118     decomposeTransform(prior, prior_Mu(1), prior_Mu(2), prior_Mu(3), prior_Mu(4), prior_Mu(5), prior_Mu(6));
00119     SymmetricMatrix prior_Cov(6); 
00120     for (unsigned int i=1; i<=6; i++) {
00121       for (unsigned int j=1; j<=6; j++){
00122         if (i==j)  prior_Cov(i,j) = pow(0.001,2);
00123         else prior_Cov(i,j) = 0;
00124       }
00125     }
00126     prior_  = new Gaussian(prior_Mu,prior_Cov);
00127     filter_ = new ExtendedKalmanFilter(prior_);
00128 
00129     // remember prior
00130     addMeasurement(StampedTransform(prior, time, "odom_combined", "base_footprint"));
00131     filter_estimate_old_vec_ = prior_Mu;
00132     filter_estimate_old_ = prior;
00133     filter_time_old_     = time;
00134 
00135     // filter initialized
00136     filter_initialized_ = true;
00137   }
00138 
00139 
00140 
00141 
00142 
00143   // update filter
00144   bool OdomEstimation::update(bool odom_active, bool imu_active, bool vo_active, const Time&  filter_time, bool& diagnostics_res)
00145   {
00146     // only update filter when it is initialized
00147     if (!filter_initialized_){
00148       ROS_INFO("Cannot update filter when filter was not initialized first.");
00149       return false;
00150     }
00151 
00152     // only update filter for time later than current filter time
00153     double dt = (filter_time - filter_time_old_).toSec();
00154     if (dt == 0) return false;
00155     if (dt <  0){
00156       ROS_INFO("Will not update robot pose with time %f sec in the past.", dt);
00157       return false;
00158     }
00159     ROS_DEBUG("Update filter at time %f with dt %f", filter_time.toSec(), dt);
00160 
00161 
00162     // system update filter
00163     // --------------------
00164     // for now only add system noise
00165     ColumnVector vel_desi(2); vel_desi = 0;
00166     filter_->Update(sys_model_, vel_desi);
00167 
00168     
00169     // process odom measurement
00170     // ------------------------
00171     ROS_DEBUG("Process odom meas");
00172     if (odom_active){
00173       if (!transformer_.canTransform("base_footprint","wheelodom", filter_time)){
00174         ROS_ERROR("filter time older than odom message buffer");
00175         return false;
00176       }
00177       transformer_.lookupTransform("wheelodom", "base_footprint", filter_time, odom_meas_);
00178       if (odom_initialized_){
00179         // convert absolute odom measurements to relative odom measurements in horizontal plane
00180         Transform odom_rel_frame =  Transform(tf::createQuaternionFromYaw(filter_estimate_old_vec_(6)), 
00181                                               filter_estimate_old_.getOrigin()) * odom_meas_old_.inverse() * odom_meas_;
00182         ColumnVector odom_rel(6); 
00183         decomposeTransform(odom_rel_frame, odom_rel(1), odom_rel(2), odom_rel(3), odom_rel(4), odom_rel(5), odom_rel(6));
00184         angleOverflowCorrect(odom_rel(6), filter_estimate_old_vec_(6));
00185         // update filter
00186         odom_meas_pdf_->AdditiveNoiseSigmaSet(odom_covariance_ * pow(dt,2));
00187 
00188         ROS_DEBUG("Update filter with odom measurement %f %f %f %f %f %f", 
00189                   odom_rel(1), odom_rel(2), odom_rel(3), odom_rel(4), odom_rel(5), odom_rel(6));
00190         filter_->Update(odom_meas_model_, odom_rel);
00191         diagnostics_odom_rot_rel_ = odom_rel(6);
00192       }
00193       else{
00194         odom_initialized_ = true;
00195         diagnostics_odom_rot_rel_ = 0;
00196       }
00197       odom_meas_old_ = odom_meas_;
00198     }
00199     // sensor not active
00200     else odom_initialized_ = false;
00201 
00202     
00203     // process imu measurement
00204     // -----------------------
00205     if (imu_active){
00206       if (!transformer_.canTransform("base_footprint","imu", filter_time)){
00207         ROS_ERROR("filter time older than imu message buffer");
00208         return false;
00209       }
00210       transformer_.lookupTransform("imu", "base_footprint", filter_time, imu_meas_);
00211       if (imu_initialized_){
00212         // convert absolute imu yaw measurement to relative imu yaw measurement 
00213         Transform imu_rel_frame =  filter_estimate_old_ * imu_meas_old_.inverse() * imu_meas_;
00214         ColumnVector imu_rel(3); double tmp;
00215         decomposeTransform(imu_rel_frame, tmp, tmp, tmp, tmp, tmp, imu_rel(3));
00216         decomposeTransform(imu_meas_,     tmp, tmp, tmp, imu_rel(1), imu_rel(2), tmp);
00217         angleOverflowCorrect(imu_rel(3), filter_estimate_old_vec_(6));
00218         diagnostics_imu_rot_rel_ = imu_rel(3);
00219         // update filter
00220         imu_meas_pdf_->AdditiveNoiseSigmaSet(imu_covariance_ * pow(dt,2));
00221         filter_->Update(imu_meas_model_,  imu_rel);
00222       }
00223       else{
00224         imu_initialized_ = true;
00225         diagnostics_imu_rot_rel_ = 0;
00226       }
00227       imu_meas_old_ = imu_meas_; 
00228     }
00229     // sensor not active
00230     else imu_initialized_ = false;
00231     
00232     
00233     
00234     // process vo measurement
00235     // ----------------------
00236     if (vo_active){
00237       if (!transformer_.canTransform("base_footprint","vo", filter_time)){
00238         ROS_ERROR("filter time older than vo message buffer");
00239         return false;
00240       }
00241       transformer_.lookupTransform("vo", "base_footprint", filter_time, vo_meas_);
00242       if (vo_initialized_){
00243         // convert absolute vo measurements to relative vo measurements
00244         Transform vo_rel_frame =  filter_estimate_old_ * vo_meas_old_.inverse() * vo_meas_;
00245         ColumnVector vo_rel(6);
00246         decomposeTransform(vo_rel_frame, vo_rel(1),  vo_rel(2), vo_rel(3), vo_rel(4), vo_rel(5), vo_rel(6));
00247         angleOverflowCorrect(vo_rel(6), filter_estimate_old_vec_(6));
00248         // update filter
00249         vo_meas_pdf_->AdditiveNoiseSigmaSet(vo_covariance_ * pow(dt,2));
00250         filter_->Update(vo_meas_model_,  vo_rel);
00251       }
00252       else vo_initialized_ = true;
00253       vo_meas_old_ = vo_meas_;
00254     }
00255     // sensor not active
00256     else vo_initialized_ = false;
00257     
00258     
00259     // remember last estimate
00260     filter_estimate_old_vec_ = filter_->PostGet()->ExpectedValueGet();
00261     tf::Quaternion q;
00262     q.setRPY(filter_estimate_old_vec_(4), filter_estimate_old_vec_(5), filter_estimate_old_vec_(6));
00263     filter_estimate_old_ = Transform(q,
00264                                      Vector3(filter_estimate_old_vec_(1), filter_estimate_old_vec_(2), filter_estimate_old_vec_(3)));
00265     filter_time_old_ = filter_time;
00266     addMeasurement(StampedTransform(filter_estimate_old_, filter_time, "odom_combined", "base_footprint"));
00267 
00268     // diagnostics
00269     diagnostics_res = true;
00270     if (odom_active && imu_active){
00271       double diagnostics = fabs(diagnostics_odom_rot_rel_ - diagnostics_imu_rot_rel_)/dt;
00272       if (diagnostics > 0.3 && dt > 0.01){
00273         diagnostics_res = false;
00274       }
00275     }
00276 
00277     return true;
00278 };
00279 
00280   void OdomEstimation::addMeasurement(const StampedTransform& meas)
00281   {
00282     ROS_DEBUG("AddMeasurement from %s to %s:  (%f, %f, %f)  (%f, %f, %f, %f)",
00283               meas.frame_id_.c_str(), meas.child_frame_id_.c_str(),
00284               meas.getOrigin().x(), meas.getOrigin().y(), meas.getOrigin().z(),
00285               meas.getRotation().x(),  meas.getRotation().y(), 
00286               meas.getRotation().z(), meas.getRotation().w());
00287     transformer_.setTransform( meas );
00288   }
00289 
00290   void OdomEstimation::addMeasurement(const StampedTransform& meas, const MatrixWrapper::SymmetricMatrix& covar)
00291   {
00292     // check covariance
00293     for (unsigned int i=0; i<covar.rows(); i++){
00294       if (covar(i+1,i+1) == 0){
00295         ROS_ERROR("Covariance specified for measurement on topic %s is zero", meas.child_frame_id_.c_str());
00296         return;
00297       }
00298     }
00299     // add measurements
00300     addMeasurement(meas);
00301     if (meas.child_frame_id_ == "wheelodom") odom_covariance_ = covar;
00302     else if (meas.child_frame_id_ == "imu")  imu_covariance_  = covar;
00303     else if (meas.child_frame_id_ == "vo")   vo_covariance_   = covar;
00304     else ROS_ERROR("Adding a measurement for an unknown sensor %s", meas.child_frame_id_.c_str());
00305   };
00306 
00307 
00308   // get latest filter posterior as vector
00309   void OdomEstimation::getEstimate(ColumnVector& estimate)
00310   {
00311     estimate = filter_estimate_old_vec_;
00312   };
00313 
00314   // get filter posterior at time 'time' as Transform
00315   void OdomEstimation::getEstimate(Time time, Transform& estimate)
00316   {
00317     StampedTransform tmp;
00318     if (!transformer_.canTransform("base_footprint","odom_combined", time)){
00319       ROS_ERROR("Cannot get transform at time %f", time.toSec());
00320       return;
00321     }
00322     transformer_.lookupTransform("odom_combined", "base_footprint", time, tmp);
00323     estimate = tmp;
00324   };
00325 
00326   // get filter posterior at time 'time' as Stamped Transform
00327   void OdomEstimation::getEstimate(Time time, StampedTransform& estimate)
00328   {
00329     if (!transformer_.canTransform("odom_combined", "base_footprint", time)){
00330       ROS_ERROR("Cannot get transform at time %f", time.toSec());
00331       return;
00332     }
00333     transformer_.lookupTransform("odom_combined", "base_footprint", time, estimate);
00334   };
00335 
00336   // get most recent filter posterior as PoseWithCovarianceStamped
00337   void OdomEstimation::getEstimate(geometry_msgs::PoseWithCovarianceStamped& estimate)
00338   {
00339     // pose
00340     StampedTransform tmp;
00341     if (!transformer_.canTransform("odom_combined", "base_footprint", ros::Time())){
00342       ROS_ERROR("Cannot get transform at time %f", 0.0);
00343       return;
00344     }
00345     transformer_.lookupTransform("odom_combined", "base_footprint", ros::Time(), tmp);
00346     poseTFToMsg(tmp, estimate.pose.pose);
00347 
00348     // header
00349     estimate.header.stamp = tmp.stamp_;
00350     estimate.header.frame_id = "odom";
00351 
00352     // covariance
00353     SymmetricMatrix covar =  filter_->PostGet()->CovarianceGet();
00354     for (unsigned int i=0; i<6; i++)
00355       for (unsigned int j=0; j<6; j++)
00356         estimate.pose.covariance[6*i+j] = covar(i+1,j+1);
00357   };
00358 
00359   // correct for angle overflow
00360   void OdomEstimation::angleOverflowCorrect(double& a, double ref)
00361   {
00362     while ((a-ref) >  M_PI) a -= 2*M_PI;
00363     while ((a-ref) < -M_PI) a += 2*M_PI;
00364   };
00365 
00366   // decompose Transform into x,y,z,Rx,Ry,Rz
00367   void OdomEstimation::decomposeTransform(const StampedTransform& trans, 
00368                                            double& x, double& y, double&z, double&Rx, double& Ry, double& Rz){
00369     x = trans.getOrigin().x();   
00370     y = trans.getOrigin().y(); 
00371     z = trans.getOrigin().z(); 
00372     trans.getBasis().getEulerYPR(Rz, Ry, Rx);
00373   };
00374 
00375   // decompose Transform into x,y,z,Rx,Ry,Rz
00376   void OdomEstimation::decomposeTransform(const Transform& trans, 
00377                                            double& x, double& y, double&z, double&Rx, double& Ry, double& Rz){
00378     x = trans.getOrigin().x();   
00379     y = trans.getOrigin().y(); 
00380     z = trans.getOrigin().z(); 
00381     trans.getBasis().getEulerYPR(Rz, Ry, Rx);
00382   };
00383 
00384 }; // namespace