Homepage | Demos | Overview | Downloads | Tutorials | Reference | Credits |
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 |