An Origin State Method for Lossy Synchronous-clock Acoustic Navigation

Transcription

1 An Origin State Method for Lossy Synhronous-lok Aousti Navigation Jeffrey M. Walls and Ryan M. Eustie Department of Mehanial Engineering Department of Naval Arhiteture and Marine Engineering University of Mihigan, Ann Arbor, MI 4819, Abstrat: This paper examines the problem of loalizing a network of underwater vehiles using inter-vehile range observations derived from measuring the one-way-travel-time (OWTT) of aousti broadasts. We report the derivation of the novel origin state method; an algorithm for distributing a loal pose-graph as a sequene of minimal bandwidth information pakets that is robust to paket loss. We demonstrate the effetiveness of this algorithm as an extension of the deentralized extended information filter (DEIF) for synhronous-lok aousti navigation through post-proess implementation using a real-world data set. Keywords: Robot navigation, Kalman filters, distributed models, ovariane, state estimation 1. INTRODUCTION Underwater vehiles typially fuse Doppler body-frame veloity, attitude, and pressure depth observations to produe dead-rekoned (DR) navigation solutions. Without absolute position measurements, (e.g., global positioning system (GPS)), DR position errors grows unbounded in time. Higher quality DR sensors are only apable of reduing the rate of unertainty growth, therefore, alternative methods for onstraining navigation errors are required. Underwater aousti navigation systems attain boundederror navigation through range-only observations to beaons with known position. Range observations are obtained from measuring the time-of-flight (TOF) of aousti signals and assuming a known sound speed profile. The long-baseline (LBL) navigation framework, for example, employs a network of fixed referene beaons to whih vehiles an measure range (Hunt et al., 1974). LBL, however, limits the area of operations to the overage footprint of the referene beaons. Furthermore, narrowband LBL laks the ability to sale up to large groups of vehiles beause only a single vehile an interrogate the beaon network at any one time. Synhronous-lok aousti navigation (Eustie et al., 211) is an appliation of ooperative loalization, whih involves a group of vehiles augmenting their position estimates with inter-platform range observations. Muh like other aousti navigation methods, reeiving platforms measure one-way-travel-time (OWTT) range to a transmitting platform. The OWTT derived distane is a funtion of the transmitter position at the time-oflaunh (TOL) and the reeiver position at the time-ofarrival (TOA). Synhronous-lok aousti navigation an be thought of as a moving long-baseline (MLBL) approah This work was supported by a grant from the National Siene Foundation under awards IIS and ANT (Vaganay et al., 24). Advantageously, synhronous-lok aousti networks an sale to arbitrarily many vehiles beause all vehiles within aousti range of the transmitting platform passively reeive a range measurement leading to onstant time update rates. Several methods have been proposed to inorporate OWTT-derived relative-range observations into a navigation framework; eah presenting a trade off between onsisteny, bandwidth, and robustness to lossy ommuniation. The rest of this paper is organized as follows. Setion 2 reviews existing approahes to ooperative loalization and synhronous-lok aousti navigation. Next, Setion 3 highlights the ability of the deentralized extended information filter (DEIF) algorithm to transmit a loal posegraph as a sum of delta information pakets. Setion 4 then presents the derivation of the origin state method, a novel reinterpretation of the DEIF that is robust to paket loss. Setion 5 demonstrates the appliation of the modified deentralized extended information filter (MDEIF) in a real-world experiment onsisting of a topside ship and an autonomous underwater vehile (AUV). Finally, Setion 6 summarizes and presents reommendations for future work. 2. COOPERATIVE LOCALIZATION The goal of general ooperative loalization is simply to estimate the position of vehiles within the network given loal information, gained via onboard sensors, and external information, gained via inter-vehile measurements (suh as OWTT range measurements). Several methods for synhronous-lok aousti navigation have been proposed in the literature. Many of these methods an be oneptualized using a pose-graph, whih is a graphial model in whih past vehile poses, or delayed states, are represented by graph nodes and their interdependene is enoded by graph edges (Fig. 1).

2 Fig. 1. Global pose-graph for a two-vehile network. Cirle nodes represent vehile poses, solid edges depit loally observable state transitions, while dashed edges represent relative-range observations. 2.1 Prior Work Eustie et al. (26) initially proposed a maximum likelihood estimate (MLE) solution for synhronously navigating subsea nodes via ranging to surfae ships. Other algorithms based upon the Kalman family of filters (Webster et al., 29; Fallon et al., 21; Bahr et al., 29), and fator-graphs (Cunningham et al., 21) also exist. These algorithms are summarized below. The entralized extended Kalman filter (CEKF) (Webster et al., 29) is a post-proess solution that assumes aess to all sensor measurements from all platforms. The CEKF is a global multi-platform pose-graph method that maintains all urrent vehile poses and all neessary TOL poses in its state vetor (Fig. 1). Eah new TOL transmitter state is added as a node on the global pose-graph. Edges are then added representing OWTT range onstraints between transmitter TOL nodes and TOA reeiver states. The CEKF reursively estimates the pose-graph within a Kalman filter framework, traking the global state mean and ovariane. Sharing inter-platform range measurements builds orrelation between vehile navigation estimates. The utility of the CEKF is that it traks the full ovariane matrix of the network and therefore serves as a baseline for omparison. A simple approah to distributing the underwater ooperative loalization problem is to have eah platform only estimate its urrent state as opposed to the full global pose-graph. OWTT measurement updates are made by inluding the transmitting platform s loal mean and ovariane estimate in eah aousti broadast. The naively distributed extended Kalman filter (NEKF) method is essentially equivalent to the CEKF but with inter-vehile orrelation atively held zero. This method does not trak the full pose-graph and orrelation that develops as a result of aousti broadasts resulting in an overonfident estimate (Walls and Eustie, 211). If, however, the orrelation remains small between platforms (e.g., when one vehile has onstant aess to GPS) the NEKF performs relatively well. Bahr et al. (29) proposed the interleaved update (IU) algorithm as a onsistent distributed ooperative loalization solution. The IU maintains a bank of NEKFs and a table that traks the origins of eah inter-vehile observation. This entire bank of filters is transmitted within eah aousti broadast. By areful bookkeeping, relative-range measurement updates are only performed using information that is guaranteed to be unorrelated. The result is a onservative, but onsistent, navigation solution. The deentralized extended information filter (DEIF) (Webster et al., 21) is another distributed navigation algorithm. The DEIF exatly mathes the CEKF result under a onstrained network topology of a single lient vehile reeiving aousti broadasts from a server vehile. The authors insightfully observed that information arued between server broadasts an be transmitted to the lient platform in a small additive paket, termed delta information. The lient filter simply adds these delta information pakets to reassemble the global posegraph. This algorithm is desribed in greater detail in Setion 3 and serves as the motivation for our origin state variant. Cunningham et al. (21) ooperatively builds landmarkbased maps of an environment. The authors present a graphial approah for sharing loal map information aross a network of vehiles. Their method, termed deentralized data fusion-smoothing and mapping (DDF-SAM), ondenses a loal landmark-based map and ommuniates this graph among the network. A global estimation module is then able to onsistently ombine loal graphs and arrive at a global solution. While DDF-SAM does provide a low bandwidth means for distributing loal pose-graphs, it does not expliitly deompose the graph to ahieve the minimal bandwidth required for the aousti hannel. 2.2 Proposed Method We onsider pose-graph navigation frameworks for their ability to easily enode orrelation between separate platforms that develops as a onsequene of relative-range observations. Ignoring orrelation between platforms an lead to inonsistent results by essentially double-ounting information. In a pose-graph ooperative loalization framework, eah platform estimates the global graph of the network. Fig. 1 illustrates the problem of estimating the position of two vehiles given loal observations and relative-range onstraints. Note that eah loal-graph must minimally ontain all nodes that are involved in relative-range observations. We laim that eah platform must aomplish two tasks: (1) Construt and distribute the loal pose-graphs of all platforms in the network. In Fig. 1 this step orresponds to building a separate graph for eah vehile using only the solid edges. (2) Compute a global estimate (i.e., global pose-graph). This ation is illustrated as adding the dashed lines in Fig. 1. The novel ontribution of this work is a method for deomposing, distributing, and later reonstruting a loal pose-graph using the onept of an origin state. Our algorithm allows relative-pose onstraints to be added within a ooperative loalization framework with bandwidth and robustness in mind. This method an be diretly applied to the DEIF algorithm to make it robust to a lossy ommuniation hannel. Moreover, this result also provides possible appliation to work suh as DDF-SAM by providing a more bandwidth onstrained method for distributing loal pose-graphs.

3 Fig. 2. Loal graph deomposition. Coarsely dashed ars represent state transitions transmitted by the DEIF algorithm. Finely dashed ars represent state transitions transmitted by the origin state method where x is the agreed upon origin node. 3. DECENTRALIZED EXTENDED INFORMATION FILTER The DEIF algorithm allows a subsea vehile to simultaneously reonstrut the topside ship s (or other support vehile s) loal pose-graph and inorporate OWTT derived relative-range onstraints. The interested reader is referred to (Webster et al., 21) for an in depth desription of the algorithm, though a brief outline is given below. In the following disussion, the subsea vehile and support vehile are referred to as the lient and server, respetively. The extended information filter (EIF) traks the distribution parametrized by the information matrix, Λ, and information vetor,, whih are defined in terms of the ovariane, Σ, and mean, µ, of the state vetor, x, as Λ = Σ 1, = Λµ. The DEIF algorithm aomplishes the previously outlined two steps of pose-graph ooperative loalization (loal pose-graph distribution and global estimation) within a filtering framework. The DEIF assumes that ommuniation, and onsequently relative-range observations, our in a single diretion (i.e, server to lient). Therefore, only the server (transmitter) is onerned with ommuniating its loal pose-graph and only the lient (reeiver) performs a global estimation proedure as it is the sole platform with aess to relative-range observations. The real insight of the DEIF algorithm lies in its ability to distribute the server s loal-pose graph as a series of delta information pakets. The simple sum of this series exatly reprodues the server pose-graph. Coneptually, eah delta information paket desribes a one-step transition adding a new node onneted to the previous node. These delta information transitions are illustrated in Fig DEIF Operation The DEIF onsists of two information filters: one maintained by the server and only inorporating loal measurements, the seond running on the lient vehile inorporating its loal DR observations as well as relative-range onstraints from the server. Server Side Implementation The server filter augments its state vetor at eah TOL with its urrent state, so that after N aousti broadasts, its state vetor at time k is x s (k) = [x s k, x s tn 1,..., x s t ], where the s subsript denotes server states and t i represents the i th TOL. This state vetor desribes the loal pose-graph up to time k with eah node representing a TOL state. At the next TOL, the server filter omputes a delta information paket to be enoded in the next transmission. The delta information, expressed as, Λ sn = Λ sn Λ sn 1 sn = sn sn 1 (1) enompasses all loal measurements and preditions that have ourred sine the previous TOL, effetively desribing a one step transition. The full derivation of this proess is desribed in detail in (Webster et al., 21). Briefly, the delta information omputation is possible beause the information matrix exhibits a sparse blok tri-diagonal form and measurement updates and preditions only additively modify a small fixed sized portion of the information matrix and vetor. Therefore, delta information represents the hange in this small area of the information matrix and vetor orresponding to the new TOL state and the previous TOL state. For example, if we onsider the server pose-graph at the seond TOL, t 1, its state vetor would also inlude the TOL state at time t, x s (t 1 ) = [x s t1, x s t ]. The orresponding information matrix and vetor are [ Λst1 Λ s1 = Λ ] [ ] st1 s t1 st st1, Λ st s t1 Λ s2 =. st s t st At the next TOL,, the server state vetor beomes x s ( ) = [x s t2, x s t1, x s t ]. The information matrix and vetor now take the form Λ st2 s t2 Λ st2 s t1 st2 Λ s2 = Λ st1 s t2 Λ s t1 s t1 Λ st1 s t s2 = s t1 Λ st s t1 Λ st s t st where the sub elements Λ s t1 s t1 Λ st1 s t1 and s t1 st1. The delta information is omputed as Λ st2 s t2 Λ st2 s t1 Λ s2 = Λ s2 Λ s1 = Λ st1 s t2 (Λ s t1 s t1 Λ st1 s t1 ) st2 s2 = s2 s1 = s t1 st1 where Λ s1 and s1 are padded with zeros for onformability. The extraneous zero rows and olumns are dropped from the delta information matrix and vetor for transmission. Eah delta information paket displays this same nonzero pattern leading to a fixed bandwidth paket. This ompat paket enodes the hanges in the server posegraph from t 1 to, whih is broadast to the lient vehile at the TOL. Note that the server is only required to keep the last two TOL states in its state vetor for delta information omputation. Therefore, in a pratial implementation older states an be marginalized out without onsequene. Advantageously, the memory requirement of the information filter grows linearly with the number of past states, O(N), as opposed to the ovariane form of the Kalman filter whih grows quadratially, O(N 2 ). Client Side Implementation The lient filter traks the global pose-graph onsisting of nodes representing its

4 urrent state as well as previous server TOL states. Server TOL states are aumulated through the sequene of delta information pakets reeived at the TOA via the reverse operation desribed in (1). This additive step reonstruts the server loal pose-graph up to its TOL. After reeiving N aousti broadasts at time k, the lient state vetor inludes x (k) = [x k, x s tn 1,..., x s t ], where the subsript denotes lient states. After inorporating the newest delta information paket, the filter proeeds with the new range measurement update following the standard Kalman proedure, ompleting the global estimation step. Note that following this relative-range measurement update, the lient DEIF estimate is exatly equivalent to the estimate maintained by the CEKF. The DEIF s sequential nature of the delta information omputation imposes a non-lossy ommuniation requirement. By missing a single delta information paket, the reeiving platform an no longer reonstrut the transmitter s loal pose-graph. A pratial workaround is to use a transmission redundany sheme; however, this vitiates the algorithm s low-bandwidth motivation. 4. ORIGIN STATE METHOD The origin state method is motivated by the non-lossy ommuniation requirement of the DEIF. While DEIF delta information pakets represent a one-step transition between onseutive nodes, there is no physial interpretation behind the meaning of eah paket. Instead, we propose an alternative alled the origin state method, whih deomposes the pose-graph into meaningful omponents that allow for the pose-graph to be reonstruted even in the presene of missed pakets. The basi idea behind the origin state method is to represent nodes in the pose-graph relative to another node, the origin, as opposed to a transition from the previous node as in the DEIF. This differene is illustrated in Fig. 2. Instead of representing the relationship between nodes as an additive delta information paket, we expliitly solve for the joint marginal distribution of the new node and the origin node. The proess of omputing an origin state paket and reonstruting the pose-graph given a new origin state paket is demonstrated below. 4.1 Computing Origin State Pakets Assume that the transmitter s state vetor, after three TOL at times t, t 1, and, is x 2 = [x, x t 1, x t ]. The orresponding information matrix and vetor are: [ ] Λt2, Λ 2 = Λ t1,, 2 = (2) Λ t,t 1 Λ t,t t where the off-diagonal zero is from the Markov property. At the next TOL state, t 3, the state vetor is augmented with the urrent platform pose. The information matrix and vetor now take the form Λ Λ 3 = t2,t 3 Λ t3, Λ t1, Λ 3 = t 2 t1,t (3) t1 Λ t,t 1 Λ t,t t with boxed elements indiating new (from augmentation) or hanged values. Note that Λ, Λ t2, and likewise t2. The transition from (2) to (3) ontains all loal information that has been gained by the transmitter from loal measurements. The origin state paket is omputed as the joint marginal of the transmitter distribution over the new TOL node, x t3, and the origin state, x t. The origin state is simply a node in the pose-graph that the transmitter and the reeiver both agree upon. In this example, let x t be the origin state. In the information form, marginalization is defined by the Shur omplement resulting in Λ s s 3 = t3,t 3 Λ s ] t 3,t Λ s t,t 3 Λ s, s 3 = t,t [ ] s t3 s, (4) t where the s supersript indiates that this is the joint marginal distribution omputed on the server. An origin state paket is omputed at eah TOL and ommuniated to the reeiver where the series of all origin state pakets is used to reonstrut the transmitter s pose-graph. The dimension of the origin state paket is equal to that of the delta information required by the DEIF. 4.2 Reonstruting Pose-Graphs from Origin State Pakets The goal of the reeiving platform is to reonstrut the transmitter loal pose-graph given the sequene of origin state pakets. In this example, assume that the reeiver has aquired the pose-graph up to the previous TOL, (i.e., up to in (2)), and then reeives the new origin state paket, expressed as (4). The reeived origin state paket enodes the joint marginal of the transmitter state at the TOL and the origin. For notational onveniene, we also define the joint marginal of the previous delayed state, x t2, and the origin state, x t, over the previous distribution, (2), Λ 2 = t2, Λ,t Λ t, Λ t,t ], 2 = [ ] t2, (5) t where this time the supersript indiates that this joint marginal is omputed on the lient platform. The lient an now reonstrut (3) using just (2), (4), and (5). By studying how the new origin state paket, (4), was omputed via the marginalization operation of (3) (refer to Appendix A), we equate terms and expliitly solve for the unknown values of the information matrix and vetor. The full pose-graph up to time t 3 is then defined in the information form by omputing the following system, Ω 1 = Λ t, 1 (Λ t,t Λ s t,t )Λ,t 1 β = Λ s t 3,t (Ω 1 Λ t2,t ) 1 Λ, t3 ν = t, Ω 1] 1 ( t s t ) = Λ s t 3,t 3 + βω 1 β = β = Ω + t 1, = s t 3 + βω 1 ν = ν +. The equations in (6) solve for the full joint distribution given the joint marginals over eah TOL state and the (6)

5 origin state. Intuitively, this method an be thought of as inferring the orrelation between a new TOL state and all other past states by observing how its orrelation with a known origin state is affeted. We an see this in the terms (Λ t,t Λ s t,t ) and ( t s t ), whih express the differene in information known about the origin state by the lient and server vehiles. If a paket is lost in transmission, the reeiving platform still reonstruts an equivalent pose-graph with subsequent pakets as if the lost TOL state had been marginalized out. The robustness of the origin state method to dropped transmissions is an important feature espeially when using an unstable ommuniation hannel, suh as the aousti hannel. (a) Vehile trajetories (b) Iver2 AUV 4.3 Numerial Stability The origin state method defines a transmission sheme robust to paket loss using a small fixed amount of bandwidth; however, there is a ath. Over time the orrelation between eah new TOL state and the origin state dereases due to proess noise. As mentioned previously, the origin state method omputes the full joint distribution over the urrent state and all past states by observing how information gained between transmissions affets the estimate of an origin state. As the orrelation between the newest state and the origin state dereases, the ability to numerially ompute the differene in information known about the origin state diminishes. Eventually, the origin state method breaks as a result of numerial instability. A simple solution to this issue is to ensure that the origin state is ontinually moved forward in time, so as to remain as lose as possible to the new TOL state. Origin shifting an easily be aomplished in the underwater senario by allowing ommuniation to our bi-diretionally among platforms so that eah platform an transmit an index relating to the most reently reeived TOL pose from all other platforms in addition to its usual origin state paket. Eah platform an then trak the most reent state reeived by all other vehiles and shift its origin state forward aordingly. The drawbak is that lient ommuniation is no longer passive. 4.4 Modified Deentralized Extended Information Filter The MDEIF operation mirrors the standard DEIF exept for the aousti paket omposition. In the MDEIF, the server vehile transmits the origin state joint marginal of eah TOL state. The lient vehile now reonstruts the loal information from the server as previously outlined, and omputes the urrent delta information exatly as if the server vehile had done so. This delta information is then handed over to the existing DEIF mahinery. The resulting MDEIF algorithm produes the same navigation solution as the DEIF while lifting the non-lossy ommuniations onstraint. For pratial implementation, an origin shifting sheme similar to the one outlined above is neessary. Fig. 3. Experimental Setup. Table 1. AUV Navigation Sensors: Sampling Frequeny and Noise Charateristis. Client Sensors Variable Frequeny Noise Mirostrain AHRS φ, θ, ψ 25. Hz 2. Doppler veloity log ẋ, ẏ, ż 3. Hz 5. m/s pressure depth z 2. Hz 1 m aousti modem slant range every 15 se 1 m Server Sensors Variable Frequeny Noise GPS x, y 1. Hz 3. m 5.1 Experimental Setup 5. EXPERIMENTS A two-node AUV trial was arried out using one ustom modified Oean-Server Iver2 AUV and a topside surfae raft. The AUV followed a lawn-mower pattern with roughly 5 m traklines spaed 5 m apart as depited in Fig. 3. The topside ship drifted to various positions around the survey area during the mission. The AUV was equipped with a typial advaned deadrekoned sensor suite (as detailed in (Brown et al., 28) and summarized in Table 1) omprised of a 6 khz RDI Doppler veloity log (DVL), a Mirostrain 3DM- GX1 attitude and heading referene system (AHRS), and a Desert Star Systems SSP-1 digital pressure sensor. We define the state of eah vehile at time k as x k = [x k, y k, ẋ k, ẏ k ], where the vehile position in the loal-level plane is desribed by the xy pair and the orresponding world-frame veloities are ẋẏ. Sine we onsider attitude to be instrumented with bounded error, we projet the DVL bodyframe veloity measurements into the world-frame and treat these as linear observations of the AUV veloity. The topside vehile only observes world-frame position as measured by a GPS reeiver. The soure of eah aousti transmission was defined by a fixed time division multiple aess (TDMA) shedule during whih eah vehile was assigned a time-slot to send a data paket. The network maintained a 145 seond TDMA yle, whih onsisted of 6 topside broadasts and 4 subsea broadasts. During the experiment, the subsea

6 x unertainty [m] y unertainty [m] 2 1 Iver 28 3 σ xy unertainty DR CEKF DEIF MDEIF DR CEKF DEIF MDEIF time [s] Fig. 4. Estimated AUV position unertainty. Note that the estimates produed by the CEKF, DEIF, and MDEIF are nearly idential. vehile reeived roughly 81% of aousti transmissions from the ship, while the topside ship reeived about 67% of the subsea transmissions. We ompared the performane of the DEIF and the MDEIF through post-proess implementation. In order to run the DEIF, we made the assumption that suessful aousti broadasts were known ahead of time in order to orretly ompute eah delta information; an assumption that would not hold true in a real-time implementation. The MDEIF uses aousti broadasts sent from the AUV to the topside ship to shift the origin state forward in time as proposed earlier. No assumptions regarding suessful transmission were made by the MDEIF so that its performane in post-proess aurately reflets its realtime performane. 5.2 Results The xy position unertainty estimates are shown in Fig. 4 for the DEIF and MDEIF, as well as for the CEKF for omparison. The position unertainty is greatly redued by inorporating OWTT relative-range measurements between the two vehiles. Fig. 5 presents the differene in xy position estimates for the DEIF and MDEIF. The differene remains under several millimeters throughout the experiment and, with the exeption of a brief period toward the end of the mission (orresponding to a period of lost ommuniation), the differene is on the order of numerial preision. During the period of lost ommuniation, the origin is not shifted forward in time so we expet the origin state method to have diffiulty with numerial preision. The differene remains small, however, and the algorithm is able to reover. We see that the MDEIF performs nearly idential to the DEIF without requiring a non-lossy ommuniation hannel or redundant transmission shemes. differene [m] 1 differene in DEIF and MDEIF means x origin state orrelation oeffiient mission time [s] Fig. 5. The top plot shows the differene between the DEIF and MDEIF position estimates, whih remain negligible throughout the experiment. The lower plot shows the orrelation oeffiient between the origin state and the urrent topside state. The orrelation oeffiient quikly goes to zero unless the origin is shifted forward in time. The differene between the DEIF and MDEIF is largest during a period when the origin is not shifted forward (t=34 to t=37), although this differene quikly dereases one regular ommuniation resumes (t=4). 6. CONCLUSION In this paper, we have shown that ooperative loalization an be segmented into two omponent tasks: loal graph synthesis and global graph estimation. The proposed origin state method is a low bandwidth solution to the first omponent. Under the origin state framework, reeiving platforms an reonstrut the transmitter s loal posegraph as a ombination of many joint marginal origin state pakets. We also presented an extension to the DEIF estimation framework, the MDEIF, whih lifts the non-lossy ommuniation requirement of the DEIF. We showed the performane of the MDEIF on an experimental data set inluding a single AUV and a topside ship, and found it to be equal to the DEIF and CEKF in terms of auray. Future work will address the numerial instability that ours when the urrent state of the transmitting vehile is no longer orrelated with the origin state. We will also examine other estimation frameworks that, in onert with the origin state method, ould inorporate relative-range observations aross a large network of vehiles. ACKNOWLEDGEMENTS We are grateful to the University of Mihigan Biologial Station for their logistial support during field experiments. REFERENCES Bahr, A., Walter, M.R., and Leonard, J.J. (29). Consistent ooperative loalization. In Pro. IEEE Int. Conf. Robot. and Automation, x y

7 Brown, H., Kim, A., and Eustie, R. (28). Development of a multi-auv SLAM testbed at the University of Mihigan. In Pro. IEEE/MTS OCEANS Conf. Exhib., 1 6. Quebe, Canada. Cunningham, A., Paluri, B., and Dellaert, F. (21). DDF- SAM: Fully distributed SLAM using onstrained fator graphs. In Pro. IEEE/RSJ Int. Conf. Intell. Robots and Syst., Eustie, R.M., Whitomb, L.L., Singh, H., and Grund, M. (26). Reent advanes in synhronous-lok one-waytravel-time aousti navigation. In Pro. IEEE/MTS OCEANS Conf. Exhib., 1 6. Boston, MA, USA. Eustie, R.M., Singh, H., and Whitomb, L.L. (211). Synhronous-lok one-way-travel-time aousti navigation for underwater vehiles. J. Field Robot., 28(1), Fallon, M.F., Papadopoulos, G., Leonard, J.J., and Patrikalakis, N.M. (21). Cooperative AUV navigation using a single maneuvering surfae raft. Int. J. Robot. Res., 29(12), Hunt, M., Marquet, W., Moller, D., Peal, K., Smith, W., and Spindel, R. (1974). An aousti navigation system. Tehnial Report WHOI-74-6, Woods Hole Oean. Inst. Vaganay, J., Leonard, J., Curio, J., and Willox, J. (24). Experimental validation of the moving long base-line navigation onept. In Pro. IEEE/OES Autonomous Underwater Vehiles Conf., Walls, J.M. and Eustie, R.M. (211). Experimental omparison of synhronous-lok ooperative aousti navigation algorithms. In Pro. IEEE/MTS OCEANS Conf. Exhib., 1 7. Kona, HI. Webster, S.E., Eustie, R.M., Singh, H., and Whitomb, L.L. (29). Preliminary deep water results in singlebeaon one-way-travel-time aousti navigation for underwater vehiles. In Pro. IEEE/RSJ Intl. Conf. Intell. Robots and Syst., St. Louis, MO. Webster, S.E., Whitomb, L.L., and Eustie, R.M. (21). Preliminary results in deentralized estimation for single-beaon aousti underwater navigation. In Pro. Robot.: Si. & Syst. Conf. Zaragoza, Spain. Appendix A. ORIGIN STATE METHOD DERIVATION This setion details the steps taken to obtain (6). This proedure allows the lient to solve for the joint distribution over all server TOL states, Λ Λ 3 = t2,t 3 Λ, Λ t1, Λ t1,t Λ t,t 1 Λ t,t 3 = t3 t (A.1) given information that the lient has aess to inluding the server joint distribution after the previous origin state paket, [ ] Λt2, Λ 2 = Λ t1,, 2 = Λ t,t 1 Λ t,t t and the new origin state paket, Λ s s 3 = t3,t 3 Λ s ] t 3,t Λ s t,t 3 Λ s, s 3 = t,t (A.2) [ ] s t3 s, (A.3) t whih is simply the joint marginal of (A.1) over the newest TOL state at t 3, and the origin state at t omputed by the server. Note that the lient will solve for the boxed elements in (A.1). To make the derivation lear, we define two more marginal distributions eah omputed by the lient. First, we define the joint marginal of (A.1) over the transmitter states at times t 3 and as well as the origin state at time t, Λ 3 = Λ t 3,t 3 Λ, Λ,t n Λ t, Λ 3 = t 3. (A.4) t,t t The reeiver does not have outright aess to this distribution, but solving for it will serve as an intermediate step. Seond, we express the joint marginal of the distribution up to time, (A.2), over the previous delayed state at time, and the origin state at time t, Λ 2 = t2, Λ ] [ ],t Λ t, Λ, 2 = t2 t,t. (A.5) t The strategy to solve for (A.1) first involves expliitly writing (A.3) by applying the Shur omplement to (A.4) and then equating known terms. We an write the new origin state paket in terms of a marginal of (A.4) as s t3,t 3 Λ s ] t 3,t Λ s t,t 3 Λ s = t,t [ Λt3,t 3 Λ,t ] Λ t, Λ t,t Λ t, Λ,t [ ] [ s t3 t3 Λ s = t3, ] t t Λ t,. Solving the above leads to Ω 1 = Λ, 1 = Λ t, 1 (Λ t,t Λ s t,t )Λ,t 1 β = Λ s t 3,t (Ω 1 Λ t2,t ) 1 ν = = t, Ω 1] 1 ( t s t ) = Λ s t 3,t 3 + βω 1 β = β = β t3 = s t 3 + βω 1 ν., (A.6) (A.7) The only remaining unknown elements from (A.1) are Λ, and. We take a similar approah to solve this by writing (A.4) after applying the Shur omplement to (A.1) and then equating terms. Λ t 3,t 3 Λ, Λ,t = Λ t, Λ t,t Λ, Λ t1, Λ t1,t Λ t,t 1 Λ t1, Λ t,t Λ t,t 1 Λ t1,t t 3 = t2. t t Λ t,t 1 (A.8) The final unknown terms an then be expressed as Λ, = Λ t 2, + Λ t1, = + (A.9). Together (A.7) and (A.9) present a method to solve for the unknown elements of (A.1). t3