# Underwater Localization and Tracking of Physical Systems

Authors: Patrick Carroll; Shengli Zhou; Hao Zhou; Xiaoka Xu; Jun-Hong Cui; Peter Willett
Journal: Journal of Electrical and Computer Engineering
 (2012)

---

## Abstract

We investigate the problem of localizing an underwater sensor node based on message broadcasting from multiple surface nodes. With the time-of-arrival measurements from a DSP-based multicarrier modem, each sensor node localizes itself based on the travel time differences among multiple senders to the receiver. Using one-way message passing, such a solution can scale to accommodate a large number of nodes in a network. We consider the issue from not only the physical layer, but also at the node processing layer by incorporating a tracking solution. We present simulation results, testing results in a swimming pool featuring both stationary and moving receivers, and results from a lake test with a mobile receiver.


---

## Body

## 1. Introduction

Underwater localization is a topic of great interest and study, with increasingly hungry applications driving the need for better and better solutions [1, 2]. Several current systems feature augmented inertial navigation methods, which use filtering and tracking methods to provide corrections and improvements upon traditional onboard navigational equipments [3, 4]. Aside from these methods, there are several localization techniques based on acoustic signaling. The first is the long base line (LBL) system, where transponders are installed at the sea floor, and an underwater vehicle interrogates the transponders for round-trip delay estimation followed by triangulation [5]. LBL has good localization accuracy, but it requires long-time calibration. The second is the short base line (SBL) system, where a mothership moves above the underwater vehicle. The ship locates the vehicle using high-frequency directional emitters. The third approach is based on floating buoys [6, 7]. This system acts like a long base line system except that the reference points are surface buoys. There are commercial products—the GPS intelligent buoys (GIB)—that route signals from an underwater node to surface buoys [7], and using radio links, the surface buoys forward all information to a mothership, wherein the localization is performed. The floating buoys are easier to deploy and calibrate than LBL systems.Several systems have been proposed which consider a network of underwater nodes instead of a single underwater vehicle or small group of nodes. These approaches typically use an anchor-client-based approach, where nodes systematically position themselves and disseminate this information with other nearby nodes in an effort to systematically localize the entire network [8–19]. However, few of these methods have been fully implemented in physical networking systems.In this paper, we propose a new localization approach based on message broadcasts from multiple surface nodes, coupled with tracking algorithms and implemented on a physical system to provide a complete analysis. With the time-of-arrival measurements, the receiver computes its own localization based on the differences of the travel time among multiple senders to the receiver. We present one solution based on exhaustive search and the other based on the least-squares formulation [20]. By implementing the localization algorithms in the OFDM modem prototypes developed in [21], we have carried out tests in swimming pool and lake environments. With these point measurements, tracking analysis was also carried out on the collected data to further refine the position estimate.Thus, we consider the problem not only at the modem physical layer with regards to timing and detection, but further analyze it in a single-point estimate, and ultimately combine the point estimates for a tracking implementation. In particular, we consider two tracking scenarios: a largely static scenario in which the nodes are assumed to be tethered or freely floating with no self-propulsion and a mobile scenario in which the object being tracked is assumed to make deliberate maneuvers and have some control of its motion, such as an autonomous underwater vehicle (AUV).The advantage of the proposed localization method is that the broadcast messages can serve an arbitrary number of underwater nodes once they are in range, in contrast to many existing solutions which can only serve a small number of users.The rest of the paper is organized as follows. We present the system overview in Section2. Section 3 discusses the timing concerns of the physical layer. In Section 4, we consider the self-localization of a node for a single-point estimate. In Section 5, we present the series of tracking algorithms that are implemented. Sections 6, 7, and 8 contain simulation results and testing results from a swimming pool and lake, respectively. Conclusions are in Section 9.


## 2. System Overview

Figure1 depicts the considered system setup, with several surface nodes and multiple underwater nodes. The surface nodes are equipped with satellite-based GPS receivers. Relying on the internal pulse provided by the GPS device that is accurate to within 1 microsecond GPS time, the surface nodes are assumed to be well synchronized. At predetermined intervals, the surface nodes sequentially broadcast their current location and time.Figure 1
An underwater sensor network with multiple surface buoys.The underwater nodes within the broadcast range will detect a series of transmissions and decode those messages. By comparing the reception time with the transmission time encoded in the message, each underwater node can obtain estimates of the time of arrivals (or time of flights) of messages from different surface nodes, based on which it tries to compute its own position. Note that the broadcast from the surface to underwater nodes is a one-way transmission, that localization quality is independent of the number of underwater nodes in the network, and that there is no additional interference involved among different underwater nodes.The overall scheme of localization refinement is presented in Figure2. For one round of transmissions, the receiver obtains the travel times from all the surface buoys, based on which a point estimate is available. Once several of these broadcast periods have occurred, individual point estimates may be combined via tracking algorithms to form a more accurate understanding of the current node position. The algorithms at different stages are described in the following sections.Figure 2
One round of surface buoy broadcasting leads to one point estimate, while tracking algorithms improve the localization accuracy when multiple-point estimates are available.


## 3. One-Way Travel Time Estimation

Let us focus on one receiver at position(xr,yr,zr). Suppose that there are N surface nodes, at positions (xn,yn,zn), n=1,…,N. Let dn denote the distance between the receiver node and the nth surface node
(1)dn=(xr-xn)2+(yr-yn)2+(zr-zn)2.
Without loss of generality, we set the first surface node at the origin, that is, x1=y1=z1=0, such that
(2)d12=xr2+yr2+zr2.
The actual time of arrival is tn=dn/c, where c is the sound propagation speed.The transmission time is encoded at each broadcast message. The receiver needs to estimate the arrival time of each message to provide an estimate on the time of arrivaltn. In this paper, we use real-time DSP-based OFDM modem prototypes [21], as shown in Figure 3. First, the communication channel is monitored to detect signal arrivals, based on a background noise level monitoring performed by the modem at initialization. When a signal is detected, the correlation of the signal with a sliding window of itself is compared to determine when the level of peak correlation in the preamble of the message occurs, indicated by a plateau in the correlation. Once this plateau is selected, the time of arrival is coarsely estimated as having been approximately halfway during this plateau period [22]. Once coarse channel estimation has occurred, the preamble, which is entirely known to the receiver, is used to estimate the instantaneous underwater channel conditions, and from there, a more refined estimation of the time of arrival is performed via the modified page test as in [23].Figure 3
The OFDM modem prototype with an attached GPS unit.Lett^n denote the estimate of tn from the OFDM modem. It can be expressed as the sum of the real transmission propagation, the delay in signal processing at both transmitter and receiver bn, and the estimation noise wn:
(3)t^n=tn+bn+wn.
Multiple tests of the OFDM modem reveal that the noise wn has standard deviation on the order of 5–10 ms. (The bandwidth used by the OFDM modem is 6 kHz, leading to a baseband sampling interval of 1/6 ms.) On the other hand, the processing delay (bias) bn has large magnitude, which might be on the order of 500 ms. However, tests have also shown that bn is nearly identical across modems with similar hardware and operating software with the assumed GPS synchronization. Thus, we will treat bn as a constant bn=b and present the localization algorithms based on
(4)t^n=tn+b+wn,n=1,…,N.


## 4. Localization

In each round of broadcasting from all surface nodes, a node collectsN travel time measurements and can form a single-point estimate of its current position. This is accomplished by way of localization algorithms based on the intersection of spherical surfaces.Since the biasb is unknown and usually large, time-of-arrival- (TOA-) based methods are not suitable. Instead, we use the time-difference-of-arrival (TDOA) method to cancel the common bias term b by forming
(5)Δt^n1=t^n-t^1,n=2,…,N.
The distance difference dn1=dn-d1 is then estimated by
(6)d^n1=cΔt^n1.The TDOA method also corrects for clock skew alongside this bias term, due to the nature of the shared GPS clock. Each receiving node will have its own internal clock, which at some update periodk will have drifted by an unknown, nonlinear skew factor ϕ(k). Each of the surface transmitters, however, will have the same clock skew, and due to the periodic corrections by the GPS clock, this value can be assumed approximately 0 for any period k. Thus, each transmission time can be represented as
(7)t^n=tn+b+ϕ(k)+wn,n=1,…,N,
and again, by taking the difference of the time-of-arrival estimates, this common clock skew is eliminated from the timing estimate.Next, we present the localization methods based on the exhaustive search and least-squares formulations.


### 4.1. Exhaustive Search

The individual time estimatest^n generally have correlated noise in the underwater channel. For simplicity, we assume instead that they are independent and identically distributed (i.i.d.) and pursue a maximum likelihood function
(8)min⁡xr,yr,zrf(xr,yr,zr)=∑n=2N‍[cΔt^n1-(dn-d1)]2.The solution to (8) is found by exhaustive search.In subtracting a common random variable,t^1, from all subsequent TOA estimates, we are correlating each measurement by a factor of approximately 1/2. As such, assuming again i.i.d. measurements, a differencing measurement bias modification can be made as follows:
(9)min⁡xr,yr,zrf(xr,yr,zr)=(ct^Δ-dΔ)TΣ-1(ct^Δ-dΔ)T,
where
(10)t^Δ=[Δt^21Δt^31⋮Δt^n1],dΔ=[d2-d1d3-d1⋮dn-d1],
and Σ is an (N-1)×(N-1) normalized covariance matrix
(11)Σ=[11212⋯1212112⋯12⋮⋮⋮⋯⋮121212⋯1].


### 4.2. Least-Squares Solution

We use the least-squares solution from [20]. Since dn=dn1+d1, we have
(12)(dn1+d1)2=xn2+yn2+zn2-2xnxr-2ynyr-2znzr+d12,
which can be simplified as
(13)xnxr+ynyr+znzr=12([xn2+yn2+zn2-dn12])-dn1d1.
Define the following matrix and vectors:
(14)H=[x2y2z2x3y3z3⋮⋮⋮xNyNzN],v=(-d^21-d^31⋮-d^N1),u=12[x22+y22+z22-d^212x32+y32+z32-d^312⋮xN2+yN2+zN2-d^N12],a=[xryrzr].
The least-squares solution can be obtained as
(15)a^=d1H†v+H†u,
where † stands for pseudoinverse. Substituting the entries of into (2) yields a quadratic equation for d1 [20]. Solving for d1 and substituting the positive root back into (15) provides the final solution for the receiver position a. We compare this LS solution with our exhaustive search in Figure 4, where timing error was assumed Gaussian distributed with zero mean and a varied standard deviation.Figure 4
Root-mean-squared (RMS) localization error as a function of the standard deviation of the distance measurements. (Two curves overlap and are not easily distinguishable).


## 4.1. Exhaustive Search

The individual time estimatest^n generally have correlated noise in the underwater channel. For simplicity, we assume instead that they are independent and identically distributed (i.i.d.) and pursue a maximum likelihood function
(8)min⁡xr,yr,zrf(xr,yr,zr)=∑n=2N‍[cΔt^n1-(dn-d1)]2.The solution to (8) is found by exhaustive search.In subtracting a common random variable,t^1, from all subsequent TOA estimates, we are correlating each measurement by a factor of approximately 1/2. As such, assuming again i.i.d. measurements, a differencing measurement bias modification can be made as follows:
(9)min⁡xr,yr,zrf(xr,yr,zr)=(ct^Δ-dΔ)TΣ-1(ct^Δ-dΔ)T,
where
(10)t^Δ=[Δt^21Δt^31⋮Δt^n1],dΔ=[d2-d1d3-d1⋮dn-d1],
and Σ is an (N-1)×(N-1) normalized covariance matrix
(11)Σ=[11212⋯1212112⋯12⋮⋮⋮⋯⋮121212⋯1].


## 4.2. Least-Squares Solution

We use the least-squares solution from [20]. Since dn=dn1+d1, we have
(12)(dn1+d1)2=xn2+yn2+zn2-2xnxr-2ynyr-2znzr+d12,
which can be simplified as
(13)xnxr+ynyr+znzr=12([xn2+yn2+zn2-dn12])-dn1d1.
Define the following matrix and vectors:
(14)H=[x2y2z2x3y3z3⋮⋮⋮xNyNzN],v=(-d^21-d^31⋮-d^N1),u=12[x22+y22+z22-d^212x32+y32+z32-d^312⋮xN2+yN2+zN2-d^N12],a=[xryrzr].
The least-squares solution can be obtained as
(15)a^=d1H†v+H†u,
where † stands for pseudoinverse. Substituting the entries of into (2) yields a quadratic equation for d1 [20]. Solving for d1 and substituting the positive root back into (15) provides the final solution for the receiver position a. We compare this LS solution with our exhaustive search in Figure 4, where timing error was assumed Gaussian distributed with zero mean and a varied standard deviation.Figure 4
Root-mean-squared (RMS) localization error as a function of the standard deviation of the distance measurements. (Two curves overlap and are not easily distinguishable).


## 5. Tracking Algorithms

To further reduce the localization error from a single-point measurement, tracking algorithms can be implemented to combine the knowledge of multiple measurements into a more accurate position estimate.In deciding which tracking approach would be best, we first consider the scenarios in which the node is being localized. There are two distinct modes in which underwater nodes typically move: either passively, with the water currents as a free-floating node, or actively as an underwater vehicle such as an AUV. Both are characterized primarily by long periods of relatively straight motion at a fairly constant speed. Typically, AUV motion differs in that at certain random intervals, it will change direction according to operator or preprogrammed instruction. Most search patterns for AUVs are defined by spiral paths, or by rectangular search grids. In either case, the vehicle is likely to alter its direction by way of a continuous turn; that is, to make a turn at a fixed angular velocity until the desired heading is achieved (or in the case of a spiral, until the search area is exhausted).


### 5.1. Kalman Filter

For simple motion models, characterized by long periods of motion free from maneuvers, a Kalman filter is the best linear state estimator for a Gaussian initial state assumption [24].In the KF, we choose to model the movement of the node as set of discrete white noise acceleration (“kinematic”) models [24], with a separate model for each possible direction, that is, x, y, and z. As such, the state equation for the Kalman filter at time index k+1 based on information from time step k becomes
(16)L(k+1)=F(k)L(k)+v(k),
with measurement
(17)z(k+1)=H(k+1)L(k+1)+w(k+1),
where
(18)F(k)=[1τ0000010000001τ0000010000001τ000001],(19)H(k+1)=[100000001000000010],v(k) is process noise, w(k) is measurement noise, and τ is the sampling interval of the discrete model in seconds.The state covariance is modeled as(20)P(k+1∣k)=F(k)P(k∣k)F(k)T+Q(k).
The corresponding process noise has a covariance given as
(21)Q=[14τ412τ3000012τ3τ200000014τ412τ3000012τ3τ200000014τ412τ3000012τ3τ2]σv2.Here,σv is a design parameter that is chosen to match the most likely level of process noise to be experienced by the object in question, which is to say it controls how much the model anticipates the object to maneuver. Given that the object in question is likely to be either stationary or altering its velocity at a slow, steady rate, a process noise level of σv=0.5m/s2 was selected to best emulate this behavior. The filter was initialized with two-point differencing [24].


### 5.2. Probabilistic Data Association Filter

During analysis of the performance of the KF, it became apparent that within a tracking window, there were point estimates which would appear as outliers by a considerable margin. On closer inspection, it was found that these were likely the result of one or more transmissions during which the direct-path signal propagation had been blocked, and a bounce was instead detected and treated as the direct path. Due to the assumption implicit to the KF approach that all of our messages are the direct-path propagation, this resulted in a drastic alteration of the point estimate, to the point where it could be classified as a false measurement. In that context, the probabilistic data association filter (PDAF) offers an improved performance over the standard KF, by allowing outlier estimates such as these to be ignored as false-alarm or clutter detections [25]. The PDAF is very similar to the KF in terms of state equations, presented here for measurement k+1:
(22)L(k+1)=F(k)L(k)+v(k),
with measurement
(23)z(k+1)=H(k+1)L(k+1)+w(k+1),
where F(k) and H(k+1) are given in (18) and (19), respectively.The state covariance is modeled similarly as(24)P(k+1∣k)=F(k)P(k∣k)F(k)T+Q(k).
The difference is on how to compute P(k∣k). First, let PD denote the probability of detection, which is a design parameter. Operating under the assumption that the measurement is always gated, define
(25)b=2(1-PDPD).
Define the following variables and matrices:
(26)ν(k)=z(k)-H(k)L(k),e(k)=e-(1/2)ν(k)TS(k)-1ν(k),S(k)=H(k)P(k∣k-1)H(k)T+R,W(k)=P(k∣k-1)H(k)TS(k)-1,
where R is the observation noise covariance. The probability of no correct measurement available is
(27)β0(k)=bb+e(k).
The probability of a correct measurement is
(28)β1(k)=e(k)b+e(k).
Further defining
(29)Pc(k∣k)=P(k∣k-1)-W(k)S(k)W(k)T,P~(k)=W(k)(β1(k)ν(k)ν(k)T-ν(k)ν(k)T)W(k)T,
the covariance matrix update is as follows:
(30)P(k∣k)=β0(k)P(k∣k-1)+β1(k)Pc(k∣k)+P~(k).We are assuming that the measurement is always gated, and that there is only a single target and a single measurement at each time step. Accordingly, thePD is then the probability that the current measurement is a valid estimate of the node being tracked. Based on experimental data, the number of “false detection” measurements was around 5% of the total samples, and so a value of 0.95 was selected for PD.


### 5.3. Interacting Multiple Model (IMM) Filter

For the more complex motion of an active underwater node, an interacting multiple model filter (IMM) was implemented, as the expected maneuvering index of underwater vehicles can easily exceed the threshold for which a single linear filter is likely to have any benefit. To this end, the IMM was a simple two-model filter, with a single, linear, low process noise(σv=0.05m/s2) KF to account for the straight motion travel, and an extended Kalman filter (EKF), configured in a coordinated-turn mode [26]. This validity of the coordinated turn assumption is dependent on the scenario in question, though given the previously described search patterns, it should be sufficiently accurate [27].The linear KF uses similar system equations as given previously, augmented with an additional column and row of zeros in order to accommodate the use of the EKF’s additional state in the IMM. The EKF in this problem uses one of two sets of state equations: the first set is an approximation used when the predicted coordinated turn rate is near 0 (Ω^(k)≈0), and the second set is used when the predicted coordinated turn rate is greater than some detection threshold (|Ω^(k)|>0) [24].The first set of EKF state equation modifications (Ω^(k)≈0) is as follows:
(31)L(k+1)=FL(k)L(k)+v(k),where(32)FL(k)=[1τ0000-12τ2η˙^(k)010000-τη˙^(k)001τ0012τ2ξ˙^(k)000100τξ˙^(k)00001τ000000100000001],
where η and ξ represent the x and y directions, respectively, and we denote η˙ as the velocity component in the η direction. When |Ω^(k)|>0,
(33)FL(k)=[1sinΩ^(k)Ω^(k)τ0-1-cosΩ^(k)τΩ^(k)00fΩ,1(k)0cosΩ^(k)τ0-sinΩ^(k)τ00fΩ,2(k)01-cosΩ^(k)τΩ^(k)1sinΩ^(k)τΩ^(k)00fΩ,3(k)0sinΩ^(k)τ0cosΩ^(k)τ00fΩ,4(k)00001τ000000100000001]
where the partial derivatives fΩ,1(k),…,fΩ,4(k) are found as:
(34)fΩ,1(k)=(cosΩ^(k)τ)τξ˙^(k)Ω^(k)-(sinΩ^(k)τ)ξ˙^(k)Ω^(k)2-(sinΩ^(k)τ)τη˙^(k)Ω^(k)-(-1+cosΩ^(k)τ)η˙^(k)Ω^(k)2,fΩ,2(k)=-(sin⁡Ω^(k)τ)τξ˙^(k)-(cos⁡Ω^(k)τ)τη˙^(k),fΩ,3(k)=(sinΩ^(k)τ)τξ˙^(k)Ω^(k)-(1-cosΩ^(k)τ)ξ˙^(k)Ω^(k)2+(cosΩ^(k)τ)τη˙^(k)Ω^(k)-(sinΩ^(k)τ)η˙^(k)Ω^(k)2,fΩ,4(k)=(cosΩ^(k)τ)τξ˙^(k)-(sinΩ^(k)τ)τη˙^(k).In both cases, the process noise covariance is determined in the following state equations:(35)P(k+1∣k)=FL(k)P(k∣k)FL(k)′+ΓEKFQ(k)ΓEKF′,
where
(36)ΓEKF=[12τ2000τ000012τ2000τ000012τ2000τ0000τ].From our assumptions of AUV motion, the value ofQ(k) was selected as:
(37)Q(k)=[rs0000rs0000rs0000rd],
where rs=(1.25m/s2)2 and rd=(0.3π/180rad)2.The IMM-CT is outlined in Figure5. It combines a set of filters (in this case a KF and EKF) and mixes the weighted previous state estimates to determine the current hypothesis of each of the filters. The linear KF is designed as described previously, whereas the nonlinear EKF has a different set of model selection parameters which define how it interprets large differences in the measurements. In particular, its covariance matrix describes how much variation is expected during a coordinated maneuver in terms of the angular velocity, represented as two directional speed components and a rate of angular change component.Figure 5
Outline of an interacting multiple-model filter.


### 5.4. Computational Complexity

For a Kalman filter, the computational complexity is approximately ofO(n3), where n=max(nx,nz), nx is the number of state variables, and nz is the number of measurements [24]. It follows that the PDAF, having a similar number of calculations, is also nearly approximated by O(n3). The IMM-CT, which is dominated by the computation of multiple filters in parallel, is approximately O(pn3), where p is the number of filters used in the IMM which are not being processed in a parallel fashion.


## 5.1. Kalman Filter

For simple motion models, characterized by long periods of motion free from maneuvers, a Kalman filter is the best linear state estimator for a Gaussian initial state assumption [24].In the KF, we choose to model the movement of the node as set of discrete white noise acceleration (“kinematic”) models [24], with a separate model for each possible direction, that is, x, y, and z. As such, the state equation for the Kalman filter at time index k+1 based on information from time step k becomes
(16)L(k+1)=F(k)L(k)+v(k),
with measurement
(17)z(k+1)=H(k+1)L(k+1)+w(k+1),
where
(18)F(k)=[1τ0000010000001τ0000010000001τ000001],(19)H(k+1)=[100000001000000010],v(k) is process noise, w(k) is measurement noise, and τ is the sampling interval of the discrete model in seconds.The state covariance is modeled as(20)P(k+1∣k)=F(k)P(k∣k)F(k)T+Q(k).
The corresponding process noise has a covariance given as
(21)Q=[14τ412τ3000012τ3τ200000014τ412τ3000012τ3τ200000014τ412τ3000012τ3τ2]σv2.Here,σv is a design parameter that is chosen to match the most likely level of process noise to be experienced by the object in question, which is to say it controls how much the model anticipates the object to maneuver. Given that the object in question is likely to be either stationary or altering its velocity at a slow, steady rate, a process noise level of σv=0.5m/s2 was selected to best emulate this behavior. The filter was initialized with two-point differencing [24].


## 5.2. Probabilistic Data Association Filter

During analysis of the performance of the KF, it became apparent that within a tracking window, there were point estimates which would appear as outliers by a considerable margin. On closer inspection, it was found that these were likely the result of one or more transmissions during which the direct-path signal propagation had been blocked, and a bounce was instead detected and treated as the direct path. Due to the assumption implicit to the KF approach that all of our messages are the direct-path propagation, this resulted in a drastic alteration of the point estimate, to the point where it could be classified as a false measurement. In that context, the probabilistic data association filter (PDAF) offers an improved performance over the standard KF, by allowing outlier estimates such as these to be ignored as false-alarm or clutter detections [25]. The PDAF is very similar to the KF in terms of state equations, presented here for measurement k+1:
(22)L(k+1)=F(k)L(k)+v(k),
with measurement
(23)z(k+1)=H(k+1)L(k+1)+w(k+1),
where F(k) and H(k+1) are given in (18) and (19), respectively.The state covariance is modeled similarly as(24)P(k+1∣k)=F(k)P(k∣k)F(k)T+Q(k).
The difference is on how to compute P(k∣k). First, let PD denote the probability of detection, which is a design parameter. Operating under the assumption that the measurement is always gated, define
(25)b=2(1-PDPD).
Define the following variables and matrices:
(26)ν(k)=z(k)-H(k)L(k),e(k)=e-(1/2)ν(k)TS(k)-1ν(k),S(k)=H(k)P(k∣k-1)H(k)T+R,W(k)=P(k∣k-1)H(k)TS(k)-1,
where R is the observation noise covariance. The probability of no correct measurement available is
(27)β0(k)=bb+e(k).
The probability of a correct measurement is
(28)β1(k)=e(k)b+e(k).
Further defining
(29)Pc(k∣k)=P(k∣k-1)-W(k)S(k)W(k)T,P~(k)=W(k)(β1(k)ν(k)ν(k)T-ν(k)ν(k)T)W(k)T,
the covariance matrix update is as follows:
(30)P(k∣k)=β0(k)P(k∣k-1)+β1(k)Pc(k∣k)+P~(k).We are assuming that the measurement is always gated, and that there is only a single target and a single measurement at each time step. Accordingly, thePD is then the probability that the current measurement is a valid estimate of the node being tracked. Based on experimental data, the number of “false detection” measurements was around 5% of the total samples, and so a value of 0.95 was selected for PD.


## 5.3. Interacting Multiple Model (IMM) Filter

For the more complex motion of an active underwater node, an interacting multiple model filter (IMM) was implemented, as the expected maneuvering index of underwater vehicles can easily exceed the threshold for which a single linear filter is likely to have any benefit. To this end, the IMM was a simple two-model filter, with a single, linear, low process noise(σv=0.05m/s2) KF to account for the straight motion travel, and an extended Kalman filter (EKF), configured in a coordinated-turn mode [26]. This validity of the coordinated turn assumption is dependent on the scenario in question, though given the previously described search patterns, it should be sufficiently accurate [27].The linear KF uses similar system equations as given previously, augmented with an additional column and row of zeros in order to accommodate the use of the EKF’s additional state in the IMM. The EKF in this problem uses one of two sets of state equations: the first set is an approximation used when the predicted coordinated turn rate is near 0 (Ω^(k)≈0), and the second set is used when the predicted coordinated turn rate is greater than some detection threshold (|Ω^(k)|>0) [24].The first set of EKF state equation modifications (Ω^(k)≈0) is as follows:
(31)L(k+1)=FL(k)L(k)+v(k),where(32)FL(k)=[1τ0000-12τ2η˙^(k)010000-τη˙^(k)001τ0012τ2ξ˙^(k)000100τξ˙^(k)00001τ000000100000001],
where η and ξ represent the x and y directions, respectively, and we denote η˙ as the velocity component in the η direction. When |Ω^(k)|>0,
(33)FL(k)=[1sinΩ^(k)Ω^(k)τ0-1-cosΩ^(k)τΩ^(k)00fΩ,1(k)0cosΩ^(k)τ0-sinΩ^(k)τ00fΩ,2(k)01-cosΩ^(k)τΩ^(k)1sinΩ^(k)τΩ^(k)00fΩ,3(k)0sinΩ^(k)τ0cosΩ^(k)τ00fΩ,4(k)00001τ000000100000001]
where the partial derivatives fΩ,1(k),…,fΩ,4(k) are found as:
(34)fΩ,1(k)=(cosΩ^(k)τ)τξ˙^(k)Ω^(k)-(sinΩ^(k)τ)ξ˙^(k)Ω^(k)2-(sinΩ^(k)τ)τη˙^(k)Ω^(k)-(-1+cosΩ^(k)τ)η˙^(k)Ω^(k)2,fΩ,2(k)=-(sin⁡Ω^(k)τ)τξ˙^(k)-(cos⁡Ω^(k)τ)τη˙^(k),fΩ,3(k)=(sinΩ^(k)τ)τξ˙^(k)Ω^(k)-(1-cosΩ^(k)τ)ξ˙^(k)Ω^(k)2+(cosΩ^(k)τ)τη˙^(k)Ω^(k)-(sinΩ^(k)τ)η˙^(k)Ω^(k)2,fΩ,4(k)=(cosΩ^(k)τ)τξ˙^(k)-(sinΩ^(k)τ)τη˙^(k).In both cases, the process noise covariance is determined in the following state equations:(35)P(k+1∣k)=FL(k)P(k∣k)FL(k)′+ΓEKFQ(k)ΓEKF′,
where
(36)ΓEKF=[12τ2000τ000012τ2000τ000012τ2000τ0000τ].From our assumptions of AUV motion, the value ofQ(k) was selected as:
(37)Q(k)=[rs0000rs0000rs0000rd],
where rs=(1.25m/s2)2 and rd=(0.3π/180rad)2.The IMM-CT is outlined in Figure5. It combines a set of filters (in this case a KF and EKF) and mixes the weighted previous state estimates to determine the current hypothesis of each of the filters. The linear KF is designed as described previously, whereas the nonlinear EKF has a different set of model selection parameters which define how it interprets large differences in the measurements. In particular, its covariance matrix describes how much variation is expected during a coordinated maneuver in terms of the angular velocity, represented as two directional speed components and a rate of angular change component.Figure 5
Outline of an interacting multiple-model filter.


## 5.4. Computational Complexity

For a Kalman filter, the computational complexity is approximately ofO(n3), where n=max(nx,nz), nx is the number of state variables, and nz is the number of measurements [24]. It follows that the PDAF, having a similar number of calculations, is also nearly approximated by O(n3). The IMM-CT, which is dominated by the computation of multiple filters in parallel, is approximately O(pn3), where p is the number of filters used in the IMM which are not being processed in a parallel fashion.


## 6. Simulation Results

We first carry out simulations using a simple noise model to generate the TOA measurements and evaluate the localization accuracy. For simplicity,z is assumed to be known, and we only solve for x and y coordinates. Four transmitters are placed on a square grid with coordinates (0,0), (100,0), (0,100), and (100,100). One receiver is placed at the (0,50) point and moves at a constant rate of 0.125m/s parallel to the x-axis.The TOAs are generated according to (3) where b is a fixed large bias, and wn is i.i.d. zero-mean white Gaussian noise with standard deviation of 7.5m. Position updates were taken every 16 seconds.The localization position error is shown in Figure6 as a function of total number of measurements acquired. We see that the Kalman filter clearly outperforms the point estimation based on exhaustive search (the LS solution has similar performance as the exhaustive search). We have also tested with a varied clock bias (not depicted), which had no effect upon the position error, as expected.Figure 6
Root-mean-squared (RMS) localization error as a function of the number of measurements acquired as the receiver moved in a straight line.In addition to the Kalman filter, simulations for the proposed IMM-CT were also run, using the relatively challenging scenario presented in Figure7, with the corresponding RMS position error given by Figure 8. The dashed lines depicted in the figure indicate the beginning or end of one of the maneuvers from Figure 7. As can be seen in Figure 8, the point estimates are drastically improved by all three of the trackers, with the KF and PDAF slightly outperforming the IMM-CT on the straight-path portion after exiting a maneuver. There was no noticeable difference in the performance of the KF and PDAF, which is to be expected, as the scenario did not feature any indirect path propagations.Figure 7
Simulation path for the IMM-CT, with distances in meters.Figure 8
Root-mean-squared (RMS) localization error as a function of the number of measurements acquired as the receiver moved in the scenario.


## 7. Pool Tests

We carried out tests in a standard competitive athletic swimming pool at The University of Connecticut, Storrs, whose dimensions are perfectly known. These tests did not use the GPS capabilities of the nodes, due to the limitation of the GPS receivers indoors. The nodes were fixed to the corner locations of the pool, such that their locations are measured accurately. The receiver was positioned approximately in the center of the pool, as outlined in Figure9. All the transducers are placed about 1 m below the surface. The pool has a depth about 2 m. Stationary test was conducted during March 2010, while the mobile test was conducted during December 2010.Figure 9
Node deployment during March 2010 pool test. The transmitters are denoted by squares, and the receiver is denoted by the diamond. The scattered plus signs are the estimates by the exhaustive search method.


### 7.1. Test Case 1 (Stationary Test, March 2010)

During the test, not all the messages from the transmitters were decoded correctly. For this reason, we use the data set with at least three measurements within one cycle of broadcasting from the four surface nodes. The favorable geometry and the known value ofz allow an estimate based on only three surface nodes. The location estimates by the exhaustive search method are shown in Figure 9, and those by the LS method are shown in Figure 10. We see that the LS estimates from these data sets are biased.Figure 10
Node deployment during March 2010 pool test. The transmitters are denoted by squares, and the receiver is denoted by the diamond. The scattered plus signs are the estimates by the least-squares method.Although advanced algorithms could be applied to fuse the data from multiple data sets, here we simply average the location estimates from multiple data sets. As more data sets are available, the localization accuracy improves, as shown in Figure11. A localization error of about 5 m is achieved with about 10 data sets.Figure 11
Localization error during March 2010 pool test.


### 7.2. Test Case 2 (Mobile Test, December 2010)

For the moving test in the pool, a simple straight-line maneuver was carried out. All of the previous conditions apply from the stationary pool test, including the use of only three nodes for localizing purposes. The scatter plot and approximate trajectory of the mobile node along with the four stationary transmitter locations are depicted in Figure12. There was a significant upgrade in the hardware and software used for the second test, which resulted in a large reduction in the overall error, as can be seen in Figure 13. The high initial error of the Kalman filter can be attributed to the very low process noise of the filter, combined with a very bad initialization which lead the filter to believe the node was traveling almost horizontally. Additionally, the measurements overall had a very low level of error, and the tracking system therefore could offer little improvement.Figure 12
Localization error in the swimming pool.Figure 13
Localization error in the swimming pool.


## 7.1. Test Case 1 (Stationary Test, March 2010)

During the test, not all the messages from the transmitters were decoded correctly. For this reason, we use the data set with at least three measurements within one cycle of broadcasting from the four surface nodes. The favorable geometry and the known value ofz allow an estimate based on only three surface nodes. The location estimates by the exhaustive search method are shown in Figure 9, and those by the LS method are shown in Figure 10. We see that the LS estimates from these data sets are biased.Figure 10
Node deployment during March 2010 pool test. The transmitters are denoted by squares, and the receiver is denoted by the diamond. The scattered plus signs are the estimates by the least-squares method.Although advanced algorithms could be applied to fuse the data from multiple data sets, here we simply average the location estimates from multiple data sets. As more data sets are available, the localization accuracy improves, as shown in Figure11. A localization error of about 5 m is achieved with about 10 data sets.Figure 11
Localization error during March 2010 pool test.


## 7.2. Test Case 2 (Mobile Test, December 2010)

For the moving test in the pool, a simple straight-line maneuver was carried out. All of the previous conditions apply from the stationary pool test, including the use of only three nodes for localizing purposes. The scatter plot and approximate trajectory of the mobile node along with the four stationary transmitter locations are depicted in Figure12. There was a significant upgrade in the hardware and software used for the second test, which resulted in a large reduction in the overall error, as can be seen in Figure 13. The high initial error of the Kalman filter can be attributed to the very low process noise of the filter, combined with a very bad initialization which lead the filter to believe the node was traveling almost horizontally. Additionally, the measurements overall had a very low level of error, and the tracking system therefore could offer little improvement.Figure 12
Localization error in the swimming pool.Figure 13
Localization error in the swimming pool.


## 8. Field Test in a Local Lake

The lake testing was performed in Mansfield Hollow lake, located in Mansfield, CT during August 2011. The average depth of the area in which the testing occurred is approximately 2.5 m, with minor variations of approximately 0.5 m. During the test, wind speed was minimal, typically less than 5 mph. The nodes were positioned in a roughly square formation, with an average separation of 110 m, as depicted in Figure14. The receiver was attached to a boat which would freely float inside of this node formation for the duration of the testing.Figure 14
Approximate node deployment of August 2011 test via Google Maps.For this test, the ground truth was determined via an onboard GPS device which would record the position of the receiver whenever a message was received. The data for a single run is presented, during which the boat moved at approximately 1 knot while moving along a slightly curved trajectory (approximately10∘). Note, however, that at a certain point during the test, the ground-truth GPS stopped updating its position, while the boat continued to move. In order to correct for this, the remaining ground truth was extrapolated from the initial GPS measurements. This introduces a nonnegligible amount of uncertainty, but still enables conclusions to be drawn regarding the behavior of the tracking algorithms. The approximate trajectory and measurements of the boat along with relative node positions are depicted in Figure 15.Figure 15
Plot of measurements and ground truth of lake test.As can be seen in Figure16, the tracking algorithms smooth the error out over the course of the maneuver and eventually reduce the overall error by a slight amount. Over the whole period, approximated error never exceeded a combined 5 m. While the raw measurements do exceed the trackers at one particular stretch, this is a combination of a rather small process noise coupled with a set of increasingly accurate measurements and is considered an acceptable trade-off due to the nature of node motion.Figure 16
Localization error during second lake test.On inspection, the PDAF does not perform much better than the Kalman filter in most cases. There was one measurement where it clearly offered an improvement, but overall they did not differentiate much in performance. In almost all cases, the IMM-CT was superior to both filters.


## 9. Conclusion

In this paper, we presented an underwater localization solution based on one-way message broadcasting from multiple surface nodes. In addition to simulation results, we provided testing results in a swimming pool and in a local lake. Future work would involve large-scale field tests featuring multiple nodes, as well as sea deployments over larger distances. Those will be pursued when there are suitable opportunities.



---

*Source: 683919-2012-07-02.xml*