Development and Testing of Navigation Algorithms for Autonomous Underwater Vehicles pp 4171  Cite as
Navigation Filter
Abstract
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 realtime. 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 realtime. 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 twelvedimensional 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 disturbancefree measurements only and recognizing and rejecting corrupted compass readings, providing at the same time a solution to estimate the threedimensional 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 UKFbased algorithm, exploiting GPS (on surface), depth, and velocity measurements in order to compute an estimate of the absolute position of the vehicle.
Following the logic of the block scheme of Fig. 4.1, this chapter is organized as follows: in Sect. 4.1, the abovementioned 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 statespace vehicle model the UKFbased 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 Earthfixed 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 bodyfixed 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 bodyframe 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 timevarying bias of the gyroscope.
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 abovementioned 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.

Hard Iron disturbances: permanent magnets and magnetized objects, such as electronic subsystems in the proximity of the sensor, give rise to the socalled 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:\(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.$$\begin{aligned} W=W_{mis}W_{sf}W_{SI} \ . \end{aligned}$$(4.6)
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 MTi300 AHRS taken while rotating such device in space. In the first case, the rotation was performed in a disturbancefree 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 zaxis 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 zaxis, has been derived.

Geometrical shape: long and slender AUVs (torpedoshaped 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).
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 sign is chosen positive if \(\mathbf m _{1}^{\Pi }\), \(\mathbf m _{2}^{\Pi }\), and \(\mathbf m _{3}^{\Pi }\) lie counterclockwise on the ellipse (see Fig. 4.7); this condition can be verified considering the sign of the gyroscope angular rate during the calibration turn.
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 disturbancefree 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
Known Directions Choice
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 realtime 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.
 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}\):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;$$\begin{aligned} D_{a}=\left \frac{\mathbf a _{f}\bar{a}}{\bar{a}} \right \ . \end{aligned}$$(4.29)

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 magnituderelated 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: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}\).$$\begin{aligned} \alpha _{m}^{1}=cos^{1}\left( \left( \mathbf m _{\bot }^{c} \right) ' \hat{\mathbf{H }}_{\bot }^{N,B} \right) \ . \end{aligned}$$(4.30)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: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} \left( \mathbf z ^{N} \right) ' \mathbf H ^{N} = \left( \mathbf a _{f} \right) ' \mathbf m ^{c} \ , \end{aligned}$$(4.31)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.$$\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}$$(4.32)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.
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 singleaxis FOG has been mounted with its sensitive axis parallel to the IMU gyroscope zaxis. Its measurement completely substitutes the angular rate change read on that axis by the IMU gyroscope within the filter.
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 statespace 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 tradeoff between required computational load (since it must be used on board in realtime) 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 torpedoshaped 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 UKFbased 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 abovementioned estimators: indeed, AUV models may be nonlinear and nondifferentiable (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 statespace 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 derivativefree; 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 UKFbased 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 statespace 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 StateSpace Model
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 lookup 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 tradeoff 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
As a concluding remark, it is worth noting that the model in (4.39), together with (4.43)–(4.44), is nonlinear and nondifferentiable (due to the presence of e.g., quadratic and sign functions), which may favor the adoption of a derivativefree 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 realtime 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.
The model in (4.52) is used within the same UKFbased 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.
 1.
the magnitude of the current in the fixed frame is greater than zero;
 2.
the vehicle moves along the positive direction of its body xaxis, 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 bodyframe thrust);
 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.
the orientation of the vehicle is not changing. Recalling that orientation is a timevarying input, this assumption equals to considering the system autonomous. Furthermore, it is assumed that roll and pitch are zero;
 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 bodyframe x and yaxis; this last hypothesis will then be relaxed.
References
 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.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.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.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.Carlton JS (2007) Marine propellers and propulsion, 2nd edn. ElsevierGoogle Scholar
 6.Costanzi R (2015) Navigation systems for unmanned underwater vehicles. PhD Dissertation, University of Florence, Florence (IT)Google Scholar
 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.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.Fitzgibbon A, Pilu M, Fisher RB (1999) Direct least square fitting of ellipses. IEEE Trans Pattern Anal Mach Intell 21(5):CrossRefGoogle Scholar
 10.GebreEgziabher 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.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.Hermann R, Krener AJ (1977) Nonlinear controllability and observability. IEEE Trans Autom Control 22(5):728–740MathSciNetCrossRefGoogle Scholar
 13.Hua MD, Rudin K, Ducard G, Hamel T, Mahony R (2011) Nonlinear attitude estimation with measurement decoupling and antiwindup gyrobias compensation. In: Proceedings of the 18th IFAC world congress 2011, Milan (IT), Aug 28–Sept 2, pp 2972–2978CrossRefGoogle Scholar
 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.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.Markovsky I, Kukush A, Van Huffel S (2004) Consistent least squares fitting of ellipsoids. Numerische Mathematik 98(1):177–194MathSciNetCrossRefGoogle Scholar
 17.Martin P, Salaün E (2010) Design and implementation of a lowcost observerbased attitude and heading reference system. Elsevier Control Eng Pract 18(7):712–722CrossRefGoogle Scholar
 18.Munson BR, Okiishi TH, Huebsch WW, Rothmayer AP (2012) Fundamentals of Fluid Mechanics, 7th edn. Wiley, USAGoogle Scholar
 19.National Geophysics Data Center. www.ngdc.noaa.gov
 20.Pivano L, Johansen TA, Smogeli ØN (2009) A fourquadrant thrust controller for marine propellers with loss estimation and antispin: theory and experiments. Mar Technol 46(4):229–242Google Scholar
 21.Siciliano B, Khatib O (2008) Handbook of robotics. Springer handbooks, Napoli and StanfordCrossRefGoogle Scholar
 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