A Machine Learning Approach for Dead-Reckoning Navigation at Sea Using a Single Accelerometer

Transcription

1 A Machine Learning Approach for Dead-Reckoning Navigation at Sea Using a Single Accelerometer 1 Roee Diamant, Yunye Jin Electrical and Computer Engineering, The University of British Columbia Institute for Infocomm Research, Singapore roeed@ece.ubc.ca, yjin@i2r.a-star.edu.sg Abstract Dead-reckoning (DR) navigation is used when GPS reception is not available or its accuracy is not sufficient. At sea, DR requires the use of inertial sensors, usually a gyrocompass and an accelerometer, to estimate the orientation and distance traveled by the tracked object with respect to a reference coordinate system. In this paper, we consider the problem of DR navigation for vessels located close to or on the sea surface, where motion is affected by ocean waves. In such cases, the vessel pitch angle is fast timevarying and its estimation via direct measurements of orientation is prone to drifts and noises of the gyroscope in use. Regarding this problem, we suggest a method to compensate on the vessel pitch angle using a single acceleration sensor. Using a constraint expectation maximization (EM) algorithm, our method classifies acceleration measurements into states of similar pitch angles. Then, for each class, we project acceleration measurements into the reference coordinate system along the vessel heading direction, and obtain distance estimations by integrating the projected measurements. Results in both simulated and actual sea environments demonstrate that by using only acceleration measurements, our method achieves accurate results. Index Terms Naval navigation, dead-reckoning, EM classification.

2 2 I. INTRODUCTION Navigation capability is crucial for any vessel at sea. While navigation at sea often involves a GPS receiver, in many cases its accuracy may not be sufficient (for example, navigating in harbor environment or collision avoidance in near shore exploration). Moreover, for vessels like autonomous underwater vehicles (AUV), GPS reception is not available when the vessel is submerged. In such scenarios, beacon-free navigation using dead-reckoning (DR) is a viable alternative [1]. Terrestrial DR navigation usually requires the use of inertial systems including an accelerometer and a three-dimensional (3-D) digital compass or gyroscope. The former is used to estimate the displacement of the tracked object, while the latter is used to measure its 3-D orientation. At sea, due to interference from metallic and electrical instruments on board vessels, instead of magnetic compass gyrocompass is preferred [2]. In DR navigation, a key challenge is to determine the orientation of the inertial sensor with respect to a reference coordinate system (e.g. the East-North-Up world coordinate system). Only then the distance traveled by the tracked object can be estimated. When the vessel is located close to or on the sea surface such that its motion is affected by the ocean waves, DR navigation poses an additional challenge as the vessel pitch angle is fast time-varying. For pedestrian DR, where the sensor is attached to the person foot or heap, this is usually dealt with by identifying reference points for which velocity is zero (for example, when the foot is placed on ground). In this way, timely orientation measurements from 3-D compass or gyroscope are calibrated [3]. However, at sea, since vessels constantly move, it is hard to identify such reference points. Furthermore, when orientation measurements of gyrocompass are directly used to compensate on the vessel pitch angle, accuracy is affected by the drifts and noises of the gyroscope in use [1]. For this reason, traditionally, DR navigation at sea involves integrating a large number of inertial sensors and applying Bayesian filtering methods to mitigate oscillatory wave-dependent components and reduce measurement drifts [4]. However, this approach limits the resolution of DR navigation, as measurements are filtered over the period of several ocean waves. Moreover, accurate gyrocompasses are expensive and may not be available. In this paper, we take a different approach and using a single 3-D acceleration sensor we estimate the heading and distance traveled by a vessel whose motion is affected by the ocean

3 3 Estimate heading angle Align measurements with vessel motion to compansate on heading angle Clasify measurements to M pitch states Per state: estimate pitch angle Per state: compansate on pitch angle Combine per state projected measurements Integrate measurements to estimate distance Fig. 1: Flow chart for the DR-A algorithm. waves. Our method, denoted as the DR-navigation-using-a-single-accelerometer (DR-A), is based on the observation that, due to ocean waves, the vessel pitch angle is periodic in nature. The intuition behind our method is that, when the vessel pitch angle is fixed, direction can be determined without using orientation measurements. Hence, by identifying classes of acceleration measurements such that in each class the pitch angle is approximately the same, we can readily project acceleration measurements into a reference coordinate system. While our method can be used also for AUVs submerged deep at sea, it is mostly designated to compensate on wave induced motions. The main contribution of our work is therefore in employing machine learning classification techniques for DR navigation of vessels located close to the sea surface, and obviating the need to compensate on the vessel time-varying pitch angle via direct measurement of the vessel orientation using dedicated hardware, e.g., gyrocompass. We offer a sequential approach as follows. First, we estimate the vessel heading angle and project acceleration measurements such that these are aligned with the vessel motion. Next, we classify the projected measurements into pitch-states of similar pitch angles. This is performed using a constraint expectation maximization (EM) algorithm. Third, per pitch-state, we project acceleration measurements (whose pitch angles are similar) to compensate on the vessel pitch angle. After this step, the vessel s projected local coordinate system is considered aligned with the the vessel heading direction and the East-North-Up coordinates system. Finally, we integrate the projected measurements to evaluate the distance traveled by the vessel. A flow chart of the above process is given in Figure 1. The remainder of this paper is organized as follows. Related work on DR navigation is

4 4 described in Section II. Our system model and assumptions are introduced in Section III. In Section IV, we present the steps of our DR-A method. Section V includes performance evaluation of our algorithm in simulations (Section V-A) and using realistic data obtained during a designated sea trial (Section V-B). Finally, conclusions are offered in Section VI. The key notations used in this paper are summarized in Table I. TABLE I: List of key notations Notation Explanation d i,j distance traveled by the vessel at time period [t i, t j ] N T c n α n ρ n number of acceleration measurements coherence time of acceleration along the horizontal plane the vessel orientation with respect to the reference system in time t n the vessel heading angle with respect to n the vessel pitch angle in time t n [a n,x, a n,y, a n,z] the vessel 3-D acceleration at time t n in the horizontal plane coordinate system â n   h  h,p M l L δ l Θ p acceleration measurement with elements [â n,x, â n,y, â n,z ] obtain at time t n vector of acceleration measurements obtained between t i and t j Matrix  projected to match the vessel heading Matrix  projected into the horizontal plane along the vessel heading pre-defined number of pitch-states group of acceleration measurements which must be classified into the same pitch-state number of groups l classifier for group l the distribution parameters of Â, estimated at the pth EM iteration µ m,x mean of the mth class for the x axis σ m,x k m standard deviation of the mth class for the x axis prior probability of the mth class II. RELATED WORK DR navigation finds its applications both for pedestrian [2] or vessel navigation [1]. DR navigation requires inertial sensors to estimate the distance traveled by the tracked object through integration of acceleration measurements while evaluating the sensor orientation. The main challenge in DR navigation is the possible drift and measurement noise of inertial sensors, which may lead to errors in the order of 10% of the traveled distance, depending on the technology

5 5 employed [5]. In [6], it was suggested to reduce sensor drift by posing constraints on the estimated distance based on an evaluated route of the tracked object. In [7], the periodic motions when walking or running are used as reference in mitigating acceleration measurement errors. For pedestrian applications, using the fact that velocity and orientation can be set to zero when the foot is on ground, it is customary to mount inertial sensors on foot or heap and estimate distance and orientation separately for each step, e.g., [8], [3], [9]. However, while at sea we can identify time instances where the vessel pitch angle is zero (i.e., at the top or bottom of the ocean wave, see Figure 2), velocity of the vessel cannot be assumed zero at these points. Instead, in areas of shallow water, Doppler velocity loggers (DVL) are used to serve as reference for DR navigation [10], and measurement drifts are controlled through fusion with range estimations to anchor nodes [11]. At areas of deep sea, DR navigation is performed by integrating measurements from large number of inertial sensors [4]. Since motion of vessels located close to the sea surface is affected by wind, currents, and wave forces, ship s DR navigation involves dynamic positioning, heading autopilots, and thruster-assisted position mooring [1]. On the sea surface, navigation also includes signals quality checking through comparing results with, e.g., GPS receivers and surface positioning references [12]. Angular velocity measurements from gyrocompass can be used to compensate over distortions of acceleration data caused by the ocean waves [2]. To compensate on noise, measurements from inertial sensors are filtered using, e.g., extended Kalman filter or particle filters (cf. [5], [13]) over a period of several ocean waves [1]. However, often the vessel pitch angle is fast timevarying and the navigation system avoids correcting motion for every single wave [4], affecting resolution. A different approach to compensate on the vessel s time-varying pitch angle is to project acceleration measurements using the principal component analysis (PCA) method [14]. PCA gives the axis coordinate system (or the transformation matrix to that system) along whose axes the variation of change is smallest [15]. If the vessel acceleration is constant in a certain plane, PCA projection aligns acceleration measurements with that plane. However, this method relies on large variance of acceleration measurements in the y and z axes, otherwise large errors occur.

6 6 ẑ ẑ z ẑ ˆx ˆx ẑ ˆx ẑ ˆx ρ n ˆx x y α n ŷ Fig. 2: Illustration of the vessel s wave-induced motion. III. SYSTEM MODEL Our system comprises of a three-axis accelerometer device attached to a vessel, and producing N i.i.d acceleration measurements during a time period of interest, [t start, t end ]. We assume that at time t start, the vessel s initial speed and heading with respect to a reference coordinate system (e.g., UTM) is given as v i, and init, respectively. Our objective is to track the vessel heading angle between times t start and t end, and to estimate the distance traveled by the vessel during the considered time period 1, di,j. Let a n,x, n, and α n, n = 1,..., N be the vessel s acceleration along the vessel heading direction, the vessel heading direction with respect to the reference coordinate system, and the vessel orientation (i.e., the direction in which the vessel bow is pointing) with respect to its heading direction n, respectively. We assume that the change in α n is only due to timely changes in n. That is, during period [t start, t end ] the vessel orientation with respect to the reference system does not change. In addition, we assume both a n,x and n are slowly timevarying such that the considered time period can be divided into W = tend t start T c duration T c (representing the coherence time of the system), in each N slot = NTc t end t start time slots of acceleration measurements are acquired, where in each time slot w, n and a n,x are assumed fixed and equal w and a w,x, respectively. For simplicity, we assume the vessel motion is perfectly correlated with ocean waves such that the vessel roll angle can be neglected. However, extension is suggested also for the case of time-varying pitch and roll angles (see Section IV-E). 1 Throughout the paper upper case bold symbols represent matrices, bold symbols represent vectors, ˆθ represents the measurement of true parameter θ, and θ represents the estimation of θ.

7 7 In the wth time slot, on time t n, n = 1,..., N slot, t start t n t end, the accelerometer device produces three-axis acceleration measurement vector, â n = [â n,x, â n,y, â n,z ], at local coordinates, which are grouped into a matrix Â. Vector â n is a projection of a vector a n = [a n,x, a n,y, a n,z ] T representing the vessel s acceleration in a 3-D coordinate system referred to as the horizontal plane. The coordinates of the horizontal plane are defined such that the x axis is aligned with the vessel s heading direction, assumably there is no movement in the y axis (since within the time slot the heading is assumed fixed), and the z axis is as in the East-North-Up coordinates system. Thus, the horizontal plane is separately defined for each time slot of duration T c and assumed fixed heading and acceleration. Referring to the illustration on Figure 2, the projection from a n into â n is modeled by rotating vector, [a n,x, a n,y, a n,z ] T about its y-axis by the vessel time-varying pitch angle, ρ n, then its z-axis by angle α n, and adding a zero-mean Gaussian noise, e n, with a per-axis standard deviation of ς 3 m/(sec)2, such that â n,x cos ρ n 0 sin ρ n cos α n sin α n 0 â n,y = sin α n cos α n 0 â n,z sin ρ n 0 cos ρ n a n,x a n,y a n,z + e n,x e n,y e n,z. (1) We note that while a n,y is assumed zero, a n,z can be non-zero. This is due to the up and down movements of the vessel with the ocean waves. However, we expect a small value for a n,z. This is required to solve an ambiguity in estimating the pitch angle 2. In fact, we are interested in cases where a n,x >> a n,z. Otherwise, the vessel is moving in almost fixed speed, v i, and projection is not required. However, it is important to note that by (1), even when a n,z is zero compensation over both α n and ρ n is required. For each time instance t n, we aim to estimate ρ n and α n, and to project the vessel s local coordinate system about the z-axis followed by its y-axis. The first projection converts the set of measurements  into a projected set Âh with elements â h n, and the second into a projected set  h,p with elements â h,p n. Since after the second projection the local coordinate system is expected to be aligned with the horizontal plane, we can readily integrate elements â h,p n estimate d i,j. Furthermore, since the vessel orientation with respect to the reference coordinate system is assumed fixed during [t start, t end ], such projection allows us to estimate the per-time slot change in the vessel s heading direction with respect to the initial heading, init. to 2 This assumption is validated using a realistic wave model as well as results from a sea trial.

8 8 By (1), the set of measurements  are affected by the pitch angle of the vessel. Much like the ocean waves, the vessel pitch angle is periodic in nature and likely, π < ρ 4 n < π. However, being 4 induced from ocean waves, we do not assume a fixed period for the vessel pitch angle. Given this periodicity, we can identify time instances for which the vessel pitch angle is approximately the same. This observation sets the stage for classifying measurements into pitch-states of assumed fixed pitch angle, given the following model. Let Â(w) with elements â n (w) = [â n,x (w), â n,z (w)] T, n = 1,..., N slot be the set of 2-D acceleration measurements acquired during the wth time slot, w = 1,..., W, and sorted by their measurement time. We consider a Gaussian mixture with M pitch-states for the distribution of Â(w) such that for Θ(w) = {ω 1,x (w), ω 1,z (w), k 1 (w),..., ω M,x (w), ω M,z (w), k M (w)}, P (Â(w) Θ(w)) = M k m (w)p m (â n,x (w) ω m,x (w))p m (â n,z (w) ω m,z (w)), (2) â n (w) Â(w) m=1 where ω m,x (w) = [ µ m,x (w), σ 2 m,x(w) ], and ω m,z (w) = [ µ m,z (w), σ 2 m,z(w) ], are the distribution m=1 parameters of the mth pitch-state with mean µ m,x (w) and µ m,z (w), and variance σm,x(w) 2 and σm,z(w), 2 respectively, and k m (w) ( M k m (w) = 1) is the a-priori probability of the mth pitchstate. Model (2) is used to classify acceleration measurements into pitch-states using a constraint EM algorithm. For clarity, in the following we drop the time slot subindex, w, and consider projection of measurements acquired during a single time slot. IV. THE DR-A METHOD Refereing to model (1), in the DR-A method we estimate angles α n and ρ n to project measurement in  into the horizontal plane and obtain an estimation for a n,x, n = 1,..., N slot. While the orientation angle, α n, is assumed fixed during the time slot, it is not the case for the pitch angle, ρ n. For this reason, we separately compensate on the vessel heading and pitch angles. The former is done jointly for all measurements acquired during a single time slot, resulting in a projected set  h. For the latter, we use machine learning approach to classify acceleration measurements into states of similar pitch angles, and per-state compensate on the pitch angle to form a projected set  h,p, considered aligned with the horizontal plane. Finally, we combine projected measurements of all time slots, and, using the initial (given) velocity v i, integrate them over the time period [t start, t end ] to obtain d i,j. In the following sections we describe in details the steps of our method.

9 9 A. Forming Âh : Estimation of the Heading Angle Before forming classes of similar pitch angles, we project â n Â, n = 1,..., N slot to form â h n = [â h n,x, â h n,y, â h n,z], for which â h n,z = â n,z and (if the vessel s pitch angle is zero) â h n,x aligns with the heading of the vessel. This projection is performed as follows. Refereing to Figure 2, α n denotes the angle between the heading direction of the vessel, n, and the accelerometer s local x-axis. Since per time slot the vessel is assumed moving in a fixed heading, without measurement noise, there should be no acceleration, caused by the vessel motion, along the axis which is perpendicular to the heading direction. Therefore, we expect â n,x sin α n â n,y cos α n 0, (3) and estimation α n = α which minimizes N â n,x sin α â n,y cos α is obtained by n=1 tan α = N â n,y n=1. (4) N â n,x Once α is determined through (4), we project measurements in  and form matrix Âh. This is performed by multiplying â n Â, n = 1,..., N slot with the rotation matrix cos α sin α 0 sin α cos α 0. (5) In the wth time slot, the estimated angle α w from (4) is compared with α w 1 to estimate the heading direction of the vessel, w, assumed fixed in each time slot. Let w be the change in the heading direction of the vessel between the w 1th time slot and the wth one. Then, w = w 1 w. Recall we assume the direction of the vessel s bow with respect to the reference coordinate system (e.g., UTM) does not change during the considered time period, [t start, t end ]. Thus, w = α w α w 1, and w = w 1 w, (6) where 0 = init. n=1

10 10 B. Forming Âh,p : Estimating the Time-varying Pitch Angle After forming matrix Âh, we estimate the vessel pitch angle to form matrix Âh,p, which will be used to readily calculate d i,j. Since the vessel pitch angle is time-varying, different from estimation (4), elements of Âh cannot be directly projected to Âh,p. For this reason, traditionally, a direct measurement of the vessel orientation is used via, e.g., gyrocompass, which, as stated in Section I, may not be available or is too noisy. In this section we describe an alternative approach, where based on the observation that the vessel pitch angle is periodic in nature, we form matrix Âh,p by grouping acceleration measurements to classes of similar pitch angles. Based on model (2), to identify measurements of similar pitch angles we use a constraint EM algorithm and classify the elements in Âh to M pitch-states, where M is a pre-defined number of states. For an assumed coherence time, T c, classification is performed separately for each time slot. Since the effect of the pitch angle is similar in the x and z axes, classification is performed jointly for both axes. In classifying Âh, we can use side information in the form of positive constraints for measurements which must belong to the same pitch-state, and negative constraint for measurements which must be classified into different pitch-states, as described in the following. 1) Formulating Positive Constraints: Positive constraints allow us to mitigate measurement noise by relaxing the element-wise clustering problem, and instead clustering sets of measurements. Since the vessel pitch angle is continuous, we limit positive constraints to consecutive measurements, â h n, â h n+1,..., â h n+ε of similar values, where ε is limited. To formulate positive constraint, let V pos be a predefined multiplication of the sensitivity level of the used accelerometer device. Also let T p be a pitchcoherence time used as an upper bound for the time period positive constraints can be defined for two acceleration measurements. Denote relation a n a n if a n,x a n,x < V pos (7a) a n,z a n,z < V pos (7b) t n t n T p, (7c) otherwise a n a n. We classify measurements â h n and â h n into the same pitch-state if âh n â h n, and if there exist no element â h j, such that t n < t end < t n and â h n â h j or â h n âh j. Since the

11 11 vessel pitch angle is affected by ocean waves, we set T p = 1 2f wave, where f wave is the fundamental frequency of the ocean waves, which can be estimated using, e.g., cyclostationary analysis (cf. [16]). Positive constraints are used to form matrixes l, l = 1,..., L, each including a chain of consecutive measurements which pair-wise are classified into the same pitch-state. The above definition of positive constrains guarantees that l are disjoint matrixes. We obtain l by grouping consecutive acceleration measurements satisfying the above constraints in disjoint matrixes such that a new matrix is formed every time positive constraints are not met for two consecutive measurements. For each vector l we assign classifier δ l = {1,..., M} such that all elements in l are classified into the same δ l pitch-state. To this end, we reduce the problem of classifying â h n Âh into classifying l. 2) Formulating Negative Constraints: Negative constraints reflect the expected change in the vessel pitch angle. Since the last element of l and the first element of l+1 do not satisfy (7), negative constraints are formed as the complementary of positive constraints. More specifically, negative constraints ensure that two consecutive subsets l and l+1 cannot share the same pitch-state. Interestingly, since the rank of vectors l, l = 1,..., L is limited by T p, if T p < T c (recall T c is the duration of each time slot) then negative constraints renders L 2. We represent negative constraints by operator 1 δ l = δ l+1 η δl,δ l+1 =, l = 1,..., L. (8) 0 otherwise Shental [17] suggested formalizing negative constraints by a directed Markov graph, where in our case classifier δ l is a hidden (decision) node, connected to both δ l 1 and δ l+1, as well as to an observation node, â h n l. Since the pitch angle associated with l+1 depends on the pitch angle associated with l, hidden nodes form a directed chain from parent, δ l, to child, δ l+1, and from hidden nodes to observation nodes, as illustrated in Figure 3. This directed chain is a form of a Bayesian belief network [18], where for Y being the set of classifiers δ l, l = 1,..., L (with δ 0 = ), we obtain the joint distribution ( ) L ) P Y Âh, Θ = P (δ l δ l 1, Θ, Âh. (9) l=1 The relation in (9) would serve us in formalizing the likelihood function to determine δ l using a constraint EM algorithm, as described next.

12 12 δ l 1 δ l δ l+1 l 1 l l+1 Fig. 3: Graph representation for positive and negative constraints. 3) Formulating the Likelihood Function: Considering both positive and negative constraints, the data likelihood function for the event of correct classification of the elements of  h is P (Âh, Y Θ) = 1 L 1 L (1 η δl,δ c l+1 ) 1 where c 1 = δ 1 =1 L 1 δ L =1 l=1 l=1 l=1 â h n l (1 η δl,δ l+1 ) L P (δ l Θ). l=1 P (δ l Θ)P (â h n,x δ l, Θ)P (â h n,z δ l, Θ), (10) For classifying subsets l, we use the EM algorithm to iteratively maximize (10). In the pth iteration, we obtain estimation Θ p of Θ, with elements µ p m,x, µ p m,z, σ p m,x, σ p m,z, and k p m, m = 1,..., M. For the Gaussian mixture model (2), given Θ p we obtain [19] Furthermore, µ p+1 m,ω = σ p+1 m,ω = k p+1 p+1 1,..., k M s.t. m=1 L P (δ l = m Âh, Θ p ) l=1 â h n,ω l â h n,ω L P (δ l = m Âh, Θ p ) l=1 L P (δ l = m Âh, Θ p (âh ) n,ω µ p+1 m â h n,ω l l=1 k m = 1. L P (δ l = m Âh, Θ p ) l=1 ( ) 1 = argmax log + k 1,...,k M c 1 m=1 log(k m ), ω {x, z} (11a) ) 2, ω {x, z}. (11b) L P (δ l = m Âh x, Âh z, Θ p ) l=1 (12a) (12b)

13 13 Since c 1 is a function of k m (recall k m = P (δ l = m Θ p )), it is hard to maximize (12). Instead, we approximate c 1. Assuming negative constraints are mutually exclusive, c 1 (1 j=1 k 2 j ) L 1 [17]. Clearly, this approximation does not hold in our case. However, since our Markov graph is sparse, this assumption has little affect on performance (as our numerical simulations show). From (12) and using a lagrange multiplier, we get ( 2(L 1) k p+1 m + P (δ l = m Âh x, Âh z, Θ p ) k p+1 m The expression in (13) can be solved for (and k p j 1 k p+1 m ( k p+1 j=1 j ) 2 ) +(2L 1) j=1 either by approximating ( k p+1 j ) 2 1 = 0. (13) ( k p+1 j j=1 ) 2 as ( k p j )2 j=1 is known from previous iteration), or using alternating optimization technique (cf. [20]). Equations (11a), (11b), and (13), are used to determine Θ p+1, which in turn is used for the next EM iteration till convergence of (10) is reached 3. In the last EM iteration, p last, we determine classifiers δ l, l = 1,..., L of vectors l by numerically solving [ δ l = argmax P ( δ l = m Âh x, Âh z, Θ ] p last ) δ l, (14) and construct matrix Âh m, m = 1,..., M including elements â h n for which δ l = m and ρ n = ρ m. Considering model (1), to project the elements in matrix Âh m into Âh,p m, we guess a solution, ρ m, and multiply â h n Âh m, n = 1,..., N by the rotation matrix cos ρ m 0 sin ρ m 0 1 0, (15) sin ρ m 0 cos ρ m Observing model (1), we note that when the noise vector e n is zero and heading compensation is ideal, multiplying â h n Âh m by the matrix in (15) gives the vector an,x( ρ h,p m ) a n,x cos ( ρ m ρ m ) + a n,z sin (ρ m ρ m ) a h,p n,y( ρ m ) = a n,y. (16) a h,p n,z( ρ m ) a n,x sin ( ρ m ρ m ) + a n,z cos ( ρ m ρ m ) 3 Note that the EM algorithm is proven to converge to a local maxima of the log-likelihood function [19].

14 14 Since a n,z can be non-zero, ρ m cannot be estimated following the same approach as in (4). Unfortunately, (16) sets a degree of freedom in choosing ρ m. Instead, since we assumed a n,x >> a n,z (see Section III), the term a n,x (cos ( ρ m ρ m ) + sin ( ρ m ρ m )) is approximated by a h,p n,x( ρ m ) + a h,p n,z( ρ m ). Since a h,p n,x( ρ m ) + a h,p n,z( ρ m ) achieves its maximum for ρ m ρ m = π 4 when a n,x > 0, and for ρ m ρ m = π 4 + π when a n,x < 0, we set ρ m,1 = argmax ρ m ρ m,2 = argmax ρ m n: â h n Âh m n: â h n Âh m a h,p n,x( ρ m ) + a h,p n,z( ρ m ) π 4 a h,p n,x( ρ m ) + a h,p n,z( ρ m ) π 4 π. (17) From the two solutions in (17), we choose the one satisfying condition π < ρ 4 m < π, and using 4 (16) form projection a h,p n,x, considered aligned with the horizontal plane along the vessel heading direction. Finally, we combine projected measurements from all time slots and pitch-states in matrix Âh,p. Due to the dependency between hidden nodes δ l, the difficulty in (11), (13), and (14) lies in calculating P (δ l = m Âh x, Âh z, Θ p ). We next describe an efficient way to obtain the posterior probabilities. 4) Finding the Posterior Probability: Finding the posterior probability is simple when only positive constraints are imposed. Here, P (δ l = m Âh, Θ p ) = P (δ l = m l, Θ p ), and since measurements in Âh are assumed independent (see (2)), P (δ l = m l, Θ p ) = k p mp ( l ω p m) P ( l Θ p ) k p m P (â h n,x ω p m)p (â h n,z ω p m) â = h n l M k p j=1 j P (â h n,x ω p j )P (âh n,z ω p j ). (18) â h n l However, at the presence of negative constraints, δ l depends on other hidden nodes, and where P (Y Âh, Θ p ) = c 2 = δ 1 =1 (1 η δl,δ l+1 ) L L 1 l=1 L 1 δ L =1 l=1 (1 η δl,δ l+1 ) l=1 â h n l L l=1 â h n l P (δ l â h n,x, Θ p )P (δ l â h n,z, Θ p ) c 2, (19) P (δ l â h n,x, Θ p )P (δ l â h n,z, Θ p ).

15 15 Here, the posterior probability can be obtained by marginalizing the joint probability (19). However, in general, this is an NP-hard problem (with similarities to the graph coloring problem) ( ) with complexity O M L 1 [18]. Instead, since the Markov graph illustrated in Figure 3 is a directed chain, belief propagation techniques can be used to significantly reduce complexity to O (LM 2 ) [18]. The general idea in belief propagation is based on observation (9), that assuming P (δ l δ l 1 ) is known, belief BEL δl (m) = P (δ l Âh, Θ p ) can be exactly calculated by receiving belief BEL δl 1 (m) from parent δ l 1 and likelihood P (Âh δ l+1, Θ p ) from child δ l+1. Following [17], we use Pearl s belief propagation algorithm for trees [18], which adaptation to our system is as follows. In Pearl s algorithm, each hidden node l exchanges messages λ δl (m) and π δl (m) with his parents and children, respectively. We start by initializing lists π δl (m) = λ δl (m) = 1, l, m, l = 1,..., L, m = 1,..., M. Upon activation, each hidden node, δ l, receives π δl 1 (m) and λ δl+1 (m) from its parent and child, respectively. It then calculates λ δl (m) = π δl (m) = n=1 n=1 λ δl+1 (n) â h l P π δl 1 (n) â h l P where π δ0 (m) = 1 (recall δ 0 = ), and sets Next, hidden node δ l sends message (δ l = n δ l 1 = m, â hn,x, â hn,z, Θ p ) ( δ l = m δ l 1 = n, â h n,x, â h n,z, Θ ) p ) BEL δl (m) = λ δ l+1 (m)π δl (m) j=1 π δl (m) π δl (j) j=1 BEL δl (j) to its child, δ l+1, and (20a), (20b). (21) λ δ l (m) λ δl (j) j=1 to its parent, δ l 1. Since our network is a directed Markov chain, we execute (20) and (21) for the sequence δ 1 δ 2 δ L, and reach convergence after L steps. Since the above procedure is performed for each pth EM iteration and wth time slot, the complexity of the DR-A algorithm is O (LM 2 p last W ), where W = tend t start T c and L N W. 5) Initial Estimation Θ 0 : The EM algorithm in (11) and (12) requires initialization Θ 0. With no prior knowledge of the effect of ocean waves on acceleration, we use the K-means clustering algorithm (cf. [19]) to

16 16 initially classify elements in each time slot into M pitch-states and form matrixes Âh m. Similar to the EM algorithm, the K-means algorithm is executed jointly for Âh x and Âh z. After this initial classification, based on the statistical mean and standard deviation of elements â h n,x, â h n,z Âh m we calculate µ 0 m,x, µ 0 m,z, and σ m,x, 0 σ m,z, 0 m = 1,..., M, respectively, and k m 0 is estimated as the ratio between rank Âh m and the number of elements in each time slot. C. Distance Estimation After projecting vector  into Âh,p with elements â h,p n,x, n = 1,..., N, considered aligned with the vessel heading direction (see Section IV-A) and horizontal plane (see Section IV-B), we can readily estimate the distance traveled by the vessel at time period [t start, t end ], d i,j. Given the initial velocity v i at time instance t start, we obtain estimation d i,j by integrating the projected acceleration measurements over the period [t start, t end ]. First, we obtain the mean velocity by, Then, we set d i,j = ˆv i,j (t end t start ). ˆv i,j = v i N n=2 â h,p n,x(t n t n 1 ). (22) D. Summarizing the Operation of the DR-A Method We now summarize the operation of our DR-A method. Referring to the pseudo-code in Algorithm 1, we first form vectors Âw, w = 1,..., W for W = tend t start T c time slots (line 1). For each wth time slot, we estimate α n and w, and project acceleration measurements in Âw into  h,w (line 3). To assist in classification of pitch-states, we set positive and negative constraints to form subsets l and Λ l, respectively (lines 4-11). Next, we perform initial classification (line 12) to calculate Θ 0 (line 13), and perform p last EM iterations to classify l (lines 14-18). For each wth time slot and mth pitch-state, we project elements in matrix Âh,w m to compensate on the vessel pitch angle (line 19), and group the projected elements to form vector  h,p (line 21). Finally, we evaluate distance d i,j (line 22). E. Discussion In this paper, we specifically assumed the vessel roll angle is fixed. However, due to ocean waves, much like the vessel pitch angle, the former can also be time-varying. While the roll

17 17 Algorithm 1 Evaluate d i,j from vector  1: Divide  into W time slots Âw 2: for w := 1 to W do {do for each time slot} 3: Estimate α n using (4) and w using (6), and project  w into Âh,w using (5) 4: l := 1, l â h,w 1, Λ l := 5: for n := 1 to Âh,w 1 do 6: if â h,w n,x Âh,w and â h,w n+1,x Âh,w satisfy positive constraints then 7: l â h,w n+1 8: else 9: l := l + 1, l â h,w n+1, Λ l 1 (n, n + 1) 10: end if 11: end for 12: classify Âh,w using the K-means algorithm to initially form matrixes (Âh,w Âh,w m 2], 13: µ 0 m,x := E[Âh,w m,x], µ 0 m,z := E[Âh,w m,z], σ m,x 0 = E[ m,x µ m,x) 0 ) 2], σ m,z 0 = E[(Âh,w m,z µ 0 m,z k0 m = Âh,w m {initial estimation} N slot 14: for p := 0 to p last 1 do {EM iterations} 15: Iteratively calculate (21) to obtain P (δ l = m Âh,w, Θ p ) 16: Solve (11) and (13) to obtain 17: end for Θ p+1 18: Classify subsets l using (14) and form Âh,w m, m = 1,..., M 19: Project elements in matrixes Âh,w m using (17) 20: end for 21: group all projected measurements to form vector  h,p 22: Evaluate d i,j using (22) angle is expected to change much slower than the pitch angle, the former may still change within a single time slot. As a result, our assumption that (per time slot) changes in acceleration are only due to the time-varying pitch angle, does not hold and classification to pitch-states cannot be made. A possible solution to this problem would be to identify the frequency of change of the vessel roll angle, and to define shorter time slots for which both acceleration in

18 18 the horizontal plane and vessel roll angle can be considered fixed. Classification of acceleration measurements is then performed for each of these (shorter) time slots. Since changes in the vessel roll angle also affects acceleration in the projected y-axis (which otherwise would be close to zero), the rate of change in the vessel roll angle can be estimated by observing periodic changes in the y component of Â. Clearly, this approach introduces more noise in estimating d i,j since classification is based on fewer measurements. V. PERFORMANCE EVALUATION We now evaluate the performance of our DR-A algorithm. The results are presented in terms of ρ err = d i,j d i,j and ρ angle = α n α n. Since the DR-A method is based on the Gaussian mixture model (2) as well as on the assumption that per-time slot acceleration in the horizontal plane, vessel heading direction, and vessel roll angle are fixed, to validate simulation results we also present results from a designated sea trial where we tested our algorithm. A. Simulation Results Our simulation setting includes a Monte-Carlo set of channel realizations. In each simulation, a vessel is moving for 60 sec in the x y plane with initial speed uniformly distributed between [0, 5] m/sec. The vector of acceleration in the horizontal plane, a n,x, n = 1,..., N, is sampled at rate 0.1 sec (i.e., N = 600), and generated as zero-mean colored Gaussian process with standard deviation of 1 m/(sec) 2 and a cross-correlation factor between adjacent samples of e 0.1 Tc, where T c = 6 sec. Likewise, the vessel heading, n is generated uniformly between [0, 2π] with a similar cross-correlation factor between adjacent samples. Furthermore, per-simulation, the (fixed) vessel orientation angle with respect to the reference coordinate system is uniformly randomized within the interval [0, 2π]. Based on the latter and n, we form vector α n, n = 1,..., N. Note that the initial speed and heading direction are known. Let h(x, y, t) be the time domain t function for the height of the sea surface for a modeled three-dimensional ocean wave in the x y plane. To simulate wave-based acceleration in the vertical plane, for t n being exactly in the middle of the time period before a local maxima and after a local minima of h(x, y, t), we uniformly randomize sample a n,z between [0.01, 0.05] g. Similarly, a n,z is generated uniformly between [ 0.05, 0.01] g for t n exactly in between a local maxima and a local minima. Acceleration then changes linearly with time such that it

19 19 6 Wave elevation [m] y [m] x [m] Fig. 4: Example of a modeled wave in the x y plane. t = 10 sec. reaches zero at both local maxima and minima of h(x, y, t). Since the derivative of h(x, y, t) is also the slope of the wave surface, the pitch angle ρ n at time sample t n and coordinates (x n, y n ) is computed from tan ρ n = the vector of acceleration measurements, Â. t n w(x, y, t n ). Using α n and ρ n and based on model (1), we form Current literature offers multiple models for the wind-based ocean surface wave function h(x, y, t) (e.g., [21], [22], [23],). In our simulations we use the analytical wave model offered in [23]. We note that similar results were obtained also for other wave models. For the ith wave frequency and jth directional angle, let θ i, φ i, and ψ i,j be the spreading directional angle, wave frequency, and an initial phase angle, respectively. The wave height is modeled as I J h(x, y, t) = 2S(φ i ) φ i θ j cos(c i x cos θ j + c i y sin θ j φ i t ψ i,j ), (23) i=1 j=1 where c i = φ2 i is the wave number, φ and θ are the increments of angles φ and θ, respectively, g ( ) ] 4 and we use the directional spectrum function S(φ) = b 1g 2 exp [ b 2 with b 1 = , b 2 = 0.74, and a wind speed U = 5 knots. For each channel realization, parameters φ i, θ i, and ψ i,j are uniformly randomized in intervals [1, 5] rad/sec, [0, 2π] rad, and [0, 2π] rad, respectively, and we use I = 10, J = 10, φ = 0.4 rad/sec, and θ = π 5 h(x, y, t) for t = 10 sec is shown in Figure 4. φ 5 g Uφ rad. An example for Since current methods for DR navigation use both acceleration and orientation measurements, we benchmark our algorithm by showing results of ρ err when an ideal gyroscope is used, i.e.,

20 20 Empirical Pr(ρ angle x) /ς=50dB 1/ς=30dB 1/ς=20dB 1/ς=10dB x [rad] Fig. 5: C-CDF results of ρ angle. perfect compensation of the vessel pitch and heading angles (Ideal-Gyro), and when orientation measurements are noisy (Noisy-Gyro). In addition, we compare results of DR-A with a direct integration of acceleration measurements with no heading and pitch compensation (Naive), and with an alternative method in which after heading estimation, using the PCA method (see Section II), per time slot we obtain measurements aligned with the horizontal plane (PCA). We also demonstrate tradeoffs between complexity and performance by replacing the constraint EM algorithm with i) the initial K-means classifier (K-means), ii) a simple slicing classifier (Slice), and iii) non-constraint EM (NC-EM). For the Ideal-Gyro and Noisy-Gyro methods, we estimate a n,x by multiplying â n with matrices (5) and (15) for the measured heading and pitch angles, ˆα i,j and ˆρ n, respectively. For the former we use ˆρ n = ρ n and ˆα n = α n, while for the latter we set ˆρ n = ρ n + ē n and ˆα n = α n + ē n, where ē n is a zero-mean Gaussian noise with variance ς rad 2, which is the same variance considered for the acceleration measurement noise. In the following, results for the DR-A method are shown for p last = 10 EM iterations and using V pos = 0.05 m/sec 2 for the positive constraints in (7). In Figure 5, we show the complementary cumulative density function (C-CDF) of ρ angle using estimation (4) for different values of 1. We observe that reasonable performance are obtained ς for relatively low noise values (around 20 db). Next, we show tradeoffs of performance vs. the number of pitch-states, M, and the assumed coherence time, Tc. In Figure 6a we show performance of the four considered classifying method in terms of ρ err for M {2, 10, 20} and

21 DR A NC EM Kmeans Slice DR A NC EM Kmeans Slice ρ err [m] 10 ρ err [m] M=2 M=5 M=10 M=20 0 T_c=3sec T_c=6sec T_c=12sec (a) (b) Fig. 6: Average results of ρ err as a function of (a) M (for Tc = T c ), (b) T c (for M = 10). T c = 6 sec. 1 ς = 20 db. We observe that for the Slice method performance are not linear with M. This is due to the fact that in the Slice method measurements are classified to all assumed M pitchstates, and thus, for each pitch-state, fewer measurements are available affecting accuracy of estimation (17). While accuracy increases with M for the DR-A, NC-EM, and Kmeans (which allow empty pitch-states), we observe that little is gained for M > 10. Next, for M = 10, in Figure 6b we show ρ err as a function of the assumed coherence time, Tc, which is used to obtain W time slices of assumed fixed acceleration in the horizontal plane. Here we observe a slight performance degradation for mismatch coherence time (i.e., when T c T c = 6 sec). When T c < T c, this is because fewer measurements are available to estimate the pitch angle in (17), while for T c > T c, performance degrade since our assumption of fixed acceleration in the horizontal plane (required for classification) does not hold. Interestingly, we observe that performance are less affected in the latter case. That is, having enough statistics to estimate the pitch angle is more important, as was also observed for the Slice method in Figure 6a. In the following we use M = 10 and T c = T c. In Figure 7, we show average results of ρ err for the considered classification methods and the benchmark methods as a function of 1. We observe a significant performance degradation for the ς

22 22 Noisy-Gyro method compared to the Ideal-Gyro one. We consider this performance gap as the maximal gain available for using our method. We also observe that for the Noisy-Gyro method, performance is not linear with 1 ς (in the logarithmic scale). This is because of the noisy orientation measurements which add non-gaussian noise to the projected acceleration measurements. As expected, performance for the Naive approach is poor, and is in fact fixed for different noise values. The latter is due to the periodic nature of the vessel pitch angle, which averages out positive and negative acceleration measurements in the x axis. Comparing the performance of the PCA method to those of our method, we observe that PCA is better than the Slice method, mostly due to the naive classification performed in the Slice method which is largely affected by measurement noise. However, using the better classification capabilities of the EM and K- means algorithm, we observe considerable performance gain compared to the PCA method. This is due to the underline assumption in PCA of the variance of acceleration measurements (see Section II), which might not hold for all modeled ocean waves. As expected, performance of the EM algorithm, which is matched to the Gaussian mixture model in (2), outperforms that of the K-means algorithm. Moreover, significant improvement is achieved using our DR-A method compared to that of the non-constraint EM algorithm, NC-EM. From Figure 7, we observe that the performance of our DR-A method is close to that of the (unrealistic) Ideal-Gyro method. Thus, we conclude that, without using orientation measurements, the DR-A method almost entirely compensates on the vessel s heading angle and time-varying pitch angle. To comment on the distribution of the performance, in Figure 8 we show the C-CDF of ρ err for 1 ς = 20 db. Results show that the above conclusions, drawn for the average ρ err results, hold true for all simulated scenarios. In the following we present results of offline processing of 3-D acceleration measurements obtained in a sea trial. B. Sea-Trial Results In this section, we describe results of our DR-A method obtained from real sea environment. The sea trial was conducted on Nov in the Singapore straight. The experiment lasted for two hours and included two boats. During the experiment, the waves height was about 0.5 m and the boats drifted with the ocean current at about 0.7 m/sec. At each boat, we obtained around N = 33.5 k 3-D acceleration measurements using the Libelium Wasp Mote s on board

23 ρ err [m] DR A NC EM Kmeans Slice PCA Ideal Gyro Noisy Gyro Naive /ς [db] Fig. 7: Results of ρ err as a function of 1 ς. M = 10, Tc = T c. Empirical Pr(ρ err x) DR A NC EM Kmeans Slice PCA Ideal Gyro Noisy Gyro x [m] Fig. 8: C-CDF results of ρ err. 1 ς = 20 db, M = 10, Tc = T c. accelerometer at rate of f = 4.8 Hz, and used its serial port for data logging purpose and offline decoding. Throughout the experiment, the location of the boats was monitored using GPS receivers at rate of 3 sec and with expected accuracy of 5 m. The boats GPS-based location is shown in Figure 9. From the figure we observe that, as a result of the ocean current, the boats changed their heading direction. However, other than around time t = 25 min (where boat 2 had to maneuver around an obstacle), this heading change is slow and fits our underline assumption. Furthermore, from Figure 9 we observe a slow change in the boats speed (with variance of 0.1 m/sec 2 ), which allows using time slices of assumed fixed acceleration in the horizontal

24 time [min] Boat 1 Boat y [m] x [m] Fig. 9: Sea trial: nodes location in Cartesian coordinates. plane. In Figure 10, we show the measured acceleration along the x axis as a function of earth gravity, g (the results for the projected accelerations are discussed further below). We observe that acceleration measurements follow a wave pattern, and that both frequency and amplitude of these waves are different for the two boats. The periodic nature of the measurements shown in Figure 10 emphasize the need for compensating on the vessel pitch angle, as direct orientation measurements may be too noisy. Moreover, due to the observed fast time-varying measurements (caused by ripples), mitigating the effect of the vessel s pitch angle using filtering is not possible without loss of resolution. First, in Figure 11 we show C-CDF performance of the heading estimate for the two boats, where the vessel heading direction with respect to the UTM coordinate system is calculated based on the boats GPS-based location. Results are compared for consecutive time slots of duration T slot such that N estimations of ρ T slot f angle are obtained. Since accuracy of estimation (4) improves with the number of measurements but decreases if the heading angle changes within the time slot, results tradeoff for T slot. We observe that best results are obtained for T slot = 200 sec. From the figure, we observe a good fit of less than 1 degree between the GPS-based heading angle and the estimated one. However, we note that when the vessels heading angle changes within the time slot, estimation accuracy is poor. In Figure 10, for the first 50 sec of the experiment we show the measured and projected acceleration along the x axis, where the latter are obtained using M = 5 and T c = 40 sec. We

25 Acceleration x axis (g) Boat 1 (projected) Boat 1 (measured) 0.04 Boat 2 (projected) Boat 2 (measured) Time (sec) Fig. 10: Sea trial: measured and projected acceleration along the x axis relative to g for M = 5 and T c = 40 sec. Empirical Pr(ρ α x) T slot =60sec T slot =120sec T slot =200sec T slot =240sec x [rad] Fig. 11: Sea trial: C-CDF results of ρ angle for the two boats. observe that the projected measurements are almost constant, which matches the expected slow change of the boats acceleration in the horizontal plane. Finally, in Figure 12 we present C-CDF results of ρ err, comparing estimation ˆd i,j to the GPS-based distance for time slots of duration T slot = 200 sec and for different values of M and Tc. For each time slot, the initial speed v i is calculated based on the first two GPS-based locations of the boats. From the figure we observe that in some cases ρ err > 10 m (which is above the expected error due to GPS uncertainty). As in the estimation of the heading angle, these cases occur when there is a sudden shift in

26 26 Empirical Pr(ρ err x) M=2,T c =20sec M=8,T c =40sec M=5,T c =60sec M=5,T c =40sec x [m] Fig. 12: Sea trial: C-CDF results of ρ err for the two boats. T slot = 200 sec the boats heading direction. A possible way to reduce the effect of this shortcoming of our method is to avoid relying on heading and distance estimations whenever a sudden change from the previous estimation is observed. However, for M = 5 and Tc = 40 sec, we observe that in more than 96% of cases ρ err is lower than the expected error. Considering this result and the low estimation error observed for the heading angle, we conclude that when the boats heading is constant, our method fully compensates for the vessel s time-varying pitch angle using only a single accelerometer device. VI. CONCLUSION In this paper, for DR navigation of a vessel whose motion is affected by the ocean waves, we described a method to estimate the vessel heading and the distance traveled by the vessel using only a single 3-D accelerometer. This is required when measurement of the vessel orientation using, e.g., gyrocompass, is not available or is too noisy to directly compensate for the vessel pitch and heading angles. Using only 3-D acceleration measurements, the major challenge in such estimating is the waves-induced vessel s time-varying pitch angle. Considering this problem, based on the periodic nature of the vessel pitch angle we described a machine learning classification approach that forms classes of acceleration measurements for which the pitch angle is similar. Per-class, our method estimates the vessel pitch angle, and projects the available acceleration measurements into the horizontal plane. The projected measurements are