Experimental Validation

≈

Figure 3-4: Histogram for the LG simulation that depicts the global uncertainties that result from the SEIF (left) and modified rule (right) sparsification strategies as compared to those of theKF. We compute the relative uncertainty for each feature as the log of the ratio between the determinant of the information filter covariance sub-block and the corresponding determinant from the actual distribution as maintained by the KF. Positive values correspond to conservative error estimates while negative log ratios indicate overconfidence. Both sparsification routines yield overconfident map estimates, though the inconsistency of the SEIFis more pronounced.

Note that in the process of expressing the map relative to the first feature, the orig-inal world origin is now included as a state element. We compute the confidence measures for the SEIF and modified rule relative to the KF based upon covariances associated with the root-shifted state, as before. Figures 3-5(b) and 3-5(a) plot the histograms associated with the modified rule and SEIF sparsification strategies, re-spectively. Unlike the global (nominal) distribution, the SEIF uncertainty estimates for the relative feature positions are closer to the values from the actual distribution.

The one exception is the estimate for the former world origin as expressed in the relative map, which remains overconfident as a result of the global inconsistency of the SEIF. Meanwhile, the modified rule remains slightly overconfident in the rela-tive estimates with confidence levels that are more similar to those of the underlying Gaussian.

The effect of sparsification on the covariance estimates is in-line with what is observed with the normalized errors. Though there is little difference between the three sets of feature position estimates, the normalized errors for the globalSEIFmap are larger due to the higher confidence attributed to the estimates. In the case of root-shifting the state, the histograms in Figure3-5 reveal a negligible difference between the relative uncertainty estimates associated with the three filters. Consequently, the uncertainty-based normalization has similar effects on each filter’s feature position errors.

3.4.2 Experimental Validation

Simulations are helpful in investigating our findings without having to take into con-sideration the effects of linearization. However, real-world SLAM applications typi-cally involve nonlinear vehicle motion and perception models, and include noise that

−0.006 −0.005 −0.004 −0.003 −0.002 −0.001 0 log of det(Σ)/det(Σ^KF)

−1.20 −0.8 −0.4 5

10 15 20 25

≈

(a) SEIF

−0.0015 −0.0010 −0.0005 0

0 10 20 30 40 50 60

log of det(Σ)/det(Σ^KF)

(b) Modified Rule

Figure 3-5: Histograms that show the uncertainties associated with the relative (a) SEIFand(b)modified rule map estimates relative to the baselineKF. We compute the uncertainty ratios based upon the relative covariance estimates that follow from root-shifting the state to the first feature added to the map. Unlike the global estimates, the SEIF is only slightly overconfident and performs similarly to the modified rule. The one outlier in the SEIF histogram corresponds to the representation for the original world origin in the root-shifted reference frame and is a consequence of the inconsistent global representation.

is not truly Gaussian. For that reason, we analyze the effects of the two sparsification strategies on a typical nonlinear dataset. As we show, the SEIF and modified rule yield posteriors with the same characteristics as those of the linear Gaussian (LG) simulations.

In our experiment, we operated an iRobot B21r wheeled robot in a gymnasium consisting of four adjacent tennis courts. A set of 64 track hurdles were positioned at known locations on the court baselines, which provide a convenient ground truth for the experiment. Figure A-1 within Appendix A presents a photograph of the environment. The vehicle recorded observations of the the relative position of the legs of neighboring hurdles with a SICK laser range finder as we drove it in a series of loops. Wheel encoders measured the vehicle’s forward velocity and rate of rotation, which we employ in the time projection step for each filter.

We independently implement two information filters, one that employs the SEIF

3.4. Experimental Results 71

Figure 3-6: The final global maps for the (a) SEIF and (b) modified rule, along with the three-sigma uncertainty ellipses. We compare each to the map generated with the standard EKF, as well as the manually-measured ground truth hurdle positions.

The SEIF maintains global feature estimates that are significantly overconfident as the uncertainty bounds do not capture the ground truth or the EKF estimates. The modified rule, meanwhile, yields estimates for absolute feature pose and uncertainty which are nearly identical to those of theEKF.

sparsification routine and a second filter that uses the modified rule to maintain a limit of Γa= 10 active features. As a basis for comparison, we also apply a standard EKF. We treat each hurdle as a single feature that we interpret as a 2D coordinate frame. The model considers one of the two hurdle legs, which we refer to as the “base”

leg, to be the origin of this frame and defines the positive x-axis in the direction of the second leg. Features are then parametrized by the translation and rotation of this coordinate frame. Each filter employs a kinematic model for the vehicle motion and treats the forward velocity and rotation rates as control inputs. The measurement model adapts the laser range and bearing observations into a measure of the position and orientation of the hurdle reference frame with respect to the vehicle’s body-fixed frame. We solve the data association problem independently in order to ensure that the correspondences are identical for all three filters. For a more detailed explanation of the experiment and the filter implementation, refer to AppendixA.2.

We first consider the posterior over the global state representation that results from the different sparsification routines. Figure 3-6 presents the final global map estimates for theSEIFand modified rule together with theEKF map and the ground truth hurdle positions. The ellipses denote the three-sigma confidence intervals

as-−10 0 10 20 30

Figure 3-7: A comparison of the relative maps estimates that result from the(a) SEIF and(b)modified rule, along with the three-sigma uncertainty ellipses. We compute the relative map by expressing the nominal state in the reference frame associated with the first hurdle added to the map. Both theSEIFand modified rule sparsification strategies yield estimates for the relative relationship between features that are nearly identical to those of the EKF.

sociated with the estimate for the base position for each hurdle. Much like the LG simulation, the final SEIF map estimates exhibit a distinctive degree of overconfi-dence. We see in the inset plot in Figure3-6(a) that the SEIF uncertainty estimates are overly tight and do not capture either the EKF position estimates or the ground truth. Empirically, this behavior supports the belief that the SEIF sparsification strategy yields global map estimates that are inconsistent. In contrast, the confi-dence intervals associated with the modified rule are much larger and account for the ground truth and EKF positions. Qualitatively, we also see that the modified rule yields estimates for the feature position and orientation that better approximate the EKF estimates. While the modified rule produces a posterior that remains overcon-fident with respect to theEKF, it maintains a distribution that better approximates that of the EKF.

As a study of the relative map estimate structure, we transform the state into the reference frame associated with the first hurdle added to the map. The result agrees with theLG analysis in that the quality of theSEIF estimates improves significantly when we consider the relative map relationships. We plot the relative maps for the SEIFand modified rule in Figure3-7alongside the root-shiftedEKFestimates and the ground truth. The ellipses again denote the three-sigma uncertainty bounds for the