Abstract-In cooperative navigation, teams of mobile robots obtain range and/or angle measurements to each other and dead-reckoning information to help each other navigate more accurately. One typical approach is moving baseline navigation, in which multiple Autonomous Underwater Vehicles (AUVs) exchange range measurements using acoustic modems to perform mobile trilateration. While the sharing of information between vehicles can be highly beneficial, exchanging measurements and state estimates can also be dangerous because of the risk of measurements being used by a vehicle more than once; such data re-use leads to inconsistent (overconfident) estimates, making data association and outlier rejection more difficult and divergence more likely. In this paper, we present a technique for the consistent cooperative localization of multiple AUVs performing mobile trilateration. Each AUV establishes a bank of filters, performing careful bookkeeping to track the origins of measurements and prevent the use any of the measurements more than once. The multiple estimates are combined in a consistent manner, yielding conservative covariance estimates. The technique is illustrated using simulation results. The new method is compared side-by-side with a naive approach that does not keep track of the origins of measurements, illustrating that the new method keeps conservative covariance bounds whereas state estimates obtained with the naive approach become overconfident and diverge. Index Terms-Cooperative Localization, Cooperative Navigation, Sensor Fusion As mobile robots become more affordable and reliable, the use of a groups of many robots becomes more feasible. While the primary reason to use a group of robots is often to speed up tasks such as map building, or add redundancy, more recently the concept cooperative navigation (CN) has been explored. In CN a member A of the group obtains exteroceptive measurements such as range or bearing (or both) to another member B and uses these measurements together with B's own position estimate, obtained through a status broadcast, to improve its own position. Robots often broadcast status information to coordinate with one another or to provide telemetry information to an operator and are equipped with sensors which enable them to locate other members. As a result, adding the CN capability often does not require any extra hardware and represents a cost-effective way to improve the navigation accuracy of the individual vehicles. With the price and size of Global Positioning System (GPS) units having dropped significantly in the past two decades, it is now possible to outfit each member of large groups of robots with its own unit. Having access to very accurate, absolute position information at rates of several Hz Fig. 1 . An Autonomous Underwater Vehicle (AUV) on the crane and two Autonomous Surface Craft (ASC) on deck after a cooperative navigation experiment. The GPS-derived position of the ASCs is obtained by the submerged AUV through an acoustic modem. Using time-stamped messages and globally synchronized clocks on all vehicles the AUV is also able to determine its range to the ASC through time-of-flight measurements. has simplified the robot navigation problem significantly for many areas. There are, however, many environments where GPS signals are either intermittently or not available at all. This mostly affects robots operating indoors or underground, but also those operating underwater such as AUVs. Vehicles operating in these environments may occasionally have access to absolute position information by surfacing for a GPS update in the case of an AUV, or when the camera on an indoor robot recognizes a landmark with a known position [1] . In between these absolute position updates, robots have to rely on dead-reckoning sensors. When relying only on dead-reckoning sensors the uncertainty in their position estimate grows without bound. For scenarios where some members have intermittent access to absolute position information, the uncertainty of their position estimate may be significantly lower than the position uncertainty of those that rely solely on deadreckoning for a long period of time. By broadcasting their low-uncertainty position estimate, all receiving members of the group, which also obtain a range or bearing measurement to the broadcasting vehicle, are then able to improve their own estimate. When robot A uses the position estimate of another robot B to update his own, their position estimates become correlated. Not taking these cross-correlations into account can have a negative effect if there is a chain of updates back to B, often leading to an overconfidence in B's position, estimate which then diverges as a result [2] . Various approaches have been devised to either properly account for the crosscovariance or to use very conservative uncertainty bounds to avoid overconfidence. The resulting failure modes and are outlined in later sections, but many of the algorithms impose additional requirements which make them unfeasible for many CN-scenarios. Our approach requires that each robot includes additional information within their status broadcast. The receiving robots can then use this information to ensure that the crosscovariances are properly accounted for. The approach does not require centralized data storage and processing as all updates are done locally on each vehicle using only data from the broadcasting vehicle. It does not enforce a particular communication hierarchy or topology and individual members can join and leave the group and do not need any awareness of previous communications or the size of the group. Unlike other methods, broadcasts do not need to be received by all participating vehicles as each transmission contains all the information which is required for a position update which accounts for the cross-correlations. The main motivation for our approach comes from working with AUVs (figure 1). In the harsh underwater environment, an unreliable navigation estimate may result in the loss of an expensive vehicle. When submerged, AUVs have to rely on dead-reckoning sensors and the number of surfacings for a GPS fix needs to be minimized to save energy or maintain covertness in military applications. Through CN, the surfacing of a single vehicle can then benefit a large number of submerged vehicles. As submerged vehicles can only communicate acoustically over a very slow and unreliable channel [3] , the approach can only rely on unacknowledged broadcast-based communication. While the underwater scenario enforces particularly hard constraints, these constraints ensure that our approach works for many other multi-robot applications outside a very controlled lab environment. Roumeliotis et al. have contributed a large body of work to the field of CN. Early work relies on a central site for data storage and processing [4] . With this setup, the authors make useful insights into relationship between the number of cooperating robots and the individual position uncertainty. The result is an analytical expression for the upper bound on the positioning uncertainty increase rate for the group [5] . In another experiment, the central filter that keeps track of the state and covariance of all vehicles is replaced by distributed filters that run on the individual members. The exchange of only local data is necessary, but as both vehicles are required to transmit, this approach does not scale as well as others that rely only on one-way broadcasts [6] . Caglioti et al. also use a distributed filter approach. While they only require oneway data exchange (broadcast), these broadcasts occur very frequently and their method relies on perfect communication as each vehicle is required to receive every broadcast. The problem of fusing measurements from several sources while properly keeping track of common information has been addressed by Grime [7] and Nettleton [8] . Unlike the work of Roumeliotis et al., they track the information parametrization of the Gaussian rather than the standard form. In the information form, the update step is simply an addition and joint information, which models codependencies among different states, can be subtracted if the communication topology is known. A general approach to the problem of fusing correlated estimates has been proposed by Julier and Uhlmann [9] , [10] . Their Covariance Intersection (CI) algorithm fuses two different estimates for a random variable, each represented by their estimated mean and covariance much like the update step in the Kalman filter. The result is a posterior covariance that guarantees consistency under the assumption of Gaussian noise. Arambel et al. present an application of the CI algorithm for a group of space vehicles, where relative position measurements are communicated in a ring topology [2] . Each of these works have examples of how the state estimator can diverge if estimates are fused with a simple Kalman update without accounting for correlation among the estimates. A disadvantage of CI algorithm is that it can only fuse two state estimates. Additionally, unlike the standard Kalman Filter, it cannot perform a partial update such as those that apply to vehicles that only have a range or bearing sensor. As a result, robots that only have a bearing sensor, such as a monocular camera, or have only range information from time-of-flight-based techniques cannot participate in a setup which relies on CI for the update of position estimates. The previous work presented thus far relies on the Kalman Filter in its original and modified (CI) form, or its inverse, the information filter, to compute an estimate. Fox et al. [11] use sample-based Markov techniques to perform cooperative localization. They represent the distribution by a large number of Monte Carlo samples rather than as a Gaussian distribution. This representation allows for complex, multi-modal uncertainty distributions and avoids problems with the linearization that is required for Kalman Filter-based methods. Transferring the distribution between vehicles requires a comparably fast communication channel which might not always be available [12] . Their approach also requires information exchange such that broadcast-based approaches cannot be used, which further increases the bandwidth necessary. The remainder of the paper is structured as follows. In section IV, we outline the assumptions we make for our group of vehicles cooperating for localization. Section III then gives a brief review of Extended Kalman Filter(EKF)-based CN for the special case where only intra-vehicle range measurements are available. Section V describes the Interleaved Update (IU) algorithm that we propose to fuse intravehicle range measurements together with status broadcasts from other vehicles to update our own position estimate. Section VI shows an example of the algorithm at work for a group of vehicles over several time steps. Section VII shows 3416 simulated results for a group of cooperating vehicles and shows how the naive approach of not taking proper care of cross-correlations leads to an overconfident (inconsistent) position estimate. Section VIII presents the conclusions and provides an outlook to future work. EKF-based approaches are the algorithms most commonly used for CN. All EKF-based methods model the estimated random variables as Gaussians, typically parametrized in terms of their mean and covariance, and generally assume that the sensor noise is zero mean, white noise. The shortcomings of these methods, such as the unimodal distribution of the state estimate and the error introduced through the required linearization, have given rise to alternative methods [11] , [12] , [13] . The EKF outlined below describes a variant that only uses range measurements for its updates. For a more detailed description of the EKF and how to incorporate bearing measurements or range/bearing measurement pairs please refer to [14] . For EKF-based CN, we assume that all n vehicles T that contains an estimate of their position at time k, and the covariance matrix describing the uncertainty associated with that estimate. Whenever vehicle i = 1 obtains proprioceptive measurements u 1 (k) from its dead-reckoning sensors, x 1 (k) and P 1 (k) are propagated where Q 1 (k + 1) is a diagonal matrix where the elements contain the variance of the proprioceptive sensor noise, which is modeled as mean-free Gaussian noise and G 1 (k+1) is the Jacobian containing the partial derivatives of g ∂g(u 1 (k + 1), x 1 (k)) ∂x 1 (k) If vehicle 1 at a later time l receives a broadcast from vehicle 2 that contains x 2 (l) and P 2 (l) together with an intravehicle range measurement r 1,2 (l), it uses this information to update its estimate of its own position as follows: First, it computes what the predicted range z 1,2 (l) between the two vehicles would be, based on their estimated position. The difference between the predicted measurement and the measured distance z 1,2 (l) − r 1,2 (l) represents the innovation. It then builds the combined covariance matrix and computes the Jacobian H 1,2 (l) that contains the derivatives of the range measurement with respect to the position of vehicle 1 and 2 (time index l omitted on matrix components). ∂r ∂y 1 ∂r ∂z1 ∂r ∂x2 ∂r ∂y 2 ∂r ∂z2 Using the residual covariance and the variance and σ r associated with the exteroceptive (range) sensor we compute the Kalman gain that represents a weighting factor for how much the measurement will affect the updated position. Using the innovation z 1,2 (l)−r 1,2 (l) and the Kalman gain, the updated position estimate is and the combined covariance is from which we can extract the updated covariance estimate for vehicle 1 P 1 (l). Note that we also obtain an updated estimate for the position and covariance of vehicle 2 P 2 (l) and x 2 (l). We make the following assumptions with regard to our group of vehicles. Vehicles The group consists of an arbitrary number of vehicles. Vehicles are not required to maintain a hierarchy, but each of them has a unique id. No vehicle needs to be aware of the size of the group. Each vehicle needs to have proprioceptive sensors to obtain dead-reckoning results and a single or several exteroceptive sensors to obtain a range, bearing or both to another vehicle. While covariance intersection, the standard algorithm for consistent data fusion, requires a range and bearing estimate, the particular strength of our approach lies in its 3417 ability to incorporate measurements from only one such sensor. Vehicle to vehicle communication is based on unacknowledged broadcast only. Our approach does not require any vehicle to receive all messages that were broadcasted. V. THE INTERLEAVED UPDATE ALGORITHM For the Interleaved Update algorithm, each vehicle i at time k now maintains a set X i (k) of state estimate vectors together with a set P i (k) of associated covariance matrices. In the elements of X i (k) and P i (k) the superscript q indicates the filter consisting of the state x q i (k) and the covariance P q i (k). As we will explain later, the maximum size of the set is 2 n where n is the total number of vehicles cooperating for navigation. Before vehicle i receives information from any other vehicle the only contents of X i (k) and Each time vehicle i receives proprioceptive sensor readings it uses the Kalman Filter prediction steps for state and covariance (eq. (1) and (2)) to propagate x 1 i (k) and P 1 i (k). When vehicle i receives a broadcast from vehicle j at time l, it first instantiates a second filter x 2 i (l),P 2 i (l) by copying the state and covariance matrix from x 1 i (l),P 1 i (l) the forward predicted version of the initial filter. The vehicle also instantiates a matrix T i where each row represents a filter and each column represents a vehicle number. The entry in matrix T i (q, i) is the time when vehicle i was last used to update filter q. Using the Kalman update equations (eq. (3) and (4)), we now only update x 2 i (l) and P 2 i (l). After this update, our sets X (l), P i (l) and the matrix T i (l) look as follows. All elements of column i in T i (l) are l as all filters are forward predicted using (1) and (2) up to the actual time l. Row 1 in T i (l) represents the filter which has never been updated by any other vehicle and contains the initial state forward predicted up to l. The first prediction for vehicle i after the update, propagates both filters using eq. (1) and (2) to X i (l + 1) and P i (l + 1) and all elements in column i in T i (l + 1) are set to l + 1. Matrix T i therefore keeps track of which vehicles have been used to update a particular filter as well as the age of the updates. Predictions after l + 1 up to the next update are propagated the same way, both filters are propagated and all elements in column i of T i are set to the actual time. All other columns remain unchanged. Every time vehicle i sends out a broadcast, the transmitted data consists out of X i , P i and T i . By maintaining a state x 1 i on vehicle i which is continuously propagated and has not been updated with information from vehicle j, we make sure that a future broadcast from vehicle i received by vehicle j contains a state which is not cross-correlated with vehicle j and can therefore be used by vehicle j for an update. The general update case when vehicle i receives a broadcast from j after both vehicles have received broadcasts from various other vehicles and have incorporated those to update their navigation filters looks as follows. We define S i as the set of all m vehicle ids which vehicle i received updates from. S i not only contains the ids of which vehicle i has directly received broadcasts from, but also those ids which have been propagated to it through other vehicles. The power set 2 Si than contains all 2 m possible subsets of these ids. Each subset then corresponds to a filter maintained in x q i ,P q i which maintains a state that has been updated by the ids in the corresponding subset A q i and therefore has cross-correlations with these vehicles. The information about which ids are in the individual subsets is maintained in line q of T i as each line in T i corresponds to a subset of A i . Similarly there is a set S j for all o ids which vehicle j has received broadcasts from. When vehicle i receives X j ,P j and T j from vehicle j it first adds entries in X i ,P i and T i for all elements of A j which are not in A i . As a result vehicle i then maintains filters for a new set A i Each filter x  We now select g and h s.t. P q i has the smallest trace of all possible combinations. Using g * and h * determined through eq. (9) we use eq. (7) to update the state. Line q in T i is updated to reflect the age of updates. All steps in section (V-F) are repeated for all 2 n filters on vehicle i and all other vehicles which overheard the broadcast update their local filters accordingly. The amount of information which needs to be transmitted during each broadcast, as well as the number of local prediction and update steps grows with O(m 2 ) where m is the size of set S i as defined in section V-F. The amount of data which needs to be transmitted per filter however is fairly small (≈ 10 bytes) and the update of each filter only requires 4 [2 × 6] · [6 × 6] matrix multiplications for a 3D environment where range and heading measurements are available. Assuming a data packet size of 10 kBytes set sizes up to 30 ids are feasible. If CN is implemented on a group of robots which is much larger than 30 it is worth noting that Roumeliotis et al. show in [15] that for a group of robots with the same level of uncertainty in their proprioceptive measurements the uncertainty growth is inversely proportional to the number of robots thus the contribution of each additional robot follows a law of diminishing return. This suggests that set sizes of 30 and less are sufficient to obtain an improvement of navigation accuracy which is close to the theoretical maximum obtained when broadcasts of all available vehicles are incorporated. Based on our available communications bandwidth and available processing cycles we can choose an upper bound b for the size of S i . If our set size grows larger than b we can incorporate the new broadcast according to section V-F and than resize S i by eliminating the id which contributes the least amount of information. The resize process consists of two steps. First we determine the vehicle (id) which contributes the least amount of information. Second we remove this id from S i and modify X i ,P i and T i accordingly. 1) Compare: One method to determine the vehicle with id q which contributes the least amount of information is to compare the trace difference between the filter which was only updated by {q, i} with the filter that has the dead reckoning result only {i}. 2) Eliminate: After we determined q * we remove all filters depending on q * from our sets X i ,P i and obtain our new sets X − i and P − i and our updated matrix T
In the decentralized estimation scheme of REF , the cross-correlations between the state estimates of different robots are not estimated.
Consistent cooperative localization
2009 IEEE International Conference on Robotics and Automation
2009 IEEE International Conference on Robotics and Automation
Computer Science Engineering