## 1. Introduction

Underwater operations attract great attention for environmental matters and potential resources, as well as scientific interest. Therefore, the need for underwater robotic systems has become more apparent [

1]. With the emergence of inspection-class autonomous underwater vehicles (AUVs), navigation and navigational accuracy are becoming increasingly important in order for the AUV to complete its task. Without an operator in the loop, the vehicle must use sensors to determine its position, velocity and orientation. Most AUVs employ an inertial navigation system (INS) as their main navigation sensor [

2,

3,

4]. This is for many reasons; one of which is that the INS is a standalone system that can provide all of the required navigation data: position, velocity and orientation. However, even with a high-grade INS, the navigation solution drifts in time due to measurement errors of its inertial sensors. Therefore, INSs are usually aided by other external sensors or data such as the Doppler velocity log (DVL) for velocity, magnetometers for heading and depth/pressure sensor for altitude [

5,

6]. For positioning, global positioning system (GPS) is commonly used in land or air vehicles, yet it cannot be used underwater [

7,

8] since the electromagnetic signals decay very quickly in the water. A common method for overcoming the localization problem of underwater vehicles is to use acoustic positioning methods such as long baseline (LBL) or ultra-short baseline (USBL) [

9]. One drawback of such approaches is the need for a priori deployment of beacons. An alternative method for obtaining localization of underwater vehicles is geophysical navigation which uses physical features of the AUV’s environment to produce an estimate of the location of the AUV [

10,

11].

A team at the Technion–Israel Institute of Technology is currently developing an AUV named the Technion autonomous underwater vehicle (TAUV). The TAUV project goal is to develop and to produce a small autonomous underwater vehicle, which will serve as a technology demonstrator and a platform for various research programs. The TAUV diameter is 300 mm and its length is 3000 mm. The TAUV has four rudders installed in an “x” configuration, and its maximum operation depth is 200 m. The TAUV navigation system based on an INS aided by a DVL, magnetometer, pressure sensor (PS), and GPS when available [

12]. When submerged, the main aiding sensor is the DVL which provides velocity measurements that can help estimate, in addition to the vehicle velocity, some of the orientation and inertial sensor error states depending on the vehicle dynamics [

13,

14].

There are two main approaches for sensor fusion between INS and other sensors: (1) loosely coupled (LC); and (2) tightly coupled (TC) [

15]. We shall refer here to INS/DVL fusion, but the underlying principles are the same for other sensors as well. A top-level block diagram of LC and TC approaches is presented in

Figure 1. The raw data of the DVL is the relative velocity in each beam direction. In the LC approach, using parameter estimation the DVL raw data is used to calculate the vehicle velocity, which in turn is compared with its INS counterpart in the navigation filter. The advantage of this method is the simplicity of integration and the ability to combine any off-the-shelf INS with any DVL. However, in order for the DVL to calculate vehicle velocity, it must operate in bottom lock, which refers to the condition when a sufficient number of beam measurements (at least three) are available. In the TC approach, the DVL raw data is directly used in the navigation filter. That is, each beam measurement of the DVL is compared with its calculated INS counterpart and independently integrated into the navigation filter. Therefore, there is no need for a bottom lock stage, and aiding may be applied even with a single beam measurement.

The selected TAUV INS can only receive from the DVL an external velocity vector measurement [

16]. Therefore, INS/DVL fusion with the TC approach is not possible, and thus the LC approach must be used. In some situations—such as operation in close proximity to the seafloor, or when experiencing extreme tilt angle or beam malfunction—one or more of the DVL beams may not provide the reflection required for determining velocity [

17]. In such cases of partial beam measurements, the DVL fails to maintain bottom lock and is unable to estimate the AUV velocity. Thus, the TAUV INS navigation solution will drift in time.

In this paper, we propose the extended loosely coupled (ELC) approach for calculating vehicle velocity when only partial DVL measurements are available. This is made possible by using the partial measured raw data from the DVL combined with additional information. This calculated vehicle velocity is fed back to the TAUV INS and used for aiding the INS, as in the regular LC approach.

Related approaches in similar situations where partial measurements are available can be found in the literature for INS/GPS fusion or LBL positioning. In [

18], a single beacon measurement and two virtual ranging measurements were used to triangulate and solve the vehicle position with the LBL method. Another method is to utilize the knowledge of the vehicle dynamics under a specific scenario. For example, in [

19] the authors utilized the vehicle dynamics in a straight trajectory scenario to improve navigation performance with low-cost GPS. In [

20] the construction of virtual GPS satellites was suggested in order to facilitate GPS receiver position and velocity solutions in cases of partial GPS availability, thereby enabling the fusion of GPS/INS in the LC approach.

In order to examine and demonstrate the improvement of the navigation performance under the ELC approach, we developed the TAUV six degrees of freedom (6DOF) simulation, which includes all functional sub-systems. This simulation consists of the AUV guidance, navigation and control subsystems and uses a complete hydrodynamic model. Simulation results show great improvement in the navigation performance using the proposed approach compared to the standalone TAUV INS solution.

The rest of the paper is organized as follows:

Section 2 describes the TAUV 6DOF simulation and in particular its navigation system. In

Section 3 the ELC approach is derived. In

Section 4 6DOF simulation results are presented to demonstrate the effectiveness of the partial measurements approach on the TAUV navigation system. Finally,

Section 5 provides the conclusions.

## 2. TAUV 6DOF Simulation

A top-level block diagram of the TAUV 6DOF simulation with its main subsystems is presented in

Figure 2. The purpose of each subsystem is discussed next.

**Hydrodynamic and Motion block**: This model calculates the forces and moments applied on the TAUV for the 6DOF rigid body motion model. The motion model solves the vehicle kinematics to produce the true values of the vehicle’s position, velocity, attitude, and acceleration. The model contains a complete hydrodynamic model derived for the TAUV.

**Control system block**: The control system is used to stabilize the vehicle attitude by determining the angles of the four TAUV servos. The vehicle velocity is stabilized by controlling the thruster rotation speed. The TAUV control system contains four proportional integrative derivative (PID) controllers: three for the attitude control and one for the velocity control.

**Guidance system block**: The guidance laws in the TAUV are based on target tracking methods [

21]. This subsystem determines the vehicle steering command and the desired velocity in order to follow the AUV’s desired trajectory.

**Sensors generator block**: The block calculates sensors’ outputs, including their errors, by statistical means. The sensors are: accelerometers, gyros, DVL, PS, magnetometer, and GPS.

**Navigation block**: The navigation block represents the navigation system of the TAUV, including the navigation equations and navigation filter. The TAUV navigation system is presented in detail in the following section.

#### 2.1. TAUV Navigation System

In

Figure 3, a top-level block diagram of the TAUV navigation system is shown. The accelerometer and gyro measurements are integrated into the INS system in order to produce its standalone solution for position, velocity, and attitude. The extended Kalman filter (EKF) [

22,

23,

24] fuses the INS with measurements from the other sensors: the magnetometer, PS, DVL and GPS. By comparing the INS state calculation and aiding the sensor measurements, the EKF estimate corrects the error state vector comprising the INS states and inertial sensor error terms. Notice that each aiding sensor operates at a different sampling rate, yet in each time instance that a measurement arrives, it is fused with the INS.

The navigation equations and the EKF are expressed in a coordinate frame, known as the platform frame. The platform frame is located at the center of buoyancy of the vehicle. The navigation frame is determined by the INS frame. For simplicity of the dynamics equations, we assume that the platform frame and the INS frame are located at the same point at the AUV and with identical orientation. The TAUV platform frame and the tangent frame are represented in

Figure 4. The vehicle velocities

$(u,v,w)$ are expressed in the platform frame, and the vehicle attitudes expressed in the tangent frame by the roll, pitch, and yaw

$(\varphi ,\theta ,\psi )$ Euler angles.

#### 2.1.1. Navigation Filter

An error state EKF is implemented in the TAUV navigation system. The error state vector

$\delta x$ is defined by the difference between the true state vector and Kalman filter estimate and expressed by [

17]:

where k is the time step. The standard INS error state vector is in accordance with [

22]:

where

$\delta {r}_{t/p}^{t}$ is the position error vector expressed in the tangent frame; the vector

$\delta \rho $ is the orientation error also referred to as the frame misalignment error;

$\delta {v}_{t/p}^{p}$ is the velocity error vector expressed in the platform frame;

$\delta {b}_{a}$ is the accelerometer biases error expressed in the platform frame; and

$\delta {b}_{g}$ is the gyro biases error expressed in the platform frame. The INS error state model has the following form:

where

${F}_{INS}\left(t\right)$ is the matrix that relates the error state to the dynamic state [

17]:

${\widehat{R}}_{p}^{t}$ is the estimated transformation matrix from platform frame to tangent frame, and

${\widehat{\mathrm{\Omega}}}_{i/e}^{t}$ is a skew symmetric matrix of the earth’s rotation rate expressed in the tangent frame. The expressions for

${F}_{32}$,

${F}_{33}$ can be found for example in [

17]. The matrix

${G}_{INS}\left(\mathrm{t}\right)$ relating the error state to the noise process is defined by:

The process noises vector is defined by:

where

${\mathrm{w}}_{a}$,

${\mathrm{w}}_{g}$ are the accelerometers and gyros noise vectors are modeled as zero-mean white Gaussian processes,

${\mathrm{w}}_{ba}$ is the noise vector of the accelerometer biases modeled as random walk and

${\mathrm{w}}_{bg}$ is the noise vector of the gyro biases modeled as random walk.

In the linear case, the measurement model is:

where

${n}_{w}$ is zero-mean Gaussian white noise. The matrix

$H$ relates the measurement to the error state and is known as the measurement matrix. For each aiding sensor and for each measurement, a measurement rejection algorithm is applied, by examining if the data is within range of three standard deviations compared to the INS estimation.

In the presented navigation system model, we augment the INS state vector from Equation (2) with five additional error states of the DVL: four to model the DVL biases of each beam, and one for common DVL scale factor error. Thus, the augmented error state vector:

where

$\delta {b}_{DVL}$ is the DVL biases error expressed in the DVL frame and

$S{F}_{DVL}$ is the DVL scale factor error. The corresponding dynamics matrix

F is given by:

The corresponding noise process matrix

G is expressed via:

where the filter process noises vector is modified via:

where

${\mathrm{w}}_{bDVL}$ is a noise vector modeled as random walk of the DVL biases, and

${\mathrm{w}}_{sfDVL}$ is zero-mean Gaussian white noise of the DVL scale factor.

#### 2.1.2. Doppler Solution

The DVL measures the Doppler frequency shift for each depth cell and each beam and then computes the component of relative flow velocity in the direction of each acoustic beam. Based on [

25], the relative velocity of each beam can be assumed as:

where

${F}_{D}$ is the Doppler frequency shift,

${b}_{{F}_{D}}$ is the frequency shift bias,

${n}_{{F}_{D}}$ is the frequency shift noise, C is the velocity of sound in water at transducer face,

${F}_{s}$ is the transmitted acoustic frequency,

$S{F}_{c}$ is the scale factor error of the velocity of sound in water, and

${n}_{{F}_{D}}$ is zero-mean Gaussian white noise. The scale factor is due to the variation of the velocity of sound under different temperatures and salinity level of the water. From Equation (12) we can observe that the DVL measurement model has three main errors: (1) bias error (2) scale factor error and (3) white noise.

The DVL assembly in the TAUV is constructed in an “x” configuration relative to the platform frame (“Janus Doppler configuration”), as shown in

Figure 5. Generally, each DVL beam direction in the vehicle frame is defined by:

where

${b}_{i}$ represents the direction of each transducer/beam of the DVL expressed in the platform frame, for i = 1,2,3,4,

$\tilde{\theta}$ is the pitch angle (relative to platform frame) of each DVL beam, and

${\tilde{\psi}}_{i}$ is the yaw angle (relative to platform frame) of each DVL beam for i = 1,2,3,4. In the TAUV DVL,

$\tilde{\theta}$ is equal to 20°.

The expression for yaw angle of each beam direction in the TAUV can be written as:

The DVL beam velocity is modeled as:

where

${l}_{{D}_{i}}$ is the position vector of each transducer expressed in the platform frame,

${\omega}_{t/p}^{p}$ is the angular rate vector of the platform expressed in the platform frame,

${b}_{DVL}$ is a 4 × 1 vector representing the bias of each beam,

$S{F}_{DVL}$ is the scale factor error, and

${n}_{{v}_{i}}$ is measurement noise. Let

A be a matrix contacting all beam direction vectors:

Thus, by using Equation (15), the DVL-based velocity measurement of the vehicle can be expressed by:

Note that the velocity solution in Equation (17) is based on four beams, of which one beam is for redundancy due to the beam configuration. Therefore, in practice only three beams are enough to calculate the vehicle velocity.

#### 2.1.3. DVL Fusion

In the LC fusion technique, the DVL output to the navigation filter is the platform velocity (Equation (17)). The corresponding measurement matrix is:

and the velocity measurement noise covariance matrix is:

where

$W={R}_{DVL}^{-1}$ and

${R}_{DVL}$ is the DVL measurement noise covariance matrix [

17], defined as:

In the case of small-level arm

${l}_{{D}_{i}}$, Equation (20) can be well approximated as:

where

${\sigma}_{{v}_{i}}$ is the velocity variance for each beam measurement for i = 1,2,3,4. The variance value of each beam is taken from the DVL spec [

26] or calculated in a lab test.

In the TC fusion technique, the DVL raw data is used to aid the INS. That is, each transducer measurement is fused with its INS counterpart (no need to calculate the DVL vehicle velocity, Equation (17)) and the measurement matrix [

17] is defined as:

The measurement noise covariance matrix is taken from Equation (21) for each beam.

## 3. INS/DVL Fusion with the ELC Approach

As mentioned in

Section 1, in some situations several or all of the DVL beams do not provide reflection. With no reflection, the beam cannot provide the relative velocity of the AUV Equation (15). In cases where only one beam is not available, the DVL can still calculate the vehicle velocity Equation (17) but without any redundant data. If two beams are not available, the DVL cannot calculate the vehicle velocity. In this case, no velocity aiding is provided to the TAUV INS.

In this section, we present the ELC approach, with four different methods for utilizing the partial measurements from the DVL (instead of not using them at all) together with external information to construct the DVL based velocity estimate. This velocity is used in the TAUV navigation filter, as illustrated in

Figure 6. The four ELC methods differ in the external information they employ as elaborated in the following subsections.

#### 3.1. Virtual Beam

This method utilizes the raw data from the DVL and the filter prediction of the velocity error in order to solve for the vehicle velocity. Without the loss of generality, we assume that beams #3 and #4 are not available. Let the matrix

${A}_{s}$ relate the velocity in the platform frame to the velocity in each beam direction. So, for small value of lever arm, following Equation (17) the velocities in beam directions (#1, #2, and #3) are:

From Equation (23), the value of the third beam may be found as:

where

${v}_{t/p}^{p}$ is taken from the last INS step. In this method we estimate only the value of the third beam. Generally, the value of the fourth beam can be estimated instead. Plugging Equation (24) into Equation (23) we solve the unknown vehicle velocity vector at time k via:

It is important to note that now, theoretically, a correlation between the measurement noise and process noise exists, since the DVL velocity measurement Equation (25) depends on the DVL partial raw data (with measurement noise) but also on the INS velocity solution Equation (24) (with process noise). Therefore:

Thus, the covariance term Equation (26), which is usually nullified in the EKF formulation, needs to be included. However, in our current analysis we neglect this term.

In order to calculate the corresponding measurement covariance matrix, we need to derive an expression for the constructed third beam measurement standard deviation (STD)

${\sigma}_{v3}$. To that end we use the error covariance matrix

${p}_{k-1}$ (last step of the fusing process), to define the velocity STD by:

Using Equation (24) we derive the expression for

${\sigma}_{v3}$:

where

${b}_{i,j}={b}_{i}(j)$ and

${n}_{f}$ is a factor to compensate for our assumption of neglecting Equation (26). Using the known values of for

${\sigma}_{v1}$ and

${\sigma}_{v2}$ and Equation (28), the DVL measurement noise covariance matrix is:

From Equation (29) the velocity measurement noise covariance matrix is expressed via:

In summary, in the proposed approach we calculate one velocity beam of the DVL via Equation (24). Combining the two beam measurement from the DVL, we solve the velocity vector of the vehicle using Equation (25).

#### 3.2. Nullfying Sway Velocity

In this method we set the sway velocity to be zero, that is

${v}_{k}=0$. This assumption is reasonable for scenarios such as straight line trajectories, which in practice are the AUV trajectories for most of the operating time (although the AUV may be influenced by some disturbances that alter the straight line). Since we assume

v = 0, only the other two velocity components (

u, w) are to be calculated using the partial DVL data. Similar to the previous method outlined in

Section 3.1, we assume that beams #3 and #4 are not available.

This matrix relates the beam velocities #1 and #2 (since beams #3 and #4 are not available) to the vehicle velocity (

u and

w). Thus, the relation between DVL beam velocities

${y}_{v1}$ and

${y}_{v2}$ to vehicle velocities is:

From Equation (32) the surge and heave velocities are:

Note that the matrix

${A}_{0}$ is not singular, because of the independence of the beams direction vectors. The DVL measurement noise covariance matrix is then:

and

From Equation (35) we extract the velocity variances of the calculated vehicle velocity components (

u and

w). Therefore, the velocity measurement noise covariance matrix that is plugged into the navigation filter is:

where

$\epsilon $ is a parameter that reflects the assumption of

v = 0 and has a value close to zero. Equation (36) is used for practical considerations since the TAUV INS requires a complete velocity vector with its corresponding measurement covariance.

In summary, in the proposed approach we assume the sway velocity component is zero. Combining this assumption with the two beams measurements from the DVL, we solve the velocity vector of the vehicle using Equation (33).

#### 3.3. Partial LC Fusing

This method utilizes the DVL setup configuration in order to calculate one component of the AUV velocity,

u or

v, depending on the active transducer order. Therefore, in this approach only one velocity component measurement is fused into the filter. As in the previous approaches, we assume that beams #3 and #4 are not available. When the DVL setup is in an “x” configuration, as in the TAUV, we can derive the following relations between the components of the DVL beam direction vectors based on Equation (13):

If the DVL setup is in a “+” configuration and the available beams are pointing in the same direction, the two components of the velocity vector—

u and

w or

v and

w—can be calculated. If the active beams are not pointing in the same direction (e.g., beam #1 is aimed toward the surge direction and beam #2 toward the sway direction) we cannot utilize any data from the DVL using this method. Here, (under the assumption that only beams #1 and #2 are available), we intend to solve the surge velocity

u, utilizing the partial measurements from the DVL. Using Equation (37) we can express the velocity of beams #1 and #2 as:

The sway velocity is found using Equation (39):

where the corresponding measurement noise covariance is found using Equation (40):

For practical considerations in the TAUV INS, the velocity measurement noise covariance matrix must have a structure of

${R}_{PLCF}\in {\mathbb{R}}^{3\times 3}$. So, the matrix that is passed to the filter is defined by:

Yet, since only the surge velocity, u, is measured, variances

${R}_{v,PLCF}$ and

${R}_{w,PLCF}$, corresponding to sway and heave velocities, are:

To summarize, in the proposed approach we calculate the surge velocity component using the two beam measurements Equation (40). The filter ignores the two other velocity components (sway and heave) by setting their measurement variance to infinity. That is, in practice only one velocity component is used to aid the INS.

#### 3.4. Virtual Heave Velocity

This method is an elaboration of the previous method derived in

Section 3.3, where we extract the surge velocity

u from the DVL raw data, while the DVL allows us access to two measured velocities. In order to utilize the two DVL measurements, we use the velocity prediction from the filter. Therefore, in this approach two vehicle velocity components are used to aid the INS:

u is taken from

Section 3.3, and

v is calculated here. To that end, we sum the two equations from Equation (38) to derive the following expression:

Thus, we have one equation, Equation (44), with two unknown velocity components,

$v$ and

$w$. To calculate them we use the estimated velocity from the navigation filter. Assuming the AUV is travelling in a straight line, the heave velocity (

$w$) estimation is probably more accurate due to the gravitation vector in heave (and even more so if the pressure sensor is active to measure the vehicle depth). Using Equation (44) and

${w}_{k-1}$ from the filter, the sway velocity is:

The corresponding velocity variance component is:

From Equations (41) and (46) the velocity measurement noise covariance matrix that is plugged into the navigation filter is given in Equation (47). As before, for practical considerations in the TAUV INS, the velocity measurement noise covariance matrix must have a structure of

${R}_{VHV}\in {\mathbb{R}}^{3\text{}\times \text{}3}$; thus we set

${R}_{w,VHV}\to \infty $, and the measurement noise covariance matrix is:

where

${R}_{u,PLCF}$ is defined in Equation (41) and

${R}_{v,VHV}$ is defined in Equation (46).

In summary, in the proposed approach we use the surge velocity component calculated as in

Section 3.3. Using the estimated heave velocity component, the sway velocity component is solved via Equation (45). The filter ignores the other velocity component (heave) by setting its measurement variance to infinity. That is, in practice only two velocity components are used to aid the INS.

#### 3.5. Summary of ELC Methods

Table 1 presents a summary of the four ELC methods. VB: virtual beam; NSV: nullifying sway velocity; PLCF: partial loosely coupled fusing; VHV: virtual heave velocity

#### 3.6. ELC Implemenation

The DVL enables access to its raw data and its calculated velocity. In the case of partial measurements, the DVL cannot calculate the vehicle velocity, and only the partial raw data is available. From the raw data, we can calculate the vehicle velocity and the variance from all four approaches as presented in the previous sections. Instead of choosing which approach to use in the TAUV, in Equation (48) we employ a simple selector to pick the best vehicle velocity components out of the four approaches and use each derived corresponding velocity to aid the INS.

The velocity measurement noise covariance matrix (with the corresponding velocity measurement) that goes to the TAUV INS is:

Figure 7 shows the implementation of the ELC approach and LC approach in the TAUV. The red lines are relevant only in partial measurement scenarios.