Navigation Filter

  • Francesco FanelliEmail author
Part of the Springer Theses book series (Springer Theses)


This chapter is dedicated to the AUV navigation filter developed during the Ph.D. period. Such filter is able to estimate the complete pose of an UUV fusing the measurements of onboard sensors in real-time. In particular, a parallel structure has been chosen (Fig. 4.1): attitude is estimated independently using IMU, compass, and FOG data, and constitutes an input for the position estimation filter.

This chapter is dedicated to the AUV navigation filter developed during the Ph.D. period. Such filter is able to estimate the complete pose of an UUV fusing the measurements of onboard sensors in real-time. In particular, a parallel structure has been chosen (Fig. 4.1): attitude is estimated independently using IMU, compass, and FOG data, and constitutes an input for the position estimation filter. The starting point is the estimator proposed in [2, 6]: sensor data and a mixed kinematic/dynamic vehicle model are exploited within an UKF in order to estimate a twelve-dimensional state composed of \(\varvec{\eta }\) and \(\varvec{\nu }\). However, the experimental activity carried out at the MDMLab highlighted the high sensitivity of magnetometers with respect to operation in the underwater environment: indeed, many typical tasks executed under the surface involve proximity to sources of magnetic disturbances (e.g., navigation close to industrial platforms or metal debris due to modern wrecks encountered during seabed inspection); since magnetometers are commonly used to estimate the heading of a vehicle, the presence of perturbed magnetic fields may possibly lead to large navigation errors.

In order to maintain a satisfying level of navigation accuracy, a dedicated strategy aiming at evaluating the validity of magnetometer output, using disturbance-free measurements only and recognizing and rejecting corrupted compass readings, providing at the same time a solution to estimate the three-dimensional attitude of an UUV without resorting to erroneous compass data, has been developed during the Ph.D. period. The resulting estimator, described in details in Sect. 4.1, allows the reduction of the dimension of the state vector with respect to [2, 6], since attitude can be accurately estimated by a standalone filter and then input into the position estimation filter. The latter consists in an UKF-based algorithm, exploiting GPS (on surface), depth, and velocity measurements in order to compute an estimate of the absolute position of the vehicle.

Additionally, the derived navigation solution has been improved by including the online estimation of sea currents acting on the vehicle. As stated in Sect.  3.1.1, neglecting the presence of currents is likely to negatively affect the performance of the navigation filter. In this work, sea current is estimated by adding a suitable current model within the state-space model of the whole system; an estimate of the Earth-fixed horizontal current components (see Fig. 4.1, where \(\varvec{\nu }_{c,h}^{N}\) represents the first two components of \(\varvec{\nu }_{c}^{N}\)) is computed on board, without the need of additional sensors (such as ADCPs), which would increase the overall cost of the system.
Fig. 4.1

Navigation filter block scheme

Following the logic of the block scheme of Fig. 4.1, this chapter is organized as follows: in Sect. 4.1, the above-mentioned attitude estimation filter is presented in details; in addition, a magnetometer calibration procedure applicable when the rotation of the vehicle is limited (e.g., for many typical AUVs, whose attitude is usually constrained within a neighborhood of the horizontal plane) is proposed. Calibration must indeed be performed prior to the estimation process, in order to increase the reliability of compass readings. Then, in Sect. 4.2, the position estimation filter is described; in particular, Sect. 4.2.1 presents the state-space vehicle model the UKF-based filter is applied to, while Sect. 4.2.2 shows how it is possible to modify such model in order to take into account the effects of local marine currents on vehicle dynamics, allowing for their online estimation without requiring additional instrumentation; finally, an observability analysis is provided in order to justify the proposed approach.

4.1 Attitude Estimation Filter

The attitude estimation filter developed during the Ph.D. period is based on the Nonlinear Explicit Complementary Filter (NECF) proposed in [15], whose name derives from the structural similarity of the algorithm to linear complementary filters. In recent years, the NECF has become a standard reference in the field of attitude estimation; furthermore, it has been proven to converge to real orientation (a formal proof, based on Lyapunov theory, is provided by the authors in [15]).

Let us assume that the vehicle whose orientation has to be estimated (with respect to the Earth-fixed frame) is equipped with a device composed of a triaxial gyroscope, a triaxial accelerometer, and a triaxial magnetometer (which will be commonly referred to as an IMU and compass system); additionally, let us assume that such device is rigidly fixed to the vehicle, so that the term sensor frame may be equivalently used to indicate the body-fixed frame, since the two differ at maximum by a constant rotation describing the sensor mounting. At each iteration of the NECF, an estimate of the orientation of the sensor is computed; the basic principle is to integrate the angular rate changes measured by the gyroscopes about the body-frame axes, and to correct the obtained values exploiting accelerometers and magnetometers readings. If at least two fixed and nonparallel Earth frame directions can be measured in the sensor frame, the algorithm converges to the exact attitude. For the considered system, this condition is satisfied: particularly, assuming that the proper acceleration of the vehicle is negligible (as it is usually the case with underwater robots) accelerometer readings give an estimate of the direction of the gravitational acceleration, and they are employed to correct roll and pitch gyro integration; magnetometers, on the other hand, measure the direction of Earth’s magnetic field (i.e. the direction of magnetic North), and thus they can be used to compute an estimate of the yaw angle. Furthermore, the NECF presented in [15] is able to estimate the time-varying bias of the gyroscope.

The filter is a dynamical system governed by the following equations:
$$\begin{aligned}&\dot{\hat{R}}^{N}_{B}=\hat{R}^{N}_{B} \left( S\left( \varvec{\omega }_{IMU}^{B} - \hat{\mathbf{b }}_{g}\right) + k_{P}S\left( \varvec{\omega }_{c} \right) \right) \ , \ \ \ \hat{R}^{N}_{B}\left( t=0 \right) = \hat{R}^{N}_{B,0} \ , \end{aligned}$$
$$\begin{aligned}&\dot{\hat{\mathbf{b }}}_{g}=-k_{I}\varvec{\omega }_{c} \ , \end{aligned}$$
$$\begin{aligned}&\varvec{\omega }_{c}=\sum _{i=1}^{n} k_{i}{} \mathbf v _{i}^{B} \times \hat{\mathbf{v }}_{i}^{B} \ , \ \ \ k_{i}\ge 0 \ , \ \ \ i=1,\ldots ,n \ . \end{aligned}$$
In (4.1), the attitude estimate \(\hat{R}^{N}_{B}\) is computed starting from \(\hat{R}^{N}_{B,0}\) exploiting the sum of a term proportional to gyro measured angular rate \(\varvec{\omega }^{B}_{IMU}\) and gyro estimated bias \(\hat{\mathbf{b }}_{g}\) and a correction term based on accelerometers and magnetometers readings (being the two terms weighted by the positive gains \(k_{P}\), \(k_{I}\)).
To give more details about the correction term, let \(\mathbf v _{i}^{N}\), with \(i=1,\ldots ,n\), be a set of known fixed-frame directions, and let
$$\begin{aligned} \mathbf v _{i}^{B}=\left( R^{N}_{B} \right) ' \mathbf v _{i}^{N}+\varvec{\delta }_{i} \end{aligned}$$
$$\begin{aligned} \hat{\mathbf{v }}_{i}^{B}=\left( \hat{R}^{N}_{B}\right) ' \mathbf v _{i}^{N} \end{aligned}$$
denote, respectively, its measurement in the sensor frame (affected by noise \(\varvec{\delta }_{i}\)) and its estimate (computed using the estimated rotation matrix \(\hat{R}^{N}_{B}\)); then, the term \(\varvec{\omega }_{c}\) is the weighted sum of the misalignments between the measured directions and their estimates output by the filter (computed as the cross product between each pair of vectors). In particular, in the considered case the direction of the vertical axis (measured by accelerometers) and the direction of the magnetic field (read by magnetometers) are taken into account; the weights \(k_{i}\) are chosen according to the relative confidence in each measurement \(\mathbf v _{i}^{B}\).

Regarding convergence, let \(\tilde{R}^{N}_{B}=\left( \hat{R}^{N}_{B}\right) ' R^{N}_{B}\) and \(\tilde{\mathbf{b }}_{g}=\mathbf b _{g}-\hat{\mathbf{b }}_{g}\) denote, respectively, the orientation and the gyro bias errors; then, for \(n>1\), through the use of Lyapunov theory it has been proven in [15] that \(\left( \tilde{R},\tilde{\mathbf{b }}_{g} \right) \) is locally exponentially stable to \(\left( I,\mathbf 0 \right) \).

However, field experiments carried out using the NECF in its original formulation (4.1)–(4.3), using a commercial IMU and compass system to estimate the attitude of an underwater vehicle, highlighted some issues related to the particular field of application and whose effects are especially relevant in the case of corrupted magnetic field measurements. Hence, a set of design changes, justified by the characteristics of the underwater environment an by the limitations imposed by the adopted sensors, have been applied to the original structure of the filter in order to better adapt it to the underwater field. The beneficial effect of such modifications has then been verified with additional experimental testing. The resulting filter [4, 7] has proven to be able to compute an accurate attitude estimate, being at the same time resilient to magnetic field disturbances. At the moment of writing, it is employed as the standard attitude estimator on board the AUVs of the University of Florence, and its output constitutes the input to the position navigation algorithm.

In view of the above-mentioned considerations, the remaining of this section is organized as follows: at first, the procedure adopted to calibrate the onboard magnetometer is presented; calibration is indeed necessary in order to compensate those magnetic disturbances which rotate with the vehicle. Then, the applied design changes to the original formulation of the NECF are illustrated and justified.

4.1.1 Magnetometer Calibration

A triaxial magnetometer measures the direction and the intensity of the total magnetic field around the device. It cannot, however, distinguish between Earth’s magnetic field and additive magnetic disturbances.

The direction of Earth’s magnetic field depends on the geographical location, and several calculators exploit one of the available geomagnetic field models [19] to determine its components from latitude and longitude. In this context, it is assumed that, given the restricted dimensions of a typical operational area of an AUV, Earth’s magnetic field is constant with respect to the NED frame. In the absence of disturbances, the measurements (expressed in the body frame) obtained by arbitrarily rotating a magnetometer in every possible orientation would lie on the surface of a sphere with its center in the origin and whose radius is the magnitude of the field at the geographical location where such rotation is performed. However, in the presence of magnets or ferromagnetic objects, the measurements locus is shifted and deformed.

Magnetic disturbances conceptually fall into two different categories: external (environmental) disturbances, and disturbances rotating with the sensor. As regards the latter, they can be further characterized as Hard Iron or Soft Iron disturbances, whose effect is the following:
  • Hard Iron disturbances: permanent magnets and magnetized objects, such as electronic subsystems in the proximity of the sensor, give rise to the so-called Hard Iron effect: these objects are the source of a permanent magnetic field, constant in all directions, whose effect is the addition of a constant bias \(\mathbf H _{d}\) on the magnetometer output (see  3.37);

  • Soft Iron, scale factor and misalignment disturbances: using the simplified affine model of ( 3.37), matrix W can be factored as follows:
    $$\begin{aligned} W=W_{mis}W_{sf}W_{SI} \ . \end{aligned}$$
    \(W_{mis}\) takes into account the misalignment between sensor axes and vehicle axes, including a non perfect orthogonality between the three axes of the magnetometer; \(W_{sf}\) models the different sensitivity of the device on its three axes, introducing a different scaling factor along the three directions; finally, \(W_{SI}\) represents the Soft Iron effect: ferromagnetic materials close to the sensor, such as iron and nickel, produce a local magnetic field, whose magnitude is related to the angle of incidence of Earth’s magnetic field on the material itself. Thus, this effect changes as the orientation of the sensor varies. All these contributions, combined into W, have the effect of deforming the measurements sphere into an ellipsoid, arbitrarily tilted in space.
If both kinds of disturbance are present, measurements taken while rotating the sensor in space would lie on the surface of an ellipsoid (due to W) centered at a certain offset from the origin (due to \(H_{d}\)).
Fig. 4.2

Magnetometer measurements in the absence of magnetic disturbances

Fig. 4.3

Magnetometer measurements in the presence of magnetic disturbances

For instance, Figs. 4.2 and 4.3 show the magnetometer measurements (expressed in the sensor frame) acquired by the triaxial magnetometer of a Xsens MTi-300 AHRS taken while rotating such device in space. In the first case, the rotation was performed in a disturbance-free environment; sensor readings lie with good approximation on the surface of a sphere centered at the origin of the axes. The radius of the sphere does not reflect the true magnitude of the magnetic field, since the measurements are given by the employed sensor in arbitrary units. On the other side, measurements shown in Fig. 4.3 were taken in the same location as in the previous test, but metal objects were preliminarily attached to the sensor case: it is easy to note the deformation of the previously obtained sphere into an ellipsoid and the shift its center is subjected to.

While external disturbances cannot be predicted (requiring a suitable identification and rejection strategy, such as the one proposed in Sect. 4.1.2), magnetic disturbances rotating with the sensor can be completely compensated exploiting suitable calibration procedures. The most common techniques are based on ellipsoid fitting methods [16]: after collecting measurements while rotating the sensor in a sufficient number of different orientations in space, the best fitting ellipsoid (in a least square sense) is determined [10]; once its center, radii and axes are known, the offset \(\mathbf H _{d}\) and the scaling W can be corrected. Other commonly used techniques involve optimization: for example, in [14, 22], the calibration process is cast as a maximum likelihood problem, and is solved using optimization tools.

While these techniques are computationally efficient and provide satisfying performance in a large number of situations, they are not applicable when sensor rotation about one or more axes is constrained. This is usually the case of the considered field of application: roll and pitch angles of an AUV are typically limited to a strict range. This restriction implies that the set of measurements to be used for the calibration procedure does not usually include enough information content along one space dimension (the vertical direction) to completely identify the shape of the ellipsoid.

Tests in a controlled environment have been conducted to verify if, by executing a complete z-axis turn of the vehicle, measurements would be sufficient to correctly fit the data to an ellipsoid. However, even in the presence of currents, which cause a rotation of the vehicle out of the horizontal plane, ellipsoid fitting has not been proven to be a reliable technique to estimate the shape of the locus of magnetometer measurements. This reflects the impossibility of identifying an ellipsoid from an (approximately) elliptical data set. Hence, a simple and fast calibration technique which could approximate at best the theoretical locus on which the uncorrupted measurements lie, using only data (which roughly lie on a plane) acquired during a complete turn of the vehicle about its z-axis, has been derived.

During the calibration turn, it is especially important that the roll and the pitch angles of the vehicle remain close to zero; to ensure this condition, the following characteristics are exploited:
  • Geometrical shape: long and slender AUVs (torpedo-shaped AUVs) naturally dampen pitch rotations;

  • Pitch control: MARTA and Typhoon AUVs actively control the pitch degree of freedom, thus guaranteeing limited values for such angle if proper reference signals are applied;

  • Hydrostatic stability: generally speaking, the correct positioning and alignment of the centers of buoyancy and gravity ensures limited roll and pitch motions and increases the overall stability of the vehicle.

The idea behind the developed calibration algorithm is to perform a planar ellipse fit (the only one available when roll and pitch angles are constrained) on the data set constituted by the complete magnetometer readings. The derived technique is exact if the measurements are taken while the vehicle is horizontal, and remains sufficiently reliable for small roll and pitch angles values (within the usual ranges for an AUV).

Let us assume that the x-axis of the NED reference frame is pointing towards geodetic North; then, on every point of Earth’s surface (exception made for the magnetic poles) Earth’s magnetic field has three nonzero components: one in the North-South direction (\(H^{N}_{x}\)), one in the East-West direction (\(H_{y}^{N}\)), and a component directed towards Earth’s center (vertical component \(H^{N}_{z}\)). Since these components are known at each location, and can be easily computed using suitable calculators (e.g., [19]), the locus given by measurements taken on the horizontal plane is a well identifiable circle on the theoretical magnetic field sphere: it is indeed a circle whose axis is parallel to the z-axis of the sensor, whose center is on such axis at the height of \(H^{N}_{z}\) and whose radius is given by
$$\begin{aligned} r_{H,h}=\sqrt{(H^{N}_{x})^{2}+(H^{N}_{y})^{2}} \ . \end{aligned}$$
Figure 4.4 is given for a better understanding: if both Hard and Soft Iron disturbances are present, the circle in Fig. 4.4 is mapped onto an ellipse which is arbitrarily rotated in space (see Fig. 4.5).
Fig. 4.4

Magnetic field measurements on the horizontal plane

The idea behind the proposed calibration procedure is to compare the theoretical circular locus of measurements on the horizontal plane with the ellipse it is deformed into by magnetic disturbances, in order to determine the transformation that maps the former onto the latter and to compensate the disturbances by applying the inverse transformation to magnetometer readings.

The developed algorithm is composed of the following steps: firstly, the measurements ellipse is rotated so that it lies on a horizontal plane; then, a planar ellipse fitting procedure is applied to determine the center, the radii and the tilt of the resulting ellipse; finally, these quantities are compensated and measurements are mapped back onto the theoretical circle.
Fig. 4.5

Magnetic field measurements locus deformation in the presence of both Hard and Soft Iron magnetic disturbances

In practice, not all the measurements are exactly acquired at zero pitch and roll; hence, magnetometer readings do not perfectly lie on a plane. The first step of the calibration algorithm consists in computing the minimal distance plane (in a least square sense) \(\Pi \) that best fits all the points (Fig. 4.6a).
Fig. 4.6

Magnetometer calibration procedure

Such calculation can be executed resorting to linear algebra. Consider the standard plane equation:
$$\begin{aligned} \mathbf P _{\Pi }'{} \mathbf C _{\Pi }=0 \ , \end{aligned}$$
where \(\mathbf P _{\Pi }=\left[ x_{\Pi } \ y_{\Pi } \ z_{\Pi } \ 1 \right] '\) is a point on the plane, represented as an augmented vector, and \(\mathbf C _{\Pi }=\left[ a_{\Pi } \ b_{\Pi } \ c_{\Pi } \ d_{\Pi } \right] '\) are the coefficients defining the plane (i.e. the unknowns of the problem). Given \(N_{\Pi }\) points (which, in the considered case, will be the compass readings), the following linear system can be written:
$$\begin{aligned} \tilde{P}_{\Pi }{} \mathbf C _{\Pi }=\mathbf e _{\Pi } \ , \end{aligned}$$
$$\begin{aligned} \tilde{P}_{\Pi }=\begin{bmatrix} \tilde{x}_{1}&\tilde{y}_{1}&\tilde{z}_{1}&1 \\ \vdots&\vdots&\vdots&\vdots \\ \tilde{x}_{i}&\tilde{y}_{i}&\tilde{z}_{i}&1 \\ \vdots&\vdots&\vdots&\vdots \\ \tilde{x}_{N_{\Pi }}&\tilde{y}_{N_{\Pi }}&\tilde{z}_{N_{\Pi }}&1 \\ \end{bmatrix} \ , \end{aligned}$$
and each \(\left[ \tilde{x}_{i} \ \tilde{y}_{i} \ \tilde{z}_{i} \right] '\) a different measurement. \(\mathbf e _{\Pi } \in \mathbb {R}^{N_{\Pi }}\) is a vector representing the error to be minimized (if all points lie on the plane, then \(\mathbf e _{\Pi }\) is the null vector). The solution of such system is the closest vector to the kernel of \(\tilde{P}_{\Pi }\), which is given by the right-singular vector corresponding to the minimum singular value obtained from the singular value decomposition of \(\tilde{P}_{\Pi }\).
Once the best fitting plane has been determined, its normal vector is given by:
$$\begin{aligned} \mathbf n _{\Pi }=\left[ a_{\Pi } \ b_{\Pi } \ c_{\Pi } \right] ' \ , \end{aligned}$$
and any point \(\mathbf P _{\Pi }\) can be found by assigning two coordinates and determining the third one by inversion of (4.8). The projection of a generic magnetometer reading \(\mathbf m _{i}^{B}\) on \(\Pi \) is then given by:
$$\begin{aligned} \mathbf m _{i}^{\Pi }=\mathbf m _{i}^{B}-\left( \mathbf n _{\Pi }' \left( \mathbf m _{i}^{B}-\mathbf P _{\Pi } \right) \right) \mathbf n _{\Pi } \ . \end{aligned}$$
The following step consists in computing the necessary rotation to align \(\Pi \) with the horizontal plane. The “outwards” pointing normal \(\mathbf n _{\Pi }^{out}\) (pointing towards the half-space which does not contain the origin of the axes) is determined: since Eq. (4.8) is invariant with respect to a constant scale factor, \(\mathbf n _{\Pi }^{out}\) can be either \(\mathbf n _{\Pi }\) or \(-\mathbf n _{\Pi }\); hence, (4.8) cannot be used to discern the correct direction of \(\mathbf n _{\Pi }^{out}\). This indeterminacy can be solved by computing \(\mathbf n _{\Pi }^{out}\) as follows:
$$\begin{aligned} \mathbf n _{\Pi }^{out}=\pm \frac{\left( \left( \mathbf m _{3}^{\Pi }-\mathbf m _{1}^{\Pi }\right) \times \left( \mathbf m _{2}^{\Pi }-\mathbf m _{1}^{\Pi }\right) \right) }{\left| \left| \left( \mathbf m _{3}^{\Pi }-\mathbf m _{1}^{\Pi }\right) \times \left( \mathbf m _{2}^{\Pi }-\mathbf m _{1}^{\Pi }\right) \right| \right| } \ . \end{aligned}$$
Fig. 4.7

Computation of \(\mathbf n _{\Pi }^{out}\)

The sign is chosen positive if \(\mathbf m _{1}^{\Pi }\), \(\mathbf m _{2}^{\Pi }\), and \(\mathbf m _{3}^{\Pi }\) lie counter-clockwise on the ellipse (see Fig. 4.7); this condition can be verified considering the sign of the gyroscope angular rate during the calibration turn.

Let \(\mathbf z ^{N}=[0 \ 0 \ 1]'\) denote the direction of the fixed z-axis; in order to align \(\Pi \) with the horizontal plane, it is necessary to rotate such plane of the angle given by:
$$\begin{aligned} \theta _{\Pi }=cos^{-1}\left( \left( \mathbf z ^{N} \right) '{} \mathbf n _{\Pi }^{out}\right) \end{aligned}$$
about the axis whose direction can be computed with the cross product:
$$\begin{aligned} \mathbf a _{\Pi }=\mathbf n _{\Pi }^{out} \times \mathbf z ^{N} \ . \end{aligned}$$
\(\theta _{\Pi }\) and \(\mathbf a _{\Pi }\) can be used to compute a suitable rotation matrix \(R(\theta _{\Pi },\mathbf a _{\Pi })\) resorting to axis-angle parametrization of rotations [21], which is then applied to the projected points (Fig. 4.6b):
$$\begin{aligned} \mathbf m _{i}^{h}=R(\theta _{\Pi },\mathbf a _{\Pi })\mathbf m _{i}^{\Pi } \ . \end{aligned}$$
In (4.16), the superscript \(^{h}\) denotes measurements parallel to the horizontal plane (referred to as horizontal measurements in the following).
Then, a standard ellipse fitting procedure can be applied to determine the best fitting ellipse (in a least square sense) to the points (Fig. 4.6c). Without going into details (see [9, 11] for theoretical proof), given a point \(\mathbf P _{E}=\left[ x_{E} \ y_{E} \right] '\), it lies on the ellipse defined by coefficients \(\mathbf C _{E}=\left[ a_{E} \ b_{E} \ c_{E} \ d_{E} \ e_{E} \ f_{E} \right] '\) if the following equation holds true:
$$\begin{aligned} a_{E}x_{E}^{2}+b_{E}x_{E}y_{E}+c_{E}y_{E}^{2}+d_{E}x_{E}+e_{E}y_{E}+f_{E}=0 \ . \end{aligned}$$
For a set of \(M_{E}\) different points \(\mathbf m _{i}^{h}=[m_{i,x}^{h} \ m_{i,y}^{h} \ m_{i,z}^{h}]^{T}, \ i=1,\ldots ,M_{E}\), the best fitting ellipse is obtained by solving the following generalized eigenvalue problem:
$$\begin{aligned}&S_{E}{} \mathbf C _{E}=\lambda _{E} K_{E} \mathbf C _{E} \end{aligned}$$
$$\begin{aligned}&\mathbf C _{E}^{T}K_{E}{} \mathbf C _{E}=1 \ , \end{aligned}$$
where \(\lambda _{E}\) is a scalar value, \(S_{E}\) is the scatter matrix built from measurements:
$$\begin{aligned}&S_{E}=\tilde{P}_{E}'\tilde{P}_{E} \in \mathbb {R}^{6\times 6} \end{aligned}$$
$$\begin{aligned}&\tilde{P}_{E}=\begin{bmatrix} (m_{1,x}^{h})^{2}&m_{1,x}^{h}m_{1,y}^{h}&(m_{1,y}^{h})^{2}&m_{1,x}^{h}&m_{1,y}^{h}&1 \\ \vdots&\vdots&\vdots&\vdots&\vdots&\vdots \\ (m_{i,x}^{h})^{2}&m_{i,x}^{h}m_{i,y}^{h}&(m_{i,y}^{h})^{2}&m_{i,x}^{h}&m_{i,y}^{h}&1 \\ \vdots&\vdots&\vdots&\vdots&\vdots&\vdots \\ (m_{M_{E},x}^{h})^{2}&m_{M_{E},x}^{h}m_{M_{E},y}^{h}&(m_{M_{E},y}^{h})^{2}&m_{M_{E},x}^{h}&m_{M_{E},y}^{h}&1 \\ \end{bmatrix} \ , \end{aligned}$$
and \(K_{E}\) is the constant matrix defined as follows:
$$\begin{aligned} K=\begin{bmatrix} 0&0&2&0&0&0 \\ 0&-1&0&0&0&0 \\ 2&0&0&0&0&0 \\ 0&0&0&0&0&0 \\ 0&0&0&0&0&0 \\ 0&0&0&0&0&0 \\ \end{bmatrix} \ . \end{aligned}$$
Once the components of \(\mathbf C _{E}\) have been determined, the center \(\left[ C_{x} \ C_{y} \right] '\), the radii \(\left[ R_{x} \ R_{y} \right] '\), and the tilt \(\epsilon \) of the ellipse (the latter indicating the directions the ellipse is scaled along) can be determined; once these quantities are known, its points can be mapped onto the desired circle using the following transformation:
$$\begin{aligned} \mathbf m _{i}^{c}&=\left( R_{z^{N}}\left( \epsilon \right) \begin{bmatrix} r_{H,h}/R_{x}&0&0 \\ 0&r_{H,h}/R_{y}&0 \\ 0&0&1 \end{bmatrix} \left( R_{z^{N}}\left( \epsilon \right) \right) ' \left( \mathbf m _{i}^{h} - \begin{bmatrix} C_{x} \\ C_{y} \\ 0 \end{bmatrix} \right) \right) + \nonumber \\&- \begin{bmatrix} 0 \\ 0 \\ m^{h}_{i,z}-H_{z}^{N} \end{bmatrix} \ , \end{aligned}$$
where \(R_{z^{N}}\left( \epsilon \right) \) denotes a rotation of an angle \(\epsilon \) about the fixed z-axis. At the end of the calibration process, the corrected (calibrated) compass measurements \(\mathbf m _{i}^{c}\) lie on a circle with radius \(r_{H,h}\) and whose center is on the z-axis at the height of \(H_{z}^{N}\) (Fig. 4.6d).

In order to validate the developed calibration procedure several tests have been conducted, where the algorithm has been employed to calibrate a compass in both disturbance-free and magnetically perturbed environments; the results of these tests are reported in Chap.  5.

4.1.2 Attitude Estimator Design Changes

As stated at the beginning of Sect. 4.1, the use of the standard formulation of the NECF with a commercial IMU and compass system to estimate the attitude of an AUV may give unsatisfying performance; hence, a set of design changes applied to better adapt the algorithm for use in the underwater field are illustrated and justified in this section.

Filter on Accelerometer Measurements

Through suitable preliminary tests performed out of the water, it has been experimental observed that oscillating movements of the IMU on the horizontal plane cause undesired variations of the roll and pitch angles extracted from the rotation matrix output by the filter; this happens because the accelerometer interprets sudden horizontal movements of the sensor as deviations of the vertical direction, which in turn affect the computation of the rotation matrix. To produce accurate attitude estimates, this phenomenon must be identified and corrected in real-time. To smooth the obtained roll and pitch angles profiles and to reduce the amplitude of these variations, accelerometer measurements have been filtered as follows:
$$\begin{aligned} \mathbf a _{f}=F_{a}\left( z \right) \mathbf a _{IMU}^{B} \ , \end{aligned}$$
being \(\mathbf a _{f}\) the filtered measurement, and \(F_{a}\left( z \right) \) the transfer function of a suitable digital filter. Particular care has to be taken when choosing the filter order and its cutoff frequency, since a wrong choice of such parameters could result in an undesirable estimate delay. Several filters with different cutoff frequencies have been tested; a second-order filter has then been adopted for the final version of the algorithm, constituting an effective trade-off between estimate accuracy and readiness of the NECF. In particular, \(F_{a}\left( z \right) \) is obtained by discretization of the filter with continuous transfer function:
$$\begin{aligned} F_{a}\left( s \right) =\frac{f_{a}}{\left( s+f_{a} \right) ^{2}} \end{aligned}$$
using the bilinear transform
$$\begin{aligned} F(z)=F(s)|_{s=\frac{2z-1}{\Delta Tz-1}} \ , \end{aligned}$$
being \(f_{a}\) the cutoff frequency, and \(\Delta T\) the time between two subsequent filter iterations. Additionally, since only the direction of the acceleration vector is needed, \(\mathbf a _{IMU}^{B}\) is normalized before being filtered by \(F_{a}\left( z \right) \); the same operation is executed on \(\mathbf a _{f}\) before its use in \(\varvec{\omega }_{c}\) (notation has not been changed for the ease of reading).

Known Directions Choice

Convergence of the NECF has been demonstrated if the direction of at least two known nonparallel fixed vectors can be measured in sensor frame. Considering devices equipped with accelerometers and magnetometers, a common choice is to use the measurements of gravitational acceleration (i.e. the vertical direction) and of Earth’s magnetic field. These quantities are employed in the correction term \(\varvec{\omega }_{c}\), where they are compared with their estimates to generate the correction term.
Fig. 4.8

Projection of the measured magnetic field on the plane orthogonal to acceleration

The proposed modification consists in choosing only the component of the measured magnetic field which is orthogonal to the direction of acceleration instead of the complete magnetometer measurement (refer to Fig. 4.8; a similar approach can be found in [13, 17]). This choice is justified by the following considerations: first of all, it is worth noting that the plane orthogonal to acceleration approximately coincides with the horizontal plane all the time; this is because the proper acceleration of the vehicle is often negligible if compared with gravity, and because rotations about the x-and y-axis are usually constrained. This means that accelerometer readings measure the body frame direction of the fixed vertical axis \(\mathbf z ^{N}\). In addition, accelerometer measurements are more reliable than magnetometer readings, since they are less susceptible to external error sources. Hence, even if no external magnetic disturbance is present, including the vertical component of the measured magnetic field does not produce further benefits with respect to only using the information given by accelerometers. Thus, once the corrected compass readings \(\mathbf m ^{c}\) have been obtained through calibration, their projection onto the plane orthogonal to acceleration is computed:
$$\begin{aligned} \mathbf m ^{c}_{\bot }=\mathbf m ^{c}-\left( \left( \mathbf a _{f}\right) ' \mathbf m ^{c}\right) \mathbf a _{f} \ . \end{aligned}$$
Thanks to the previous observations, \(\mathbf m ^{c}_{\bot }\) (once expressed in the fixed frame) is a vector pointing towards magnetic North; thus, it is compared in \(\varvec{\omega }_{c}\) with the estimate:
$$\begin{aligned} \hat{\mathbf{H }}_{\bot }^{N,B}=\left( \left( R^{N}_{B} \right) ' \mathbf H ^{N}\right) _{\bot } \ , \end{aligned}$$
where subscript \(_{\bot }\) indicates the same projection operation applied to the measurements; additionally, the same considerations made for measured acceleration (concerning normalization) hold.

Time Varying Gains

In Eq. (4.3), the gains \(k_{i}\) of the correction term are constant and fixed before execution. However, during the normal functioning of the filter, unpredictable transitory errors may affect the measurements provided by accelerometers or magnetometers. These dangerous situations should be detected in real-time and the corresponding gains should be scaled according to the actual reliability of each measurement, in order to preserve the accuracy of the computed estimate.

Hence, after setting a suitable initial value for each gain, the latter are eventually scaled over time (never exceeding the initial value) so as to reflect the real-time reliability of each measurement. Since in the underwater field proper vehicle accelerations are usually small, while it is quite common to encounter sources of magnetic disturbance, acceleration measurements will be, generally speaking, more reliable than compass readings; in view of this consideration, the accelerometer-related initial gain will be higher than its magnetometer-related counterpart. The scaling criterion is different for acceleration and magnetic field readings, and is detailed here below.
Fig. 4.9

Computation of gain \(k_{1}\)

  • Acceleration gain: in order to avoid that sudden accelerations along some directions generate a wrong contribution to the correction term of the filter, the acceleration gain \(k_{1}\) is linearly decreased with acceleration magnitude if high accelerations occur. During initialization, the average acceleration magnitude \(\bar{a}\) is computed to be used as a reference term. Then, \(k_{1}\) is set according to the relative distance \(D_{a}\) between the norm of acceleration measurements and \(\bar{a}\):
    $$\begin{aligned} D_{a}=\left| \frac{||\mathbf a _{f}||-\bar{a}}{\bar{a}} \right| \ . \end{aligned}$$
    Referring to Fig. 4.9: \(D_{th}\) represents a threshold value which, if exceeded, causes the decrease of the gain until the maximum value \(D_{max}\) is reached, at which \(k_{1}\) is set to zero. Denoting with \(k_{1}^{in}\) the initial (constant) gain, \(k_{1}\) is determined according to Algorithm 3;
  • Magnetic field gain: as stated in Sect. 4.1.1, only disturbances which rotate with the sensor can be compensated, regardless of the calibration technique adopted. External metal objects (whose presence is not uncommon in the field of underwater robotics) are inevitably a source of magnetic disturbance that affects the yaw estimate. Since these disturbances cannot be corrected, the only possible countermeasure is to readily detect corrupted readings and to avoid the use of magnetometer measurements in such situations, relying only on gyroscope integration for yaw estimation.

    The gain \(k_{2}\) associated with magnetometer readings is changed according to a different strategy with respect to the accelerometer gain \(k_{1}\). Since it is likely that external disturbances modify the direction of the local magnetic field without considerably altering its magnitude, a magnitude-related law would be unreliable, leading to the use of wrong information for correction purposes. The idea is then to scale down \(k_{2}\) if two particular angular constraints are violated; in the case that accurate gyroscope data are available, on a brief period of time angular rate integration leads to an error which is lower than that generated by using corrupted magnetic measurements.

    Figure 4.10 is given for a better understanding; the first angle to be checked is the angle between the projection of the corrected measurement onto the plane orthogonal to the acceleration vector (which can be assumed to be the horizontal plane) and the estimate of magnetic North direction:
    $$\begin{aligned} \alpha _{m}^{1}=cos^{-1}\left( \left( \mathbf m _{\bot }^{c} \right) ' \hat{\mathbf{H }}_{\bot }^{N,B} \right) \ . \end{aligned}$$
    In ideal conditions, the magnitude of \(\alpha _{m}^{1}\) is zero (or, since sensors are affected by measurement noise, it has zero mean value). If a source of magnetic disturbance gets close to the sensor, the change of magnetometer readings causes the estimated rotation \(\hat{R}^{N}_{B}\) to vary. However, since the dynamics of the magnetometer are much faster than the filter’s, if the disturbance approaches the sensor sufficiently rapidly a deviation of \(\mathbf m _{\bot }^{c}\) is registered before a relevant change in \(\hat{R}^{N}_{B}\) may occur, thus causing the increase of \(\alpha _{m}^{1}\).
    Nonetheless, the use of \(\alpha _{m}^{1}\) only to verify the correctness of magnetometer readings has a drawback: if the magnetic disturbance approaches the sensor very slowly, the velocity of the drift of the sensor reading may match the dynamics of the filter: in this case, a slow but continuous change of the yaw estimate is registered, with \(\alpha _{m}^{1}\) remaining close to zero even if a disturbance is present. A second control angle \(\alpha _{m}^{2}\) has then been introduced to overcome this problem. The choice of \(\alpha _{m}^{2}\) is based on the following consideration: in a given geographical location Earth’s magnetic field can be considered constant, i.e. the angle between \(\mathbf H ^{N}\) and the vertical direction \(\mathbf z ^{N}\) is constant. The same angle must be measured between the same vectors rotated in the body frame. Mathematically, this equality is expressed by the following relation:
    $$\begin{aligned} \left( \mathbf z ^{N} \right) ' \mathbf H ^{N} = \left( \mathbf a _{f} \right) ' \mathbf m ^{c} \ , \end{aligned}$$
    where the body frame vertical direction is given by filtered accelerometer measurements. Hence, \(\alpha _{m}^{2}\) measures the angular distance between the angle \(\left( \mathbf a _{f} \right) ' \mathbf m ^{c}\) obtained at each iteration and the corresponding value \(\left( \mathbf z ^{N} \right) ' \mathbf H ^{N}\):
    $$\begin{aligned} \alpha _{m}^{2}=\left| \left| cos^{-1}\left( \left( \mathbf a _{f} \right) ' \mathbf m ^{c} \right) - cos^{-1}\left( \left( \mathbf z ^{N} \right) ' \mathbf H ^{N} \right) \right| \right| \ . \end{aligned}$$
    The magnitude of \(\alpha _{m}^{2}\) is independent from the speed at which a magnetic disturbance is applied. Note that, in Eqs. (4.31) and (4.32), only the directions of the involved vectors are considered. Two threshold values \(\alpha _{th}^{1}\) and \(\alpha _{th}^{2}\) are set; if either one is reached, \(k_{2}\) is forced to zero in a finite number of iterations; if both angles fall below the threshold values, \(k_{2}\) is increased back to the initial value, with the decrease much faster than the increase.
    Let \(k^{u}\) and \(k^{d}\) be suitable increase and decrease counters, and let \(k^{u}_{max}\) and \(k^{d}_{max}\) denote the number of iterations allowed for the variation of the gain \(k_{2}\). If \(k_{2}^{in}\) is the initial value for \(k_{2}\), then Algorithm 4 illustrates how the magnetometer gain is computed.
    Even if no magnetic disturbances are present, it is possible for both angles to become greater than the threshold values; this can occur if large accelerations arise (which is not likely to happen in the field of underwater robotics) or during motion transients. However, this effect is only temporary and \(\alpha _{m}^{1}\), \(\alpha _{m}^{2}\) fall again below the threshold in a short amount of time. This solution may appear quite conservative; nevertheless, if a precise gyroscope is available, it is indeed better to discard good magnetometer readings than running the risk of exploiting corrupted magnetic measurements, thus compromising the accuracy of the yaw estimate. The choice of the threshold values depends on the sensitivity of the selected hardware: suitable tests must be executed to evaluate the different effects of both magnetic disturbances and motion transients on the control angles, in order to set threshold values which guarantee that corrupted readings are promptly discarded and, at the same time, reliable measurements are taken into account.
Fig. 4.10

Control angles associated with the magnetic field gain

After the introduced modifications, it is now possible to specify the particular form of \(\varvec{\omega }_{c}\) for the considered application:
$$\begin{aligned} \varvec{\omega }_{c}=k_{1}{} \mathbf a _{f} \times \left( \hat{R}^{N}_{B}\right) '{} \mathbf z ^{N} + k_{2}{} \mathbf m _{\bot }^{c} \times \hat{\mathbf{H }}_{\bot }^{N,B} \ . \end{aligned}$$
FOG Integration

The majority of commercial IMU and compass systems possess internal algorithms that fuse the raw data coming from the sensors they are equipped with in order to estimate their own orientation. The use of a standalone attitude estimation filter, on the other side, yields the possibility of increasing the accuracy of the computed estimate by using data that come from sensors that are not originally present within the device. This is extremely useful, e.g., in low cost applications, where cheaper MEMS sensors can be used together with more precise devices. In the considered case, a single-axis FOG has been mounted with its sensitive axis parallel to the IMU gyroscope z-axis. Its measurement completely substitutes the angular rate change read on that axis by the IMU gyroscope within the filter.

This choice is justified by the following consideration: the magnetometer-related contribution in \(\varvec{\omega }_{c}\) has the purpose of obtaining an accurate estimate of the yaw angle. In the presence of magnetic disturbances the gain \(k_{2}\) is set to zero, and the yaw estimate is obtained by raw integration of angular velocity. Due to the relatively low precision of a MEMS gyroscope, a significant yaw drift is registered even if the sensor is not moving. However, since the FOG possesses a much lower drift with respect to MEMS gyroscopes, the employment of its reading for the z-axis angular rate change measurement allows to reach a high level of accuracy; through its use, the risk of unacceptable growth of the integration error when the magnetometer is not employed is avoided.
Fig. 4.11

FOG correction term computation

Nonetheless, due to the high sensitivity of a FOG, the device is able to sense Earth’s rotation, producing a nonzero output of up to \(15^{\circ }\) per hour (which is, in this context, detrimental) even if the component is not rotating. However, this effect can be compensated exploiting the knowledge of the latitude at which the sensor is operating and the information regarding the attitude of the vehicle it is mounted on. Referring to Fig. 4.11: it is assumed that the sensor is operating in Earth’s northern hemisphere; however, the compensation procedure is conceptually the same on the whole planet surface. Let \(\varvec{\omega }_{E}\) denote Earth’s angular velocity; its magnitude, expressed in radians per second, is then
$$\begin{aligned} \omega _{E}=||\varvec{\omega }_{E}|| \cong 7.2921 \cdot 10^{-5} \ . \end{aligned}$$
Latitude \(\Psi \) is supposed to be known. The idea is to determine the component of Earth’s angular rate acting on the body z-axis and subtract it from the sensor reading. At first, it is convenient to express Earth’s angular velocity in the NED frame as follows:
$$\begin{aligned} \varvec{\omega }_{E}^{N}=\begin{bmatrix} \omega _{E}cos\left( \Psi \right) \\ 0 \\ -\omega _{E}sin\left( \Psi \right) \ . \end{bmatrix} \end{aligned}$$
Exploiting the current attitude estimate, the bias to be subtracted from the device’s measurement is the third component of the vector:
$$\begin{aligned} \varvec{\omega }_{E}^{B}=\left( \hat{R}^{N}_{B} \right) '\varvec{\omega }_{E}^{N} \ , \end{aligned}$$
which gives the corrected FOG measurement:
$$\begin{aligned} \omega _{FOG}^{c}=\omega _{FOG}^{m}-\omega _{E,z}^{B} \ . \end{aligned}$$
Applying the proposed correction, it is possible to considerably reduce the drift of the device, substantially increasing its accuracy.

4.2 Position Estimation Filter

This section is dedicated to the position estimation filter developed during the Ph.D. period. As stated at the beginning of this chapter, such filter is based on the UKF; it is thus necessary to derive a state-space model for the whole AUV system. The choice of a suitable vehicle model to adopt is indeed very delicate: the latter must constitute a smart and efficient trade-off between required computational load (since it must be used on board in real-time) and accuracy. In literature, AUV models can be usually classified into two main categories: complete dynamic models, which draw directly from ( 3.5), and kinematic models. While the former are capable of capturing more information about the evolution of the system, they inevitably require additional computational resources with respect to the latter; where hardware limitations prevail, it may be possible to resort to a completely kinematic model in favor of a reduced burden on the processing unit of the vehicle. In this work, a mixed kinematic and dynamic model has been adopted, suitably modifying ( 3.5) in order to take into account longitudinal dynamics only. This choice is justified by the fact that, for underwater vehicles, considerable dynamics takes place only on the direction of forward motion: most of the time AUVs move along the longitudinal direction (since it usually constitutes the direction of minimal resistance, this allows to reduce power consumption); this is especially true for torpedo-shaped vehicles, where lateral and vertical motion is strongly dampened by drag forces. Hence, the chosen model offers a satisfying level of accuracy, better describing the physics of a real vehicle compared with a kinematic only model, offering at the same time a reduction of the overall number of unknown parameters of the system and requiring less computational resources with respect to a complete dynamic model.

The derived model is then used on board, within the position estimation filter of the vehicle; the latter is based on the UKF introduced in Sect.  3.2. The choice of an UKF-based position estimator is investigated as an alternative solution to the KF and the EKF, whose use in the marine field is quite common and extensively documented in literature. The UKF has been proposed in order to cope with the issues that may arise with the two above-mentioned estimators: indeed, AUV models may be nonlinear and non-differentiable (as is the case of the adopted model), preventing the use of the KF and hindering the use of the EKF, since the latter requires the explicit computation of the derivatives of the equations of the state-space model of the system, which can be problematic in the case of functions with discontinuities and/or jumps. The UKF, on the other side, is completely derivative-free; in addition, the computational load it requires can be efficiently handled by the hardware which is today present on the majority of AUVs.

In addition to position estimation, it is shown how the UKF-based navigation filter can be used to simultaneously estimate sea currents acting on the vehicle. Such operation is performed without the need for further instrumentation: indeed, Sect. 4.2.2 shows how the state-space model of the system, described in details in Sect. 4.2.1, can be suitably modified in order to take into account the effects of currents; in particular, the model is rewritten in terms of the relative velocity \(\varvec{\nu }_{r}\), and a contribution proportional to current intensity is included into lateral dynamics. Then, current velocity components are added to the state vector and estimated on board, increasing at the same time the accuracy and the reliability of the estimated position.

4.2.1 AUV State-Space Model

To describe the behavior of the AUV, the following state variables have been used:
$$\begin{aligned} \mathbf x =\begin{bmatrix} \varvec{\eta }_{1} \\ \varvec{\nu }_{1} \end{bmatrix} \ . \end{aligned}$$
\(\mathbf x \) is a vector in \(\mathbb {R}^{6}\) composed of the NED frame position and of the body frame linear velocity of the vehicle. The state evolves according to a strictly causal, mixed kinematic and dynamic model, taking into account longitudinal dynamics:
$$\begin{aligned} \begin{bmatrix} \varvec{\eta }_{1} \\ \varvec{\nu }_{1} \end{bmatrix}_{k} = \begin{bmatrix} \varvec{\eta }_{1} \\ \varvec{\nu }_{1} \end{bmatrix}_{k-1} + \Delta T \begin{bmatrix} R^{N}_{B}\left( \left( \varvec{\eta }_{2}\right) _{k-1} \right) \left( \varvec{\nu }_{1}\right) _{k-1} \\ \begin{bmatrix} \frac{\tau _{1,x}\left( \varvec{\nu }_{k-1},\mathbf u _{k-1} \right) }{m} - \frac{D_{x}\left( \left( \nu _{1,x}\right) _{k-1} \right) \left( \nu _{1,x} \right) _{k-1}}{m} \\ 0 \\ 0 \end{bmatrix} \end{bmatrix} + \mathbf w _{k-1} \ . \end{aligned}$$
In (4.39), \(\Delta T\) is the (fixed) sampling time of the filter, and process noise \(\mathbf w \) has been modeled as a zero mean stationary white noise. Furthermore, it is worth noting that, since attitude is accurately estimated by the dedicated filter presented in Sect. 4.1, there is no need to include it within the state vector of the system; instead, the orientation of the vehicle constitutes an input to the system evolution equation.

For what concerns longitudinal dynamics, the fourth equation of (4.39) derives from the complete dynamic model ( 3.5); additionally, gravitational, centripetal and Coriolis effects have been neglected, and only longitudinal force has been taken into account (to a first approximation, moments generated by the propellers and acting on the AUV can be neglected as their influence can be greatly reduced thanks to a smart mechanical design of the vehicle). The damping coefficient \(D_{x}\) has been computed as in ( 3.14), and drag coefficients \(C_{D,x}\) have been identified for all the vehicles employed for the validation of the model resorting to experimental tests. For long and slender vehicles (such as MARTA AUV or Typhoon class vehicles), the area coefficient \(A_{f,x}\) has been computed, according to [18], as the product of length and diameter of an equivalent cylinder representing the vehicle.

Particular attention has to be given to the determination of the total force and moment acting on the AUV: such effects are a function of the thrust generated by the propellers of the vehicle, which depends on their rotational speed (i.e. the control input \(\mathbf u \) to the system) and on their advance speed (which is, in turn, a function of the body frame velocity \(\varvec{\nu }\)); the thrust exerted by all the propellers is then mapped onto total force and moment by \(\varvec{\tau }\left( \varvec{\nu },\mathbf u \right) \). Many literature sources provide the relation between thrust, rotational speed, and advance speed in the form of experimentally derived curves or look-up tables [5, 20]; additionally, it is not uncommon for these curves to be derived for particular operating conditions only (e.g., only for positive rotational and advance speeds). These relations, even if accurate, are hardly exploitable within a recursive estimation filter, where an analytical expression for such functions would be preferred. In this work, the propulsion system model presented in [6] has been adopted; such model provides indeed an analytical expression for the sought after relation, valid for all operating conditions, and constitutes a suitable trade-off between computational load and modeling accuracy. A sketch of the derivation of the chosen propulsion model is illustrated here below; for a detailed discussion, refer to [6] and references contained therein.

Propulsion System Model

The first step is the determination of the advance speed of a propeller. Let \(\mathbf P _{p,i}^{B}\) and \(\mathbf V _{p,i}^{B}\) denote, respectively, the position and the velocity of the i-th propeller, expressed in the body frame; then, the following equation holds:
$$\begin{aligned} \mathbf V _{p,i}^{B}=\varvec{\nu }_{1}+\varvec{\nu }_{2} \times \left( \mathbf P _{p,i}^{B} - \mathbf O ^{B} \right) \ ; \end{aligned}$$
additionally, let \(\mathbf n _{p,i}^{B}\) indicate its line of action. The advance speed of the propeller is given by the projection of its velocity on \(\mathbf n _{p,i}^{B}\):
$$\begin{aligned} V_{a,i}=\left( \mathbf V _{p,i}^{B} \right) ' \mathbf n _{p,i}^{B} \ . \end{aligned}$$
In particular, since an AUV usually moves in the direction of forward motion, the transverse velocity \(\mathbf V _{p,i,t}^{B}=\mathbf V _{p,i}^{B}-V_{a,i}{} \mathbf n _{p,i}^{B}\) can be neglected for its rear propellers, and (4.40) is simplified as follows:
$$\begin{aligned} V_{a,i}\cong \left| \left| \mathbf V _{p,i}^{B} \right| \right| \ . \end{aligned}$$
Let \(n_{i}\) denote the rotational speed of the i-th propeller; then, once its advance speed is known, the exerted thrust is computed according to the following equation [1, 6]:
$$\begin{aligned} T_{i}\left( n_{i},V_{a,i} \right) =sgn \left( n_{i} \right) \left( k_{b}n_{i}^{2} - \frac{k_{b} \left| n_{i} \right| g\left( sgn \left( n_{i} \right) V_{a,i} \right) }{p_{p}} \right) \ , \end{aligned}$$
where \(sgn\left( \mathord {\cdot } \right) \) is the sign function, and \(g\left( \mathord {\cdot } \right) \) is a piecewise function defined as
$$\begin{aligned} g\left( x \right) ={\left\{ \begin{array}{ll} 0 \ \ \ for \ x \le 0 \\ x \ \ \ for \ 0<x \le \left| n_{i} \right| p_{p} \\ \left| n_{i} \right| p_{p} \ \ \ for \ x>\left| n_{i} \right| p_{p} \end{array}\right. } \ . \end{aligned}$$
In (4.43)–(4.44), \(p_{p}\) indicates the propeller pitch, while the bollard thrust coefficient \(k_{b}\) is derived from:
$$\begin{aligned} T_{i}\left( n_{i},0 \right) =k_{b}n_{i}^{2} \ . \end{aligned}$$
As shown in Eq. (4.45), it is assumed that the relation between the rotational speed of a propeller and the thrust exerted at zero advance speed (bollard thrust) is (at least to a first approximation) quadratic; this behavior can be observed while performing suitable experimental test, which can be used to determine the value of \(k_{b}\).
After the thrust exerted by each propeller of the vehicle has been computed, the resulting force and moment acting on the AUV can be determined. Let
$$\begin{aligned} \mathbf U =\left[ T_{1} \ T_{2} \ \cdots \ T_{i} \ \cdots \ T_{n_{p}} \right] ' \in \mathbb {R}^{n_{p}} \ , \ \ \ T_{i}=T_{i}\left( n_{i},V_{a,i} \right) \end{aligned}$$
denote the vector collecting the thrust given by the \(n_{p}\) thrusters of the vehicle; then, \(\varvec{\tau }\left( \varvec{\nu },\mathbf u \right) \) is given by:
$$\begin{aligned} \varvec{\tau }\left( \varvec{\nu },\mathbf u \right) =B\mathbf U \left( \varvec{\nu },\mathbf u \right) \ , \ \ \ \mathbf u =\left[ n_{1} \ n_{2} \ \cdots \ n_{p} \right] ' \ . \end{aligned}$$
Matrix \(B \in \mathbb {R}^{6\times n_{p}}\) is constant and dependent on vehicle geometry, and is expressed as follows:
$$\begin{aligned} B=\begin{bmatrix} B_{1} \\ B_{2} \end{bmatrix} \ , \end{aligned}$$
$$\begin{aligned} B_{1}=\left[ \cdots \ \mathbf n _{p,i}^{B} \ \cdots \right] \ , \ \ \ B_{2}=\left[ \cdots \ \left( \mathbf P _{p,i}^{B} - \mathbf O ^{B} \right) \times \mathbf n _{p,i}^{B} \ \cdots \right] \ . \end{aligned}$$
For what concerns the available measurements, the proposed filter is able to exploit position measurements, them being either GPS fixes (used to initialize the filter while the vehicle is on surface) or position fixes coming from a dedicated underwater acoustic localization system (for instance, in the following equation the availability of an USBL system is assumed), and depth and velocity measurements; the measurement vector at the k-th time instant can be expressed as follows:
$$\begin{aligned} \mathbf y _{k}=\left[ \left( \mathbf P _{GPS}^{N}\right) ' \ \left( \mathbf P _{USBL}^{N}\right) ' \ d_{DS}^{N} \ \left( \mathbf v _{DVL}^{B}\right) ' \right] _{k}' \ . \end{aligned}$$
From (4.50), it is easy to note that \(\mathbf y \) contains direct measurements of the components of the state vector; hence, the measurement equation is affine with respect to \(\mathbf x \):
$$\begin{aligned} \mathbf y _{k}=H_{k}{} \mathbf x _{k}+\mathbf v _{k} \ , \end{aligned}$$
where matrix \(H_{k}\) contains only 1 or 0 elements. As in (4.39), measurement noise in (4.51) is assumed additive, stationary and with zero average value; additionally, initial state and process and measurement noises are assumed to have zero cross-correlation.

As a concluding remark, it is worth noting that the model in (4.39), together with (4.43)–(4.44), is nonlinear and non-differentiable (due to the presence of e.g., quadratic and sign functions), which may favor the adoption of a derivative-free estimation algorithm such as the UKF.

4.2.2 Sea Current Estimation

Marine currents can considerably affect the motion of an underwater vehicle performing an autonomous mission, and they may even pose a serious threat to the success of the task itself. Several solutions exist to measure sea current, involving e.g., the use of dedicated instrument such as ADCPs. However, these devices are quite expensive and require adequate deployment, increasing the cost of operations at sea. Within this work, an alternative approach is proposed: currents are estimated in real-time by the vehicle, exploiting measurements which are already employed by the position estimation filter and the knowledge of the dynamic model of the vehicle, without requiring the purchase and the installation of additional devices. Additionally, the availability of a current estimate on board is likely to improve the accuracy of the computed position estimate as well.

In particular, the state vector in (4.38) is augmented in order to include the first two components of \(\varvec{\nu }_{c}^{N}\) (i.e. North and East current components); at this stage, the vertical component of the current has not been taken into account. The complete system model (taking currents into account) has been rewritten in terms of the relative velocity \(\varvec{\nu }_{r}\), and evolves according to the following equations [3, 8]:
$$\begin{aligned} \begin{bmatrix} \varvec{\eta }_{1} \\ \varvec{\nu }_{1} \\ u_{c}^{N} \\ v_{c}^{N} \end{bmatrix}_{k} =&\begin{bmatrix} \varvec{\eta }_{1} \\ \varvec{\nu }_{1} \\ u_{c}^{N} \\ v_{c}^{N} \end{bmatrix}_{k-1} + \nonumber \\ + \Delta&T \begin{bmatrix} R^{N}_{B}\left( \left( \varvec{\eta }_{2}\right) _{k-1} \right) \left( \varvec{\nu }_{1}\right) _{k-1} \\ \begin{bmatrix} \frac{\tau _{1,x}\left( \left( \varvec{\nu }_{r}\right) _{k-1},\mathbf u _{k-1} \right) }{m} - \frac{D_{x}\left( \left( \nu _{1,x,r}\right) _{k-1} \right) \left( \nu _{1,x,r} \right) _{k-1}}{m} \\ - \frac{D_{y}\left( \left( \nu _{1,y,r}\right) _{k-1} \right) \left( \nu _{1,y,r} \right) _{k-1}}{m} \\ 0 \end{bmatrix} \\ \begin{bmatrix} 0 \\ 0 \end{bmatrix} \end{bmatrix} + \mathbf w _{k-1} \ . \end{aligned}$$
As anticipated in Sect.  3.1.1, current has been considered constant (in the NED frame) over the operational area of the vehicle. A drag force contribution due to currents has been included into lateral dynamics; the lateral damping coefficient \(D_{y}\) has been computed according to ( 3.14), with a resulting drag coefficient \(C_{D,y}\) which is much higher than \(C_{D,x}\). To a first approximation, the current-related contribution \(\dot{J}^{c}\varvec{\nu }_{c}^{N}\) has been neglected, since it is null when the vehicle reaches steady-state speed or, more generally, when the latter moves in a straight line; finally, an additive process noise (with the same spectral characteristics as those introduced for the other state variables) has been included in order to take into account slow current variations.

The model in (4.52) is used within the same UKF-based navigation filter presented in previous sections, which is employed to simultaneously estimate the position of the vehicle and sea currents acting on the latter without a relevant computational cost increase (with respect to position only estimation), and without requiring any additional instrumentation.

Observability of the System in the Presence of Sea Currents

In order to justify the proposed current estimation strategy, the observability of the system in the presence of marine currents, exploiting the same measurements as the position estimator, is provided here below.

For the ease of reading, the analysis is carried out on the continuous-time version (prior to discretization) of the model (4.52) with the measurement equation (4.50); additionally, the following assumptions have been made:
  1. 1.

    the magnitude of the current in the fixed frame is greater than zero;

  2. 2.

    the vehicle moves along the positive direction of its body x-axis, propelled by two rear thrusters (whose line of action is indeed \(\mathbf x ^{B}\)); they exert the same thrust, so that the only control input is their common positive rotational speed \(n_{t}\) (conventionally, a positive \(n_{t}\) implies a positive body-frame thrust);

  3. 3.

    the advance speed of the thrusters with respect to the current, expressed in body frame (equal to that of the vehicle, i.e. \(\nu _{1,x,r}\)) is positive and less than \(n_{t}p_{p}\); together with the previous hypothesis, this implies that the propulsion model (4.43)–(4.44) is differentiable with respect to state components;

  4. 4.

    the orientation of the vehicle is not changing. Recalling that orientation is a time-varying input, this assumption equals to considering the system autonomous. Furthermore, it is assumed that roll and pitch are zero;

  5. 5.

    with respect to (4.50), despite their heterogeneity, GPS and USBL fixes are treated as generic position measurements.


Additionally, it is worth reminding that it is assumed that absolute velocity measurements are available (i.e. obtained with a DVL with constant bottom lock). Finally, let us assume that the vehicle is not moving in the direction of the current or in a direction perpendicular to it, so that current can be experienced on both the body-frame x- and y-axis; this last hypothesis will then be relaxed.

In light of assumptions 1–5, the state-space representation of the system is the following:
$$\begin{aligned} \dot{\mathbf{x }}&=f\left( \mathbf x ,n_{t},\mathbf w \right) = \begin{bmatrix} \dot{\varvec{\eta }}_{1} \\ \dot{\varvec{\nu }}_{1} \\ \dot{u}_{c}^{N} \\ \dot{v}_{c}^{N} \end{bmatrix} = \begin{bmatrix} f_{1}\left( \mathbf x ,n_{t}\right) \\ \vdots \\ f_{n_{x}}\left( \mathbf x ,n_{t}\right) \end{bmatrix} +\mathbf w = \nonumber \\&= \begin{bmatrix} R^{N}_{B}\left( \varvec{\eta }_{2}\right) \varvec{\nu }_{1} \\ \frac{2}{m}\left( k_{b}n_{t}^{2}-\frac{k_{b}n_{t}\nu _{1,x,r}}{p_{p}} \right) - \frac{\rho A_{f,x} C_{D,x}}{2m} \nu _{1,x,r}^{2} \\ \frac{\rho A_{f,y} C_{D,y}}{2m}\nu _{1,y,r}^{2} \\ 0 \\ 0 \\ 0 \end{bmatrix} + \mathbf w \in \mathbb {R}^{n_{x}} \ , \end{aligned}$$
$$\begin{aligned} \mathbf y&=\begin{bmatrix} h_{1}\left( \mathbf x ,\mathbf v \right) \\ \vdots \\ h_{n_{y}}\left( \mathbf x ,\mathbf v \right) \end{bmatrix}=\begin{bmatrix} I_{6\times 6}&0_{6\times 2} \end{bmatrix}{} \mathbf x +\mathbf v \in \mathbb {R}^{n_y} \ ; \end{aligned}$$
in the considered case, \(n_{x}=8\) and \(n_{y}=6\). For the purpose of the observability analysis, the sign of the fifth equation in (4.53) can be arbitrarily chosen (it has been assumed positive in this context); additionally, the x- and y-axis components of the relative velocity are given by:
$$\begin{aligned} \nu _{1,x,r}=u-c_{\psi }u_{c}^{N}-s_{\psi }v_{c}^{N} \ , \end{aligned}$$
$$\begin{aligned} \nu _{1,y,r}=v+s_{\psi }u_{c}^{N}-c_{\psi }v_{c}^{N}\ , \end{aligned}$$
being \(c_{\psi }\) and \(s_{\psi }\) the sine and the cosine of the yaw angle.
Due to the nonlinearity of the system, the observability analysis is carried out exploiting differential geometry as explained in [12]. In particular, a sufficient condition to guarantee the local weak observability of the system (4.53)–(4.54) (see [12] for the definition and the corresponding proof) is that:
$$\begin{aligned} dim\left( dO\right) =n_{x} \ , \end{aligned}$$
$$\begin{aligned} dO=span\left\{ \frac{\partial \mathcal {L}^{k}_\mathbf{f \left( \mathbf x ,n_{t},\mathbf w \right) }h_{j}\left( \mathbf x ,\mathbf v \right) }{\partial \mathbf x } \right\} , \ \ \ j=1,\ldots ,n_{y}, \ \ \ k \in \mathbb {N} \ . \end{aligned}$$
In (4.58), \(\mathcal {L}^{k}_\mathbf{f \left( \mathbf x ,n_{t},\mathbf w \right) }h_{j}\left( \mathbf x ,\mathbf v \right) \) is the k-th order Lie derivative of the j-th measurement function with respect to the vector field \(\mathbf f \left( \mathbf x ,n_{t},\mathbf w \right) \), computed as follows:
$$\begin{aligned}&\mathcal {L}^{0}_\mathbf{f \left( \mathbf x ,n_{t},\mathbf w \right) }h_{j}\left( \mathbf x ,\mathbf v \right) =h_{j}\left( \mathbf x ,\mathbf v \right) \nonumber \\&\mathcal {L}^{1}_\mathbf{f \left( \mathbf x ,n_{t},\mathbf w \right) }h_{j}\left( \mathbf x ,\mathbf v \right) =\left( \frac{\partial h_{j}\left( \mathbf x ,\mathbf v \right) }{\partial \mathbf x }\right) '{} \mathbf f \left( \mathbf x ,n_{t},\mathbf w \right) \nonumber \\&\vdots \nonumber \\&\mathcal {L}^{k}_\mathbf{f \left( \mathbf x ,n_{t},\mathbf w \right) }h_{j}\left( \mathbf x ,\mathbf v \right) =\left( \frac{\partial \mathcal {L}^{k-1}_\mathbf{f \left( \mathbf x ,n_{t},\mathbf w \right) }h_{j}\left( \mathbf x ,\mathbf v \right) }{\partial \mathbf x }\right) '{} \mathbf f \left( \mathbf x ,n_{t},\mathbf w \right) \ . \end{aligned}$$
In this context, it is equivalently shown that the matrix
$$\begin{aligned} \overline{dO}=\begin{bmatrix} \frac{\partial h_{1}\left( \mathbf x ,\mathbf v \right) }{\partial \mathbf x }&\cdots&\frac{\partial h_{n_{y}}\left( \mathbf x ,\mathbf v \right) }{\partial \mathbf x }&\frac{\partial \mathcal {L}^{1}_\mathbf{f \left( \mathbf x ,n_{t},\mathbf w \right) }h_{4}\left( \mathbf x ,\mathbf v \right) }{\partial \mathbf x }&\frac{\partial \mathcal {L}^{1}_\mathbf{f \left( \mathbf x ,n_{t},\mathbf w \right) }h_{5}\left( \mathbf x ,\mathbf v \right) }{\partial \mathbf x } \end{bmatrix} \end{aligned}$$
has full rank (i.e. eight linearly independent columns). From (4.54), it is easy to observe that:
$$\begin{aligned} \overline{dO}=\begin{bmatrix} \begin{bmatrix} I_{6\times 6} \\ 0_{2\times 6} \end{bmatrix}&\frac{\partial \mathcal {L}^{1}_\mathbf{f \left( \mathbf x ,n_{t},\mathbf w \right) }h_{4}\left( \mathbf x ,\mathbf v \right) }{\partial \mathbf x }&\frac{\partial \mathcal {L}^{1}_\mathbf{f \left( \mathbf x ,n_{t},\mathbf w \right) }h_{5}\left( \mathbf x ,\mathbf v \right) }{\partial \mathbf x } \end{bmatrix} \ ; \end{aligned}$$
this implies that, in addition to being linearly independent, the last two columns of \(\overline{dO}\) must contain non-zero elements on the seventh and eighth rows in order to guarantee local weak observability.
For the ease of reading, let us define
$$\begin{aligned} K_{x}=\frac{\rho A_{f,x}C_{D,x}}{2m} \ , \ \ \ K_{y}=\frac{\rho A_{f,y}C_{D,y}}{2m} \ ; \end{aligned}$$
hence, explicitly computing the Lie derivatives yields:
$$\begin{aligned}&\mathcal {L}^{1}_\mathbf{f \left( \mathbf x ,n_{t},\mathbf w \right) }h_{4}\left( \mathbf x ,\mathbf v \right) =\left( \frac{\partial h_{4}\left( \mathbf x ,\mathbf v \right) }{\partial \mathbf x }\right) '{} \mathbf f \left( \mathbf x ,n_{t},\mathbf w \right) =f_{4}\left( \mathbf x ,n_{t}\right) = \nonumber \\&=\frac{2}{m}\left( k_{b}n_{t}^{2}-\frac{k_{b}n_{t}}{p_{p}}\left( u-c_{\psi }u_{c}^{N}-s_{\psi }v_{c}^{N}\right) \right) -K_{x}\left( u-c_{\psi }u_{c}^{N}-s_{\psi }v_{c}^{N}\right) ^{2} \end{aligned}$$
$$\begin{aligned}&\mathcal {L}^{1}_\mathbf{f \left( \mathbf x ,n_{t},\mathbf w \right) }h_{5}\left( \mathbf x ,\mathbf v \right) =\left( \frac{\partial h_{5}\left( \mathbf x ,\mathbf v \right) }{\partial \mathbf x }\right) '{} \mathbf f \left( \mathbf x ,n_{t},\mathbf w \right) =f_{5}\left( \mathbf x ,n_{t}\right) = \nonumber \\&=K_{y}\left( v+s_{\psi }u_{c}^{N}-c_{\psi }v_{c}^{N}\right) ^{2} \ . \end{aligned}$$
Differentiating with respect to state components allows to obtain the last two columns of \(\overline{dO}\):
$$\begin{aligned} \frac{\partial \mathcal {L}^{1}_\mathbf{f \left( \mathbf x ,n_{t},\mathbf w \right) }h_{4}\left( \mathbf x ,\mathbf v \right) }{\partial \mathbf x }&=\Big [ \begin{matrix} 0&0&0&-\frac{2k_{b}n_{t}}{mp_{p}}-2K_{x}\nu _{1,x,r}&0 \end{matrix} \Big . \nonumber \\&\Big . \begin{matrix} 0&\frac{2k_{b}n_{t}c_{\psi }}{mp_{p}}+2K_{x}c_{\psi }\nu _{1,x,r}&\frac{2k_{b}n_{t}s_{\psi }}{mp_{p}}+2K_{x}s_{\psi }\nu _{1,x,r} \end{matrix} \Big ]' \end{aligned}$$
$$\begin{aligned} \frac{\partial \mathcal {L}^{1}_\mathbf{f \left( \mathbf x ,n_{t},\mathbf w \right) }h_{5}\left( \mathbf x ,\mathbf v \right) }{\partial \mathbf x }&=\Big [ \begin{matrix} 0&0&0&0&2K_{y}\nu _{1,y,r} \end{matrix} \Big . \nonumber \\&\Big . \begin{matrix} 0&2K_{y}s_{\psi }\nu _{1,y,r}&-2K_{y}c_{\psi }\nu _{1,y,r} \end{matrix} \Big ]' \ . \end{aligned}$$
Observing (4.64)–(4.65), it is easy to verify that rank loss of \(\overline{dO}\) is associated with those points (in state space) that zero the determinant of the following matrix:
$$\begin{aligned} \overline{\underline{dO}}=\begin{bmatrix} \frac{2k_{b}n_{t}c_{\psi }}{mp_{p}}+2K_{x}c_{\psi }\nu _{1,x,r}&\frac{2k_{b}n_{t}s_{\psi }}{mp_{p}}+2K_{x}s_{\psi }\nu _{1,x,r} \\ 2K_{y}s_{\psi }\nu _{1,y,r}&-2K_{y}c_{\psi }\nu _{1,y,r} \end{bmatrix} \ , \end{aligned}$$
given by:
$$\begin{aligned} \begin{vmatrix} \overline{\underline{dO}} \end{vmatrix}=-4K_{y}\nu _{1,y,r}\left( \frac{k_{b}n_{t}}{mp_{p}}+K_{x}\nu _{1,r,x}\right) \ . \end{aligned}$$
In light of the assumptions made at the beginning of the paragraph, the determinant of \(\overline{\underline{dO}}\) is always different than zero, thus guaranteeing the local weak observability of the current estimator.
As anticipated, it is possible to relax the assumption that the vehicle is not moving in a direction that is parallel or perpendicular to the direction of the current. Let us now consider, for instance, the first case: the observability analysis is simplified due to the fact that the motion on the horizontal plane is constrained on a line parallel to the direction of the current, reducing the dimension of the system. In particular, without taking into account the depth of the vehicle (a direct measurement of it is obtained using the available sensors), observability can be evaluated on the following system:
$$\begin{aligned} \dot{\mathbf{x }}^{\parallel }&=\begin{bmatrix} \dot{x}^{\parallel } \\ \dot{\nu }^{\parallel } \\ \dot{\nu }_{c}^{\parallel } \end{bmatrix} = \mathbf f ^{\parallel }\left( \mathbf x ^{\parallel },n_{t},\mathbf w ^{\parallel }\right) = \begin{bmatrix} f_{1}^{\parallel }\left( \mathbf x ^{\parallel },n_{t}\right) \\ f_{2}^{\parallel }\left( \mathbf x ^{\parallel },n_{t}\right) \\ f_{3}^{\parallel }\left( \mathbf x ^{\parallel },n_{t}\right) \end{bmatrix} + \mathbf w ^{\parallel }= \nonumber \\&=\begin{bmatrix} \nu ^{\parallel } \\ \frac{2}{m}\left( k_{b}n_{t}^{2}-\frac{k_{b}n_{t}}{p_{p}}\nu _{r}^{\parallel }\right) - K_{x}\left( \nu _{r}^{\parallel }\right) ^{2} \\ 0 \end{bmatrix} + \mathbf w ^{\parallel } \end{aligned}$$
$$\begin{aligned} \mathbf y ^{\parallel }&=\begin{bmatrix} h_{1}^{\parallel }\left( \mathbf x ^{\parallel },\mathbf v ^{\parallel }\right) \\ h_{2}^{\parallel }\left( \mathbf x ^{\parallel },\mathbf v ^{\parallel }\right) \end{bmatrix} + \mathbf v ^{\parallel }=\begin{bmatrix} 1&0&0 \\ 0&1&0 \end{bmatrix}\begin{bmatrix} x^{\parallel } \\ \nu ^{\parallel } \\ \nu _{c}^{\parallel } \end{bmatrix} + \mathbf v ^{\parallel } \ , \end{aligned}$$
where \(K_{x}\) has the same meaning as before, and \(\nu _{r}^{\parallel }=\nu ^{\parallel }-\nu _{c}^{\parallel }\); the state is composed of position, velocity, and current velocity along a line parallel to the direction of the current (recall that the latter is the only current component that needs to be estimated). Exploiting the same tools used for the previous case, the local weak observability of (4.68)–(4.69) is guaranteed by showing that the matrix
$$\begin{aligned} \overline{dO}^{\parallel }=\begin{bmatrix} \frac{\partial h_{1}^{\parallel }\left( \mathbf x ^{\parallel },\mathbf v ^{\parallel }\right) }{\partial \mathbf x ^{\parallel }}&\frac{\partial h_{2}^{\parallel }\left( \mathbf x ^{\parallel },\mathbf v ^{\parallel }\right) }{\partial \mathbf x ^{\parallel }}&\frac{\partial \mathcal {L}^{1}_\mathbf{f ^{\parallel }\left( \mathbf x ^{\parallel },n_{t},\mathbf w ^{\parallel }\right) }h_{2}^{\parallel }\left( \mathbf x ^{\parallel },\mathbf v ^{\parallel }\right) }{\partial \mathbf x ^{\parallel }} \end{bmatrix} \end{aligned}$$
has full rank.
Explicitly computing the elements of \(\overline{dO}^{\parallel }\) yields:
$$\begin{aligned} \overline{dO}^{\parallel }=\begin{bmatrix} 1&0&0 \\ 0&1&-\frac{2k_{b}n_{t}}{mp_{p}}-2K_{x}\nu _{r}^{\parallel } \\ 0&0&\frac{2k_{b}n_{t}}{mp_{p}}+2K_{x}\nu _{r}^{\parallel } \end{bmatrix} \ ; \end{aligned}$$
hence, the system is locally weakly observable since the last element of the third column is always positive.
Analogously, observability is guaranteed even if the vehicle moves in a direction perpendicular to that of the current. In this case, the system under study is the following:
$$\begin{aligned} \dot{\mathbf{x }}^{\perp }&=\begin{bmatrix} \dot{x}^{\perp } \\ \dot{\nu }^{\perp } \\ \dot{\nu }_{c}^{\perp } \end{bmatrix} = \mathbf f ^{\perp }\left( \mathbf x ^{\perp },\mathbf w ^{\perp }\right) = \begin{bmatrix} f_{1}^{\perp }\left( \mathbf x ^{\perp }\right) \\ f_{2}^{\perp }\left( \mathbf x ^{\perp }\right) \\ f_{3}^{\perp }\left( \mathbf x ^{\perp }\right) \end{bmatrix} + \mathbf w ^{\perp }= \nonumber \\&=\begin{bmatrix} \nu ^{\perp } \\ K_{y}\left( \nu _{r}^{\perp }\right) ^{2} \\ 0 \end{bmatrix} + \mathbf w ^{\perp } \end{aligned}$$
$$\begin{aligned} \mathbf y ^{\perp }&=\begin{bmatrix} h_{1}^{\perp }\left( \mathbf x ^{\perp },\mathbf v ^{\perp }\right) \\ h_{2}^{\perp }\left( \mathbf x ^{\perp },\mathbf v ^{\perp }\right) \end{bmatrix} + \mathbf v ^{\perp }=\begin{bmatrix} 1&0&0 \\ 0&1&0 \end{bmatrix}\begin{bmatrix} x^{\perp } \\ \nu ^{\perp } \\ \nu _{c}^{\perp } \end{bmatrix} + \mathbf v ^{\perp } \ , \end{aligned}$$
where the used terms have obvious meaning. As before, the sign of the second equation in (4.72) is arbitrary. The condition (4.57) for the system (4.72)–(4.73) is verified evaluating the rank of the following matrix:
$$\begin{aligned} \overline{dO}^{\perp }=\begin{bmatrix} \frac{\partial h_{1}^{\perp }\left( \mathbf x ^{\perp },\mathbf v ^{\perp }\right) }{\partial \mathbf x ^{\perp }}&\frac{\partial h_{2}^{\perp }\left( \mathbf x ^{\perp },\mathbf v ^{\perp }\right) }{\partial \mathbf x ^{\perp }}&\frac{\partial \mathcal {L}^{1}_\mathbf{f ^{\perp }\left( \mathbf x ^{\perp },\mathbf w ^{\perp }\right) }h_{2}^{\perp }\left( \mathbf x ^{\perp },\mathbf v ^{\perp }\right) }{\partial \mathbf x ^{\perp }} \end{bmatrix} \ . \end{aligned}$$
Indeed, computation of the Lie derivatives yields:
$$\begin{aligned} \overline{dO}^{\parallel }=\begin{bmatrix} 1&0&0 \\ 0&1&2K_{y}\nu _{r}^{\perp } \\ 0&0&-2K_{y}\nu _{r}^{\perp } \end{bmatrix} \ , \end{aligned}$$
showing that \(\overline{dO}^{\perp }\) has full rank since its last column has nonzero elements under assumptions 1–5, thus guaranteeing local weak observability.


  1. 1.
    Allotta B, Caiti A, Chisci L, Costanzi R, Di Corato F, Fantacci C, Fenucci D, Meli E, Ridolfi A (2016) An unscented Kalman filter based navigation algorithm for autonomous underwater vehicles. J Mechatron 39:185–195CrossRefGoogle Scholar
  2. 2.
    Allotta B, Caiti A, Costanzi R, Fanelli F, Fenucci D, Meli E, Ridolfi A (2016) A new AUV navigation system exploiting unscented Kalman filter. J Ocean Eng 113:121–132CrossRefGoogle Scholar
  3. 3.
    Allotta B, Costanzi R, Fanelli F, Monni N, Paolucci L, Ridolfi A (2017) Sea currents estimation during AUV navigation using Unscented Kalman Filter. In: Proceedings of the IFAC 2017 world congress, Toulouse (FR), July 9–14Google Scholar
  4. 4.
    Allotta B, Costanzi R, Fanelli F, Monni N, Ridolfi A (2015) Single axis FOG aided attitude estimation algorithm for mobile robots. J Mechatron 30:158–173CrossRefGoogle Scholar
  5. 5.
    Carlton JS (2007) Marine propellers and propulsion, 2nd edn. ElsevierGoogle Scholar
  6. 6.
    Costanzi R (2015) Navigation systems for unmanned underwater vehicles. PhD Dissertation, University of Florence, Florence (IT)Google Scholar
  7. 7.
    Costanzi R, Fanelli F, Monni N, Ridolfi A, Allotta B (2016) An attitude estimation algorithm for mobile robots under unknown magnetic disturbances. IEEE/ASME Trans Mechatron 21:1900–1911CrossRefGoogle Scholar
  8. 8.
    Costanzi R, Fanelli F, Ridolfi A, Allotta B (2016) Simultaneous navigation state and sea current estimation through augmented state Unscented Kalman Filter. In: Proceedings of the MTS/IEEE OCEANS’16 Monterey, Monterey (CA, US), Sept 19–22Google Scholar
  9. 9.
    Fitzgibbon A, Pilu M, Fisher RB (1999) Direct least square fitting of ellipses. IEEE Trans Pattern Anal Mach Intell 21(5):CrossRefGoogle Scholar
  10. 10.
    Gebre-Egziabher D, Elkaim G, David Powell J, Parkinson B (2006) Calibration of strapdown magnetometers in magnetic field domain. J Aerosp Eng 19(2):CrossRefGoogle Scholar
  11. 11.
    Halíř R, Flusser J (1998) Numerically stable direct least square fitting of ellipses. In: Proceedings of the WSCG international conference in central Europe on computer graphics and visualizationGoogle Scholar
  12. 12.
    Hermann R, Krener AJ (1977) Nonlinear controllability and observability. IEEE Trans Autom Control 22(5):728–740MathSciNetCrossRefGoogle Scholar
  13. 13.
    Hua M-D, Rudin K, Ducard G, Hamel T, Mahony R (2011) Nonlinear attitude estimation with measurement decoupling and anti-windup gyro-bias compensation. In: Proceedings of the 18th IFAC world congress 2011, Milan (IT), Aug 28–Sept 2, pp 2972–2978CrossRefGoogle Scholar
  14. 14.
    Kok M, Schön TB (2014) Maximum likelihood calibration of a magnetometer using inertial sensors. In: Proceedings of the 19th world congress of the international federation of automatic control, Cape Town (ZA), Aug 24–29, pp 92–97CrossRefGoogle Scholar
  15. 15.
    Mahony RE, Hamel T, Pflimlin JM (2008) Nonlinear complementary filters on the special orthogonal group. IEEE Trans Autom Control 53(5):1203–1218MathSciNetCrossRefGoogle Scholar
  16. 16.
    Markovsky I, Kukush A, Van Huffel S (2004) Consistent least squares fitting of ellipsoids. Numerische Mathematik 98(1):177–194MathSciNetCrossRefGoogle Scholar
  17. 17.
    Martin P, Salaün E (2010) Design and implementation of a low-cost observer-based attitude and heading reference system. Elsevier Control Eng Pract 18(7):712–722CrossRefGoogle Scholar
  18. 18.
    Munson BR, Okiishi TH, Huebsch WW, Rothmayer AP (2012) Fundamentals of Fluid Mechanics, 7th edn. Wiley, USAGoogle Scholar
  19. 19.
    National Geophysics Data Center.
  20. 20.
    Pivano L, Johansen TA, Smogeli ØN (2009) A four-quadrant thrust controller for marine propellers with loss estimation and anti-spin: theory and experiments. Mar Technol 46(4):229–242Google Scholar
  21. 21.
    Siciliano B, Khatib O (2008) Handbook of robotics. Springer handbooks, Napoli and StanfordCrossRefGoogle Scholar
  22. 22.
    Vasconcelos JF, Elkaim G, Silvestre C, Oliveira P, Cardeira B (2011) A geometric approach to strapdown magnetometer calibration in sensor frame. IEEE Trans Aerosp Electron Syst 47(2):1293–1306CrossRefGoogle Scholar

Copyright information

© Springer Nature Switzerland AG 2020

Authors and Affiliations

  1. 1.Department of Industrial EngineeringUniversity of FlorenceFlorenceItaly

Personalised recommendations