Implementation of a Low-Cost Navigation System Using Data Fusion of a Micro-Electro-Mechanical System Inertial Sensor and an Ultra Short Baseline on a Microcontroller

Abstract

In this work, a low-cost low-power navigation solution for autonomous underwater vehicles is introduced utilizing a Micro-Electro-Mechanical System (MEMS) inertial sensor and an ultra short baseline (USBL) system. The complete signal processing is implemented on a cheap 16-bit fixed-point arithmetic microcontroller. For data fusion and calibration, an error state Kalman filter in square root form is used, which preserves stability in case of rounding errors. To further reduce the influence of rounding errors, a stochastic rounding scheme is applied. The USBL measurements are integrated using tightly coupled data fusion by deriving the observation functions separately for range, elevation, and azimuth angles. The effectiveness of the fixed point implementation with stochastic rounding is demonstrated on a simulation, and the the complete setup is tested in a field test. The results of the field test show an improved accuracy of the tightly coupled data fusion in comparison with loosely coupled data fusion. It is also shown that the applied rounding schemes can bring the fixed-point estimates to a near floating point accuracy.

1. Introduction

Autonomous Underwater Vehicles (AUVs) have revolutionized oceanographic research and exploration by enabling autonomous operations in deep, inaccessible, and hazardous underwater environments. A pivotal challenge in their deployment, however, lies in achieving precise underwater navigation. Unlike terrestrial or aerial systems, AUVs cannot reliably utilize Global Navigation Satellite Systems (GNSSs) due to rapid signal attenuation in water, restricting GNSS availability to brief surface intervals [1]. Consequently, navigation predominantly relies on inertial systems such as Fiber-Optic Gyroscopes (FOGs) in combination with dead reckoning based on Doppler Velocity Logs (DVLs). While these systems offer high accuracy, their substantial cost—particularly for high-precision FOG and DVL units—poses a significant barrier to affordability, especially for smaller AUVs, where they may constitute over $50\%$ of total platform expenses.

To bridge this gap, recent advancements focus on integrating low-cost Micro-Electro-Mechanical System (MEMS)-based inertial sensors with acoustic positioning through sensor fusion techniques, such as Kalman filtering [2].

In this work, we present a novel, cost-effective navigation solution for AUVs combining MEMS-based inertial measurement with acoustic positioning, optimized for real-time operation on low-power microcontrollers. By prioritizing computational efficiency without sacrificing performance, this system aims to democratize AUV access for scientific and commercial applications—from marine biodiversity mapping to underwater infrastructure inspection—where high-cost solutions remain prohibitive. The integration of modular, low-power hardware further ensures scalability across AUV classes, underscoring our goal to redefine the balance between precision, affordability, and operational versatility in underwater navigation.

2. Related Work

Underwater navigation makes extensive use of inertial navigation. Because pure inertial navigation is not applicable for more than a few minutes [3], it is normally integrated in multi-sensor data fusion [4].

Combined navigation and sensor data fusion in underwater applications is mostly done using extended Kalman filters [5,6,7]. In [8], an unscented Kalman filter is used to model the distribution of quantization noise. For three-dimensional navigation, error state Kalman filters are used in combination with quaternion calculations, as detailed in [9]. Such filters are typically implemented with 32-bit or 64-bit floating-point calculation. Before sufficient calculation power was available, Kalman filters were often implemented in square root form [10,11]. Square root Kalman filters are typically used for simple systems with a small number of optimizable states. With digital signal processors capable of performing millions of fixed-point multiplications per second, it has become viable to use square root Kalman filters for complex navigation and data fusion tasks, while embedded class processors are often much more cost and power efficient than application class processors.

In [12], a navigation solution for AUVs is proposed based on tightly coupled data fusion that combines an inertial sensor and DVL. A chi-square detection-aided dual-factor adaptive filter is used to suppress outliers. Ref. [13] proposes Inertial Navigation System (INS)/DVL data fusion using a square-root unscented information filter to ensure the symmetry and positive definiteness of the covariance matrix or information matrix and to enhance the stability of the filter. Ref. [14] proposes a virtual beam predictor based on multi-output least-squares support vector regression in order to allow for data fusion when not all DVL beams are available. In [15], an extended exponential weighted Kalman filter is combined with a long short-term memory neural network to make the DVL/INS data fusion more robust against outliers. These advanced filtering algorithms often come with high computational complexity and are therefore difficult to realize in a small, low-cost AUVs. Therefore, ref. [16] proposed a low-cost AUV navigation solution using an extended Kalman filter of only five dimensions in combination with complementary filtering.

In summary, this literature review highlights the importance of having high calculation precision. Our proposed method combines low-cost hardware with advanced implementation methods.

While data fusion between MEMS inertial sensors and USBL is well-established [16,17,18], our contribution focuses on implementing this fusion using 16-bit fixed-point arithmetic and analyzing the impact of rounding errors in this context. Our method is able to effectively reduce the influence of rounding errors introduced by 16-bit fixed point representation. The reduced calculation cost allows us to use a high-dimensional state vector without losing real-time capabilities when implemented on an inexpensive microcontroller. This can, for example, be used to perform calibration and data fusion with the same filter.

3. System Overview

The navigation solution used in this work consists of a Seatrac X150 Ultra-Short Baseline (USBL) beacon, an Advanced Navigation Spatial MEMS inertial sensor, and a Microchip dsPIC33CH512MP508 microcontroller. For laboratory tests, the MEMS sensor was replaced by a much cheaper Bosch BNO055. The microcontroller and the MEMS sensor were directly connected via an RS232 interface, and acceleration, gyroscope, and magnetic measurements were sent to the microcontroller with a sampling rate of 100 Hz. The microcontroller’s main loop also operated at 100 Hz, synchronizing itself to the MEMS sampling rate. A second RS232 interface linked the microcontroller to the AUV’s main PC to transmit the position and velocity estimates. This connection also facilitated the transfer of additional data, such as USBL pings, to the microcontroller and enabled the storage of raw MEMS measurements for post-processing. When using the BNO055 sensor, an I2C interface was employed to connect it to the microcontroller. The dsPIC33CH microcontroller family is a dual-core system, with both cores featuring DSP units. In our setup, one core was dedicated to real-time signal processing, while the other handled communication with the host PC and the MEMS sensor.

4. Data Fusion Method

A common way to describe the position and motion of a vehicle in three-dimensional space is an indirect or error state Kalman filter combining a quaternion equation and linear algebra equations, as described in detail in [9]. The proposed data fusion method uses an error state Kalman filter with a 15-dimensional state vector $\mathbf{x}$ including position $\mathbf{p}$, velocity $\mathbf{v}$, rotation vector $\mathit{\theta }$, accelerometer bias ${\mathbf{a}}_b$, and gyroscope bias ${\mathit{\omega }}_b$. Each of them is three-dimensional.

$$\mathbf{x}={\left[\begin{array}{ccccc}\mathbf{p}& \mathbf{v}& \mathit{\theta }& {\mathbf{a}}_b& {\mathit{\omega }}_b\end{array}\right]}^{\top }$$

(1)

The measurement input vector ${\mathbf{u}}_m$ consists of the accelerometer measurement ${\mathbf{a}}_m$ and the gyroscope measurement ${\mathit{\omega }}_m$.

$${\mathbf{u}}_m={\left[\begin{array}{cc}{\mathbf{a}}_m& {\mathit{\omega }}_m\end{array}\right]}^{\top }$$

(2)

For the nominal state, the rotation vector $\mathit{\theta }$ is replaced by unit quaternion $\mathbf{q}\left\{\mathit{\theta }\right\}$, where $\mathbf{q}\{·\}$ is the quaternion exponential map. Quaternion multiplications are written as $\mathbf{q}={\mathbf{q}}_1\otimes {\mathbf{q}}_2$ in order to be distinguished from normal matrix multiplications.

The state transition ${\mathbf{x}}_{k+1}=\mathbf{f}\left({\mathbf{x}}_k\right)$ is defined in the Equations (3)–(5), where the index k marks the current time step and $k+1$ marks the next time step of a time discrete system. $\Delta t$ is the sampling period, $\mathbf{g}$ is the gravity vector, and $\mathbf{R}\left\{\mathit{\theta }\right\}$ is the exponential map of the rotation vector $\mathit{\theta }$ as rotation matrix.

$$\begin{array}{cc}\hfill {\mathbf{p}}_{k+1}& ={\mathbf{p}}_k+{\mathbf{v}}_k\Delta t+{\displaystyle \frac{1}{2}}\left(\mathbf{R}\left\{{\mathit{\theta }}_k\right\}({\mathbf{a}}_{m,k}-{\mathbf{a}}_{b,k})+\mathbf{g}\right){\Delta t}^2\hfill \end{array}$$

(3)

$$\begin{array}{cc}\hfill {\mathbf{v}}_{k+1}& ={\mathbf{v}}_k+\left(\mathbf{R}\left\{{\mathit{\theta }}_k\right\}·({\mathbf{a}}_{m,k}-{\mathbf{a}}_{b,k})+\mathbf{g}\right)\Delta t\hfill \end{array}$$

(4)

$$\begin{array}{cc}\hfill \mathbf{q}\left\{{\mathit{\theta }}_{k+1}\right\}& =\mathbf{q}\left\{{\mathit{\theta }}_k\right\}\otimes \mathbf{q}\{({\mathit{\omega }}_{m,k}-{\mathit{\omega }}_{b,k})·\Delta t\}\hfill \end{array}$$

(5)

The state transition function can be linearized around a given working point of $\mathbf{x}$ and ${\mathbf{u}}_m$ into a matrix ${\mathbf{F}}_{\mathbf{x}}$ and ${\mathbf{F}}_{\mathbf{u}}$ such that ${\mathbf{x}}_{k+1}={\mathbf{F}}_{\mathbf{x},k}·{\mathbf{x}}_k+{\mathbf{F}}_{\mathbf{u},k}·{\mathbf{u}}_{m,k}$. Similar to [9], ${\mathbf{F}}_{\mathbf{x}}$ and ${\mathbf{F}}_{\mathbf{u}}$ can be derived as shown in Equations (6) and (7). $\mathbf{I}$ is the $3\times 3$ identity matrix and ${[·]}_{\times }$ is the skew operator, such that $\mathbf{a}\times \mathbf{b}={\left[\mathbf{a}\right]}_{\times }·\mathbf{b}$. The rotation matrix is written as $\mathbf{R}=\mathbf{R}\left\{\mathit{\theta }\right\}$.

$$\begin{array}{l}{\mathbf{F}}_{\mathbf{x}}={\left.{\displaystyle \frac{\partial \mathbf{f}}{\partial \delta \mathbf{x}}}\right|}_{\mathbf{x},{\mathbf{u}}_m}=\hfill \\ \left[\begin{array}{ccccc}\mathbf{I}& \mathbf{I}\Delta t& 0& 0& 0\\ 0& \mathbf{I}& -{\left[\mathbf{R}({\mathbf{a}}_m-{\mathbf{a}}_b)\right]}_{\times }\Delta t& -\mathbf{R}\Delta t& 0\\ 0& 0& \mathbf{I}& 0& -\mathbf{R}\Delta t\\ 0& 0& 0& \mathbf{I}& 0\\ 0& 0& 0& 0& \mathbf{I}\end{array}\right]\hfill \end{array}$$

(6)

$$\begin{array}{cc}\hfill & {\mathbf{F}}_{\mathbf{u}}={\left.{\displaystyle \frac{\partial \mathbf{f}}{\partial \delta {\mathbf{u}}_m}}\right|}_{\mathbf{x},{\mathbf{u}}_m}=\left[\begin{array}{cc}0& 0\\ \mathbf{R}\Delta t& 0\\ 0& \mathbf{R}\Delta t\\ 0& 0\\ 0& 0\end{array}\right]\hfill \end{array}$$

(7)

For data fusion and filtering, the state vector $\mathbf{x}$ will be estimated on each time step k as a state estimate vector ${\widehat{\mathbf{x}}}_k$ with a covariance matrix ${\mathbf{P}}_k$. The Kalman filter splits the estimation into a prediction and correction step. The prediction step is defined as shown in Equations (8) and (9), where $\mathbf{Q}$ is the covariance matrix of $\mathbf{u}$. The prediction step gives the a priori estimate marked with index $k|k-1$.

$$\begin{array}{cc}\hfill {\widehat{\mathbf{x}}}_{k|k-1}& ={\mathbf{F}}_{\mathbf{x}}·{\widehat{\mathbf{x}}}_{k-1}+{\mathbf{F}}_{\mathbf{u}}·{\mathbf{u}}_k\hfill \end{array}$$

(8)

$$\begin{array}{cc}\hfill {\mathbf{P}}_{k|k-1}& ={\mathbf{F}}_{\mathbf{x}}·{\mathbf{P}}_{k-1}·{\mathbf{F}}_{\mathbf{x}}^{\top }+{\mathbf{F}}_{\mathbf{u}}·\mathbf{Q}·{\mathbf{F}}_{\mathbf{u}}^{\top }\hfill \end{array}$$

(9)

Given an external observation of the system state $\mathbf{y}=\mathbf{h}\left(\mathbf{x}\right)$ and the associated observation matrix $\mathbf{H}={\left.{\displaystyle \frac{\partial \mathbf{h}}{\partial \delta \mathbf{x}}}\right|}_{\mathbf{x}}$ and observation covariance matrix $\mathbf{V}$, the correction step can be defined as shown in Equations (10)–(12). The intermediate matrix $\mathbf{K}$ is the Kalman gain.

$$\begin{array}{cc}\hfill {\mathbf{K}}_k& ={\mathbf{P}}_{k|k-1}{\mathbf{H}}^{\top }{\left(\mathbf{H}{\mathbf{P}}_{k|k-1}{\mathbf{H}}^{\top }+\mathbf{V}\right)}^{-1}\hfill \end{array}$$

(10)

$$\begin{array}{cc}\hfill {\widehat{\mathbf{x}}}_k& ={\widehat{\mathbf{x}}}_{k|k-1}+{\mathbf{K}}_k\left(\mathbf{y}-\mathbf{H}·{\widehat{\mathbf{x}}}_{k|k-1}\right)\hfill \end{array}$$

(11)

$$\begin{array}{cc}\hfill {\mathbf{P}}_k& =\left(\mathbf{I}-{\mathbf{K}}_k\mathbf{H}\right){\mathbf{P}}_{k|k-1}\hfill \end{array}$$

(12)

The Kalman filter steps can be transferred into a square root form. In this work, the square root form of Cholesky factors is used. The Cholesky factor of a matrix $\mathbf{M}$ is here written as ${\mathbf{M}}^{1/2}$, such that $\mathbf{M}={\mathbf{M}}^{1/2}·{\mathbf{M}}^{\top /2}$. To create the square root form, all covariance matrices are replaced by their Cholesky factors. As shown in [19], the prediction step in square root form can be written as shown in Equation (13) where $\mathbf{S}={\mathbf{P}}^{1/2}$ is the Cholesky factor of the covariance of the state estimate and $\mathbf{G}$ is an orthogonal matrix used to compress $\mathbf{S}$ back in its original size.

$$\left[\begin{array}{c}{\left({\mathbf{S}}_{k|k-1}\right)}^{\top }\\ 0\end{array}\right]=\mathbf{G}\left[\begin{array}{c}{({\mathbf{F}}_{\mathbf{x}}·{\mathbf{S}}_{k-1})}^{\top }\\ {({\mathbf{F}}_{\mathbf{u}}·{\mathbf{Q}}^{1/2})}^{\top }\end{array}\right]$$

(13)

The correction step is transferred into its square root form according to [19], resulting in Equation (14).

$$\begin{array}{cc}\hfill & \left[\begin{array}{ccc}{(\mathbf{V}+\mathbf{H}{\mathbf{P}}_k{\mathbf{H}}^{\top })}^{\top /2}& & {(\mathbf{V}+\mathbf{H}{\mathbf{P}}_k{\mathbf{H}}^{\top })}^{\top /2}{\mathbf{K}}_k^{\top }\\ 0& & {\mathbf{S}}_k^{\top }\end{array}\right]\hfill \\ \hfill & =\mathbf{G}\left[\begin{array}{ccc}{\mathbf{V}}^{\top /2}& & 0\\ {\mathbf{S}}_{k|k-1}^{\top }{\mathbf{H}}^{\top }& & {\mathbf{S}}_{k|k-1}^{\top }\end{array}\right]\hfill \end{array}$$

(14)

After extracting $\mathbf{K}$ and $\mathbf{V}$ from the result of Equation (14), the new state estimate $\widehat{\mathbf{x}}$ can be obtained the same way as in Equation (11).

Each time when calculating Equations (13) and (14), an orthogonal transformation represented by $\mathbf{G}$ has to be found, which creates the required zeros in the bottom left part of the result matrix. This is the same process as applying a QR-decomposition, which splits a square matrix into a triangular and an orthogonal part. The orthogonal matrix can be discarded, and the triangular matrix corresponds to the left hand side of Equation (13) or (14). There are multiple methods for performing the QR-decomposition. The most common methods utilize either Givens rotations or Householder transformations. The method using Householder transformations is in general more efficient for non-sparse matrices, as it can be applied per column instead of per element. Therefore, Householder transformations are used in this work.

4.1. QR Decomposition Using the Householder Transformation

As described in [20], the Householder transformation can be performed as a series of Householder reflections, where each column vector of a matrix $\mathbf{M}$ is mirrored in the same hyperplane. As shown in Figure 1, the column vector is called $\mathbf{x}$, the normal vector of the hyperplane is $\mathbf{u}$, and the basis vector of the target direction is ${\mathbf{e}}_1$. To create a triangular matrix, the reflection plane must be positioned in such a way that all elements of the first column vector except the first are 0 and the column vector lies on the first basis vector ${\mathbf{e}}_1$. Because the amounts of the column vectors must not change during an orthogonal transformation, the first column vector $\mathbf{x}$ must therefore become $\pm \left|\right|\mathbf{x}\left|\right|·{\mathbf{e}}_1$ after the transformation.

The normal vector $\mathbf{u}$ of the hyperplane can be found by subtracting the desired first column vector $\pm \left|\right|\mathbf{x}\left|\right|·{\mathbf{e}}_1$ from the current first column vector $\mathbf{x}$, as shown in Equation (15). The normalized normal vector $\mathbf{v}$ is then calculated from this, as shown in Equation (16).

$$\begin{array}{cc}\hfill \mathbf{u}& =\mathbf{x}\pm \left|\right|\mathbf{x}\left|\right|·{\mathbf{e}}_1\hfill \end{array}$$

(15)

$$\begin{array}{cc}\hfill \mathbf{v}& ={\displaystyle \frac{\mathbf{u}}{\left|\right|\mathbf{u}\left|\right|}}\hfill \end{array}$$

(16)

The new sign for the first element of the new column vector is arbitrary, but it should be the opposite of the previous sign. This prevents the vector $\mathbf{u}$ from becoming very small, leading to rounding errors.

$$\mathbf{u}=\mathbf{x}+\mathrm{sgn}\left({\mathbf{x}}_1\right)·\left|\right|\mathbf{x}\left|\right|·{\mathbf{e}}_1$$

To mirror each column vector $\mathbf{x}$ on the plane with the normal vector $\mathbf{v}$, $\mathbf{x}$ must first be projected onto $\mathbf{v}$ and then subtracted twice from $\mathbf{x}$.

$$\mathbf{x}\leftarrow \mathbf{x}-2\mathbf{v}\left({\mathbf{v}}^{\top }\mathbf{x}\right)$$

In each step, two square root operations are required to determine the amounts of $\mathbf{x}$ and $\mathbf{v}$. The second root operation can be saved by using $\mathbf{u}$ directly instead of $\mathbf{v}$.

$$\mathbf{x}\leftarrow \mathbf{x}-2{\displaystyle \frac{\mathbf{u}}{\left|\right|\mathbf{u}\left|\right|}}\left({\left({\displaystyle \frac{\mathbf{u}}{\left|\right|\mathbf{u}\left|\right|}}\right)}^{\top }\mathbf{x}\right)=\mathbf{x}-2\mathbf{u}{\displaystyle \frac{{\mathbf{u}}^{\top }\mathbf{x}}{{\left|\right|\mathbf{u}\left|\right|}^2}}=\mathbf{x}-2\mathbf{u}{\displaystyle \frac{{\mathbf{u}}^{\top }\mathbf{x}}{{\mathbf{u}}^{\top }\mathbf{u}}}$$

The same process can then be applied to matrix $\mathbf{M}$ again, except that one column and one row are omitted each time. This is repeated until $\mathbf{M}$ becomes an upper triangular matrix.

$$\left[\begin{array}{cccc}x& x& x& x\\ x& x& x& x\\ x& x& x& x\\ x& x& x& x\end{array}\right]\stackrel{{\mathbf{Q}}_1}{\to }\left[\begin{array}{cccc}x& x& x& x\\ 0& x& x& x\\ 0& x& x& x\\ 0& x& x& x\end{array}\right]\stackrel{{\mathbf{Q}}_2}{\to }\left[\begin{array}{cccc}x& x& x& x\\ 0& x& x& x\\ 0& 0& x& x\\ 0& 0& x& x\end{array}\right]\stackrel{{\mathbf{Q}}_3}{\to }\left[\begin{array}{cccc}x& x& x& x\\ 0& x& x& x\\ 0& 0& x& x\\ 0& 0& 0& x\end{array}\right]$$

4.2. Fixed-Point Implementation

To implement the whole filter on a microcontroller with only fixed point arithmetic, a new data structure has been designed to hold the Cholesky factor $\mathbf{S}$ of the covariance of state estimate, which adds an additional per-row exponent to the matrix containing only fixed point representations of its elements. As shown in Equation (17), the matrix $\mathbf{S}$ is split into a diagonal exponent matrix $\mathbf{E}$ containing exponents with base 2 and a mantissa matrix $\mathbf{M}$ containing fixed point values. This way, the $\mathbf{E}$ matrix can be ignored during the QR-decomposition, because it is diagonal and thus also triangular. Only the $\mathbf{M}$ matrix needs to be QR-decomposed. This means that the computation-intensive Householder transforms can be calculated completely in fixed point arithmetic, while the complete $\mathbf{S}$ matrix still has some advantages of floating point numbers.

$$\begin{array}{cc}\hfill \mathbf{S}& =\mathbf{E}·\mathbf{M}=\left[\begin{array}{ccc}{2}^{e_1}& 0& 0\\ 0& 2^{e_2}& 0\\ 0& 0& 2^{e_3}\end{array}\right]·\left[\begin{array}{ccc}{m}_{1,1}& m_{1,2}& m_{1,3}\\ m_{2,1}& m_{2,2}& m_{2,3}\\ m_{3,1}& m_{3,2}& m_{3,3}\end{array}\right]\hfill \\ \hfill & =\left[\begin{array}{ccc}{m}_{1,1}·2^{e_1}& m_{1,2}·2^{e_1}& m_{1,3}·2^{e_1}\\ m_{2,1}·2^{e_2}& m_{2,2}·2^{e_2}& m_{2,3}·2^{e_2}\\ m_{3,1}·2^{e_3}& m_{3,2}·2^{e_3}& m_{3,3}·2^{e_3}\end{array}\right]\hfill \end{array}$$

(17)

4.3. Stochastic Rounding

When using limited precision values like 16-bit signed integers for the mantissas in $\mathbf{M}$, each multiplication of two of these numbers results in a 32-bit integer, which needs to be rounded to be stored in a 16-bit register again. With the typical Round to Nearest (RN) approach, this can lead to some unfavorable effects, as the rounding errors often sum up quickly over the iterations of the filter. By replacing the RN method with a Stochastic Rounding (SR) method, the behavior can be made much more predictable, as the accumulated rounding error over time can now be described with a random walk function [22], whose expected magnitude over time follows a square root function. In addition to multiplications, the Householder algorithm also requires square root and division operations whose results have to be rounded as well. The SR implementations for the different cases are shown in Algorithms 1–3. The given pseudo code uses ≫ to represent bit-wise right shift and ≪ for bit-wise left shift. $\mathrm{PRNG}32\left(\right)$ and $\mathrm{PRNG}16\left(\right)$ are 32-bit and 16-bit Pseudorandom Number Generator (PRNG) functions.

Algorithm 1 Rounding last n bits of 32-bit integer with stochastic rounding

function ROUND16_STOCHASTIC(x, n)     $\mathrm{mask}\leftarrow (1\ll n)-1$     $p\leftarrow \mathrm{PRNG}32\left(\right)\&\mathrm{mask}$     $\mathrm{residual}\leftarrow x\&\mathrm{mask}$     $x\leftarrow x\gg n$     if $p<\mathrm{residual}$ then         $x\leftarrow x+1$     end if     return x end function

Algorithm 2 Integer division with stochastic rounding

function DIVISION_STOCHASTIC($x_1$, $x_2$)     $x_1\leftarrow x_1+((x_2·\mathrm{PRNG}16\left(\right))\gg 16)$     return $x_1/x_2$ end function

Algorithm 3 Integer square root with stochastic rounding

function SQRT_STOCHASTIC($\mathrm{value}$)     $\mathrm{result}\leftarrow 0$     $\exp \leftarrow 0\mathrm{x}8000$     while $exp\ne 0$ do         $\mathrm{temp}\leftarrow \mathrm{result}|\exp$         if ($\mathrm{temp}·\mathrm{temp})\le \mathrm{value}$ then            $\mathrm{result}\leftarrow \mathrm{temp}$         end if         $\exp \leftarrow \exp \gg 1$     end while                                                                 ▹ rounding last bit with stochastic rounding     ${\mathrm{temp}}_{32}\leftarrow (\mathrm{result}\ll 16)+\mathrm{PRNG}16\left(\right)$     if $(({\mathrm{temp}}_{32}·{\mathrm{temp}}_{32})\gg 32)<\mathrm{value}$ then         $\mathrm{result}\leftarrow \mathrm{result}+1$     end if     return $\mathrm{result}$ end function

To illustrate the effectiveness of the stochastic rounding method, a simplified abstract system is constructed. The system consists of a three-dimensional state vector and uses an identity matrix as the state transition model. At each iteration, the filter receives a one-dimensional observation—a linear combination of the system states—with a constant standard deviation. Following each update, the predicted state uncertainty is evaluated using the filter’s covariance matrix, or the square root of the covariance matrix in the case of the fixed-point implementation. The deviation is expected to go down as the filter incorporates more observations over time.

As can be seen in Figure 2, the SR implementation result is much closer to the result from floating-point arithmetic. Because of the low precision of the 16-bit numerical representation, there is only a very limited amount of possible step sizes. It can be clearly seen that the RN implementation utilizes the nearest possible step size, but it diverges quickly from the floating-point results.

To assess the impact of rounding errors in a more realistic scenario, the following setup was simulated. The initial three-dimensional position, velocity, and orientation are assumed to be known. Then, for 5 s, only noisy inertial measurements are available. After the first 5 s, an additional noisy position measurement becomes available. The three-dimensional angular velocity measurement has a noise spectral density of $3\times {10}^{-7}\mathrm{rad}/\mathrm{s}/\sqrt{\mathrm{Hz}}$, and the acceleration measurement has a noise spectral density of $0.3\mathrm{m}/{\mathrm{s}}^2/\sqrt{\mathrm{Hz}}$. The additional noisy position measurement has a noise spectral density of $1.5\mathrm{m}/\sqrt{\mathrm{Hz}}$. Figure 3a shows the simulation results for floating-point arithmetic, averaged over 100 repeated simulations, with the absolute actual error plotted against the predicted error. In the first 5 s, the position estimation uncertainty increases mainly because of the acceleration measurement noise. After the position measurement becomes available, the uncertainty stabilizes at above 2 m error. Figure 3b shows the difference between the results with floating point and fixed point arithmetic. Consistent with Figure 2, the SR method provides significantly more accurate predicted uncertainty compared to RN. This difference has minimal impact on the actual error during the pure inertial phase. However, when the position measurement is available, the inaccurately predicted uncertainty affects data fusion, resulting in a slight increase in the actual error.

4.4. Tightly Coupled Data Fusion with USBL

The data fusion with USBL measurements can be implemented either using a Loosely Coupled (LC) or Tightly Coupled (TC) approach [23]. The TC method is chosen here because it allows us to have separate certainty values for the measurement of range and angle. The known beacon position in global coordinates ${\mathbf{p}}_{\mathrm{beacon}}$ is first transferred to the body coordinate system, as shown in Equation (18). Then, the observation vector $\mathbf{y}={\left[\begin{array}{ccc}{r}_m& {\beta }_{az,m}& {\beta }_{el,m}\end{array}\right]}^{\top }$ including range, azimuth angle, and elevation angle can be predicted using the $\mathbf{h}\left(\mathbf{x}\right)$ function given in Equation (19).

$$\begin{array}{cc}\hfill {\mathbf{p}}_B& ={\mathbf{R}}^{\top }\left\{\mathit{\theta }\right\}({\mathbf{p}}_{\mathrm{beacon}}-\mathbf{p})\hfill \end{array}$$

(18)

$$\begin{array}{cc}\hfill \mathbf{h}& =\left[\begin{array}{c}\left|\right|{\mathbf{p}}_B\left|\right|\\ \mathrm{atan}2\left({\mathbf{p}}_{B,y},{\mathbf{p}}_{B,x}\right)\\ \mathrm{atan}2\left({\mathbf{p}}_{B,z},\left|\right|{\mathbf{p}}_{B,xy}\left|\right|\right)\end{array}\right]\hfill \end{array}$$

(19)

This function is then linearized around the current state estimate to obtain the observation matrix ${\mathbf{H}}_{\mathbf{x}}$ given in Equation (20).

$${\mathbf{H}}_{\mathbf{x}}={\left.{\displaystyle \frac{\partial \mathbf{h}\left(\mathbf{x}\right)}{\partial \mathbf{x}}}\right|}_{\mathbf{x}}=\left[\begin{array}{ccccc}{\displaystyle \frac{\partial \mathbf{h}}{\partial \mathbf{p}}}& 0& {\displaystyle \frac{\partial \mathbf{h}}{\partial \mathit{\theta }}}& 0& \dots \end{array}\right]$$

(20)

Here, the derivative of the observation vector relative to the position vector is given by ${\displaystyle \frac{\partial \mathbf{h}}{\partial \mathbf{p}}}=-{\displaystyle \frac{\partial \mathbf{h}}{\partial {\mathbf{p}}_B}}{\mathbf{R}}^{\top }\left\{\mathit{\theta }\right\}$, with the derivative of the observation relative to the beacon position in body coordinates given in Equation (21). The derivative of the observation relative to the rotation vector $\mathit{\theta }$ is given by ${\displaystyle \frac{\partial \mathbf{h}}{\partial \mathit{\theta }}}={\displaystyle \frac{\partial \mathbf{h}}{\partial {\mathbf{p}}_B}}{\mathbf{R}}^{\top }\left\{\mathit{\theta }\right\}{[{\mathbf{p}}_{\mathrm{beacon}}-\mathbf{p}]}_{\times }$.

$$\begin{array}{cc}\hfill {\displaystyle \frac{\partial r}{\partial {\mathbf{p}}_B}}& ={\displaystyle \frac{{\mathbf{p}}_B}{\left|\right|{\mathbf{p}}_B\left|\right|}}\hfill \\ \hfill {\displaystyle \frac{\partial {\beta }_{az}}{\partial {\mathbf{p}}_B}}& ={\displaystyle \frac{\left[\begin{array}{ccc}{\mathbf{p}}_{B,y}& -{\mathbf{p}}_{B,x}& 0\end{array}\right]}{\left|\right|{\mathbf{p}}_{B,xy}{\left|\right|}^2}}\hfill \\ \hfill {\displaystyle \frac{\partial {\beta }_{el}}{\partial {\mathbf{p}}_B}}& ={\displaystyle \frac{\left[\begin{array}{ccc}-\frac{{\mathbf{p}}_{B,z}·{\mathbf{p}}_{B,x}}{\left|\right|{\mathbf{p}}_{B,xy}\left|\right|}& -\frac{{\mathbf{p}}_{B,z}·{\mathbf{p}}_{B,y}}{\left|\right|{\mathbf{p}}_{B,xy}\left|\right|}& \left|\right|{\mathbf{p}}_{B,xy}\left|\right|\end{array}\right]}{\left|\right|{\mathbf{p}}_B{\left|\right|}^2}}\hfill \\ \hfill {\displaystyle \frac{\partial \mathbf{h}}{\partial {\mathbf{p}}_B}}& =\left[\begin{array}{c}{\displaystyle \frac{\partial r}{\partial {\mathbf{p}}_B}}\\ {\displaystyle \frac{\partial {\beta }_{az}}{\partial {\mathbf{p}}_B}}\\ {\displaystyle \frac{\partial {\beta }_{el}}{\partial {\mathbf{p}}_B}}\end{array}\right]\hfill \end{array}$$

(21)

5. Sensor Calibration

While the sensor calibration process typically requires precise reference measurement systems, the previously described Kalman filter can also be extended to estimate the sensors’ orientation together with calibration parameters. This way, the calibration process can be performed without additional reference measurements. Therefore, the state estimate $\mathbf{x}$ of Equation (1) has to be extended as shown in Equation (22), where ${\mathbf{a}}_s$ and ${\mathit{\omega }}_s$ are the sensitivity correction factors for each axis of the accelerometer and the gyroscope, and $\mathit{\alpha }={\left[\begin{array}{ccc}{\alpha }_{yz}& {\alpha }_{zy}& {\alpha }_{zx}\end{array}\right]}^{\top }$ and $\mathit{\gamma }={\left[\begin{array}{cccccc}{\gamma }_{yz}& {\gamma }_{zy}& {\gamma }_{zx}& {\gamma }_{xy}& {\gamma }_{xy}& {\gamma }_{yx}\end{array}\right]}^{\top }$ are the axis misalignment angles for the accelerometer and gyroscope.

$$\mathbf{x}={\left[\begin{array}{ccccccccc}\mathbf{p}& \mathbf{v}& \mathit{\theta }& {\mathbf{a}}_b& {\mathit{\omega }}_b& {\mathbf{a}}_s& {\mathit{\omega }}_s& \mathit{\alpha }& \mathit{\gamma }\end{array}\right]}^{\top }$$

(22)

To define the axis misalignment angles, a reference coordinate system is needed. This reference system is defined as the Accelerometer Orthogonal Frame (AOF) [24], where the measured accelerometer x-axis lies directly on the x-axis of the AOF. The y-axis of the AOF is fixed to be in the plane spanned by the x-axis and y-axis of the accelerometer measurement axes. The z-axis of the AOF is then given solely by the orthogonality of the system. Therefore, three calibration angles are needed to align the accelerometer to the AOF, and six calibration angles are needed to align the gyroscope. An accelerometer measurement vector ${\mathbf{a}}_m$ can be transformed into the AOF by multiplying it matrix ${\mathbf{T}}_a$ as shown in Equation (23). Similarly, the gyroscope measurement is aligned as shown in Equation (24).

$$\begin{array}{cc}\hfill {\mathbf{a}}_{\mathrm{AOF}}& ={\mathbf{T}}_a·{\mathbf{a}}_m=\left[\begin{array}{ccc}1& -{\alpha }_{yz}& {\alpha }_{zy}\\ 0& 1& -{\alpha }_{zx}\\ 0& 0& 1\end{array}\right]·{\mathbf{a}}_m\hfill \end{array}$$

(23)

$$\begin{array}{cc}\hfill {\mathit{\omega }}_{\mathrm{AOF}}& ={\mathbf{T}}_g·{\mathit{\omega }}_m=\left[\begin{array}{ccc}1& -{\gamma }_{yz}& {\gamma }_{zy}\\ {\gamma }_{xy}& 1& -{\gamma }_{zx}\\ -{\gamma }_{xy}& {\gamma }_{yx}& 1\end{array}\right]·{\mathit{\omega }}_m\hfill \end{array}$$

(24)

The state transition $\mathbf{f}\left(\mathbf{x}\right)$ previously defined in Equations (3)–(5) is extended as shown in Equations (25)–(27), where the ⊙ operator represents an elementwise multiplication between two vectors.

$$\begin{array}{cc}\hfill {\mathbf{p}}_{k+1}& ={\mathbf{p}}_k+{\mathbf{v}}_k\Delta t+{\displaystyle \frac{1}{2}}\left(\mathbf{R}\left\{{\mathit{\theta }}_k\right\}{\mathbf{T}}_a{\mathbf{a}}_k+\mathbf{g}\right){\Delta t}^2\hfill \end{array}$$

(25)

$$\begin{array}{cc}\hfill {\mathbf{v}}_{k+1}& ={\mathbf{v}}_k+\left(\mathbf{R}\left\{{\mathit{\theta }}_k\right\}{\mathbf{T}}_a{\mathbf{a}}_k+\mathbf{g}\right)\Delta t\hfill \end{array}$$

(26)

$$\begin{array}{cc}\hfill \mathbf{q}\left\{{\mathit{\theta }}_{k+1}\right\}& =\mathbf{q}\left\{{\mathit{\theta }}_k\right\}\otimes \mathbf{q}\{{\mathbf{T}}_g{\mathit{\omega }}_k\Delta t\}\hfill \\ \hfill {\mathbf{a}}_k& ={\mathbf{a}}_s\odot {\mathbf{a}}_{m,k}-{\mathbf{a}}_b\hfill \\ \hfill {\mathit{\omega }}_k& ={\mathit{\omega }}_s\odot {\mathit{\omega }}_{m,k}-{\mathit{\omega }}_b\hfill \end{array}$$

(27)

The new state transition is linearized around a given working point of $\mathbf{x}$, as shown in Equation (28), replacing the ${\mathbf{F}}_{\mathbf{x}}$ previously defined in (6).

$${\mathbf{F}}_{\mathbf{x}}={\left.{\displaystyle \frac{\partial \mathbf{f}}{\partial \delta \mathbf{x}}}\right|}_{\mathbf{x},{\mathbf{u}}_m}=\left[\begin{array}{ccccccccc}\mathbf{I}& \mathbf{I}\Delta t& 0& 0& 0& 0& 0& 0& 0\\ 0& \mathbf{I}& {\mathbf{F}}_1& {\mathbf{F}}_2& 0& {\mathbf{F}}_4& 0& {\mathbf{F}}_6& 0\\ 0& 0& \mathbf{I}& 0& {\mathbf{F}}_3& 0& {\mathbf{F}}_5& 0& {\mathbf{F}}_7\\ 0& 0& 0& \mathbf{I}& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& \mathbf{I}& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& \mathbf{I}& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& \mathbf{I}& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& \mathbf{I}& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& \mathbf{I}\end{array}\right]$$

(28)

where submatrices ${\mathbf{F}}_1$ to ${\mathbf{F}}_7$ are defined as follows:

$$\begin{array}{cc}\hfill {\mathbf{F}}_1& =-{\left[\mathbf{R}\left\{\mathit{\theta }\right\}{\mathbf{T}}_a\mathbf{a}\right]}_{\times }\Delta t\hfill \\ \hfill {\mathbf{F}}_2& =-\mathbf{R}\left\{\mathit{\theta }\right\}{\mathbf{T}}_a\Delta t\hfill \\ \hfill {\mathbf{F}}_3& =-\mathbf{R}\left\{\mathit{\theta }\right\}{\mathbf{T}}_g\Delta t\hfill \\ \hfill {\mathbf{F}}_4& =-\mathbf{R}\left\{\mathit{\theta }\right\}{\mathbf{T}}_a·\mathrm{Diag}\left({\mathbf{a}}_m\right)\Delta t\hfill \\ \hfill {\mathbf{F}}_5& =-\mathbf{R}\left\{\mathit{\theta }\right\}{\mathbf{T}}_g·\mathrm{Diag}\left({\mathit{\omega }}_m\right)\Delta t\hfill \\ \hfill {\mathbf{F}}_6& =\mathbf{R}\left\{\mathit{\theta }\right\}\left[\begin{array}{ccc}-{\mathbf{a}}_y& {\mathbf{a}}_z& 0\\ 0& 0& -{\mathbf{a}}_z\\ 0& 0& 0\end{array}\right]\Delta t\hfill \\ \hfill {\mathbf{F}}_7& =\mathbf{R}\left\{\mathit{\theta }\right\}\left[\begin{array}{cccccc}-{\mathit{\omega }}_y& {\mathit{\omega }}_z& 0& 0& 0& 0\\ 0& 0& -{\mathit{\omega }}_z& {\mathit{\omega }}_x& 0& 0\\ 0& 0& 0& 0& -{\mathit{\omega }}_x& {\mathit{\omega }}_y\end{array}\right]\Delta t\hfill \end{array}$$

Often, integrated MEMS sensors come with integrated magnetometers. The magnetometer can be included in the calibration, as shown in [25]. Since the magnetometer measurements are not involved in the prediction step, the state transition function and transition matrix are not effected.

6. Results

For the field test, the AUV was driving close to the surface on a triangular path marked with buoys, as shown in Figure 4. The test side was located in the Kiel Fjord, and the triangle had a side length of around 30 m. The water depth at the test side was between 3 m and 8 m. The vehicle used for the test was a custom-designed AUV with a length of about 1 m and a weight of about 50 kg. The placement of the navigational sensors is shown in Figure 5. It is noted that both sensors were relative close to each other in the horizontal direction, so rotations around the vertical axes had less impact on the lever arm. The vehicle was driving in non-autonomous mode using a tethering cable, and the actual path was recorded from above the water to measure the reference position.

During the field test, the AUV was driving for about an hour while recording data from MEMS and USBL. As our method performs joined calibration and data fusion, the accuracy grows over time. For this reason, only the position accuracy during a 10 min time period at the end of the test was taken into account for the evaluation. The recorded reference position, the USBL fixes, and the estimated vehicle path with best filter configuration are shown in Figure 6 for the time period used for evaluation. The east and north positions are also plotted separately over time in Figure 7. Different configurations of the described filter are applied to the data recorded from the field test. The maximum error and the Root Mean Squared Error (RMSE) are calculated for the estimated positions over the same 10 min time period. The filter has been processed with different types of input data enabled. This includes inertial measurements of the MEMS, the USBL measurements with either TC or LC data fusion, and feedback of the thruster set points. Additionally, the implementation method can be floating-point or fixed-point with SR or RN rounding methods. The results are summarized in

Table 1. Results with different data fusion configurations.

Type	MEMS	USBL	Thrust	RMSE	Max. Error
		raw		$3.51$ m	$17.61$ m
float	yes	TC	yes	$3.18$ m	$6.09$ m
float	yes	TC		$4.5$ m	$13.38$ m
float	yes	LC	yes	$5.28$ m	$8.67$ m
float	yes	LC		$7.02$ m	$17.07$ m
fix16 SR	yes	TC	yes	$3.36$ m	$6.21$ m
fix16 RN	yes	TC	yes	$3.69$ m	$6.75$ m
complementary filter [16]	yes	LC	yes	$3.62$ m	$8.54$ m

. It can be seen that the RMSE of the filter output is close to the RMSE of the raw USBL measurements, despite the USBL measurements being only available in a 5 s interval. In addition, the maximum error is strongly reduced from $17.62$ m to $6.09$ m. It can be seen that the SR method clearly reduces the precision difference between the 16-bit fixed-point and 32-bit floating-point implementations. The RMSE of the SR fixed-point implementation is only 5% higher than with the floating-point results. In contrast, the RN implementation had a 16% higher error. The comparison with a few-state Kalman filter extended with complementary filtering, as described in [16], showed a clear advantage of our proposed method. The complementary filtering setup was limited to LC USBL data fusion. As shown in Table 1, the maximum error was almost identical between the complementary filtering setup and our method with LC USBL data fusion, while our method with TC USBL data fusion had a reduced maximum error.

An advantage of the proposed method is its ability to perform the calibration jointly with the data fusion. To test the effectiveness of this approach, the estimated bias parameters of the MEMS sensor were compared between a standalone calibration in the laboratory and a joined calibration during the field test. The results are presented in

Table 2. Comparison between standalone calibration in the laboratory and joined calibration during the field test.

Element	Standalone Calibration	Joined Calibration
accelerometer bias x	$-8.73\times {10}^{-2}$ ${\displaystyle \frac{\mathrm{m}}{{\mathrm{s}}^2}}$	$2.95\times {10}^{-2}{\displaystyle \frac{\mathrm{m}}{{\mathrm{s}}^2}}\pm 6.58\times {10}^{-2}{\displaystyle \frac{\mathrm{m}}{{\mathrm{s}}^2}}$
accelerometer bias y	$-3.51\times {10}^{-2}$ ${\displaystyle \frac{\mathrm{m}}{{\mathrm{s}}^2}}$	$6.39\times {10}^{-1}{\displaystyle \frac{\mathrm{m}}{{\mathrm{s}}^2}}\pm 4.83\times {10}^{-2}{\displaystyle \frac{\mathrm{m}}{{\mathrm{s}}^2}}$
accelerometer bias z	$-8.77\times {10}^{-3}{\displaystyle \frac{\mathrm{m}}{{\mathrm{s}}^2}}$	$-9.64\times {10}^{-2}{\displaystyle \frac{\mathrm{m}}{{\mathrm{s}}^2}}\pm 4.41\times {10}^{-2}{\displaystyle \frac{\mathrm{m}}{{\mathrm{s}}^2}}$
gyroscope bias x	$2.50\times {10}^{-3}{\displaystyle \frac{\mathrm{rad}}{\mathrm{s}}}$	$2.53\times {10}^{-3}{\displaystyle \frac{\mathrm{rad}}{\mathrm{s}}}\pm 8.20\times {10}^{-6}{\displaystyle \frac{\mathrm{rad}}{\mathrm{s}}}$
gyroscope bias y	$-4.31\times {10}^{-4}{\displaystyle \frac{\mathrm{rad}}{\mathrm{s}}}$	$-4.82\times {10}^{-4}{\displaystyle \frac{\mathrm{rad}}{\mathrm{s}}}\pm 8.14\times {10}^{-6}{\displaystyle \frac{\mathrm{rad}}{\mathrm{s}}}$
gyroscope bias y	$-1.11\times {10}^{-3}{\displaystyle \frac{\mathrm{rad}}{\mathrm{s}}}$	$-1.21\times {10}^{-3}{\displaystyle \frac{\mathrm{rad}}{\mathrm{s}}}\pm 1.48\times {10}^{-5}{\displaystyle \frac{\mathrm{rad}}{\mathrm{s}}}$

. It can be seen that the estimated gyroscope bias parameters closely match between the two calibrations. The accelerometer bias parameters differ strongly, which is also reflected in the estimation uncertainty of around $5\times {10}^{-2}{\displaystyle \frac{\mathrm{m}}{{\mathrm{s}}^2}}$. The accelerometer calibration works much better in the laboratory test because the sensor can be freely rotated. In the field test, the sensor always has the z-axis aligned with the gravity of the earth. For the same reason, the sensitivity and orthogonality parameters are difficult to estimate in a short field test. Therefore, these parameters were fixed during the field test and not included in the comparison.

The existing methods can be broadly classified into three categories. The first relies on high-precision sensors, offering high accuracy at a high cost [2,4,12]. The second uses low-cost MEMS sensors combined with complex, high-rate, many-state data fusion algorithms, which reduce hardware costs but impose significant computational load [6]. The third category also employs low-cost sensors, but it minimizes computational demand through simplified data fusion techniques [16]. Our method belongs to the second category, with the key advantage of offloading the data fusion to a dedicated signal processing unit. This approach eliminates the computational burden on the application processor, making its full processing power available for other tasks. A comparison of the advantages and disadvantages can be seen in

Table 3. Advantages and disadvantages of different categories of underwater navigation solutions.

Method	Advantages	Disadvantages
Precise sensors	High accuracy	High cost
MEMS sensors + high rate, many states data fusion	Low cost	Limited accuracy High computational load
MEMS sensors + few states, simplified data fusion	Low cost Low computational load	Limited accuracy Suboptimal data fusion
Our method: MEMS sensors + high rate, many-state data fusion on a dedicated signal processing unit	Low cost No computational load on application processor	Limited accuracy

.

7. Conclusions

This work presents a novel implementation method to achieve near-floating-point precision on a limited 16-bit microcontroller, enabling the development of a low-cost, low-power standalone navigation system. While the microcontroller’s power consumption may be minor in the overall power management of an AUV, this approach offers the advantage of enhanced numerical stability, regardless of computational constraints. The implementation leverages a square-root form of the error state Kalman filter to establish a numerically stable inertial navigation system that allows for data fusion from external sensors. The implementation has been adapted and optimized for fixed-point calculation to be used on a 16-bit microcontroller. A stochastic rounding method has been applied to increase the precision of the fixed-point implementation. The algorithm has been adapted for TC USBL data fusion. Additional optimizable states have been added for calibration of bias, sensitivity, and orthogonality errors of the MEMS sensor.

The proposed implementation has been tested on different calibration and data fusion tasks. Data collected from a field test have been used to compare the performance of different filter configurations. It was shown that the accuracy of the fixed-point calculation could be brought close to the floating-point performance using the SR method. Also, the use of TC data fusion with USBL and the inclusion of thruster feedback led significant error reduction.