amcl: amcl_odom.cpp Source File

00001 /*
00002  *  Player - One Hell of a Robot Server
00003  *  Copyright (C) 2000  Brian Gerkey   &  Kasper Stoy
00004  *                      gerkey@usc.edu    kaspers@robotics.usc.edu
00005  *
00006  *  This library is free software; you can redistribute it and/or
00007  *  modify it under the terms of the GNU Lesser General Public
00008  *  License as published by the Free Software Foundation; either
00009  *  version 2.1 of the License, or (at your option) any later version.
00010  *
00011  *  This library is distributed in the hope that it will be useful,
00012  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00013  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00014  *  Lesser General Public License for more details.
00015  *
00016  *  You should have received a copy of the GNU Lesser General Public
00017  *  License along with this library; if not, write to the Free Software
00018  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00019  *
00020  */
00022 //
00023 // Desc: AMCL odometry routines
00024 // Author: Andrew Howard
00025 // Date: 6 Feb 2003
00026 // CVS: $Id: amcl_odom.cc 7057 2008-10-02 00:44:06Z gbiggs $
00027 //
00029 
00030 #include <algorithm>
00031 
00032 #include <sys/types.h> // required by Darwin
00033 #include <math.h>
00034 
00035 #include "amcl_odom.h"
00036 
00037 using namespace amcl;
00038 
00039 static double
00040 normalize(double z)
00041 {
00042   return atan2(sin(z),cos(z));
00043 }
00044 static double
00045 angle_diff(double a, double b)
00046 {
00047   double d1, d2;
00048   a = normalize(a);
00049   b = normalize(b);
00050   d1 = a-b;
00051   d2 = 2*M_PI - fabs(d1);
00052   if(d1 > 0)
00053     d2 *= -1.0;
00054   if(fabs(d1) < fabs(d2))
00055     return(d1);
00056   else
00057     return(d2);
00058 }
00059 
00061 // Default constructor
00062 AMCLOdom::AMCLOdom() : AMCLSensor()
00063 {
00064   this->time = 0.0;
00065 }
00066 
00067 void
00068 AMCLOdom::SetModelDiff(double alpha1, 
00069                        double alpha2, 
00070                        double alpha3, 
00071                        double alpha4)
00072 {
00073   this->model_type = ODOM_MODEL_DIFF;
00074   this->alpha1 = alpha1;
00075   this->alpha2 = alpha2;
00076   this->alpha3 = alpha3;
00077   this->alpha4 = alpha4;
00078 }
00079 
00080 void
00081 AMCLOdom::SetModelOmni(double alpha1, 
00082                        double alpha2, 
00083                        double alpha3, 
00084                        double alpha4,
00085                        double alpha5)
00086 {
00087   this->model_type = ODOM_MODEL_OMNI;
00088   this->alpha1 = alpha1;
00089   this->alpha2 = alpha2;
00090   this->alpha3 = alpha3;
00091   this->alpha4 = alpha4;
00092   this->alpha5 = alpha5;
00093 }
00094 
00095 
00097 // Apply the action model
00098 bool AMCLOdom::UpdateAction(pf_t *pf, AMCLSensorData *data)
00099 {
00100   AMCLOdomData *ndata;
00101   ndata = (AMCLOdomData*) data;
00102 
00103   // Compute the new sample poses
00104   pf_sample_set_t *set;
00105 
00106   set = pf->sets + pf->current_set;
00107   pf_vector_t old_pose = pf_vector_sub(ndata->pose, ndata->delta);
00108 
00109   if(this->model_type == ODOM_MODEL_OMNI)
00110   {
00111     double delta_trans, delta_rot, delta_bearing;
00112     double delta_trans_hat, delta_rot_hat, delta_strafe_hat;
00113 
00114     delta_trans = sqrt(ndata->delta.v[0]*ndata->delta.v[0] +
00115                        ndata->delta.v[1]*ndata->delta.v[1]);
00116     delta_rot = ndata->delta.v[2];
00117 
00118     // Precompute a couple of things
00119     double trans_hat_stddev = (alpha3 * (delta_trans*delta_trans) +
00120                                alpha1 * (delta_rot*delta_rot));
00121     double rot_hat_stddev = (alpha4 * (delta_rot*delta_rot) +
00122                              alpha2 * (delta_trans*delta_trans));
00123     double strafe_hat_stddev = (alpha1 * (delta_rot*delta_rot) +
00124                                 alpha5 * (delta_trans*delta_trans));
00125 
00126     for (int i = 0; i < set->sample_count; i++)
00127     {
00128       pf_sample_t* sample = set->samples + i;
00129 
00130       delta_bearing = angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]),
00131                                  old_pose.v[2]) + sample->pose.v[2];
00132       double cs_bearing = cos(delta_bearing);
00133       double sn_bearing = sin(delta_bearing);
00134 
00135       // Sample pose differences
00136       delta_trans_hat = delta_trans + pf_ran_gaussian(trans_hat_stddev);
00137       delta_rot_hat = delta_rot + pf_ran_gaussian(rot_hat_stddev);
00138       delta_strafe_hat = 0 + pf_ran_gaussian(strafe_hat_stddev);
00139       // Apply sampled update to particle pose
00140       sample->pose.v[0] += (delta_trans_hat * cs_bearing + 
00141                             delta_strafe_hat * sn_bearing);
00142       sample->pose.v[1] += (delta_trans_hat * sn_bearing - 
00143                             delta_strafe_hat * cs_bearing);
00144       sample->pose.v[2] += delta_rot_hat ;
00145       sample->weight = 1.0 / set->sample_count;
00146     }
00147   }
00148   else //(this->model_type == ODOM_MODEL_DIFF)
00149   {
00150     // Implement sample_motion_odometry (Prob Rob p 136)
00151     double delta_rot1, delta_trans, delta_rot2;
00152     double delta_rot1_hat, delta_trans_hat, delta_rot2_hat;
00153     double delta_rot1_noise, delta_rot2_noise;
00154 
00155     // Avoid computing a bearing from two poses that are extremely near each
00156     // other (happens on in-place rotation).
00157     if(sqrt(ndata->delta.v[1]*ndata->delta.v[1] + 
00158             ndata->delta.v[0]*ndata->delta.v[0]) < 0.01)
00159       delta_rot1 = 0.0;
00160     else
00161       delta_rot1 = angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]),
00162                               old_pose.v[2]);
00163     delta_trans = sqrt(ndata->delta.v[0]*ndata->delta.v[0] +
00164                        ndata->delta.v[1]*ndata->delta.v[1]);
00165     delta_rot2 = angle_diff(ndata->delta.v[2], delta_rot1);
00166 
00167     // We want to treat backward and forward motion symmetrically for the
00168     // noise model to be applied below.  The standard model seems to assume
00169     // forward motion.
00170     delta_rot1_noise = std::min(fabs(angle_diff(delta_rot1,0.0)),
00171                                 fabs(angle_diff(delta_rot1,M_PI)));
00172     delta_rot2_noise = std::min(fabs(angle_diff(delta_rot2,0.0)),
00173                                 fabs(angle_diff(delta_rot2,M_PI)));
00174 
00175     for (int i = 0; i < set->sample_count; i++)
00176     {
00177       pf_sample_t* sample = set->samples + i;
00178 
00179       // Sample pose differences
00180       delta_rot1_hat = angle_diff(delta_rot1,
00181                                   pf_ran_gaussian(this->alpha1*delta_rot1_noise*delta_rot1_noise +
00182                                                   this->alpha2*delta_trans*delta_trans));
00183       delta_trans_hat = delta_trans - 
00184               pf_ran_gaussian(this->alpha3*delta_trans*delta_trans +
00185                               this->alpha4*delta_rot1_noise*delta_rot1_noise +
00186                               this->alpha4*delta_rot2_noise*delta_rot2_noise);
00187       delta_rot2_hat = angle_diff(delta_rot2,
00188                                   pf_ran_gaussian(this->alpha1*delta_rot2_noise*delta_rot2_noise +
00189                                                   this->alpha2*delta_trans*delta_trans));
00190 
00191       // Apply sampled update to particle pose
00192       sample->pose.v[0] += delta_trans_hat * 
00193               cos(sample->pose.v[2] + delta_rot1_hat);
00194       sample->pose.v[1] += delta_trans_hat * 
00195               sin(sample->pose.v[2] + delta_rot1_hat);
00196       sample->pose.v[2] += delta_rot1_hat + delta_rot2_hat;
00197       sample->weight = 1.0 / set->sample_count;
00198     }
00199   }
00200 
00201   return true;
00202 }