Homepage Demos Overview Downloads Tutorials Reference
Credits
Main Page | Namespace List | Class Hierarchy | Alphabetical List | Compound List | File List | Namespace Members | Compound Members | File Members | Related Pages | Search

afsMeasurementUpdate.cc

Go to the documentation of this file.
00001 /*
00002  * Measurement update code for the AIBO FastSLAM.
00003  * See header file for details.
00004  */
00005 
00006 #ifdef __cplusplus
00007 extern "C" {
00008 #endif
00009 
00010 #include "afsParticle.h"
00011 #include "afsTriangulator.h"
00012 #include "afsUtility.h"
00013 
00014 #include <math.h>
00015 #include <stdio.h>
00016 
00017 /* Prototypes */
00018 void doPriming(afsParticle *p, int landmark, double theta);
00019 void doKalmanUpdate(afsParticle *p, int landmark, double theta);
00020 
00021 /* The main measurement update routine. Calls one of the other specific
00022  * update routines and doesn't do much by itself. */
00023 void afsMeasurementUpdate(afsParticle *p, int landmark, double theta)
00024 {
00025   /* Add theta to the current heading of the robot to put it in
00026    * world coordinates. */
00027   theta += p->pose.theta;
00028   /* Constrain it to [0, 2pi) */
00029   while(theta < 0) theta += 2*M_PI;
00030   while(theta >= 2*M_PI) theta -= 2*M_PI;
00031 
00032   if(p->landmarks[landmark].state == PRIMING) doPriming(p, landmark, theta);
00033   else /* state = PRIMED */ doKalmanUpdate(p, landmark, theta);
00034 }
00035 
00036 /* This routine is used to generate an initial fix on the location of the
00037  * landmark by priming it with a position estimation derived from
00038  * triangulation. */
00039 void doPriming(afsParticle *p, int landmark, double theta)
00040 {
00041   /* We do this for brevity's sake. */
00042   afsLastObservation *l = &(p->landmarks[landmark].priming);
00043 
00044   /* If this is the very first time the landmark has been observed, we
00045    * simply store information about the observation. */
00046   if(l->empty) {
00047     l->x = p->pose.x;
00048     l->y = p->pose.y;
00049     l->theta = theta;
00050     l->empty = 0;
00051   }
00052   /* If the landmark has already been observed once before, we try to do
00053    * the triangulation. */
00054   else {
00055     if(afsTriangulator(l->x,l->y,l->theta, p->pose.x,p->pose.y,theta,
00056            &(p->landmarks[landmark]))) {
00057       /* Succesful triangulation--the triangulator has taken care of
00058        * everything. */
00059       p->landmarks[landmark].state = PRIMED;
00060     }
00061     else {
00062       /* Priming was not successful--chances are we were in a bad location
00063        * for triangulation. Just store the last measurement away for now--
00064        * we'll try again later. */
00065       l->x = p->pose.x;
00066       l->y = p->pose.y;
00067       l->theta = theta;
00068     }
00069   }
00070 
00071   /* We didn't make any weighty contribution */
00072   p->gotweight = 0;
00073   /* Finished */
00074 }
00075 
00076 /* We already have some idea of where the landmark is. Use Kalman filtering
00077  * to integrate information from this latest sensor reading into a position
00078  * estimate for the landmark. */
00079 /* Thanks to Mike "Mr. FastSLAM" Montemerlo for help with this. */
00080 void doKalmanUpdate(afsParticle *p, int landmark, double theta)
00081 {
00082   /* Here are all the matrices 'n stuff we need to do the Kalman filter */
00083   double H[2], K[2], expected_theta;
00084   /* Shorthand */
00085   afsLandmarkLoc *l = &(p->landmarks[landmark]);
00086   /* This q variable is used to compute the Jacobean, H. It's also used
00087    * later to alter the measurement covariance based on distance. See the
00088    * note below. */
00089   double q;
00090 
00091   /* What we now have is a great deal of 2D matrix math. */
00092   /* First we compute the Jacobean, H, which is as I understand it a
00093    * transformation from 2D location space to "measurement space" (?)
00094    * or at least the kind of numbers that bearings only sensors give us.
00095    * Actually, it's a linear approximation of such a transformation. */
00096   {
00097     double rel_x, rel_y;
00098 
00099     rel_x = l->mean.x - p->pose.x;
00100     rel_y = l->mean.y - p->pose.y;
00101 
00102     q = rel_x*rel_x + rel_y*rel_y;
00103     /* Check against divide by 0--hopefully it won't ever happen */
00104     if(q < 0.0000001) {
00105       fputs("RARE ERROR: near divide by 0 in doKalmanUpdate\n", stderr);
00106       p->gotweight = 0;
00107       return;
00108     }
00109 
00110     H[0] = -rel_y / q;
00111     H[1] = rel_x / q;
00112 
00113     /* While we're at it, let's also find the value of theta we expected to
00114      * see for this landmark. */
00115     expected_theta = atan2(rel_y, rel_x);
00116   }
00117 
00118   {
00119     /* Now we compute K, the Kalman gain. */
00120     double H_Sigma[2];
00121     double divisor;
00122     double dtheta;
00123 
00124     /* This is the measurement covariance we use to compute the Kalman gain.
00125      * Here, in bearings-only FastSLAM, it's dynamic. We increase the
00126      * covariance value linearly with the distance of the object. Measurements
00127      * about distant objects therefore have less effect on where the objects
00128      * are placed in the particle's map and don't drastically affect the
00129      * particle's weighting. This is a kludge, but then so is the EKF. */
00130     /* AFS_MEASURE_VARIANCE is now the measurement variance at a
00131      * distance of a meter. AFS_VARIANCE_MULTIPLIER is multiplied by the
00132      * distance and added to AFS_MEASUREMENT_VARIANCE to describe a linear
00133      * change in covariance. It is a Good Idea to set this value such that
00134      * covariance never becomes negative! */
00135     /* If you don't like any of this, set AFS_VARIANCE_MULTIPLIER to 1
00136      * and don't think about it. */
00137     double R = AFS_VARIANCE_MULTIPLIER*(sqrt(q) - 1000) + AFS_MEASURE_VARIANCE;
00138     /* We should never let R get below AFS_MEASURE_VARIANCE, so we correct
00139      * that here. */
00140     if(R < AFS_MEASURE_VARIANCE) R = AFS_MEASURE_VARIANCE;
00141 
00142     H_Sigma[0] = H[0] * l->variance.x  + H[1] * l->variance.xy;
00143     H_Sigma[1] = H[0] * l->variance.xy + H[1] * l->variance.y;
00144 
00145     /* Note measurement variance fudge factor */
00146     divisor = H_Sigma[0] * H[0] + H_Sigma[1] * H[1] + R;
00147 
00148     K[0] = H_Sigma[0] / divisor;
00149     K[1] = H_Sigma[1] / divisor;
00150 
00151     /* Hooray--now we can compute the new mean for the landmark */
00152     dtheta = find_dtheta(expected_theta, theta);
00153     l->mean.x += K[0] * dtheta;
00154     l->mean.y += K[1] * dtheta;
00155 
00156     /* Since we have the divisor and expected value already, we can go
00157      * ahead and compute the weight for this particle. */
00158     p->gotweight = 1;
00159     /* 2 * M_PI is removed -- scaling doesn't need it */
00160     /* Why do we multiply here? If we have multiple measurements before a
00161      * resampling, we want to be able to use them all to influence the
00162      * resampling. We multiply together all the weights from the measurements
00163      * to create a final weight that is used for resampling. DANGER: May
00164      * fall to zero after too many improbable measurements. 300 weights of
00165      * 0.05 is zero in double precision. The lesson: for now, move around
00166      * and resample frequently. */
00167     p->weight *= (1/sqrt(divisor)) * exp(-0.5*dtheta*dtheta/divisor);
00168   }
00169 
00170   /* As well as the new covariance! */
00171   {
00172     double KH_x, KH_y, KH_tr, KH_bl;
00173     double KHP_x, KHP_y, KHP_xy;
00174 
00175     KH_x  = K[0]*H[0];
00176     KH_y  = K[1]*H[1];
00177     KH_tr = K[0]*H[1];
00178     KH_bl = K[1]*H[0];
00179 
00180     KHP_x  = KH_x * l->variance.x + KH_tr * l->variance.xy;
00181     KHP_y  = KH_bl * l->variance.xy + KH_y * l->variance.y;
00182     KHP_xy = KH_bl * l->variance.x + KH_y * l->variance.xy;
00183 
00184     l->variance.x -= KHP_x;
00185     l->variance.y -= KHP_y;
00186     l->variance.xy -= KHP_xy;
00187   }
00188 
00189   /* Kalman filters are rad, even if I don't understand them very well */
00190 }
00191 
00192 #ifdef __cplusplus
00193 }
00194 #endif

Tekkotsu v1.4
Generated Sat Jul 19 00:06:29 2003 by Doxygen 1.3.2