THE INTERNATIONAL JOURNAL OF ROBOTICS RESEARCH / October-November 2002
Fig. 1. Hand-measured model of a corridor (total length approximately 25 m).
Fig. 2. Laser (left) and sonar (right) data taken with a B21 mobile robot in the corridor shown in Figure 1, referenced to the dead-reckoning position estimate. The vehicle traveled back-and-forth three times following roughly the same path. Each sonar and laser return is shown referenced to odometry. The laser data are slightly smeared during turning due to a latency between the odometry and the laser data.
follow this formulation include Davison using vision (1998), Guivant and Nebot (2001), Castellanos et al. (2000), Dissanayake et al. (1999), and Jensfelt (2001) using laser sensing, and Leonard and Feder (2001), Tards et al. (2001), and Williams (2001) using sonar. Gibbens et al. have analyzed the closed-form solution of the linear, single-degree-of-freedom CML problem, yielding some insights into convergence. This body of work can be contrasted with other approaches to CML that do not rely on feature-based models, such as Gutmann and Konolige (1999), Thrun (2001) and Choset and Nagatani (2001). The general problem of CML incorporating data association ambiguity can be cast as a hybrid (mixed continuous/discrete) estimation problem in which tracking and data association are intertwined (Bar-Shalom and Fortmann 1988). General theoretical models for hybrid state estimation, such as multiple hypothesis tracking (Reid 1979; Mori, Chong, Tse, and Wishner 1986), present a staggeringly large computational burden when applied to CML. The navigation error of
the platform prevents one from splitting the solution into separate subgroups (known as clusters in the tracking literature (Kurien 1990)) corresponding to different parts of the environment (Cox and Leonard 1994). Most implementations of stochastic mapping decouple the discrete (decision-making) and continuous (ltering) parts of the problem. The extended Kalman lter (EKF) is typically used for continuous state estimation (Smith, Self, and Cheeseman 1987; Feder 1999; Guivant and Nebot 2001; Castellanos and Tards 2000). This is the method used in this paper, but it is not the only state estimation algorithm which could be used. For instance, sequential Monte Carlo algorithms (Doucet, de Freitas, and Gordan 2001; Thrun 2001) could be chosen instead. Discrete state estimationmaking decisions about the origins of measurementsis usually performed in CML with maximum likelihood methods such as nearest-neighbor gating (Bar-Shalom and Fortmann 1988). Such methods encounter difculties when the distance between features in the environment is smaller than the uncertainty in the robot

Leonard et al. / Mapping Partially Observable Features position. Unfortunately, this situation can arise frequently in practice. For example, in the data for the corridor shown in Figure 2, the doors are recessed about 10 cm from the corridor wall. The odometric uncertainty is much larger. In this situation, it is very difcult to associate measurements with features when considered in isolation. A more powerful technique that tests the joint compatibility of multiple sensor measurements, using a branch and bound algorithm, has been developed by Neira and Tards (2001) and successfully applied to CML using laser data (Newman, Leonard, Neira, and Tards 2002) and sonar data (Tards, Neira, Newman, and Leonard 2001). Data association is not the primary focus of this paper, however the topic is intimately linked with the problem of mapping partially observable features. It is desirable to employ methods for perceptual grouping that can reject outliers while nding sets of measurements that, when interpreted together, yield a consistent explanation. In this paper we focus on the state estimation aspects of this problem, describing a representation that allows the output of perceptual grouping routines to be consistently applied for mapping from multiple uncertain vantage points. In the computer vision community, the analogous problem to CML is structure from motion (SFM) (Faugeras 1993; Taylor and Kriegman 1995; Chiuso, Favaro, Jin, and Soatto 2000; Yagi, Shouya, and Yachida 2000; Hartley and Zisserman 2001). An early approach to SFM that used the EKF was developed by Ayache and Faugeras (1989). This work was one of the rst to apply geometric constraints in the state estimation process, such as the fusion of two features in the map that are asserted to be the same. Recently, constraint application has been incorporated in stochastic mapping algorithms by Chong and Kleeman (1997a), Tards et al. (2001) and Williams et al. (2001). Of recent work in computer vision, the work of Chiuso et al. (2000) and McLauchlan (2000) are the most closely related to the method presented in this paper. The algorithm of Smith, Self and Cheeseman (1987) for CML uses three models: (1) a robot motion model, (2) a feature mapping model, and (3) a measurement model. We refer to these as the functions f(), g(), and h(), respectively. The robot motion model uses knowledge of the robots dynamics for state projection. The feature mapping model uses sensor observations to estimate the location of a new geometric feature, so that it may be added to the map. The measurement model predicts observations of mapped features. The basic method assumes that features are stationary, and that there is only one robot, but it can be extended to accommodate dynamic features and/or multiple robots (Fenwick, Newman, and Leonard 2002). A signicant limitation of Smith, Self and Cheeseman (1987) relates to the initialization of new features. The method assumes that the full state of an object can be completely initialized using the measurement data available from a single vehicle position, obtained by a single robot. However, this situation is not satised in many important cases of practical

interest. In this paper, we present a generalized framework for CML that permits mapping of partially observable features. This can be applied to situations when the measurement data from a single location are insufcient to completely estimate the feature location, such as with wide-beam sonar measurements or angle-only measurements. It also enables composite features, comprised of multiple simple features, to be mapped, such as joining points and lines together to form polygons. The method also has application to mapping by multiple robots. The issue of estimating partially observable features with measurements obtained from multiple uncertain vantage points is clearly shared in both the CML and SFM problems. State-of-the-art vision algorithms for SFM use bundle adjustment (non-linear least-squares optimization) to concurrently estimate scene structure and camera motion (Triggs, McLauchlan, Hartley, and Fitzgibbon 2000). These techniques typically solve the association problem by using random sample consensus (RANSAC) (Fischler and Bolles 1981) or least medians (Faugeras, Luong, and Papadopoulo 2001) to nd groups of measurements that originate from the same point in the scene across multiple images. While most work has considered the batch SFM problem, there have been some approaches that adopt a recursive approach, which is highly important for navigation applications. Chiuso et al. (2000) have developed a real-time SFM system that can deal with occlusion. In their system, tracked features are not included into the full SFM solution until there is a high certainty that they provide good quality data. McLauchlan has developed the variable state dimension lter (VSDF), which is a hybrid batch/recursive technique that combines the characteristics of the EKF and bundle adjustment (McLauchlan 2000; McLauchlan and Murray 1996; McLauchlan and Murray 1995). The VSDF maintains a dynamic time window of observations and camera motion parameters. We employ a similar idea in this paper. Deans and Hebert (2000) have developed a related method for CML with bearing-only measurements and have performed an experimental investigation of its performance. More recently, Deans (2002) has provided several improvements to the VSDF, including an interpolation scheme that reduces the linearization error and a factorization method that yields computational efciency. The structure of this paper is as follows. Section 2 formally denes the problem under consideration. Previous work is reviewed and the problem of mapping with partial observability is formulated. Section 3 describes our new approach to this problem. The key idea is to add past vehicle positions to the state vector and to maintain explicitly estimates of the correlations between current and previous vehicle states. By incorporating past vehicle locations in the state vector, it becomes possible to consistently initialize new map features by combining data from multiple vantage points. We present two different types of experimental results with the method. In Section 4, we present a series of simplied examples that use manual data association to demonstrate the

Before considering strategies for computing eq. (1), consider rst the more restrictive problem of localization and mapping with prior knowledge of all the features and with no data association uncertainty. With perfect knowledge of A(k), we could discard the outliers and combine the remaining measurements of Z(k) into a composite measurement vector z(k). With prior knowledge of the number of features, and prior state estimates for all features, we are left with a conventional, xed-dimension state estimation problem. The general recursive solution applicable for fully non-linear and non-Gaussian systems is well known (Bucy and Senne 1971; Sorenson 1988) and is given by the following two equations p(x(k)|Z k1 , U k1 ) = p(x(k)|x(k 1), (2) u(k 1))p(x(k 1)|Z and p(x(k)|Z k , U k1 ) = ck p(z(k)|x(k))p(x(k)|Z k1 , U k1 ), k = 1, 2,. (3)

)dx(k 1)

where c1k = p(z(k)|x(k))p(x(k)|Z k1 , U k1 )dx(k). Equation 2 is the ChapmanKomolgorov equation, and represents the use of the dynamic model p(x(k)|x(k 1), u(k 1)) for state projection. Equation 3 is Bayes theorem, where p(z(k)|x(k)) is the measurement model. The direct application of eqs. (2) and (3) entails a computational burden that grows exponentially with the number of features, rendering such application computationally intractable for typical feature-based CML applications in environments with hundreds or more features. Recent work in sequential Monte Carlo methods (Doucet, de Freitas, and Gordan 2001) has achieved successful performance for many challenging nonlinear, non-Gaussian state estimation problems; difculties are encountered, however, in the application of sequential Monte Carlo methods in high-dimensional state spaces (MacCormick 2000).
Leonard et al. / Mapping Partially Observable Features Equations 2 and 3 assume that the correspondence problem is known. When data association uncertainty (the correspondence problem) is added to the formulation, we are left with a hybrid (mixed continuous/discrete) estimation problem. Mori et al. (1986) published a general recursive non-linear, nonGaussian algorithm for state estimation with assignment ambiguity. Their solution generalized an earlier linear-Gaussian method by Reid (1979), known as multiple hypothesis tracking (MHT). The solution builds an exponentially growing tree of hypotheses, with each leaf of the tree implementing a different solution to eqs. (2) and (3), based on different hypothesized assignments. Probabilities are assigned recursively to each discrete hypothesis, and pruning is used to restrict the number of hypotheses. While the Mori et al. (1986) solution can accommodate general non-linear, non-Gaussian models, to our knowledge it has never been implemented without simplifying assumptions. Even with the linear-Gaussian assumptions made by Reids algorithm, the method is exponentially complex due to the combinatorics of discrete decision making. The problem bears some resemblance to object recognition in computer vision (Grimson 1990). It is unclear how to incorporate variable-dimensionality (initialization of new features based on state estimates for the robot and other features in the map) into the Mori et al. (1986) algorithm. Hence, it is unclear if we can consider the Mori et al. (1986) as the general solution to eq. (1) for the CML problem. Our current opinion is that, because of the interactions between uncertainty and computational complexity, from a general theoretical perspective CML is an unsolved problem. 2.2. Linear-Gaussian Approximate Algorithms for CML The method published in Smith, Self, and Cheeseman (1987) is a linear-Gaussian approximation to the general solution of eqs. (2) and (3). Non-linear functions are linearized via a Taylor series expansion and all probability distributions are approximated by Gaussian distributions. State updates are performed with the EKF. With these approximations, and assuming that data association is known, the computational complexity is reduced to O(n2 ) (Moutarlier and Chatila 1989). The method recursively computes a state estimate x(k|k) = [ r (k|k)T xf (k)T ]T at each discrete time step k, where xr (k|k)T x and xf (k)T = [ f1 (k)T. xfn (k)T ]T are the robot and feature x state estimates, respectively. Based on assumptions about linearization and data association, this estimate is the approximate conditional mean of p(x(k)|Z k , U k1 ): x(k|k) E(x(k)|Z k , U k1 ). (4)

For example, for a sensor providing range and bearing measurements, zj (k) = [r ]T , the feature initialization function for a point g() takes the following form: xfn+1 (k) = g( (k|k), zj (k)) = x xr + r cos( + ). yr + r sin( + ) (8)
The new feature is integrated into the map by expanding the state vector x(k|k) and covariance P(k|k) as shown below x(k|k) Prr (k|k) P(k|k) Pf r (k|k) Pfn+1 r (k|k) where
T T Pfn+1 fn+1 (k|k) = Gx P(k|k)Gx + Gz R(k)Gz ,

x(k|k) , xfn+1 (k)

(9) Prfn+1 (k|k) Pffn+1 (k|k) , Pfn+1 fn+1 (k|k) (10)
Prf (k|k) Pff (k|k) Pfn+1 f (k|k)

Pfn+1 r (k|k)

Pfn+1 f (k|k) =
Pfn+1 r (k|k) Pfn+1 f (k|k)

= Gx P(k|k). (12)

Gx is the Jacobian of g() with respect to the state vector, and Gz is the Jacobian of g() with respect to the measurement.
3. Mapping Partially Observable Features using an Extended Representation
As mentioned above, the method of Smith, Self and Cheeseman (1987) assumes that there is sufcient information in the set of measurements available from a single robot position to completely and consistently initialize a new feature into the map. To enable CML in situations where this is not the case, we add past vehicle positions to the state vector and maintain explicitly estimates of the correlations between current and previous vehicle states. By incorporating past vehicle locations in the state vector, it becomes possible to make improved probabilistic data association and feature classication decisions and to initialize new map features by consistently combining data from multiple vantage points. The motivation for the new approach is the following. If the sensor observations available from a single time step do not provide sufcient information to initialize the state estimate of a newly detected feature, then information from multiple
Leonard et al. / Mapping Partially Observable Features vehicle positions must be used. To maintain consistent error bounds, correlations between different vehicle locations must be taken into account by the CML algorithm. Furthermore, decisions that are difcult based on the data from a single position (such as the disposition of an individual sonar return) can be made much easier when considered as delayed decisions, using data from multiple vehicle positions. Measurements can also be applied asynchronously, in batches of data from sequences of positions. To achieve these capabilities, we expand the representation to add a number of previous vehicle locations to the state vector. We refer to past vehicle states that are part of the stochastic map as trajectory states. We introduce the notation xtk (equivalent to xr (k)) to refer to the true state (pose) of the robot at time k = t. Using trajectory states, the CML problem embodied by eq. (1) is restated in expanded form as the recursive computation of p(x(k), Ak |Z k , U k1 ) = p(xt0 , xt1 ,. , xtk1 , xr (k), xf1 (k),. , xfnk (k), Ak |Z k , U k1 ). (13) The associated covariance matrix is Prr (k|k) P(k|k) = Ptr (k|k) Pf r (k|k) or equivalently,

New feature initialised by combining observations from multiple uncertain vantage points
xf [k|k]=g(Z,T) P ff [k-1|k]

z[k-1]

P rr [k|k]

xr [k-1|k]

Stored Observations Z

xr [k|k]

P rr [k-1|k]
Stored, Uncertain Vehicle Trajectory, T
Fig. 4. Illustration of initialization from multiple vantage points.
An appropriate predicted measurement vector is constructed: h( r (k 0 ), xf (k)) x h( r (k 1 ), xf (k)) x h( r (k 2 ), xf (k)) x zB = . . . h( r (k n ), xf (k)) x
R= (26) 0 0. 0 R(k 0 ) 0 R(k 1 ) 0. 0 0 R(k 2 ). . . 0. R(k n ) Using these components and the standard Kalman update equations, all robot trajectory positions and all features that are currently in map are concurrently updated. 3.4. Composite Initialization In general, the feature initialization function can use any of the information in the state estimation x for the stochastic map, including the locations of other previously mapped features as well as as previous vehicle states. The new feature location can be a function of one or more previous mapping features, xfi , one or more measurements, and the robot state estimate corresponding to these measurements. For example, we can initialize a line that passes through a point feature xfi and is tangent to one sonar return zj (k). In this case, the feature initialize function is of the form: xfn+1 = g( fi , xr (k), zj (k)). x (27)
This leads to an innovation which includes observations from across multiple time steps: z(k 0 ) h( r (k 0 ), xf (k)) x z(k 1 ) h( r (k 1 ), xf (k)) x x = z(k 2 ) h( r (k 2 ), xf (k)). . . z(k n ) h( r (k n ), xf (k)) x
Similarly, the measurement Jacobian Hx is constructed, as is the measurement covariance R: Hxr (k0 ),xf (k) Hxr (k1 ),xf (k) Hx = Hxr (k2 ),xf (k) . . Hxr (kn ),xf (k)
Alternatively, we can initialize a point that lies at the intersection of a line currently in the map and a new sonar return. The equations for these two initialization scenarios are described in Appendix A 1.3. We can also initialize a new feature without
Leonard et al. / Mapping Partially Observable Features any measurements, for example, hypothesizing the constraint that a new point feature exists at the intersection of two line segments currently in the map. Examples with real data for several of these scenarios are given below in Section 4. 3.5. Extension to Mapping by Multiple Robots Cooperative stochastic mapping with perfect communications requires adding robots to the state vector (Fenwick, Newman, and Leonard 2002). Although there are numerous technical issues to overcome, from a state estimation perspective it is relatively straightforward to perform feature initialization using data from multiple robots. By using trajectory states, such initializations can occur on a delayed basis, removing the requirement for the two robots to sense the features in question at precisely the same time. It is also possible for one robot to map the location of another robot (add the other robot to its state vector), through processing measurements of common features (assuming that association is known). A simplied example of how this process can work is shown below in Section 4.

Table 1. Details for the Ten Manually-selected Returns Used for Feature Initialization Return number Robot Time Step Odometry X (m) Odometry Y (m) 0.0 0.0 0.0045 0.0399 0.0235 0.0235 (unused) (unused) (unused) (unused) 0.0 0.0 0.4992 1.7637 2.537 2.5373 (unused) (unused) (unused) (unused)
Range (m) 1.0036 1.8058 1.8380 0.9504 2.7205 1.4192 0.9773 1.7851 1.0054 1.5692
Table 2. Method of Initialization for Features Feature Point object Lower right vertex of triangle Right plane of triangle Upper right vertex of triangle Position 1 for robot 2 Position 2 for robot 2
Initialization Method Return 1 and Return 5 Return 2 and Return 6 Lower right vertex of triangle and Return 4 Right plane of triangle and Return 3 Point object and Returns 7 and 8 Point object and Returns 9 and 10
Table 3. Comparison of Hand-measured and Estimated Feature Locations for Points Features (in meters) Hand measured CML estimated Feature x y x y Point object Lower right vertex of triangle Upper right vertex of triangle Left vertex of triangle 1.0 1.0 1.0 1.4763 0.0 1.5 2.05 1.77 1.0054 0.99 0.9922 1.4908 0.0109 1.5045 2.0837 1.7846
Leonard et al. / Mapping Partially Observable Features

y, in meter s

Fig. 5. Dead-reckoned trajectory of robot 1. The robot started at the bottom and moved upwards. The ellipses represent the 99% condence interval for each position. The triangle is an aluminum sonar target, and the small lled circle at the position x = 1.0 and y = 0.0 is a shing bobber, which served as a point object.

x(j)x(k)

0.4 0.3
Fig. 6. Correlation coefcients between the x components of the trajectory of robot 1. Each line represents the correlations between a specic time step and all other time steps.

2 x, in meter s

10,k 9,k 8,k 7,k 6,k

2 y, in meter s

Fig. 7. Set of observations used to initialize the side of the triangle and the point object. Two observations are needed to initialize the point target, two more are needed to initialize the corner of the triangle. Given the constraint of the corner, only one measurement was needed to map the wall. Given the wall, only one more measurement was needed to map the top corner of the triangle.

y, in meters

2 x, in meters
Fig. 8. Initial map. 99% condence intervals for the corners and the point object are shown.
experiment 4: complex initializations of partially observable objects 4
Fig. 9. Map after a batch update. Note the improved condence intervals for the features and the robot.
It is determined that robot 2 has observed features in the map of robot 1. From the ranges to the point object and the bottom corner of the triangle, (Returns 710 as labeled in Figure 10) the rst two positions of robot 2 can be observed. Those two positions are initialized into the map, and their initialization function is of the form g(xfr1 , zr2 ), meaning that robot 2 is added to the map of robot 1 using robot 2s observations of features that have been previously mapped by robot 1 (Figure 11). Next, using the rst two estimated positions for robot 2, the initial heading and velocity of robot 2 are mapped. Using this information, along with the control inputs, a dead-reckoned trajectory for robot 2 is established (Figure 12). Because neither compass nor velocity observations are available, and because the initial estimates of velocity and heading for robot 2 are imprecise, the trajectory is imprecise and has large error bounds. Having a dead-reckoned trajectory for robot 2 and its measurements, robot 1 then maps the (otherwise unobservable) back side of the triangle and performs a batch update to get an improved map and an improved estimate of where robot 2 traveled (Figure 13). Robot 2 obtained 22 total measurements. Of these, four measurements were used to initialize the position of robot 2 in the stochastic map of robot 1. Two measurements were used in initializing features on the left side of the triangle, and the remaining 16 returns were used in batch updating.

The error bounds for robot 2 do not exhibit the growth prole that is normally seen. Normally, since the robot starts with an initial position estimate and then moves, the uncertainty grows with time. In this case, since the second robot is mapped and localized with respect to previously mapped features, the smoothed estimate of its trajectory has the least uncertainty in the middle (Figure 14). The next experiment is for data from a simple box environment made of plywood, demonstrating the process of multiple vantage point initialization, batch updating, and composite feature initialization. The data association and feature modeling techniques utilize the a priori knowledge of the structure of the box, namely that each corner of the box was created by two walls, and that each wall was bounded at each end by a corner. The input data consists of 600 sonar returns acquired from a sequence of 50 positions that form one-anda-half loops around the inside of the box. The vehicle started in the lower left corner facing upward. The data processing proceeded as follows. First, state projection was performed and trajectory states were created for time steps 150, without any measurements being processed. At each processing cycle, a new vehicle trajectory state was added to the system state vector, using eqs. (17) and (18). The dead-reckoned vehicle trajectory is shown in Figure 16(b). After 50 cycles, a manually-guided search strategy was performed to nd nine returns that were used to initialize nine features (the four corners and four walls of the box and a
Fig. 10. Adding in the second robot. The true trajectory for robot 2 is the line on the left. Observations of the bottom corner of the triangle and of the shing bobber are reversed to nd the rst two vantage points.
Fig. 11. The rst two positions for robot 2 are mapped. Using these two positions, it is possible to estimate the initial heading and velocity.

2 s x, in meter

Fig. 12. Using the estimated initial heading and velocity for robot 2, along with its control inputs, a dead-reckoned trajectory is constructed. With poor initial estimates of heading and velocity, and no compass or velocity measurements for updates, the trajectory is very imprecise.
experiment 8: cooperative using a robot of convenience 4
Fig. 13. Using information from robot 2, robot 1 is able to map the back side of the triangle, which it otherwise could not observe. After the batch update, its estimate of robot 2 improves considerably.

meters

6 timestep
Fig. 14. Error in the smoothed estimate and three- condence interval for the x position of robot 2. The minimum uncertainty is in the middle of the trajectory due to forward/backward smoothing.

Fig. 15. B21 mobile robot in the plywood box.

North (m)

0 East (m)
(b) Fig. 16. (a) Set of all measurements processed, from 50 vehicle positions. Each sonar return is shown as a circular arc, with rays drawn from the center of the dead-reckoned robot position to the center of each arc. (b) Dead-reckoned vehicle trajectory, with three- error ellipses.
THE INTERNATIONAL JOURNAL OF ROBOTICS RESEARCH / October-November 2002 The data from a standard ring of Polaroid sonar sensors can be notoriously difcult to interpret. This leads many researchers away from a geometric approach to sonar mapping. However, using a physics-based sensor model, the geometric constraints provided by an individual sonar return can be formulated (Leonard and Durrant-Whyte 1992). Each return could originate from various types of features (point, plane, etc.) or could be spurious. For each type of feature, there is a limited range of locations for a potential feature that are possible. Given these constraints, the Hough transform (Ballard and Brown 1982) can be used as a voting scheme to identify point and planar features. More details on this technique are contained in Tards et al. (2001). The method is similar in spirit to the TBF method of Wijk and Christensen (Wijk and Christensen 2000), but can also directly identify specular planar reectors from sonar data, which is vitally important in typical man-made environments with many smooth walls. The output from the Hough transform gives sets of measurements with a high likelihood to originate from a single point or plane feature. Each candidate set from the Hough typically contains between 10 and 40 sonar returns hypothesized to originate from a new feature. For each candidate set, two returns are chosen to serve as seed features for the initialization, to be used in the function g(), and the remaining returns are used in a batch update. The rst of the two seed measurements is chosen to be the return in the candidate set that originates from the earliest vehicle position from the set of trajectory states. The other seed measurement is chosen to be the earliest return that, when combined with the rst seed measurement, achieves a sufcient minimum baseline for feature initialization (typically 0.6 m). Once a new feature is initialized, it is discarded if it has too small a baseline. To successfully distinguish doors from the walls in the corridor experiment shown in Figure 1, a minimum valid line length of 1.2 m is used for adding a feature into the map. (This restriction can be removed if the joint compatibility method of Neira and Tards (2001) is applied.) For state estimation, we have a choice between two basic strategies: (1) attempt to match individual measurements to pre-existing features, or (2) use measurements exclusively for new feature initialization and batch updating, followed by feature fusion with previously mapped features to obtain error reduction. A hybrid strategy that mixes both policies is also possible. While we have had good success with either (1) only, (2) only, or a mix of both, in this paper we focus on option (2), namely new feature mapping followed by feature fusion; see Ayache and Faugeras (Ayache and Faugeras 1989), Chong and Kleeman (Chong and Kleeman 1997a), Tards et al. (2001) and Williams et al. (2001) for further discussion of feature fusion. To determine when features should be fused together, we use the Mahalanobis distance and nearest neighbor. To illustrate the performance of the implementation, we present results from two different simplied settings: one experiment with two point objects only (cylinders of known

6. Conclusion

In this paper we have described a generalized framework for CML that incorporates temporal as well as spatial correla-
(b) Fig. 19. (a) Sonar measurements that uniquely gated with the nine initialized features, to be used in the batch update. (b) Feature location estimates, vehicle trajectory, and error ellipses after the batch update.
ASYNCHRONOUS DATA AQUISITION

SYNCHRONOUS PROCESSING

Any applicable technique can be applied here, for example Hough transform, RANSAC, least medians, maximum likely-hood

State Projection

new data available
Data Collection Perceptual Grouping
new and existing unexplained data is combined with a history of vehicle states to search for a stable initialisation of a new feature
raw sensor data Individual Observations

grouped obsevations

DATA ASSOCIATION

Data Storage

positive associations
unexplained observations no new data

Map Update

Feature Manufacture

stable initialisation

ambiguity

Batch Update

Delayed State Management

new feature

all newly explained observations are used during initialisation Feature Management Addition and removal of past vehicle states
Features can be fused with each other (equivalence assertion). Stagnant features can be removed (garbage collection). Compound features can be built.
Fig. 20. Information ow for cycle integrated real-time CML incorporating delayed state management, perceptual grouping, multiple vantage point initialization, batch updating, and feature fusion.
odometry trajectory and measurements referenced to the odometry trajectory
cml trajectory and measurements referenced to the cml trajectory 1
Fig. 21. (a) Raw sonar data for experiment with two point objects, referenced to odometry. (b) Sonar returns matched to the two features, referenced to the CML estimated trajectory. The experiment was 50 min long. The vehicle moved continuously under manual control at a speed of 0.1 m per second, making about 15 loops of the two cylinders. (See Extension 1.)
vehicle east error bound vs. time 1 meters meters 0.5 meters 0 0. meters 1 meters 0.5 meters 0 0. Feature 0 north error bound vs. time Feature 0 east error bound vs. time Feature 0 north error bound vs. time Feature 0 east error bound vs. time Vehicle heading error bound xy cor. coefficient vs. time 0 vehicle north error bound vs. time

3500 time (seconds) Fig. 22. Estimated error bounds for the experiment: top plots, three- bounds for x and y of the vehicle; next plot, xy correlation coefcient; next plot, three- bounds for vehicle heading; bottom four plots, three- bounds for the x and y locations of the two features. There is no ground-truth for this experiment, however the vehicle returned to within a few inches of the start position, commensurate with the CML algorithm state estimation error.

degrees

Fig. 23. Raw data for corridor experiment, referenced to odometry.
ing the current laser scans with another previously obtained laser scan. A benet of this type of approach is that the data association problem does not need to be solved for individual sensor measurements. Very impressive experimental results have been obtained with both approaches. With sonar, the raw data is usually too noisy and ambiguous for these correlationbased approaches to work. Recent work in feature-based CML has shown the importance of maintaining spatial correlations between the state estimates for different features, in order to maintain consistent error bounds (Castellanos and Tards 2000; Dissanayake et al. 1999). The representation of spatial correlations results in an O(n2 ) growth in computational cost (Moutarlier and Chatila 1989), where n is the number of features in the environment. This has motivated techniques to address the map scaling problem through spatial and temporal partitioning (Davison 1998; Leonard and Feder 2001; Guivant and Nebot 2001). In the current paper we have not addressed the map scaling problem, however the paper provides a framework for increasing the reliability of local map building. This is anticipated to greatly expand the range of environments in which CML can be successfully performed. For a given new type of environment, it is essential to establish reliable local mapping before considering the large-scale mapping problem. An alternative to achieve the sonar mapping results pre-
sented here is to use a custom sonar array that can classify and initialize geometric primitives from a single vantage point. The state-of-the-art in this area is exhibited by the work of Kleeman et al. (Kleeman 2001; Heale and Kleeman 2001; Chong and Kleeman 1997a; Kleeman and Kuc 1993). For example, Chong and Kleeman (Chong and Kleeman 1997) have used custom advanced sonar arrays to very good effect in testing large-scale CML algorithms; since this is a scanning sonar, the robot has to stop and scan at each location. However, more recently, Heale and Kleeman (2001) have demonstrated a small, multi-element sensor that performs rapid classication to enable mapping while moving. Nonetheless, attempting to perform CML with the standard ring of Polaroid sensors is an interesting and important problem from both a practical and a basic science perspective. The challenges of range-only interpretation explicitly capture many important uncertainty management problems posed by CML. The fundamental essence of sonar as a range-only sensor providing only sparse information is maintained in a manner that can be applied to alternative, more general situations, such as multi-robot mapping. Much further research is necessary to extend the approach to complex environments, such as the mapping of underwater terrain; however, we anticipate the generalized framework for CML presented here to be broadly applicable in a variety of environments.

Leonard et al. / Mapping Partially Observable Features This is the rst leg of a right triangle. The hypotenuse is the range measurement. The angle between the two is = arccos d r. (51)
Knowing the bearing to the line is, by denition, , the two intersections are therefore x + cos ( ) x1 = y + sin ( ) y1 x + cos ( + ) x1 =. y + sin ( + ) y1 (52)
Appendix B: Index to Multimedia Extensions
The multimedia extension page is found at http://www. ijrr.org. Table of Multimedia Extensions Extension Type Description Video Video Video jpg Sonar experiment with two objects Measurement processing for corridor experiment Mapped features for corridor experiment Map and odometry trajectory for large-scale experiment Perceptual grouping output for large-scale experiment using RANSAC

Acknowledgments

This research has been funded in part by NSF Career Award BES-9733040, the MIT Sea Grant College Program under grant NA86RG0074 (project RCM-3), and the Ofce of Naval Research under grant N00014-97-0202. The authors would like to thank J.D. Tards and J. Neira for helpful discussions and the contribution of the Hough transform feature classication technique to the results reported in Section 5.

References

Anderson, B. D. O., and Moore, J. B. 1979. Optimal Filtering. Englewood Cliffs, NJ: Prentice-Hall. Au, W. 1993. The Sonar of Dolphins. New York: SpringerVerlag. Ayache, N., and Faugeras, O. 1989. Maintaining representations of the environment of a mobile robot. IEEE Trans. Robotics Automation 5(6):804819.
Ballard, D., and Brown, C. 1982. Computer Vision. Englewood Cliffs, NJ: Prentice-Hall. Bar-Shalom, Y., and Fortmann, T. E. 1988. Tracking and Data Association. New York: Academic. Bosse, M., Newman, P., Leonard, J., and Teller, S. 2002a. An atlas framework for scalable mapping. Technical Report Marine Robotics Laboratory Technical Memorandum 02-04, Massachusetts Institute of Technology. Bosse, M., Rikoski, R., Leonard, J., and Teller, S. September 2002b. Vanishing points and 3d lines from omnidirectional video. In Int. Conf. on Image Processing, Rochester, NY. Brooks, R. A. 1984. Aspects of mobile robot visual map making. In 2nd Int. Symp. Robotics Research, Tokyo, Japan, pp. 287293. MIT Press. Bucy, R. S., and Senne, K. D. 1971. Digital synthesis of nonlinear lters. Automatica 7:287298. Carpenter, R. N. 2001. Concurrent mapping and localization and map matching on autonomous underwater vehicles. In IEEE Oceans, Hawaii, pp. 380389. Castellanos, J. A., and Tards, J. D. 2000. Mobile Robot Localization and Map Building: A Multisensor Fusion Approach. Boston: Kluwer Academic Publishers. Chiuso, A., Favaro, P., Jin, H., and Soatto, S. 2000. 3-d motion and structure from 2-d motion causually integrated over time: Implementation. In 6th European Conference on Computer Vision. Chong, K. S., and Kleeman, L. 1997a. Sonar based map building in large indoor environments. Technical Report MECSE-1997-1, Department of Electrical and Computer Systems Engineering, Monash University. Chong, K. S., and Kleeman, L. 1997b. Sonar feature map building for a mobile robot. In Proc. IEEE Int. Conf. Robotics and Automation. Choset, H., and Nagatani, K. April 2001. Topological simultaneous localization and mapping (slam): toward exact localization without explicit localization. IEEE Trans. Robotics Automation 17(2):125137. Cox, I. J., and Leonard, J. J. April 1994. Modeling a dynamic environment using a Bayesian multiple hypothesis approach. Articial Intelligence 66(2):311344. Davison, A. J. 1998. Mobile Robot Navigation Using Active Vision. Ph. D. thesis, University of Oxford. Deans, M. May 2002. Maximally informative statistics for localization and mapping. In Proc. IEEE Int. Conf. Robotics and Automation, pp. 18241829. Deans, M., and Hebert, M. December 2000. Experimental comparison of techniques for localization and mapping using a bearings only sensor. In Proc. ISER 00 7th International Symposium on Experimental Robotics. Dissanayake, M. W. M. G., Newman, P., Durrant-Whyte, H. F., Clark, S., and Csorba, M. March 1999. An experimental and theoretical investigation into simultaneous localization and map building. In 6th Int. Symposium on Experimental Robotics, pp. 265274.

THE INTERNATIONAL JOURNAL OF ROBOTICS RESEARCH / October-November 2002 Robotics Automation 12:566580. Kleeman, L. 2001. Advanced sonar sensing. In R. Jarvis and A. Zelinsky, eds., Robotics Research: The 10th Int. Symposium, Lorne, Australia. Berlin: Springer Verlag. Kleeman, L., and Kuc, R. 1993. Mobile robot sonar for target localization and classication. Technical Report ISL-9301, Intelligent Sensors Laboratory, Yale University. Kuc, R. 1996. Fusing binaural sonar information for object recognition. In IEEE/SICE/RSJ Int. Conf. on Multisensor Fusion and Integration for Intelligent Systems, pp. 727 735. Kuipers, B. J. 2000. The spatial semantic hierarchy. Articial Intelligence. Kurien, T. 1990. Issues in the design of practical multitarget tracking algorithms. In Y. Bar-Shalom, ed., MultitargetMultisensor Tracking: Advanced Applications, pp. 4383. Boston: Artech House. Leonard, J. J., Cox, I. J., and Durrant-Whyte, H. F. August 1992. Dynamic map building for an autonomous mobile robot. International Journal of Robotics Research 11(4):286298. Leonard, J. J., and Durrant-Whyte, H. F. 1992. Directed Sonar Sensing for Mobile Robot Navigation. Boston: Kluwer Academic Publishers. Leonard, J. J., and Feder, H. J. S. 2001. Decoupled stochastic mapping. IEEE J. Ocean Eng. 26(4):561571. Leonard, J. J., Newman, P., Rikoski, R., Neira, J., and Tards, J. 2001. Towards robust data association and feature modeling for concurrent mapping and localization. In R. Jarvis and A. Zelinsky, eds., Robotics Research: The 10th Int. Symposium, Lorne, Australia. Berlin: Springer Verlag. MacCormick, J. (2000). Probabilistic modelling and stochastic algorithms for visual localisation and tracking. Ph. D. thesis, University of Oxford. McLauchlan, P. F. 2000. A batch/recursive algorithm for 3d scene reconstruction. In Int. Conf. Computer Vision and Pattern Recognition, Volume 2, Hilton Head, SC, pp. 738 743. McLauchlan, P. F. and Murray, D. W. 1995. A unifying framework for structure and motion recovery from image sequences. In Proc. Int. Conference on Computer Vision, Boston, MA, pp. 314320. McLauchlan, P. F. and Murray, D. W. January 1996. Active camera calibration for a head-eye platform using the variable state-dimension lter. IEEE Trans. Pattern Anal. Mach. Intell. 18(1):1422. Mori, S., Chong, C., Tse, E., and Wishner, R. May 1986. Tracking and classifying multiple targets without a priori identication. IEEE Trans. Automatic Control 31(5). Moutarlier, P., and Chatila, R. 1989. Stochastic multi-sensory data fusion for mobile robot location and environment modeling. In 5th Int. Symposium on Robotics Research, pp. 207216. Neira, J., and Tards, J. 2001. Data association in stochastic

more powerful technique that tests the joint compatibility of multiple sensor measurements, using a branch and bound algorithm, has been developed by Neira and Tard s (2001) and successfully applied o to CML using laser data (Newman, Leonard, Neira, and Tard s 2002) and sonar data (Tard s, Neira, o o Newman, and Leonard 2001). Data association is not the primary focus of this paper, however the topic is intimately linked with the problem of mapping partially observable features. It is desirable to employ methods for perceptual grouping that can reject outliers while nding sets of measurements that, when interpreted together, yield a consistent explanation. This paper focuses on the state estimation aspects of this problem, describing a representation that allows the output of perceptual grouping routines to be consistently applied for mapping from multiple uncertain vantage points. In the computer vision community, the analogous problem to CML is Structure from Motion (SFM) (Faugeras 1993; Taylor and Kriegman 1995; Chiuso, Favaro, Jin, and Soatto 2000; Yagi, Shouya, and Yachida 2000; Hartley and Zisserman 2001). An early approach to SFM that used the EKF was developed by Ayache and Faugeras (1989). This work was one of the rst to apply geometric constraints in the state estimation process, such as the fusion of two features in the map that are asserted to be the same. Recently, constraint application has been incorporated in stochastic mapping algorithms by Chong and Kleeman (1997a), Tard s et al. (2001) and Williams et al. (2001). Of recent work in o computer vision, the work of Chiuso et al. (2000) and McLauchlan (2000) are the most closely related to the method presented in this paper. The Smith, Self and Cheeseman (1987) algorithm for CML uses three models: (1) a robot motion model, (2) a feature mapping model, and (3) a measurement model, which we refer to as the functions state projection. The feature mapping model uses sensor observations to estimate the location of a new geometric feature, so that it may be added to the map. The measurement model predicts observations of mapped features. The basic method assumes that features are stationary, and that there is only one robot, but it can be extended to accommodate dynamic features and/or multiple robots (Fenwick, Newman, and Leonard 2002). A signicant limitation of Smith, Self and Cheeseman (1987) relates to the initialization of new features. The method assumes that the full state of an object can be completely initialized using the measurement data available from a single vehicle position, obtained by a single robot. However, this situation is not satised in many important cases of practical interest. In this paper, we present a generalized framework for CML that permits mapping of partially observable features. This can be applied to situations when the measurement data from a single location is insufcient to completely 4 , , and

respectively. The robot motion model uses knowledge of the robots dynamics for
estimate the feature location, such as with wide-beam sonar measurements or angle-only measurements. It also enables composite features, comprised of multiple simple features, to be mapped, such as joining points and lines together to form polygons. The method also has application to mapping by multiple robots. The issue of estimating partially observable features with measurements obtained from multiple uncertain vantage points is clearly shared in both the CML and SFM problems. State-of-the-art vision algorithms for SFM use bundle adjustment (non-linear least squares optimization) to concurrently estimate scene structure and camera motion (Triggs, McLauchlan, Hartley, and Fitzgibbon 2000). These techniques typically solve the association problem by using random sample consensus (RANSAC) (Fischler and Bolles 1981) or Least Medians (Faugeras, Luong, and Papadopoulo 2001) to nd groups of measurements that originate from the same point in the scene across multiple images. While most work has considered the batch SFM problem, there have been some approaches that adopt a recursive approach, which is highly important for navigation applications. Chiuso et al. (2000) have developed a real-time SFM system that can deal with occlusion. In their system, tracked features are not included into the full SFM solution until there is a high certainty that they provide good quality data. McLauchlan has developed the variable state dimension lter (VSDF), which is a hybrid batch/recursive technique that combines the characteristics of the EKF and bundle adjustment (McLauchlan 2000; McLauchlan and Murray 1996; McLauchlan and Murray 1995). The VSDF maintains a dynamic time-window of observations and camera motion parameters. We employ a similar idea in this paper. Deans and Hebert (2000) have developed a related method for CML with bearing-only measurements and have performed an experimental investigation of its performance. More recently, Deans (2002) has provided several improvements to the VSDF, including an interpolation scheme that reduces the linearization error and a factorization method that yields computational efciency. The structure of this paper is as follows: Section 2 formally denes the problem under consideration. Previous work is reviewed and the problem of mapping with partial observability is formulated. Section 3 describes our new approach to this problem. The key idea is to add past vehicle positions to the state vector and to maintain explicitly estimates of the correlations between current and previous vehicle states. By incorporating past vehicle locations in the state vector, it becomes possible to consistently initialize new map features by combining data from multiple vantage points. We present two different types of experimental results with the method. In Section 4, we present a series of simplied examples that use manual data association to demonstrate the processes of multi5

The growth of the state vector in this manner increases the computational burden as
Q k Q T s# Q r u i Q Q T Q ks g Q Q s Q T s# Q gs s Q i s# Q s Q s

Q

f t t f S S

Q 4 T s

Q ks# Q gs s

jif ! S

Q r Q g q
! t f & if z & t if z S S S

. , so caution

New feature initialised by combining observations from multiple uncertain vantage points

xr [k-1|k]

P rr [k-1|k]
Stored, Uncertain Vehicle Trajectory, T
Figure 4: Illustration of initialization from multiple vantage points.
active trajectory states of the stochastic map. Initialization can then be performed by picking a minimal be the set of vehicle positions for the measurements in

can be computed as:

For example, consider the initialization of a new point feature in two dimensions, using two range intersection of two circles. The algebra is very simple and is reviewed in Appendix A. The beamwidth of the sonar sensor can be used as an angle constraint to rule out multiple solutions (Leonard and Durrant-Whyte 1992; Wijk and Christensen 2000). The covariance for the new feature is initialized in a similar fashion as shown above in Equations 10 to 12, except that the Jacobian matrix additional non-zero terms corresponding to the trajectory states and the Jacobian matrix
differentiation of the multiple vantage point initialization function can be cumbersome. It is possible, however, to compute the initialization Jacobians by inverting the Jacobian of the composite function of minimal observations, as follows:

measurements,

, taken at time steps

. The function

H 5! 1 ks" f

subset

of seed features from

candidate measurements,

, from a set of vehicle positions that are currently contained in the set of that are sufcient the estimate the state of the new feature. Let. Then, the location of the new feature (19)
represents a solution for the
xf [k|k]=g(Z,T) P ff [k-1|k]

z[k-1]

P rr [k|k]

Stored Observations Z

xr [k|k]

will contain. The direct

matrix with full rank. An alternative is to compute the Jacobians numerically (Julier, J. K. Uhlmann and H. F. DurrantWhyte 1996); this is the approach used for the results in this paper. For initialization of a line feature common tangents of two circles. The general procedure is the same if the feature initialization function using three range values. 3.3 Batch Updating Once a new feature is initialized, the map can be simultaneously updated using all other previously obtained measurements that can be associated with the new feature (measurements that are elements consisting of measurements obtained for different time steps. We call this procedure a batch update. It allows the maximum amount of information to be extracted from all past measurements. Empirically, we have observed that the EKF can be less prone to divergence when batch updates are performed. constructed from an appropriate temporal observation set. To update the robot history and features using a batch of measurements, a measurement vector is of but not elements of.) This update is performed using a composite measurement vector, from two positions with two sonar measurements, the function

represents the solution for the
is a function of measurements from more than two time steps, for example to initialize a cylinder
An appropriate predicted measurement vector is constructed:

" r ! r x r f

v u v v u v v wu u
" ! r f " ! " r ! f r f
r x r(y x x y v p x v y v x v ry v wu

x (y p x (y x r(y

' ' '

! 1 k5 0

is the state estimate for the new feature. This is possible because

3 H wj

(21) is a square
This leads to an innovation which includes observations from across multiple time steps:
Similarly, the measurement Jacobian
is constructed, as is the measurement covariance
Using these components and the standard Kalman update equations, all robot trajectory positions and all features that are currently in map are concurrently updated. 3.4 Composite Initialization In general, the feature initialization function can use any of the information in the state estimation for
the stochastic map, including the locations of other previously mapped features as well as as previous vehicle states. The new feature location can be a function of one or more previously mapping feature, , one or more measurements, and the robot state estimate corresponding to these measurements. For and is tangent to one sonar. In this case, the feature initialize function is of the form: example, one can initialize a line that passes through a point feature return
Alternatively, one can initialization a point that lies at the intersection of a line currently in the map and a new sonar return. The equations for these two initialization scenarios are described in Appendix 1.3.

r x gy

z! 6"
" g(y ' (x g( P ! r x r x u g f
" ! r f " ! " r ! f r f 24s 24s4s2
QP 6" R z! 5! 1 k60 g f f
D 5 rD h iT us x D p iD h v D p 1 iD h v h v D iD v p# u4Y v u i4

x (y p x (y x r(y F F

(xy F
' (x (y x p ' (x (y x ' (x i(y x F p (xy F mn n n n no F Y

P n mnn P n no

F F x r(
F mn n n n no QP R z! 6"

One can also initialize a new feature without any measurements, for example, hypothesizing the constraint that a new point feature exists at the intersection of two line segments currently in the map. Examples with real data for several of these scenarios are given below in Section 4. 3.5 Extension to Mapping by Multiple Robots Cooperative stochastic mapping with perfect communications requires adding robots to the state vector (Fenwick, Newman, and Leonard 2002). Although there are numerous technical issues to overcome, from a state estimation perspective it is relatively straightforward to perform feature initialization using data from multiple robots. By using trajectory states, such initializations can occur on a delayed basis, removing the requirement for the two robots to sense the features in question at precisely the same time. In is also possible for one robot to map the location of another robot (add the other robot to its state vector), through processing of measurements of common features (assuming that association is known). A simplied example of how this process can work is shown below in Section 4.
4 Examples using manual association
We now present several illustrations of the concepts presented above using real sonar data sets with manual data association. The rst experiment uses 500 kHz underwater sonar data acquired in a testing tank, and the second uses experiment uses data from a ring of 24 Polaroid sonar sensors. Both experiments use manually-guided data association strategies that exploit a priori knowledge of the environment. While both environments are highly simplied, they are useful in illustrating the state estimation process for mapping from multiple uncertain vantage points. Fully automatic data association is used in the experiments in Section 5, as part of an integrated system that can perform CML in real-time using odometry and wide-beam sonar measurements. 4.1 Tank Experiment An experiment was conducted using a robotic gantry to emulate the motion and sensing of an underwater vehicle. A 500 KHz binaural sonar was used (Kuc 1996; Au 1993). To show mapping of partially observable features, bearing information was discarded. Two objects were placed in the tank, a metal triangle and a point like object (a shing bobber). The gantry was moved through two trajectories, one 19
to the left and one to the right of the objects, emulating cooperative CML by two vehicles. All processing was post processing. Data association was done by hand since it is not the focus of this paper. The manually-associated returns used for feature initialization are labeled in Figures 7 and 10 and are listed in Table 1. The initialization strategies used for each feature and for the position of the second robot are listed in Table 2. We consider the set of measurements from the right side of the objects to originate from robot 1 and the measurements from the left side to be from robot 2. The gantry operates in a tank that is 10 meter long by 3 meter wide by 1 meter deep. The mechanism provides ground-truth good to a few millimeters. Simulated speed and heading measurements were generated and used for dead-reckoning. Initially, the sensor dead-reckoned through a trajectory of 11 positions, as shown in Figure 5. Upon completing this trajectory, the robot had a state vector and covariance matrix which contained only robot states, one estimate for each position. In Figure 6, the correlation coefcients for the x components of the trajectory are plotted. Each line represents the correlations between one timestep and all other saved timesteps. Because this is a short deadreckoned trajectory, each curve has only one maxima; more complex trajectories may have numerous local maxima. The assumed range measurement standard deviation was 3 centimeters for each measurement. The step in heading. The data processing was performed as follows. First, state projection was performed and trajectory states were created for robot 1 for time steps 1 through 11, without any measurements being processed. The three-sigma error bounds for the dead-reckoned ure 6. Having an entire trajectory of positions, robot 1 starts to construct its map. Because the robot uses a range measurements only, features must be observed from multiple vantage points to be mapped. By combining returns 1 and 5 (labeled in Figure 7), the robot initializes the point object at the bottom of its map (Figure 7). Similarly, by intersecting two more arcs (returns 2 and 6), the bottom corner of the triangle is mapped. The equations for arc intersection are given in Appendix 1.1. Next, return 4 is used in conjunction with the estimated location of the bottom corner of the triangle to add the right wall of the triangle to the map. Finally, by intersecting return 3 with the estimated line corresponding to the right wall, the top corner is added to the map. Using these observations, the point object and the side of the triangle are initialized (Figure 8). 20 The correlation coefcients between the coordinates of the robot trajectory states are plotted in Figadded process noise had a standard deviation of 1 cm per time step in and

trajectory of robot 1 are shown in Figure 5.

and 2 degrees per time

and offset of the normal with respect to the origin. The dashed line in Figures 7 and 8 show the extension of the estimated line. After initializing the features, all other observations are used for a batch update of the newly initialized features (Figure 9). Mapping and navigation are improved substantially. Robot 1 obtained 29 total measurements. Of these, six were used for initialization and 23 were used batch updating. Next, robot 1 tries to use information from robot 2. We assume that there is no a priori information for the initial location of robot 2, and that hence robot 2 must be initialized into the map using shared measurements of features seen by both robots. It is determined that robot 2 has observed features in robot 1s map. From the ranges to the point object and the bottom corner of the triangle, (returns 7 through 10 as labeled in Figure 10) robot 2s rst two positions can be observed. Those two positions are initialized into the map, their initialization observations of features that have been previously mapped by robot 1 (Figure 11). Next, using the rst two estimated positions for robot 2, the initial heading and velocity of robot 2 are mapped. Using this information, along with the control inputs, a dead-reckoned trajectory for robot 2 is established (Figure 12). Because neither compass nor velocity observations are available, and because the initial estimates of velocity and heading for robot 2 are imprecise, the trajectory is imprecise and has large error bounds. Having a dead-reckoned trajectory for robot 2 and its measurements, robot 1 then maps the (otherwise unobservable) backside of the triangle and performs a batch update to get an improved map and an improved estimate of where robot 2 traveled (Figure 13). Robot 2 obtained 22 total measurements. Of these, four measurements were used to initialize the position of robot 2 in the stochastic map of robot 1. Two measurements were used in initializing features on the left side of the triangle, and the remaining 16 returns were used in batch updating. The error bounds for robot 2 do not exhibit the growth prole that is normally seen. Normally, since the robot starts with an initial position estimate and then moves, the uncertainty grows with time. In this case, since the second robot is mapped and localized with respect to previously mapped features, the smoothed estimate of its trajectory has the least uncertainty in the middle (Figure 14). The next experiment is for data from a simple box environment made of plywood, demonstrating the process of multiple vantage point initialization, batch updating, and composite feature initialization. The data association and feature modeling techniques utilize the a priori knowledge of the structure of 21 function is of the form

Figure 11: First two position for robot 2 are mapped. Using these two positions is is possible to estimate the initial heading and velocity. Condence intervals for robot 2s positions shown in blue.
Figure 12: Using the estimated initial heading and velocity for robot 2, along with its control inputs, a deadreckoned trajectory is constructed. With poor initial estimates of heading and velocity, and no compass or velocity measurements for updates, the trajectory is very imprecise.
experiment 8: cooperative using a robot of convenience 4
Figure 13: Using information from robot 2, robot 1 is able to map the back side of the triangle, which it otherwise could not observe. After the batch update its estimate of robot 2 improves considerably.

meters

6 timestep
Figure 15: B21 mobile robot in the plywood box.
Figure 14: Error in the smoothed estimate and three- condence interval for robot 2s uncertainty is in the middle of the trajectory due to forward/backward smoothing.

position. The minimum

the box, namely that each corner of the box was created by two walls, and that each wall was bounded at each end by a corner. The input data consists of 600 sonar returns acquired from a sequence of 50 positions that form one-and-a-half loops around the inside of the box. The vehicle started in the lower left corner facing upward. The data processing proceeded as follows. First, state projection was performed and trajectory states were created for time step 1 through time step 50, without any measurements being processed. At each processing cycle, a new vehicle trajectory state was added to the system state vector, using Equations 17 and 18. The dead-reckoned vehicle trajectory is shown in Figure 16(b). After fty cycles, a manually-guided search strategy was performed to nd nine returns that were used to initialize nine features (the four corners and four walls of the box and a prominent crack on the bottom wall). The nine returns and the nine features are each labeled in Figure 17, and details of initialization sequence are shown in Tables 4 and 5. Azimuth information from the sonar returns was not used for gating but not for estimation. The processing proceeded as follows: rst, Returns 1 and 2 were used to initialize Corner 1 using a two position range-only initialization (circle intersection). Next, Return 3 was used in conjuction with the state estimate for Corner 1 to initialize Plane 1, and Return 4 was used in conjuction with the state estimate for Corner 1 to initialize Plane 2. After this, Return 5 was used in conjuction with the state estimate for Plane 1 to initialize Corner 2, and Return 6 was used in conjunction with the state estimate for Plane 2 to initialize Corner 3. Likewise, Return 7 was used in conjuction with the state estimate for Corner 2 to initialize Plane3, and Return 8 was used in conjuction with the state estimate for Corner 3 to initialize Plane 4. Next, Return 9 and Plane 4 were used to initialized the crack on plane 4. Finally, the state estimates for Plane 3 and Plane 4 were used to initialize the nal feature, Corner 4. After the initializations, nine constrained features (shown in Figure 17) were mapped using nine range measurements (shown in Figure 18). Once these features were initialized, nearest-neighbor gating was performed between all of the remaining sonar measurements and the newly initialized map features. A total of 217 of the original 600 measurements were uniquely matched to one of the nine features shown in Figure 19(a). Finally, Figure 19(b) shows the result when all these measurements are applied in a single batch update, resulting in a dramatic reduction in the uncertainty ellipses for the estimated feature locations and in the complete trajectory of the vehicle.

Figure 19: (a) Sonar measurements that uniquely gated with the nine initialized features, to be used in the batch update. (b) Feature location estimates, vehicle trajectory, and error ellipses after the batch update.
ASYNCHRONOUS DATA AQUISITION

SYNCHRONOUS PROCESSING

State Projection

new data available

Data Collection Perceptual Grouping

raw sensor data

grouped obsevations

Individual Observations

DATA ASSOCIATION

Data Storage

positive associations
unexplained observations no new data

Map Update

Feature Manufacture

stable initialisation

ambiguity

Batch Update

Delayed State Management

new feature

all newly explained observations are used during initialisation Feature Management Addition and removal of past vehicle states
Features can be fused with each other (equivalence assertion). Stagnant features can be removed (garbage collection). Compound features can be built.
Figure 20: Information ow for cycle integrated real-time CML incorporating delayed state management, perceptual grouping, multiple vantage point initialization, batch updating, and feature fusion.
Any applicable technique can be applied here, for example Hough transform, RANSAC, least medians, maximum likely-hood
new and existing unexplained data is combined with a history of vehicle states to search for a stable initialisation of a new feature
odometry trajectory and measurements referenced to the odometry trajectory
cml trajectory and measurements referenced to the cml trajectory 1
(b) Figure 21: (a) Raw sonar data for experiment with two point objects, referenced to odometry. (b) sonar returns matched to the two features, referenced to the CML estimated trajectory. The experiment was 50 minutes long. The vehicle moved continuously under manual control at a speed of 0.1 meters per second, making about 15 loops of the two cylinders. 34
vehicle east error bound vs. time 1
1 vehicle north error bound vs. time
0 Vehicle heading error bound xy cor. coefficient vs. time

degrees

50 0.5
Feature 0 east error bound vs. time
0 0.Feature 0 north error bound vs. time
1 Feature 0 east error bound vs. time
0.5 Feature 0 north error bound vs. time

0 0.time (seconds) 3500

Figure 22: Estimated error bounds for the experiment. (Top plots: three-sigma bounds for and of the vehicle; next plot: - correlation coefcient; next plot: three-sigma bounds for vehicle heading; bottom four plots: three-sigma bounds for the and locations of the two features. There is no ground-truth for this experiment, however the vehicle returned to within a few inches of the start position, commensurate with the CML algorithm state estimation error.
Figure 23: Raw data for corridor experiment, referenced to odometry.

&%$" #!

are accessible at: http://oe.mit.edu/jleonard/data/videos http://oe.mit.edu/jleonard/data//cylinders/cylinders7.mpg http://oe.mit.edu/jleonard/data/videos/jan24_all_start.mpg http://oe.mit.edu/jleonard/data/corridor/corridor_all.mpg. All of the raw data for these experiments has been made publicly available on the web at: http://oe.mit.edu/jleonard/data/ The method has also been implemented running in real-time under manual control. To our knowledge, this is the rst successful feature-based CML implementation using sonar sensing for which the robot was continually in motion and the CML output was generated in real-time. (Chong and Kleeman (1997a) implemented sonar-based mapping with a high-performance sonar array that stopped to perform mechanical scanning for each data acquisition cycle.) The method uses the standard Polaroid sonar array on the B21 robot and could be readily ported to any B21 mobile robot. Such a result has not been achieved before because it has not been possible without the expanded representation accounting for temporal correlations, described in Section 3. The method presented in this paper also been used extensively in two other experimental systems. With sonar, using RANSAC for perceptual grouping and the ATLAS framework for scalable mapping (Bosse, Newman, Leonard, and Teller 2002), we have mapped a large portion of the MIT campus and demonstrated closure of large loops, using only sonar and odometry data. A representative result is shown in Figure 25. We have also extended the framework presented in this paper to achieve robust 3-D local mapping from omnidirectional video data (Bosse, Rikoski, Leonard, and Teller 2002).

6 Conclusion

This paper has described a generalized framework for CML that incorporates temporal as well as spatial correlations, allowing features to be initialized from multiple uncertainty vantage points. The method has been applied to Polaroid sonar data from a B21 mobile robot, demonstrating the ability to perform CML with sparse and ambiguous data. These experiments illustrate the benets of adding past vehicle positions to the state vector, enabling stochastic mapping to be performed in situations where the state of a feature can only by partially observed from a single vehicle position and the ambiguity of individual measurements is high.

-140 160

Figure 25: Map produced from B21 sonar data for several corridors of the MIT campus, created using the state estimation framework described in Section 3 combined with RANSAC for perceptual grouping and the ATLAS framework for large-scale mapping (Bosse, Newman, Leonard, and Teller 2002). The mission was 50 minutes in duration and the vehicle traveled a distance of 481 meters, with a peak velocity of 0.3 meters per second and an average velocity of 0.163 meters per second.

% 3 G8! A 7

Y X7 e Q e a 3 i 7 T E7 T BEhQ 3 gf% 3D P Q 33 P ! D %D 7 7! c %D A dIXWQ 3 2! b A

%D A9D 7 IHGFE8!

Y 33 P Q 3 % 3 WV8! T 3 % 3 SR8! A Q A 7 Q 7 Y `D3 P Q 3 IXWV8! U3 BESR8! %D A Q A T %D 7 Q 7

% ACB@8!

(28) (29)
tensen 2000). 1.2 Initializing a line from two range measurements There can be as many as four lines which are tangent to two circles. Considering only the cases where the two circles are tangent to the same side of the line, the two solutions are:
designates the angle of the normal of the line and
1.3 Initializing a line from a range measurement and a colinear point Initializing the line which is tangent to circle solutions. The two results, and
to nding the line which is tangent to two circles when one of the circles has zero radius. There are two

, are :

xx % IDE7SQ 3 B9XSQ 3 Ag! FpIyY a 7 DA i w Y %D A % IHASQ 3 2! T 3 BDE7Q 3 g! r e % 3 ! % 9 BD FD ! w w

% 9D A9D 7 ID P FHG5Eg!

the origin. When
is imaginary then the circles are concentric and do not have a cotangent.
Y c 9 ! %D A A %D 3 %ID P Q 3 P Q 3 IXWQ 3 2! T 3 IESQ 3 g! r %D A A %D 3 IXWQ 3 2! T 3 BESQ 3 g! Y 9 % IDHA 3 P Q 3 $D P IHWQ 3 8! T dIH$EhQ 3 $E8! b A ! %D A A c %D AD 7 AD % 7! Q c %D A 3 A % BD P Q P !EBDESQ 8udIHWQ 8! b GpByY xx c %D 7 dBESQ 3 8! 7 w q
is imaginary, then the circles do not intersect. See also Wijk and Christensen (Wijk and Chris-
3 ! 3 t 3 ID t Y c % % P Q P uQ 3 e F3 e Q 3 ID P T P ! s s r Y pA e Q e a 3 i A T HA T $HWQ 3 8E% 3D P Q 33 P ! D %D A A! c %D 7 HBESQ 3 8! 7 q A %D v %D A @3 IXSQ 3 8! T 3 BEhQ 3 8! Y 3 e

and passes through point

is the perpendicular offset of the line from

is equivalent

(37) (38)
1.4 Initializing a point from a line and a range measurement A robot position and a range dene a circle line, which is dened as:. Provided that the circle intersects the line

Dissanayake, M. W. M. G., P. Newman, H. F. Durrant-Whyte, S. Clark, and M. Csorba (1999, March). An experimental and theoretical investigation into simultaneous localization and map building. In Sixth International Symposium on Experimental Robotics, pp. 265274. Dissanayake, M. W. M. G., P. Newman, H. F. Durrant-Whyte, S. Clark, and M. Csorba (2001, June). A solution to the simultaneous localization and map building (slam) problem. IEEE Transactions on Robotic and Automation 17(3), 229241. Doucet, A., N. de Freitas, and N. Gordan (Eds.) (2001). Sequential Monte Carlo Methods in Practice. Springer-Verlag. Faugeras, O. (1993). Three-Dimensional Computer Vision: A Geometric Viewpoint. MIT Press. Faugeras, O., Q.-T. Luong, and T. Papadopoulo (2001). The Geometry of Multiple Images. MIT Press. Feder, H. J. S. (1999). Simultaneous Stochastic Mapping and Localization. Ph. D. thesis, Massachusetts Institute of Technology. Feder, H. J. S., J. J. Leonard, and C. M. Smith (1999, July). Adaptive mobile robot navigation and mapping. Int. J. Robotics Research 18(7), 650668. Fenwick, J., P. Newman, and J. Leonard (2002, May). Collaborative concurrent mapping and localization. In Proc. IEEE Int. Conf. Robotics and Automation, Washington, DC, pp. 18101817. Fischler, M. A. and R. C. Bolles (1981). Random sample consensus: A paradigm for model tting with applications to image analysis and automated cartography. Communications of the ACM 24(6), 381395. Fleischer, S. D. (2000). Bounded-Error Vision-Based Navigation of Autonomous Underwater Vehicles. Ph. D. thesis, Stanford University. Grimson, W. E. L. (1990). Object Recognition by Computer: The Role of Geometric Constraints. MIT Press. (With contributions from T. Lozano-Perez and D. P. Huttenlocher). Guivant, J. and E. Nebot (2001, June). Optimization of the simultaneous localization and map building algorithm for real time implementation. IEEE Transactions on Robotic and Automation 17(3), 242257. Gutmann, J.-S. and K. Konolige (1999). Incremental mapping of large cyclic environments. In International Symposium on Computational Intelligence in Robotics and Automation. Hartley, R. I. and A. Zisserman (2001). Multiple View Geometry in Computer Vision. Cambridge University Press, ISBN: 0521623049. Heale, A. and L. Kleeman (2001). Fast target classication using sonar. In Proc. IEEE Int. Workshop on Intelligent Robots and Systems, pp. 14461451. Jensfelt, P. (2001). Approaches to Mobile Robot Localization in Indoor Environments. Ph. D. thesis, Royal Institute of Technology, Stockholm, Sweden. Julier, J. K. Uhlmann and H. F. Durrant-Whyte, S. J. (1996). A New Approach for the Nonlinear Transformation of Means and Covariances in Linear Filters. IEEE Transactions on Automatic Control. Kailath, T., A. Sayed, and B. Hassibi (2000). Linear Estimation. Prentice-Hall. Kavraki, L., P. Svestka, J.-C. Latombe, and M. Overmars (1996, August). Probabilistic roadmaps for path planning in high-dimensional conguration spaces. IEEE Trans. Robotics and Automation 12, 566580. Kleeman, L. (2001). Advanced sonar sensing. In R. Jarvis and A. Zelinsky (Eds.), Robotics Research: The Tenth International Symposium, Lorne, Australia. Proceedings to be published by Springer Verlag. Kleeman, L. and R. Kuc (1993). Mobile robot sonar for target localization and classication. Technical Report ISL-9301, Intelligent Sensors Laboratory, Yale University. Kuc, R. (1996). Fusing binaural sonar information for object recognition. In IEEE/SICE/RSJ International Conference on Multisensor Fusion and Integration for Intelligent Systems, pp. 727735. Kuipers, B. J. (2000). The spatial semantic hierarchy. Articial Intelligence.

Taylor, C. J. and D. J. Kriegman (1995, November). Structure and motion from line segments in multiple images. IEEE Trans. Pattern Analysis and Machine Intelligence 17(11), 10211032. Thrun, S. (2001, May). A probabilistic online mapping algorithm for teams of mobile robots. Int. J. Robotics Research 20(5), 335363. Triggs, W., P. McLauchlan, R. Hartley, and A. Fitzgibbon (2000). Bundle adjustment for structure from motion. Wijk, O. and H. Christensen (2000, December). Triangulation based fusion of sonar data with application in robot pose tracking. IEEE Trans. Robotics and Automation 16(6), 740752. Williams, S. (2001). Efcient solutions to autonomous navigation and mapping problems. Ph. D. thesis, University of Sydney. Williams, S., G. Dissanayake, and H. Durrant-Whyte (2001, June). Constrained initialisation of the simultaneous localisation and mapping algorithm. In Field and Service Robot 2001, Finland, pp. 309314. ISBN 952-5183-13-0. Yagi, Y., K. Shouya, and M. Yachida (2000, May). Environmental map generation and ego-motion estimation in a dynamic environment for an omni-directional image sensor. In Proc. IEEE Int. Conf. Robotics and Automation, pp. 34933498. Zunino, G. and H. I. Christensen (2001, July). Slam in realistic environments. In M. Devy (Ed.), Symposium on Intelligent Robotic Systems, Toulouse, F.