Tightly Coupled INS/APS Passive Single Beacon Navigation

Unlike aerial or terrestrial navigation, the Global Navigation satellite system (GNSS) is not available underwater. This is a big challenge for underwater navigation. The inertial navigation system (INS) aided by the single-beacon acoustic positioning system (APS) provides one solution, but the long-range case is limited by low SNR conditions. Inspired by passive synthetic aperture detection, we proposed a new tightly coupled navigation algorithm based on spatial synthesis and one-way-travel-time (OWTT) range measurement. We design two estimators: the DOA/Range estimator using the model-based method and the tightly coupled INS/APS navigation estimator. Based on the improved UKF, all information is combined. Simulation is carried out in MATLAB. Compared with range-only tightly coupled INS/APS navigation, synthetic long baseline (SLBL) algorithm and Doppler velocity logger (DVL) aided centralized extended Kalman filter (CEKF) based single beacon INS/OWTT navigation, the proposed method's performance is proven. The main contributions of this work are: 1. Propose a new architecture of underwater integrated navigation; 2. Apply the passive acoustic detecting method in the navigation to improve accuracy. 3. Apply the tightly coupled method to improve availability.

Keywords:

INS/APS single beacon navigation

;

tightly coupled navigation

;

passive synthetic aperture

;

joint DOA/OWTT estimation

Subject:

Engineering - Marine Engineering

1. Introduction

The increasing requirement for ocean exploration promotes the development of underwater vehicles. Autonomous underwater vehicles (AUVs) are now widely used in resource exploitation, defence, and environmental monitoring. One of the most urgent problems limiting AUVs’ application is its long-term and long-range navigation. The navigation scenarios of AUVs can be concluded into three types:

1.: Surface cruise.
2.: Periodically sink and float.
3.: Underwater cruise.

This paper will focus on the underwater phase of scenario 2 and scenario 3, where GNSS is not available. Widely acknowledged, inertial navigation system (INS) aided by other sub-navigation system is the most commonly used scheme underwater. Although acoustic positioning systems like Long Baseline (LBL), Short Baseline (SBL), and Ultra-short Baseline (USBL) can substitute for the role of GNSS, their effective range is limited. Another challenging fact for long-range underwater navigation is the low signal-noise ratio (SNR) which influences its accuracy. So, the positioning method based on one-way travel time (OWTT) range measurement using long-period ranging signals provides a potential way for long-range underwater navigation.

OWTT acoustic range measurement for underwater navigation has been discussed in [1,2,3]. All the research focuses on the underwater positioning methodology without GNSS. In [1], Webster et al. used Frequency modulation (FM) sweep signal to measure the range from beacon to AUV. The central frequency of the signal is 780Hz, and the bandwidth is 5Hz. After a 155.6km voyage, the final positioning error is about 13.7 km with the aid of range measurement, whereas pure dead reckoning’s error is about 74.1km. In [3], Webster et al. used a 40s-long 900Hz central FM signal with 25Hz bandwidth. The result shows that the range error at approximately 200-250km is 40m from beacon to beacon. In [2], Graupe et al. used the acoustic arrival matching (AAM) method. The signal for range measurement is with 250Hz central frequency and 100 Hz bandwidth which lasts 135s. The best standard deviation is 835m at the range of 480km.

On the other hand, researchers show great interest in DOA estimation with passive acoustic detection. One method that provides super-resolution is passive synthetic aperture (PASA). Sullivan and Stergiopoulo proposed this concept at the end of the last century [4]. And in the following decades, systems like the extended towed array method (ETAM) [5,6], Fourier transform synthetic aperture method (FFTSA) [7,8], and maximum likelihood estimation method (ML) [9] were continuously proposed. Sullivan also proposed the underwater model-based acoustic signal processing method using state models to estimate the target’s bearing angle, which provides a new aspect to realizing PASA [10]. Yang used the PASA method to estimate the depth of underwater objects [11].

Integrating all sensors’ information and providing the best estimation for long-range underwater navigation is also a problem. INS/DVL or INS/USBL/LBL/SBL integrated navigation schemes are well researched [9,12,13]. Most of them are based on the loosely coupled strategy. The vehicles should receive acoustic signals from more than 3 beacons. This feature limited the working area and makes the deployment of APS more complex. To find a more flexible approach, we attach interest to INS/Single Beacon integrated navigation algorithms. On the other hand, to keep the stealth of underwater vehicles, the passive OWTT acoustic positioning method is more attractive. The synthetic long baseline (SLBL) algorithm was proposed by Larsen which used one beacon to realize localization[14]. Casey combined the INS and single acoustic positioning beacon for UUV navigation [15]. Webster et al. proposed a centralized Kalman filter algorithm for single-beacon OWTT acoustic navigation aided by Doppler Velocity Logger (DVL) [16]. Qin et al. proposed a variational Bayesian approximation method for single-beacon-based integrated navigation [17]. But for long-range acoustic navigation, both PASA and OWTT range measurement needs a relatively long period to improve the accuracy of bearing and range estimation.

In addition, [15,18,19,20,21,22] also provide some ideas to solve the problem of underwater navigation. To summarize, recent research about underwater navigation which is related to this study are shown in Table 1.

This paper focuses on improving the INS/APS navigation accuracy in low SNR conditions for long-range underwater navigation. We proposed an integrated INS/APS navigation algorithm based on spatial synthetic DOA/Range joint estimation. Two Unscented Kalman Filter (UKF) based joint estimators are designed to integrate the received acoustic signal and inertial measurement. The tightly coupled INS/APS integrated navigation architecture is designed to improve the availability of the navigation system. In addition, due to the OWTT method being used, the underwater vehicle is not required to transmit acoustic signals to external devices or beacons. The simulation is carried out in MATLAB. We compare the proposed method with range-only tightly coupled INS/APS navigation, SLBL and the method proposed by [16]. The results prove the performance of the proposed method.

The remainder of this paper is organized as follows: Section 2 discusses the basic navigation system model. Section 3 describes the mathematical derivation and the whole process of the proposed navigation algorithm. Section 4 presents the results of the simulation experiments and compares the proposed algorithm with other methods. Section 5 describes the scheme of the future field experiment. Section 6 presents the conclusions and our plan for further research.

2. The Basic Underwater Navigation Model

After reviewing several studies, [13,23] describe the detailed mathematical derivation of the state-space model based on the INS/acoustic beacon systems. In this paper, we establish the underwater integrated navigation model based on these studies. Four frames are used in this paper: body frame (subscript b denotes body frame), East-North-Up local-level frame (subscript n denotes body frame), Earth-Cantered Earth-Fixed Frame (subscript

E C E F

denotes this frame), Longitude-Latitude-Altitude frame (subscript

L L A

denotes this frame). Detailed information about these frames can be found in [24].

The overview block diagram of the integrated navigation system is displayed in Figure 1. Aiming to improve the accuracy of navigation in low SNR conditions, we try to use any possible information that helps reduce the accumulating error of INS. Using the APS system, we can analyse the signal from beacons and get DOA and range information directly. The beacon transmits a narrow-band signal (single-frequency signal) and a modulated broadband signal in the meantime. The underwater vehicle equipped with a hydrophone array receives and processes the signal, and then gets bearing and distance information.

Furthermore, transforming temporal gain to spatial gain is also possible to increase the accuracy of acoustic DOA and Range measurement. Inspired by the model-based signal processing methods for underwater array [10], we design a model-based estimator and realize the joint spatial synthetic DOA/Range estimation.

The diagram in Figure 1 shows that the system can be divided into DOA/Range estimation and tightly coupled navigation. The former estimates the bearing angle and distance between AUV and beacons. This process is aided by the navigation result. The latter uses the bearing and range information to correct the results of INS Dead Reckoning.

Two discrete-time state-space models are established. A detailed description of the models will be discussed in section 3. INS computation and cross-correlation algorithms are well-researched [24] which will not be discussed in this paper.

3. Methodology

In this section, mathematical modelling and derivation of the DOA/Range estimator and the tightly coupled INS/APS integrated estimator will be discussed. Both estimators are based on the Unscented Kalman Filter (UKF). In the discrete-time domain, the state-space model is as follows:

\begin{matrix} X_{k + 1} & = F (X_{k}) + W_{k} \end{matrix}

(1)

\begin{matrix} Z_{k + 1} & = H (X_{k + 1}) + V_{k} \end{matrix}

(2)

Here,

X_{k}

Z_{k}

W_{k}

and

V_{k}

represent the state vector, the measurement vector, the system noise and the measurement noise respectively. Both

W_{k}

and

V_{k}

are viewed as zero-mean, Gaussian white noise.

F ()

and

H ()

represent the state transition function and measurement transition function respectively. Although the model is not restricted to the single beacon situation, to simplify the problem, we only discuss the single beacon case in this section.

3.1. DOA/Range Estimator

One possible way to improve positioning accuracy in low SNR conditions is transforming temporal gain into spatial gain. Based on the theory of Sullivan [10], we establish a DOA/Range joint estimation model. Figure 2 shows the basic application scenario of the moving hydrophone array.

3.1.1. The System Model

The state vector is defined as follows:

X_{k} = {[θ_{k}, b_{k}, R_{k}, V_{k}, ω_{k}, ϵ_{k}, a_{k}]}^{T}

(3)

Here

θ_{k}

represents the bearing angle;

b_{k}

represents the bearing rate;

R_{k}

represents the distance between the vehicle (can be viewed as the first hydrophone node) and the beacon;

V_{k}

represents the velocity component in the voyage direction;

ω_{k}

is the frequency of the single-frequency signal;

ϵ_{k}

is the frequency deviation due to environmental disturbance;

a_{k}

is the amplitude of the single-frequency signal. The subscript k represents the

k^{t h}

epoch.

Based on Figure 2, the acoustic signal can be viewed as a plane wave. The dynamic model of the vehicle is assumed to be the mild-change constant-velocity model. So, in system estimation, the bearing rate

b_{k}

and vehicle’s voyage velocity

V_{k}

of the current epoch are equal to the values of the next epoch. We define T as the time interval of each epoch. Then we can get

θ_{k} = θ_{k - 1} + b_{k - 1} T

. According to cosine law (as Figure 3 shows), the recurrent equation between

R_{k}

and

R_{k - 1}

can also be established. The frequency

ω_{k}

can be viewed as invariant in the system model. Therefore, the system state predicting equations of each component in

X_{k}

are shown in (4). In this way, the recurrence relation between the state vectors in the current and the next epochs is built.

\{\begin{matrix} θ_{k} = θ_{k - 1} + b_{k - 1} T \\ b_{k} = b_{k - 1} \\ R_{k} = \sqrt{R_{k - 1}^{2} + {(V_{k - 1} T)}^{2} - 2 R_{k - 1} (V_{k - 1} T sin θ_{k - 1})} \\ V_{k} = V_{k - 1} \\ ω_{k} = ω_{k - 1} \\ ϵ_{k} = ϵ_{k - 1} \\ a_{k} = a_{k - 1} \end{matrix}

(4)

In real applications, before the next epoch starts, the value of

V_{k}

will be assigned to the velocity calculated by the navigation update.

3.1.2. The Measurement Model

The measurement vector

Z_{k}

is defined as (5), (6), (7):

\begin{matrix} Z_{k} & = {[\Pr_{k}, {Dis}_{k}, Δ ω_{k}]}^{T} \end{matrix}

(5)

\begin{matrix} \Pr_{k} & = [P r_{k, 1}, P r_{k, 2} . . . P r_{k, N}] \end{matrix}

(6)

\begin{matrix} {Dis}_{k} & = [D i s_{k, 1}, D i s_{k, 2} . . . D i s_{k, N}] \end{matrix}

(7)

P r_{k, i}

in (6) represents the received sound pressure of

i^{t h}

hydrophone element. The sound pressure describes the arriving sound signal.

\Pr_{k}

represents the vector consisting of

P r_{k, i}

D i s_{k, i}

in (7) represents the measured distance between the

i^{t h}

hydrophone element and the beacon.

\Pr_{k}

is the vector consist of

P r_{k, i}

Δ ω_{k}

represents the difference in frequency between the measured narrow-band signal and the ideal frequency of the original signal. Note that

P r_{k, i}

is directly measured by the hydrophones;

D i s_{k, i}

is indirectly obtained by calculating the distance between each hydrophone element and beacon position.

Δ ω_{k}

is obtained from the frequency spectrum of a certain length of the signal sequence (a sliding temporal window).

According to Sullivan’s theory [10], the received sound pressure can be expressed by a function of

θ_{k}

b_{k}

a_{k}

ω_{k}

and

V_{k}

. As more information can be provided by INS and OWTT, in this model, the received sound pressure

P r_{k, i}

is a function of

θ_{k}

b_{k}

a_{k}

ω_{k}

V_{k}

and

ϵ_{k}

. To avoid carrying the amplitude as a nuisance parameter, the phase of

P r_{k, i}

is expressed by

θ_{k}

b_{k}

ω_{k}

and

ϵ_{k}

instead of using one parameter. The

D i s_{k, i}

can be expressed as a function of

R_{k}

and

θ_{k}

based on the shape of the array shown in Figure 2. According to the definition of

Δ ω_{k}

Δ ω_{k}

contains two components: Doppler shift and environmental disturbance. In this way,

Δ ω_{k}

can be expressed as a function of

θ_{k}

V_{k}

ω_{k}

and

ϵ_{k}

. So, the predictive equations of measurement are shown in (8).

\{\begin{matrix} P r_{k, i} = a_{k} cos [(ω_{k} + ϵ_{k}) t_{k} + β_{i} (t_{k}) sin θ_{k}] \\ D i s_{k, i} = R_{k} - (i - 1) d sin θ_{k} \\ Δ ω_{k} = (ω_{k} + ϵ_{k}) (1 + \frac{V_{k}}{c} sin θ_{k}) - ω_{0} \end{matrix}

(8)

β_{i} (t_{k}) = \frac{ω_{k}}{c} [d (i - 1) + V_{k} t_{k}]

(9)

In (8),

t_{k}

represents the time point of the

k^{t h}

epoch. The definition of

β_{i} (t_{k})

is shown in (9). c represents the sound speed

ω_{0}

represents the ideal frequency of the narrow-band signal.

In this way, equations sets (4) and (8) can be written as two functions

F_{D} (X_{k})

and

H_{D} (X_{k})

. So, the state model is finally written as (10).

\{\begin{matrix} X_{k + 1} = F_{D} (X_{k}) + W_{k}^{D} \\ Z_{k} = H_{D} (X_{k}) + V_{k}^{D} \\ W_{k}^{D} \sim N (0, Q_{k}) \\ V_{k}^{D} \sim N (0, R_{k}) \end{matrix}

(10)

The resolution of the hydrophone array is approximately

0.886 \frac{λ}{D}

, where

λ

and D are the wavelengths of the signal and the aperture of the array. After iterations, the vehicle’s trajectory just synthesizes a long aperture of detection (although it is not formed at the same time). In this way, the temporal gain is transformed into spatial gain. The DOA estimation will be more accurate.

3.1.3. Observability Analysis

The observability of linear systems is well-researched while the observability of nonlinear systems is still a field of interest for many researchers. Based on the theorem established by these publications [25,26,27,28], one possible and convenient way to analyze the observability of the proposed model is linearizing the nonlinear system model and testing the observability matrix rank conditions.

The first-order linearized system model of the system (10) can be written as (11), which is a time-varying system. To be concise, we omit the noise terms in the model.

\{\begin{matrix} X_{k + 1} = \frac{\partial F_{D}}{\partial X} |_{X_{k}} X_{k} = A_{k | k + 1} X_{k} \\ Z_{k} = \frac{\partial H_{D}}{\partial X} |_{X_{k}} X_{k} = C_{k} X_{k} \end{matrix}

(11)

The detailed components of the matrices

A_{k | k + 1}

and

C_{k}

is shown in (12)-(15). Unlike time-invariant systems, several different observability methods are defined for time-varying systems. They are complete observability, differential observability, instantaneous observability and local observability. [28,29,30]. In this paper, we analyze the local observability of the proposed model. In this way, we will know the restrictions when the model is available.

A_{k | k + 1} = [\begin{matrix} 1 & T & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 & 0 \\ A_{3, 1} & 0 & A_{3, 3} & A_{3, 4} & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 1 \end{matrix}]

(12)

\{\begin{matrix} A_{3, 1} = & - {[R_{k}^{2} + {(V_{k} T)}^{2} - 2 R_{k} (V_{k} T sin θ_{k})]}^{- \frac{1}{2}} \\ \cdot (R_{k} V_{k} T) cos θ_{k} \\ A_{3, 3} = & {[R_{k}^{2} + {(V_{k} T)}^{2} - 2 R_{k} (V_{k} T sin θ_{k})]}^{- \frac{1}{2}} \\ \cdot (R_{k} - V_{k} T sin θ_{k}) \\ A_{3, 4} = & {[R_{k}^{2} + {(V_{k} T)}^{2} - 2 R_{k} (V_{k} T sin θ_{k})]}^{- \frac{1}{2}} \\ \cdot (T^{2} V_{k} - R_{k} T sin θ_{k}) \end{matrix}

(13)

C_{k} = [\begin{matrix} P r_{1, 1} & 0 & 0 & P r_{1, 4} & P r_{1, 5} & P r_{1, 6} & P r_{1, 7} \\ P r_{2, 1} & 0 & 0 & P r_{2, 4} & P r_{2, 5} & P r_{2, 6} & P r_{2, 7} \\ ⋮ & ⋮ & ⋮ & ⋮ & ⋮ & ⋮ & ⋮ \\ P r_{N, 1} & 0 & 0 & P r_{N, 4} & P r_{N, 5} & P r_{N, 6} & P r_{N, 7} \\ D i s_{1, 1} & 0 & 1 & 0 & 0 & 0 & 0 \\ D i s_{2, 1} & 0 & 1 & 0 & 0 & 0 & 0 \\ ⋮ & ⋮ & ⋮ & ⋮ & ⋮ & ⋮ & ⋮ \\ D i s_{N, 1} & 0 & 1 & 0 & 0 & 0 & 0 \\ Δ ω_{1, 1} & 0 & 0 & Δ ω_{1, 4} & Δ ω_{1, 5} & Δ ω_{1, 6} & 0 \end{matrix}]

(14)

\{\begin{matrix} P r_{i, 1} = - a_{k} \cdot β_{i} (t_{k}) \cdot cos θ_{k} sin [(ω_{k} + ϵ_{k}) t_{k} + β_{i} (t_{k}) sin θ_{k}] \\ P r_{i, 4} = - a_{k} \frac{ω_{k} t_{k} sin θ_{k}}{c} sin [(ω_{k} + ϵ_{k}) t_{k} + β_{i} (t_{k}) sin θ_{k}] \\ P r_{i, 5} = - a_{k} (t_{k} + \frac{[d \cdot (i - 1) + V_{k} t_{k}] \cdot sin θ_{k}}{c}) sin [(ω_{k} + ϵ_{k}) t_{k} \\ + β_{i} (t_{k}) sin θ_{k}] \\ P r_{i, 6} = - a_{k} t_{k} \cdot sin [(ω_{k} + ϵ_{k}) t_{k} + β_{i} (t_{k}) sin θ_{k}] \\ P r_{i, 7} = cos [(ω_{k} + ϵ_{k}) t_{k} + β_{i} (t_{k}) sin θ_{k}] \\ {Dis}_{i, 1} = (1 - i) \cdot d \cdot cos θ_{k} \\ Δ ω_{i, 1} = (ω_{k} + ϵ_{k}) \cdot cos θ_{k} \\ Δ ω_{i, 4} = (ω_{k} + ϵ_{k}) \cdot \frac{sin θ_{k}}{c} \\ Δ ω_{i, 5} = 1 + \frac{V_{k}}{c} sin θ_{k} \\ Δ ω_{i, 6} = 1 + \frac{V_{k}}{c} sin θ_{k} \\ (i = 1, 2, 3, \dots N) \end{matrix}

(15)

Definition 1

(Local Observability). Let n denote the dimension of the state vector

x_{k}

. And a time-varying system is defined as:

x_{k + 1} = f_{k} (x_{k})

y_{k} = h_{k} (x_{k})

If the state

x_{k}

can be determined from

y_{j}

where

j = k, k + 1, \dots, k + n - 1

, then the system is locally observable.

Then the local observability matrix of the system (11) on the

k^{t h}

iteration can be written as (16).

O_{k} = [\begin{matrix} C_{k} \\ C_{k + 1} A_{k} \\ C_{k + 2} A_{k + 1} A_{k} \\ ⋮ \\ C_{k + n - 1} A_{k + n - 2} . . . A_{k + 1} A_{k} \end{matrix}]

(16)

If the system is locally observable, the rank of

O_{k}

should be of rank n, where

n = 7

in this case. After testing the local observability matrix of the (11), two interesting results are obtained:

1.: When there is only one element of the hydrophone "array", $r a n k (O_{k}) = 5$ . So the system is unobservable if the hydrophone "array" only contains one element. This property is also proven by [10,31], but in a different way.
2.: The system (10) is undifferentiable if $R_{k}^{2} + {(V_{k} T)}^{2} - 2 R_{k} (V_{k} T sin θ_{k}) = 0$ . This will happen if and only if $θ = \pm \frac{π}{2}$ and $R_{k} = \pm V_{k} \cdot T$ . Therefore, the model is unobservable if the trajectory of the AUV overlaps the position of the beacon. This result is also supported by the research of [9] but in a different aspect. Wang discussed the basic TOA/DOA single beacon localization geometric model and pointed out that the geometric state transformation matrix is not invertible when the vehicle’s trajectory overlaps the position of the beacon. But unlike pure DOA or pure TOA localization method, when $θ = \frac{π}{2}$ and $R_{k} \neq V_{k} \cdot T$ (the trajectory of the vehicle is collinear with the beacon but doesn’t overlap the beacon), the system is still observable.

3.1.4. UKF

One fact is that

F_{D} (X_{k})

and

H_{D} (X_{k})

are not linear function. The conventional Kalman Filter is not a suitable choice when we consider the data-fusion problem. While using the Extended Kalman Filter to solve the problem, it is hard work to calculate the time-varying Jacobian matrix in every iteration. So, we finally choose the Unscented Kalman Filter(UKF). The mathematical principle of UKF is well researched [32], so this paper will not list the concrete derivation of UKF. The specific steps of UKF in this model are shown in Table 2.

Because of the floating-point error in real computation, when the elements of

P_{k}

are small,

P_{k}

will sometimes become negative definite using the standard UKF algorithm. There is no solution for Cholesky decomposition in this case. To guarantee the positive semidefiniteness of

S_{k}

, unlike the standard UKF algorithm, we add a singular value decomposition (SVD) step before Cholesky decomposition, which is shown in the “Generate sigma points” step of Table 2.

The covariance matrix

P_{k}

can always be decomposed as (17):

P_{k} = U A_{k} V

(17)

Based on the definition of SVD,

A_{k}

always meets the condition of positive semidefinite. Because the covariance matrix is symmetric. Then we can get:

\begin{matrix} P_{k} = U \sqrt{A_{k} A_{k}^{T}} U^{T} \end{matrix}

(18)

\begin{matrix} C h o l e s k y (P_{k}) = U \cdot C h o l e s k y (A_{k}) \end{matrix}

(19)

This method prevents the anomaly of Cholesky decomposition over iterations which is also used in the filter of the tightly coupled INS/APS navigation Estimator.

3.2. Tightly Coupled INS/APS Navigation Estimator

For underwater vehicles’ navigation, yaw angle and depth are instrumented adequately with the compass and the depth gauge. So, long-range underwater navigation can be viewed as a 2-D problem. Traditional single beacon integrated navigation algorithms should calculate the position of the vehicle, and then position error model is used for data-fusion. This strategy has disadvantages, such as motion restriction and multi-results. So, in the proposed model, inspired by tightly coupled INS/GNSS navigation, tightly coupled strategy is chosen for INS/APS navigation. Kepper proposes an open-loop architecture for 2D underwater integrated navigation [33]. But it has been proven in INS/GNSS integrated navigation that the closed-loop architecture is more stable and has better performance in practice [34]. The closed-loop architecture is then used in the proposed model.

3.2.1. The System Model

The state vector is defined as follows:

\begin{matrix} {Xn}_{k} = [δ P_{E, k}, δ P_{N, k}, δ V_{E, k}, δ V_{N, k}, δ ϕ_{k}, δ θ_{k}, \\ δ γ_{k}, δ b_{x, k}, δ b_{y, k}, δ b_{z, k}, δ g_{x, k}, δ g_{y, k}, δ g_{z, k}, δ c l k_{k}]^{T} \end{matrix}

(20)

δ P_{E, k}, δ P_{N, k}

represent the error in Longitude and Latitude respectively;

δ V_{E, k}

and

δ V_{N, k}

represent the error of eastward velocity and northward velocity respectively;

ϕ_{k}, δ θ_{k}

and

δ γ_{k}

represent the error of the pitch, roll and yaw angle respectively;

δ b_{x, k}, δ b_{y, k}

and

δ b_{z, k}

are the error of 3-axis accelerometers’ bias in body frame;

δ g_{x, k}, δ g_{y, k}

and

δ g_{z, k}

are the error of 3-axis gyroscopes’ bias in body frame;

δ c l k_{k}

is the drift of OWTT caused by clock defects or propagation channel.

Because the changing process of bias and drift are modelled with first-order Gauss-Markov process [13], the bias can be written in the form of (21), where

T c

is the correlation time,

d t

is the sample period of the filter and

w_{b}

is the zero mean Gaussian noise of bias.

b_{k} = e^{\frac{d t}{T_{c}}} + w_{b}

(21)

Based on the classic inertial error model ([24]), the state transition model can be finally written as (22) to (27).

\begin{matrix} [\begin{matrix} δ P_{E, k + 1} \\ δ P_{N, k + 1} \end{matrix}] = [\begin{matrix} 0 & \frac{d t}{R_{M} + h} \\ \frac{d t}{(R_{N} + h) cos (L o n)} & 0 \end{matrix}] [\begin{matrix} δ V_{E, k} \\ δ V_{N, k} \end{matrix}] \\ + [\begin{matrix} δ P_{E, k} \\ δ P_{N, k} \end{matrix}] \end{matrix}

(22)

\begin{matrix} [\begin{matrix} δ V_{E, k + 1} \\ δ V_{N, k + 1} \end{matrix}] = [\begin{matrix} δ V_{E, k} \\ δ V_{N, k} \end{matrix}] + C_{b}^{n} (1 : 2, 1 : 3) [\begin{matrix} δ b_{x, k} \\ δ b_{y, k} \\ δ b_{z, k} \end{matrix}] \end{matrix}

(23)

\begin{matrix} [\begin{matrix} δ ϕ_{k + 1} \\ δ θ_{k + 1} \\ δ γ_{k + 1} \end{matrix}] = [\begin{matrix} δ ϕ_{k} \\ δ θ_{k} \\ δ γ_{k} \end{matrix}] + C_{b}^{n} [\begin{matrix} δ g_{x, k} \\ δ g_{y, k} \\ δ g_{z, k} \end{matrix}] d t \\ + [\begin{matrix} 0 & \frac{d t}{R_{M} + h} \\ \frac{- d t}{R_{N} + h} & 0 \\ \frac{- d t tan (l o n)}{R_{N} + h} & 0 \end{matrix}] [\begin{matrix} δ V_{E, k} \\ δ V_{N, k} \end{matrix}] \end{matrix}

(24)

\begin{matrix} [\begin{matrix} δ b_{x, k + 1} \\ δ b_{y, k + 1} \\ δ b_{z, k + 1} \end{matrix}] = e^{- \frac{d t}{T c_{1}}} [\begin{matrix} δ b_{x, k} \\ δ b_{y, k} \\ δ b_{z, k} \end{matrix}] \end{matrix}

(25)

\begin{matrix} \begin{matrix} [\begin{matrix} δ g_{x, k + 1} \\ δ g_{y, k + 1} \\ δ g_{z, k + 1} \end{matrix}] & = e^{- \frac{d t}{T c_{2}}} [\begin{matrix} δ g_{x, k} \\ δ g_{y, k} \\ δ g_{z, k} \end{matrix}] \end{matrix} \end{matrix}

(26)

\begin{matrix} δ c l k_{k + 1} & = e^{- \frac{d t}{T c_{3}}} \cdot δ c l k_{k} \end{matrix}

(27)

R_{N}

represents the local normal radius;

R_{M}

represents the local meridian radius;

C_{b}^{n}

is the 3×3 rotation matrix from body frame to ENU frame;

d t

is the updating period of navigation.

T c_{1}, T c_{2}, T c_{3}

are introduced which are the correlation time constants of accelerators, gyroscopes, and clock respectively.

3.2.2. The Measurement Model

The measurement vector is defined as (22) to (24).

\begin{matrix} {Zn}_{k} & = {[δ ρ_{k}, δ sin θ_{k}]}^{T} \end{matrix}

(28)

\begin{matrix} δ ρ_{k} & = ρ_{k}^{I N S} - ρ_{k}^{A P S} \end{matrix}

(29)

\begin{matrix} δ sin θ_{k} & = δ sin θ_{k}^{I N S} - δ sin θ_{k}^{A P S} \end{matrix}

(30)

δ ρ_{k}

represents the difference between the distance measured by OWTT range and calculated by the navigation results of the last iteration;

δ sin θ_{k}

represents the differences between the bearing angle measured by DOA and calculated by the navigation results of the last iteration.

The measurement vector can be predicted by state vector as equations in (31) to (36) show.

\begin{matrix} P_{E C E F}^{E S T} & = [\begin{matrix} x_{k} \\ y_{k} \\ z_{k} \end{matrix}] = C_{L L A}^{E C E F} [\begin{matrix} P_{E, k} - δ P_{E, k} \\ P_{N, k} - δ P_{N, k} \\ P_{U, k} \end{matrix}] \end{matrix}

(31)

\begin{matrix} P_{E C E F}^{b e a c o n} & = {[\begin{matrix} x 1_{k} \\ y 1_{k} \\ z 1_{k} \end{matrix}]}_{E C E F} \end{matrix}

(32)

\begin{matrix} P_{L L A}^{b e a c o n} & = {[\begin{matrix} P_{E}^{b e a c o n} \\ P_{N}^{b e a c o n} \\ P_{U}^{b e a c o n} \end{matrix}]}_{L L A} \end{matrix}

(33)

\begin{matrix} P_{E C E F}^{I N S} & = C_{L L A}^{E C E F} [\begin{matrix} P_{E, k} \\ P_{N, k} \\ P_{U, k} \end{matrix}] \end{matrix}

(34)

\begin{matrix} δ ρ = & ∥P_{E C E F}^{I N S} - P_{E C E F}^{b e a c o n}∥ - ∥P_{E C E F}^{E S T} - P_{E C E F}^{b e a c o n}∥ \\ - c \cdot δ c l k (k) \end{matrix}

(35)

\begin{matrix} δ sin θ_{k} = sin [b e a r i n g ((P_{E, k}, P_{N, k}), (P_{m}^{E}, P_{m}^{N}))] - \\ sin [b e a r i n g ((P_{E, k} - δ P_{E, k}, P_{N, k} - δ P_{N, k}), (P_{m}^{E}, P_{m}^{N}))] \end{matrix}

(36)

C_{L L A}^{E C E F}

is 3×3 rotation matrix from LLA frame to ENU frame;

P_{E C E F}^{b e a c o n}

is the beacon’s position in ECEF frame;

P_{L L A}^{b e a c o n}

is the beacon’s position in LLA frame;

b e a r i n g (A, B)

represents the bearing of point B from point A.

By combining the transformation equations, the state model of the tightly coupled model can be written in a concise form as (37).

\{\begin{matrix} {Xn}_{k + 1} = F_{T C} ({Xn}_{k}) + W_{k}^{T C} \\ {Zn}_{k + 1} = H_{T C} ({Xn}_{k + 1}) + V_{k + 1}^{T C} \\ W_{k}^{T C} \sim N (0, {Qn}_{k}) \\ V_{k}^{T C} \sim N (0, {Rn}_{k}) \end{matrix}

(37)

Note that the model can be transformed into the multi-beacon form by extending the measurement vector and transform function. Like (38) to (41) show, where m is the number of beacons. The only difference between each

H_{T C}^{i} ()

is the position of the

i_{t h}

beacon.

\begin{matrix} {Zn}_{k} & = {[{δ ρ}_{k}, {δ bear}_{k}]}^{T} \end{matrix}

(38)

\begin{matrix} {δ ρ}_{k} & = [δ ρ_{k}^{1}, δ ρ_{k}^{2}, δ ρ_{k}^{3} . . . δ ρ_{k}^{m}] \end{matrix}

(39)

\begin{matrix} {δ bear}_{k} & = [δ sin θ_{k}^{1}, δ sin θ_{k}^{2}, δ sin θ_{k}^{3} . . . δ sin θ_{k}^{m}] \end{matrix}

(40)

\begin{matrix} {Zn}_{k + 1} & = [\begin{matrix} H_{T C}^{1} ({Xn}_{k + 1}) \\ H_{T C}^{2} ({Xn}_{k + 1}) \\ H_{T C}^{3} ({Xn}_{k + 1}) \\ ⋮ \\ H_{T C}^{m} ({Xn}_{k + 1}) \end{matrix}] + W_{k + 1}^{m u l t i - T C} \end{matrix}

(41)

The observability of the tightly coupled model won’t be discussed in this paper because it is a well-researched topic in INS/GNSS tightly coupled navigation area. And some of the analyses can be found in [24,35,36].

3.2.3. UKF

UKF is also used to realize the data fusion procedure of the tightly coupled nonlinear state model. The whole streamline is shown in Table 3.

According to 3.1 and 3.2, it is possible if we combine two estimators into one by merging the state vectors and reorganizing the state-space model. But the dimension of the combined estimator will be higher which is not cost-efficient.

We quantified the computational complexity of UKF’s every step to analyse both structures. As Table 4 shows. Assuming there are E elements in the hydrophone array. In the two estimators’ architecture, the total number of flops is

(2173 \times E) / 3 + (183 \times {(2 \times E + 1)}^{2}) / 2 + (8 \times {(2 \times E + 1)}^{3}) / 3 + 169697 / 6

and the total number of square roots is 21. In the one estimator’s architecture, the total number of flops is

(9733 \times E) / 3 + (519 \times {(2 \times E + 3)}^{2}) / 2 + (8 \times {(2 \times E + 3)}^{3}) / 3 + 152861 / 2

and the total number of square roots are also 21. Because

E \in N^{+}

, the one estimator’s architecture will always execute at least more 56593 flops than the two estimators’ architecture each epoch.

On the other hand, the robustness of UKF will decrease if the dimension is too high. The results of the high-dimension filter will be hard to converge in some cases. Furthermore, it is difficult to select the best value of tuning parameters for high-dimension UKF. In consideration of these aspects, we choose the two estimators’ architectures in the proposed algorithm.

3.3. Parameters’ Selection and Motion Restriction

3.3.1. Parameters’ Selection

The system noise covariance matrix Q and the measurement covariance matrix R are the most important parameters for estimation for both estimators. It’s hard to summarize a quantified rule of parameter selection. To say the least, however, we can get a qualitative rule. For DOA/Range estimator, the selection of Q is based on the user’s trust in the constant-velocity dynamic model. R can be obtained from the noise of the navigation results and the measurement noise of sound pressure. For the tightly coupled navigation estimator, the selection of Q is based on the measurement noise of IMU and clock source. R’s selection is influenced by the measurement noise of OWTT and the estimation covariance of DOA. Most of the time, the value of Q should not be set too large, or the filter would be hard to converge. The value of R should be slightly larger than the measurement noise which will help stabilize the filter’s results.

3.3.2. The Motion Restriction

There is no motion restriction for the tightly coupled navigation estimator, and it has already been proven observable all time [24]. But for DOA/Range estimator, because the basic assumption of the system model is constant velocity, the vehicle should move with a relatively stable velocity during estimation or the result of DOA/Range estimation will deteriorate.

On the other hand, most of the single beacon navigation algorithms require the vehicle not to move radially towards or away from the beacon or the system will be unobservable. It has been proven in section 3.1.3, the proposed DOA/Range estimator is also observable in the radial motion case if the trajectory does not overlap the position of beacons. And there should be more than one element in the hydrophone array. The numeric analysis is presented in section 4.1.

3.4. The OWTT Measurement

According to [1,3,13] and our experiments in East China Sea in 2021 (ECS2021), measuring the TOA of long-range through long-period modulated signal in shallow water is possible.

Figure 4 shows the error distribution of OWTT range measurement in the real stable experiment in ECS2021 (the acoustic transceiver and the hydrophones array are stable). The acoustic transceiver sent m-sequence data modulated by BPSK. The source level is about 170dB re micro-pascal and the carrier frequency of the signal is 160Hz with 16Hz bandwidth. The correlation period is 30 seconds. The distance between the two ends is about 50 kilometres. The result shows that the standard deviation of range measurement is about 8.5m at 50km. But the error distribution shows that the noise is not zero-mean Gaussian noise, so we add a noise estimation variable

δ c l k_{k}

in the state vector of the tightly coupled navigation model.

The pseudo-code of the proposed algorithm is shown in Algorithm 1 (list in appendix).

4. Simulation

To verify the proposed method, we design a series of simulations including motion restriction and comparison of the proposed method with the pure inertial update and some recently proposed single-beacon algorithms in the same condition. The simulations are carried out in MATLAB. And the block graph of the simulation system is shown in Figure 5.

4.1. Motion Restriction Research

In this section, we will use the numeric method to research the motion restriction of DOA/Range estimator. To get the variation in accuracy of DOA/Range estimator when the initial bearing is different, we set different start points of the vehicle in simulation. Except for bearing, the other parameters including heading direction, velocity and UKF parameters are all the same. The detailed configuration is shown in Table 5.

Assuming that the DOA measuring range of the hydrophone array is [-90°, 90°] because of the array’s directivity, we set the bearing of the initial point from -90° to 90° with 1° step. For each iteration, we generate the same 450s eastward trajectory, acoustic signal after propagation and IMU data with noise. Then we calculate the error between the true value and the output of the DOA/Range estimator. The simulation repeats 50 times to eliminate the randomness. The averaged root mean square error (ARMSE, defined as (34)) is used to evaluate the accuracy. M represents the times of repetition in simulation; N is the number of sample points;

x_{i j}

is the output results of simulation at each sampling moment;

{\hat{x}}_{i j}

is the true value.

A R M S E = \frac{1}{M} \sum_{i = 1}^{M} \sqrt{\frac{1}{N} \sum_{j = 1}^{N} (x_{i j} - {\hat{x}}_{i j})}

(42)

The updating rate of the navigation (100Hz) is much lower than the updating rate of the DOA/Range estimator (4000Hz, which is equal to the sampling rate of the hydrophone). So, before the next navigation update, the range measurement input of the DOA/Range estimator is viewed as invariant.

The results show that when the initial bearing angle between the vehicle and the beacon is -90°~-60° or 60°~90°, the DOA estimation deteriorates, but there is no obvious degradation in range estimation. So, radial or near radial movement will influence the accuracy of estimation. Such kinds of movement should be avoided in real applications of the proposed method. In future work, the bearing of the current position can be a criterion to determine the confidence of the DOA/Range estimator and adjust the parameters of UKF adaptively.

4.2. Performance Comparison

To verify the navigation performance of the proposed method, we compare it in the same conditions with INS only, the INS/APS tightly coupled model with range-only measurement (algo1), the algorithm (algo2 with DVL) proposed in [16] and the synthetic baseline algorithm (algo3) which is proposed in [14]. The configuration is shown in Table 6. The simulating trajectory is shown in Figure 8. The simulation results are shown in Figure 9.

Figure 6. The variation in accuracy of bearing estimation when initial bearing is different.

Figure 7. The variation in accuracy of range estimation when initial bearing is different.

The average root means square error (ARMSE) after the simulation of every algorithm are listed in Table 7. From the results, we can see that with the aid of the acoustic positioning system, the navigation accuracy of three integrated algorithms are improved apparently. They all effectively restrict the accumulating error which is caused by the drift of Inertial sensors. From Table 7, algo2 and the proposed algorithm are much better than algo1. Although the ARMSE in the east direction of algo3 is smaller than other algorithms, the majority of the positioning error lies in the north direction. The total ARMSE of algo1 and algo3 are on the same level. The accuracy of the proposed method is relatively worse than the algo2. Regarding algo2 with the aid of DVL, the simulation results reflect that the positioning accuracy of the proposed method is the best in INS/OWTT integrated navigation algorithms. The results verify the superiority of the proposed method.

Figure 8. The simulating trajectory.

Figure 9. (a) Error curve of INS only, algo1, algo2, algo3 and the proposed method in east position; (b) Error curve of algo1, algo2, algo3 and the proposed method in east position; (c) Error curve of INS only, algo1, algo2, algo3 and the proposed method in north position; (d) Error curve of algo1, algo2, algo3 and the proposed method in north position.

Table 6. Simulation Configuration in Performance Comparison.

Parameter	Value
Number of hydrophone elements	10
Spatial interval of elements	5m
Sample rate of hydrophone	4000Hz
Sample rate of IMU	100Hz
Time of simulation of each iteration	2460s
Times of repetition	10
Initial distance between the beacon and the vehicle	50km
Initial bearing angle	45°
Voyage direction	Easst(yaw=90°)
Voyage speed	2m/s
Accuracy of depth sensor	0.1m
Accuracy of Compass	0.1°
SNR	-30dB
Frequency of narrowband signal	100Hz
Broadband signal (a period)	LFM( $130 \sim 230 H z$ , 40s)
$Q_{k}$	$D i a g o n a l (10^{- 7}, 10^{- 8}, 10^{- 6}, 10^{- 9}, 10^{- 4}, 10^{- 5}, 10^{- 6})$
$R_{k}$	$I_{21 \times 21} \cdot 10^{- 3}$
$P_{0}$	$D i a g o n a l (0.1, 0.01, 0.01, 0.01, 0.01, 0.01, 10^{- 8})$
${Qn}_{k}$	$D i a g o n a l (10^{- 10}, 10^{- 10}, 10^{- 8}, 10^{- 8}, 10^{- 10}, 10^{- 10}, 10^{- 10}, 10^{- 12},$ $10^{- 12}, 10^{- 12}, 10^{- 12}, 10^{- 12}, 10^{- 12}, 10^{- 12})$
${Rn}_{k}$	$D i a g o n a l (10^{4}, 10^{4})$
${Pn}_{0}$	$D i a g o n a l (10^{- 8}, 10^{- 8}, 10^{- 6}, 10^{- 6}, 10^{- 8}, 10^{- 8}, 10^{- 10}, 10^{- 8}, 10^{- 8},$ $10^{- 8}, 10^{- 10}, 10^{- 10}, 10^{- 10}, 10^{- 10})$

Table 7. The process of UKF in tightly coupled navigation.

	East ARMSE (m)	North ARMSE (m)	Total (m)
INS Only	608.48	966.39	1142.00
Algo1	91.65	49.58	104.20
Algo2	15.67	14.43	21.30
Algo3	9.15	102.31	102.72
Proposed	20.47	25.77	32.91

5. Field-Experimental Design

The field experiment is required if we want to verify the real performance of the proposed algorithm. Currently, the field experiment is not carried out. But the design of the field experiment is presented.

5.1. Experimental Requirements

Firstly, the field experiment should be conducted in the ocean, and the area should be at least 2500 nmile square. An AUV is needed as an experiment platform. The AUV should be equipped with a hydrophone array, GNSS receiver, IMU, an on-vehicle computer, DVL and so on. The AUV should automatically travel through the pre-set route, in the meantime, record the data including IMU’s data, hydrophones’ data and navigation results. In addition, according to the field experiments in previous research, the number of elements of the hydrophone array should be more than four.

Secondly, a high-power acoustic beacon is needed as the signal transmitter. The bandwidth of the beacon should at least cover 50Hz to 1000Hz. The beacon should be programmable so that the researchers can freely set the signal’s format. And the maximum source level of the beacon should be no less than 180 dB.

Last but not least, a high-accuracy LBL system is needed. This system should be deployed near the AUV and the effective range of the LBL system should cover the trajectory of the AUV. This system is served as a reference system which provides the benchmark for the experiment.

5.2. Experimental Procedure

Before the field experiment, several position points and areas should be preset which are: the voyage area of the AUV where the LBL system is deployed; a point locates 50km away from the voyage area; a point locates 100km away from the voyage area; a point locates 150km away from the voyage area. The whole procedure of the experiment is as follows.

1.: Two ships carry the AUV and the beacon respectively and reach the preset positions. The beacon should be firstly at the 50km point. And then the AUV and the beacon are released into the ocean.
2.: The beacon plays the acoustic signal (single-frequency signal + wide bandwidth ranging signal). Note that the bandwidth of the signal should not overlap the LBL’s bandwidth. In the meantime, the AUV voyages along with the preset trajectory (the velocity of the AUV should be approximately constant and no more than 4m/s). The AUV records the sensors’ data and the LBL positions. The whole process should be at least lasting one hour. The 50km-point test repeats three times.
3.: Then, the beacon should be deployed at the 100km-point and play the same signal. The AUV repeats the same process as 50km-point test.
4.: Repeat the same process for the 150km-point test.
5.: Collect the data recorded by the AUV. Post-process the IMU’s data and the hydrophones data using the conventional single beacon method, range-only tightly coupled method, some state-of-art single beacon methods and the proposed method respectively. Compare every method’s results with LBL data. Attention should be attached to three values: the average positioning error, the system available time and the effective range of the acoustic aid.

6. Conclusions

This work focuses on improving the accuracy of underwater navigation in long-range and low SNR conditions. To achieve this target, a new scheme of INS/APS integrated navigation is proposed. This method combines passive synthetic aperture detection and tightly coupled navigation. The signal for range and bearing measurement is a composite signal which contains a single-frequency signal and a modulated signal. We design a DOA/Range estimator and a tightly coupled INS/APS integrated estimator based on UKF. The two estimators’ architecture is proven to be slighter in computation compared with one estimator’s architecture. The motion restriction simulation reveals that the accuracy of the proposed method varies from the relative direction between the voyaging direction and the beacon. Meanwhile, the simulation results shows its superiority compared with other INS/APS integerated navigation algorithms in the same condition. Future research of this work will focus on these aspects:

1.: The practical experiments verification. The real environment will be much more complex. Some parameters which are suitable for simulation may not be the same case in the real world. Practical experiments will help improve the model and verify its real performance.
2.: The implementation of auto-tuning and adaptive data-fusion algorithm will be also important in the next step of the work. Although there are lots of KF-based adaptive data-fusion algorithms, most of them just introduce some new hyperparameters like fading factors. So, it is still a game of tuning. We hope that we can design an adaptive estimator which can provide the appropriate UKF parameters based on the underwater environment.
3.: Generalize the model. The current model is established based on the constant velocity model. But a more general model is needed, as the motion type of the underwater vehicle is required to be more flexible.
4.: A better updating strategy. During the research of this work, we find that the updating rate of each subsystem is different. Especially, the OWTT range measurement is much lower than others. This problem causes the fluctuation of estimation error. The error curve is saw-toothed because of the long updating period. On the other hand, due to the disturbance of ocean circumstances, we cannot guarantee a stable updating rate of OWTT data. We want to find some methods like temporal predictions to eliminate such fluctuation when OWTT measurement is not available.

Further research of Long-range underwater navigation without GNSS will make a contribution to deep-sea exploration, oceanic science, and flexible navy application.

Author Contributions

Conceptualization, Zhuoyang Zou and Wenrui Wang; methodology, Zhuoyang Zou; software, Zhuoyang Zou; validation, Zhuoyang Zou, Wenrui Wang and Bin Wu; formal analysis, Zhuoyang Zou; data curation,Zhuoyang Zou; writing—original draft preparation, Zhuoyang Zou; writing—review and editing,Wenrui Wang; visualization, Zhuoyang Zou; supervision, Lingyun Ye and Washington Yotto Ochieng. All authors have read and agreed to the published version of the manuscript.

Funding

This research received no external funding

Conflicts of Interest

The authors declare no conflict of interest.

Abbreviations

The following abbreviations are used in this manuscript:

GNSS	Global navigation satellite system
INS	Inertial navigation system
APS	Acoustic positioning system
SNR	Signal-noise ratip
OWTT	One-way-travel-time
DOA	Direction-of-arrival
UKF	Unscented Kalman filter
DVL	Doppler velocity logger
CEKF	Centralized extended Kalman filter
LBL	Long Baseline
SBL	Short Baseline
USBL	Ultra Short Baseline
FM	Frequency modulation
AAM	Acoustic arrival matching
PASA	Passive synthetic aperture
ETAM	Extended towed array method
ML	Maximum likelihood
FFT	Fourier transform synthetic aperture method
SLBL	Synthetic long baseline
UUV	Unmanned underwater vehicles
ENU	East-North-Up
ECEF	Earth-Cantered Earth-Fixed Frame
LLA	Longitude-Latitude-Altitude frame
SVD	singular value decomposition
LFM	Linear frequency modulation

Appendix A

Algorithm 1: Pseudo code of the proposed algorithm

Require: $N > 0$
$X_{k}, Z_{k}, P_{k}, Q_{k}, R_{k} \leftarrow X_{0}, Z_{0}, P_{0}, Q, R$
${Xn}_{i}, {Zn}_{i}, {Pn}_{i}, {Qn}_{i}, {Rn}_{i} \leftarrow {Xn}_{0}, {Zn}_{0}, {Pn}_{0}, Qn, Rn$

▹ Initialize the vectors and matrices
$N a v_{j} \leftarrow N a v_{0}$
$R a n g e_{k}, P r e s s u r e_{k}, I M U_{k} \leftarrow$ MeasureInit()

▹ Navigation and measurement initialization
while $k \leq N$ do
if $m o d (k, f_{D O A} / f_{N A V}) = 0$ then
if $m o d (j, f_{N A V} / f_{O W T T})$ then
${Zn}_{i} \leftarrow MeasureUpdt 1 (O W T T_{i}, X_{k - 1})$
${Xn}_{i}, {Pn}_{i} \leftarrow IntNav ({Xn}_{i - 1}, {Zn}_{i}, {Pn}_{i - 1})$
$N a v_{j - 1} \leftarrow N a v C r c t ({Xn}_{i})$
$i + +$
end if
$N a v_{j} \leftarrow InsUpdt (N a v_{j - 1})$
$R a n g e_{j} \leftarrow RangeGet (N a v_{j})$
j++;
end if
$Z_{k} \leftarrow MeasureUpdt 2 (R a n g e_{j}, P r e s s u r e_{k})$
$X_{k}, P_{k} \leftarrow DOA_RangeEst (X_{k - 1}, Z_{k}, P_{k - 1})$
$k + +$
end while

f_{D O A}

is the updating rate of DOA estimator;

f_{N A V}

is the updating rate of INS navigation;

f_{O W T T}

is the updating rate of OWTT range; MeasureInit() is the measerement initialization function; MeasureUpdt1() is the function to calculate

{Zn}_{i}

with the input of OWTT range measurement and the result of DOA/Range estimation; IntNav() is the function to realize the proposed tightly coupled navigation; NavCrct() is the function to correct results of navigation; InsUpdt() is the function of inertial navigation update; RangeGet() is the function to calculate the range from each element to the beacon; MeasureUpdt2() is the function to calculate

Z_{k}

with the input of range and the detected value of hydrophone; DOA_RangeEst is the function to realize the proposed DOA/Range estimator.

References

Webster, S.E.; Lee, C.M.; Gobat, J.I. Preliminary results in under-ice acoustic navigation for seagliders in Davis Strait. 2014 Oceans - St. John’s, 2014, pp. 1–5. ISSN: 0197-7385. [CrossRef]
Graupe, C.E.; van Uffelen, L.J.; Webster, S.E.; Worcester, P.F.; Dzieciuch, M.A. Preliminary results for glider localization in the Beaufort Duct using broadband acoustic sources at long range. OCEANS 2019 MTS/IEEE SEATTLE, 2019, pp. 1–6. ISSN: 0197-7385. [CrossRef]
Webster, S.E.; Freitag, L.E.; Lee, C.M.; Gobat, J.I. Towards real-time under-ice acoustic navigation at mesoscale ranges. 2015 IEEE International Conference on Robotics and Automation (ICRA), 2015, pp. 537–544. ISSN: 1050-4729. [CrossRef]
Stergiopoulos, S.; Sullivan, E.J. Extended towed array processing by an overlap correlator. The Journal of the Acoustical Society of America 1989, 86, 158–171. Publisher: Acoustical Society of America. [CrossRef]
Wang, Y.; Gong, Z.; Zhang, R. An improved phase correction algorithm in extended towed array method for passive synthetic aperture. Proceedings of Meetings on Acoustics 2017, 30, 055007. Publisher: Acoustical Society of America. [CrossRef]
Bee, T.P.; Siong, L.H.; Swee, C.C.; Passerieux, J.M. Extended Towed Array Measurement: Beam-domain phase estimation and coherent summation. OCEANS 2006 - Asia Pacific, 2006, pp. 1–6. [CrossRef]
Lim, H.S.; Tong, P.B.; Chia, C.S. Scenario Based Study of ETAM and FFTSA in Realistic Simulated Environment. OCEANS 2006 - Asia Pacific, 2006, pp. 1–7. [CrossRef]
Song, L.; Zhi-Xiang, Y. Influencing Factors of Passive Synthetic Aperture Technology Based on FFTSA. 2021 OES China Ocean Acoustics (COA), 2021, pp. 955–960. [CrossRef]
Wang, Y.; Sun, G.C.; Wang, Y.; Zhang, Z.; Xing, M.; Yang, X. A High-Resolution and High-Precision Passive Positioning System Based on Synthetic Aperture Technique. IEEE Transactions on Geoscience and Remote Sensing 2022, 60, 1–13. Conference Name: IEEE Transactions on Geoscience and Remote Sensing. [CrossRef]
Sullivan, E.J. Model-Based Processing for Underwater Acoustic Arrays; SpringerBriefs in Physics, Springer International Publishing: Cham, 2015. [CrossRef]
Yang, T.C. Source depth estimation based on synthetic aperture beamfoming for a moving source. The Journal of the Acoustical Society of America 2015, 138, 1678–1686. Publisher: Acoustical Society of America. [CrossRef]
Fischell, E.M.; Rypkema, N.R.; Schmidt, H. Relative Autonomy and Navigation for Command and Control of Low-Cost Autonomous Underwater Vehicles. IEEE Robotics and Automation Letters 2019, 4, 1800–1806. Conference Name: IEEE Robotics and Automation Letters. [CrossRef]
Kepper, J.H.; Claus, B.C.; Kinsey, J.C. A Navigation Solution Using a MEMS IMU, Model-Based Dead-Reckoning, and One-Way-Travel-Time Acoustic Range Measurements for Autonomous Underwater Vehicles. IEEE Journal of Oceanic Engineering 2019, 44, 664–682. Conference Name: IEEE Journal of Oceanic Engineering. [CrossRef]
Larsen, M. Synthetic long baseline navigation of underwater vehicles. OCEANS 2000 MTS/IEEE Conference and Exhibition. Conference Proceedings (Cat. No.00CH37158), 2000, Vol. 3, pp. 2043–2050 vol.3. [CrossRef]
Casey, T.; Guimond, B.; Hu, J. Underwater Vehicle Positioning Based on Time of Arrival Measurements from a Single Beacon. OCEANS 2007, 2007, pp. 1–8. ISSN: 0197-7385. [CrossRef]
Webster, S.E.; Eustice, R.M.; Singh, H.; Whitcomb, L.L. Advances in single-beacon one-way-travel-time acoustic navigation for underwater vehicles. The International Journal of Robotics Research 2012, 31, 935–950. Publisher: SAGE Publications Ltd STM. [CrossRef]
Qin, H.D.; Yu, X.; Zhu, Z.B.; Deng, Z.C. A variational Bayesian approximation based adaptive single beacon navigation method with unknown ESV. Ocean Engineering 2020, 209, 107484. [CrossRef]
De Palma, D.; Arrichiello, F.; Parlangeli, G.; Indiveri, G. Underwater localization using single beacon measurements: Observability analysis for a double integrator system. Ocean Engineering 2017, 142, 650–665. [CrossRef]
Paull, L.; Saeedi, S.; Seto, M.; Li, H. AUV Navigation and Localization: A Review. IEEE Journal of Oceanic Engineering 2014, 39, 131–149. Conference Name: IEEE Journal of Oceanic Engineering. [CrossRef]
Leonard, J.J.; Bahr, A. Autonomous Underwater Vehicle Navigation. In Springer Handbook of Ocean Engineering; Dhanak, M.R.; Xiros, N.I., Eds.; Springer Handbooks, Springer International Publishing: Cham, 2016; pp. 341–358. [CrossRef]
Chen, Y.; Zheng, D.; Miller, P.A.; Farrell, J.A. Underwater Inertial Navigation With Long Baseline Transceivers: A Near-Real-Time Approach. IEEE Transactions on Control Systems Technology 2016, 24, 240–251. Conference Name: IEEE Transactions on Control Systems Technology. [CrossRef]
Claus, B.; Kepper IV, J.H.; Suman, S.; Kinsey, J.C. Closed-loop one-way-travel-time navigation using low-grade odometry for autonomous underwater vehicles. Journal of Field Robotics 2018, 35, 421–434. _eprint: https://onlinelibrary.wiley.com/doi/pdf/10.1002/rob.21746. [CrossRef]
Zhang, L.; Hsu, L.T.; Zhang, T. A Novel INS/USBL Integrated Navigation Scheme Via Factor Graph Optimization. IEEE Transactions on Vehicular Technology 2022, pp. 1–1. Conference Name: IEEE Transactions on Vehicular Technology. [CrossRef]
Noureldin, A.; Karamat, T.B.; Georgy, J. Fundamentals of Inertial Navigation, Satellite-based Positioning and their Integration; Springer: Berlin, Heidelberg, 2013. [CrossRef]
Morin, P.; Eudes, A.; Scandaroli, G. Uniform Observability of Linear Time-Varying Systems and Application to Robotics Problems. Geometric Science of Information; Nielsen, F.; Barbaresco, F., Eds.; Springer International Publishing: Cham, 2017; Lecture Notes in Computer Science, pp. 336–344. [CrossRef]
Tuna, S.E. Sufficient Conditions on Observability Grammian for Synchronization in Arrays of Coupled Linear Time-Varying Systems. IEEE Transactions on Automatic Control 2010, 55, 2586–2590. Conference Name: IEEE Transactions on Automatic Control. [CrossRef]
NIJMEIJER, H. Observability of autonomous discrete time non-linear systems: a geometric approach. International Journal of Control 1982, 36, 867–874. Publisher: Taylor & Francis _eprint. [CrossRef]
Batista, P.; Silvestre, C.; Oliveira, P. Single range aided navigation and source localization: Observability and filter design. Systems & Control Letters 2011, 60, 665–673. [CrossRef]
Batista, P.; Silvestre, C.; Oliveira, P. Optimal position and velocity navigation filters for autonomous vehicles. Automatica 2010, 46, 767–774. [CrossRef]
Chen, Z.; Jiang, K.; Hung, J. Local observability matrix and its application to observability analyses. [Proceedings] IECON ’90: 16th Annual Conference of IEEE Industrial Electronics Society, 1990, pp. 100–103 vol.1. [CrossRef]
Northardt, T. Observability Criteron Guidance for Passive Towed Array Sonar Tracking. IEEE Transactions on Aerospace and Electronic Systems 2022, 58, 3578–3585. Conference Name: IEEE Transactions on Aerospace and Electronic Systems. [CrossRef]
Wan, E.; Van Der Merwe, R. The unscented Kalman filter for nonlinear estimation. Proceedings of the IEEE 2000 Adaptive Systems for Signal Processing, Communications, and Control Symposium (Cat. No.00EX373), 2000, pp. 153–158. [CrossRef]
Kepper, J.H.; Claus, B.C.; Kinsey, J.C. MEMS IMU and one-way-travel-time navigation for autonomous underwater vehicles. OCEANS 2017 - Aberdeen, 2017, pp. 1–9. [CrossRef]
Groves, P. Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems, Second Edition, 2 ed.; Artech House Publishers, 2013.
Jiang, J.; Yu, F.; Lan, H.; Dong, Q. Instantaneous Observability of Tightly Coupled SINS/GPS during Maneuvers. Sensors 2016, 16, 765. Number: 6 Publisher: Multidisciplinary Digital Publishing Institute. [CrossRef]
Rhee, I.; Abdel-Hafez, M.; Speyer, J. Observability of an integrated GPS/INS during maneuvers. IEEE Transactions on Aerospace and Electronic Systems 2004, 40, 526–535. Conference Name: IEEE Transactions on Aerospace and Electronic Systems. [CrossRef]

Figure 1. The block diagram of the underwater integrated navigation system

Figure 2. The basic application scenario of the moving hydrophone array. If the distance between the moving hydrophone array is long enough (

d i s t a n c e > {(N \times d)}^{2} / λ

), the reaching angle

θ

of the signal at each element can be viewed as the same. So,

θ

is defined as the bearing angle between array and beacon. The interval between adjacent elements is d. N is the number of elements.

λ

is the wavelength of the signal.

Figure 2. The basic application scenario of the moving hydrophone array. If the distance between the moving hydrophone array is long enough (

d i s t a n c e > {(N \times d)}^{2} / λ

), the reaching angle

θ

of the signal at each element can be viewed as the same. So,

θ

is defined as the bearing angle between array and beacon. The interval between adjacent elements is d. N is the number of elements.

λ

is the wavelength of the signal.

Figure 3. The relation of the distance between the vehicle and the beacon at two different moments.

Figure 4. The histogram of the error in the acoustic OWTT range estimates.

Figure 5. The block diagram of the simulation process.

Table 1. Some related studies about underwater detection and navigation

Field	Institution	Research
PASA	Defence Research Establishment Atlantic	Extended Towed Array Method
	Naval Undersea Warfare Center of USA	The model-based space-time array processing approach
	Zhejiang University	Source depth estimation based on synthetic aperture beam- -forming;
OWTT	University of Washington	Real-Time Under-Ice Acoustic Navigation
	University of Rhode Island	Localization using broadband acoustic sources at long-range;
INS/OWTT integrated navigation	Woods Hole Oceanographic Institution	A Navigation Solution Using a MEMS IMU, Model-Based Dead- Reckoning, and OWTT Acoustic Range Measurements;
		Long-Range Acoustic Commun- -ications and Navigation in the Arctic;
		Closed-loop OWTT navigation using low-grade odometry for AUVs;
	Johns Hopkins University	Centralized extended Kalman filter based single beacon INS /OWTT navigation

Table 2. The process of UKF in DOA/Range estimation.

Set up parameters
$N_{x}$ —the dimension of the state vector $X_{k}$	$\{\begin{matrix} G_{m}^{0} = \frac{λ}{N_{x} + λ} \\ G_{m}^{i} = \frac{λ}{2 (N_{x} + λ)} \\ G_{m}^{i + N_{x}} = \frac{λ}{2 (N_{x} + λ)} \\ G_{c}^{0} = \frac{λ}{N_{x} + λ} + 1 + β - α^{2} \\ G_{c}^{i} = \frac{λ}{2 (N_{x} + λ)} \\ G_{c}^{i + N_{x}} = \frac{λ}{2 (N_{x} + λ)} \\ i = 1, 2, 3 . . . N_{x} \end{matrix}$
$N_{y}$ —the dimension of the measurement vector $Z_{k}$
$α$ — a tuning parameter, commonly set to be $10^{- 3}$
$β$ — a tuning parameter, commonly set to be 2
$κ$ — a tuning parameter, a heuristic rule commonly used is to set $N_{x} + κ = 3$
$λ$ — $λ = α^{2} \times (N_{x} + κ) - N_{x}$
$G_{m}^{i}$ — the weight value in state prediction
$G_{c}^{i}$ — the weight value in state error covariance prediction
$Q_{k}$ —the system noise covariance matrix
$R_{k}$ —the measurement noise covariance matrix
$P_{k}$ —-the state error covariance matrix
Generate sigma points
Define $C h o l e s k y (A)$ as the Cholesky decomposition of $A$ .	$P_{k} = U A_{k} V^{T}$ —(SVD)
	$S_{k} = U \cdot C h o l e s k y (A_{k})$

Define ${(A)}_{i}$ as the $i^{t h}$ column of matrix $A$ . The total number of the sigma points is $2 N_{x} + 1$ .	$\{\begin{matrix} X_{k}^{0} = X_{k} \\ X_{k}^{i} = X_{k} + {(\sqrt{N_{x} + λ} S_{k})}_{i} \\ X_{k}^{i} = X_{k} - {(\sqrt{N_{x} + λ} S_{k})}_{i} \end{matrix}$
	$i = 1, 2, 3 . . . N_{x}$
State prediction
Calculate the results after state transformation of all sigma points, and then calculate the weighted sum- -mation.	$X_{k \| k + 1}^{i} = F_{D} (X_{k}^{i})$
	$i = 0, 1, 2, 3 . . . 2 N_{x}$
	$X_{k \| k + 1} = \sum_{i = 0}^{2 N_{x}} G_{m}^{i} X_{k \| k + 1}^{i}$
State error covariance prediction
Predict the State error covariance matrix.	${\tilde{X}}_{k \| k + 1}^{i} = X_{k \| k + 1}^{i} - X_{k \| k + 1}$
	$i = 0, 1, 2, 3 . . . 2 N_{x}$
	$P_{k \| k + 1} = \sum_{i = 0}^{2 N_{x}} G_{c}^{i} {\tilde{X}}_{k \| k + 1}^{i} \cdot {({\tilde{X}}_{k \| k + 1}^{i})}^{T} + Q_{k}$
Update sigma points
Generate the new sigma points after state prediction.	$P_{k \| k + 1} = U A_{k \| k + 1} V^{T}$ —(SVD)
	$S_{k \| k + 1} = U \cdot C h o l e s k y (A_{k \| k + 1})$
	$\{\begin{matrix} Y_{k \| k + 1}^{0} = X_{k \| k + 1} \\ Y_{k \| k + 1}^{i} = X_{k \| k + 1} + {(\sqrt{N_{x} + λ} S_{k \| k + 1})}_{i} \\ Y_{k \| k + 1}^{i} = X_{k \| k + 1} - {(\sqrt{N_{x} + λ} S_{k \| k + 1})}_{i} \end{matrix}$
	$i = 1, 2, 3 . . . N_{x}$
Measurement prediction
Predict the measurement vector by state vector.	$Z_{k \| k + 1}^{i} = H_{D} (Y_{k \| k + 1}^{i}), i = 1, 2, 3 . . . 2 N_{x}$
	$Z_{k \| k + 1} = \sum_{i = 0}^{2 N_{x}} G_{m}^{i} Z_{k \| k + 1}^{i}$
	${\tilde{Z}}_{k \| k + 1}^{i} = Z_{k \| k + 1}^{i} - Z_{k \| k + 1}$
	$P_{k \| k + 1}^{z} = \sum_{i = 0}^{2 N_{x}} G_{c}^{i} {\tilde{Z}}_{k \| k + 1}^{i} \cdot {({\tilde{Z}}_{k \| k + 1}^{i})}^{T} + R_{k}$
	$P_{k \| k + 1}^{x z} = \sum_{i = 0}^{2 N_{x}} G_{c}^{i} {\tilde{X}}_{k \| k + 1}^{i} \cdot {({\tilde{Z}}_{k \| k + 1}^{i})}^{T} + R_{k}$
State update
Update the state by introduce real measurement.	$K_{k + 1} = P_{k \| k + 1}^{x z} {(P_{k \| k + 1}^{z})}^{- 1}$
	${\hat{X}}_{k + 1} = X_{k \| k + 1} + K_{k + 1} (Z_{k + 1} - Z_{k \| k + 1})$
	$P_{k + 1} = P_{k \| k + 1} - K_{k + 1} P_{k \| k + 1}^{z} K_{k + 1}^{T}$

Table 3. The process of UKF in tightly coupled navigation.

Set up parameters
$N_{x 1}$ —the dimension of the state vector ${Xn}_{k}$	$\{\begin{matrix} G_{m}^{0} = \frac{λ}{N_{x 1} + λ} \\ G_{m}^{i} = \frac{λ}{2 (N_{x 1} + λ)} \\ G_{m}^{i + N_{x}} = \frac{λ}{2 (N_{x 1} + λ)} \\ G_{c}^{0} = \frac{λ}{N_{x 1} + λ} + 1 + β - α^{2} \\ G_{c}^{i} = \frac{λ}{2 (N_{x 1} + λ)} \\ G_{c}^{i + N_{x}} = \frac{λ}{2 (N_{x 1} + λ)} \\ i = 1, 2, 3 . . . N_{x 1} \end{matrix}$
$N_{y} 1$ —the dimension of the measurement vector ${Zn}_{k}$
$α, β, κ, λ, G_{m}^{i}, G_{c}^{i},$ — the definition of these parameters are the same as in Table 2
${Qn}_{k}$ —the system noise covariance matrix
${Rn}_{k}$ —the measurement noise covariance matrix
${Pn}_{k}$ —-the state error covariance matrix
Generate sigma points
	${Pn}_{k} = U {An}_{k} V^{T} - - - (S V D)$
	${Sn}_{k} = U \cdot C h o l e s k y ({An}_{k})$
	$\{\begin{matrix} {Xn}_{k}^{0} = {Xn}_{k} \\ {Xn}_{k}^{i} = {Xn}_{k} + {(\sqrt{N_{x 1} + λ} {Sn}_{k})}_{i} \\ {Xn}_{k}^{i} = {Xn}_{k} - {(\sqrt{N_{x 1} + λ} {Sn}_{k})}_{i} \end{matrix}$ $i = 1, 2, 3 . . . 2 N_{x 1}$
State prediction
	${Xn}_{k \| k + 1}^{i} = F_{T C} ({Xn}_{k}^{i}), i = 0, 1, 2, 3 . . . 2 N_{x 1}$ ${Xn}_{k \| k + 1} = \sum_{i = 0}^{2 N_{x}} G_{m}^{i} {Xn}_{k \| k + 1}^{i}$
State error covariance prediction
	${\tilde{Xn}}_{k \| k + 1}^{i} = {Xn}_{k \| k + 1}^{i} - {X n}_{k \| k + 1}$
	$i = 0, 1, 2, 3 . . . 2 N_{x 1}$
	${Pn}_{k \| k + 1} = \sum_{i = 0}^{2 N_{x 1}} G_{c}^{i} {\tilde{Xn}}_{k \| k + 1}^{i} \cdot {({\tilde{Xn}}_{k \| k + 1}^{i})}^{T} + {Qn}_{k}$
Update sigma points
	${Pn}_{k \| k + 1} = U {An}_{k \| k + 1} V^{T}$ —(SVD)
	${Sn}_{k \| k + 1} = U \cdot C h o l e s k y ({An}_{k \| k + 1})$
	$\{\begin{matrix} {Yn}_{k \| k + 1}^{0} = {Xn}_{k \| k + 1} \\ {Yn}_{k \| k + 1}^{i} = {Xn}_{k \| k + 1} + {(\sqrt{N_{x 1} + λ} {Sn}_{k \| k + 1})}_{i} \\ {Yn}_{k \| k + 1}^{i} = {Xn}_{k \| k + 1} - {(\sqrt{N_{x 1} + λ} {Sn}_{k \| k + 1})}_{i} \end{matrix}$
	$i = 1, 2, 3 . . . N_{x 1}$
Measurement prediction
	${Zn}_{k \| k + 1}^{i} = H_{T C} (Y_{k \| k + 1}^{i}), i = 1, 2, 3 . . . 2 N_{x 1}$
	${Zn}_{k \| k + 1} = \sum_{i = 0}^{2 N_{x}} G_{m}^{i} {Zn}_{k \| k + 1}^{i}$
	${\tilde{Zn}}_{k \| k + 1}^{i} = {Zn}_{k \| k + 1}^{i} - {Z n}_{k \| k + 1}$
	${Pn}_{k \| k + 1}^{z} = \sum_{i = 0}^{2 N_{x 1}} G_{c}^{i} {\tilde{Zn}}_{k \| k + 1}^{i} \cdot {({\tilde{Zn}}_{k \| k + 1}^{i})}^{T} + {Rn}_{k}$
	${Pn}_{k \| k + 1}^{x z} = \sum_{i = 0}^{2 N_{x}} G_{c}^{i} {\tilde{Xn}}_{k \| k + 1}^{i} \cdot {({\tilde{Zn}}_{k \| k + 1}^{i})}^{T} + {Rn}_{k}$
State update
Update the state by introducing the real measurement, note that the updating step of ${\hat{Xn}}_{k + 1}$ is different.	${Kn}_{k + 1} = {Pn}_{k \| k + 1}^{x z} {({Pn}_{k \| k + 1}^{z})}^{- 1}$
	${\hat{Xn}}_{k + 1} = {Xn}_{k \| k + 1} + {Kn}_{k + 1} {Zn}_{k + 1}$
	${Pn}_{k + 1} = {Pn}_{k \| k + 1} - {Kn}_{k + 1} {Pn}_{k \| k + 1}^{z} {Kn}_{k + 1}^{T}$

Table 4. The Computational Complexity of UKF

Step	Computational Complexity (N is the dimension of state vector, L is the dimension of measurement vector)
Flops in State Prediction	$\frac{13}{3} N^{3} + 11 N^{2} + \frac{14}{3} N$
Flops in Measurement Update	$\frac{8}{3} L^{3} + 12 N L^{2} + 2 N^{2} L + \frac{15}{2} L^{2} + 34 N L + 4 N^{2}$ $+ \frac{157}{6} L + N + 3$
Flops in SVD	$\frac{8}{3} N^{3}$
Square Roots in Cholesky decomposition	N

Table 5. Simulation Configuration in Motion Restriction Research

Parameter	Value
Number of hydrophone elements	10
Spatial interval of elements	5m
Sample rate of hydrophone	4000Hz
Sample rate of IMU	100Hz
Time of simulation of each iteration	450s
Times of repetition	50
Initial distance between the beacon and the vehicle	50km
Voyage direction	Easst(yaw=90°)
Voyage speed	2m/s
Accuracy of depth sensor	0.1m
Accuracy of Compass	0.1°
SNR	-20dB
Frequency of narrowband signal	100Hz
Broadband signal (a period)	LFM( $130 \sim 230 H z$ , 40s)
$Q_{k}$	$D i a g o n a l (10^{- 7}, 10^{- 8}, 10^{- 6}, 10^{- 9}, 10^{- 4}, 10^{- 5}, 10^{- 6})$
$R_{k}$	$I_{21 \times 21} \cdot 10^{- 3}$
$P_{0}$	$D i a g o n a l (0.1, 0.01, 0.01, 0.01, 0.01, 0.01, 10^{- 8})$
${Qn}_{k}$	$D i a g o n a l (10^{- 10}, 10^{- 10}, 10^{- 8}, 10^{- 8}, 10^{- 10}, 10^{- 10}, 10^{- 10},$ $10^{- 12}, 10^{- 12}, 10^{- 12}, 10^{- 12}, 10^{- 12}, 10^{- 12}, 10^{- 12})$
${Rn}_{k}$	$D i a g o n a l (10^{4}, 10^{4})$
${Pn}_{0}$	$D i a g o n a l (10^{- 8}, 10^{- 8}, 10^{- 6}, 10^{- 6}, 10^{- 8}, 10^{- 8}, 10^{- 10},$ $10^{- 8}, 10^{- 8}, 10^{- 8}, 10^{- 10}, 10^{- 10}, 10^{- 10}, 10^{- 10})$

Disclaimer/Publisher’s Note: The statements, opinions and data contained in all publications are solely those of the individual author(s) and contributor(s) and not of MDPI and/or the editor(s). MDPI and/or the editor(s) disclaim responsibility for any injury to people or property resulting from any ideas, methods, instructions or products referred to in the content.

© 2023 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (http://creativecommons.org/licenses/by/4.0/).

Copyright: This open access article is published under a Creative Commons CC BY 4.0 license, which permit the free download, distribution, and reuse, provided that the author and preprint are cited in any reuse.

Downloads

161

Views

Comments

Subscription

Notify me about updates to this article or when a peer-reviewed version is published.

MDPI Initiatives

Important Links

Choose an area of interest and we will send you notifications of new preprints at your preferred frequency.

Disclaimer