A Hybrid Model and Learning-Based Adaptive Navigation Filter

Autonomous vehicles, such as autonomous underwater vehicles (AUV) or quadrotors, are commonly equipped with an inertial navigation system (INS) and other sensors [1] to provide real-time information about their position, velocity, and orientation [2, 3, 4, 5, 6, 7]. The INS has two types of inertial sensors; namely, gyroscopes and accelerometers. The former measures the angular velocity vector, and the latter measures the specific force vector. The inertial sensors are combined in an inertial measurement unit (IMU). Their use for navigation is challenging due to their high noise levels, which eventually accumulate, leading to a navigation solution drift over time. To solve this problem, an external aiding sensor, such as the global navigation satellite system (GNSS) receiver, is regularly used in a navigation filter [2, 3, 6]. Such fusion is carried out using a nonlinear filter, where the error state implementation of the extended Kalman filter (es-EKF) is usually employed [8, 1]. In the filter, the process noise covariance matrix is based on the IMU measurements characteristics and/or vehicle dynamics while the external aiding measurements specification determines the measurement noise covariance matrix. Those two covariances matrices have a major influence on the filter performance. A common practice is to assume that both covariances are fixed during the vehicle operation. However, as the sensor characteristics are subject to change during operation and physical constraints do not allow capturing the vehicle dynamic optimally, those covariances should vary over time as the amount of uncertainty varies and is unknown. Thus, tuning the process or measurement noise covariances optimally can lead to a significant improvement in the filter performance [8].

To cope with this problem, several model-based attempts have been made in the literature to develop optimal adaptive filters [9, 10, 11, 12, 13, 14, 15]. The most common among them is based on the filter innovation process [16], where a measure of the new information is calculated at every iteration, leading to an update of the process noise covariance matrix. Still, the question of the optimal approach to tune the system noise covariance matrix is considered open and is addressed in detail in [17].

Recently, deep neural network (DNN) approaches were integrated in model based pedestrian dead-reckoning (PDR) algorithms. One of the initial works in the field is the robust IMU double integration, RIDI approach [18], where neural networks were trained to regress linear velocities from inertial sensors to constrain the accelerometer readings. Although it was the first work in the field, double integration is still needed and the error accumulates rapidly. In [19], the device orientation, in addition to the accelerometers and gyroscopes readings, was also used as input to a DNN architecture to regress the user velocity in 2D. The velocity is then integrated to obtain the user position. Later, PDRNet [20], utilizes a smartphone location recognition classification network followed by a change of heading and distance regression network to estimate the used 2D position. One of the main challenges in model or learning based PDR is the estimation of the user walking direction as the device is not always aligned with the user motion. To cope with that, in [21] a novel DNN structure was designed for extracting the motion vector in the device coordinates, using accelerometer readings. More pedestrian inertial navigation examples are described by [22] where recent databases, methods and real-time inferences can be found.

In AUV navigation, an end-to-end DNN approach was proposed to regress missing Doppler velocity log (DVL) beam measurements to provide the AUV velocity vector, only when a single beam is missing [23]. Later, [24], a DNN approach was used to address with a DVL failure scenarios to predict the DVL output.

For land vehicle navigation a DNN model was presented to overcome the inaccurate dynamics and observation models of wheel odometry for localization, named RINS-W [25]. Their novel approach exploited DNNs to identify patterns in wheeled vehicle motions using the IMU sensor only. To overcome the navigation solution drift in pure inertial quadrotor navigation, QuadNet, a hybrid framework to estimate the quadrotor’s three-dimensional position vector at any user-defined time rate was proposed [26].

Focusing on the estimation process, a self-learning square root-cubature Kalman filter was proposed [27]. This hybrid navigation filter enhanced the navigation accuracy by providing observation continuously, even in GNSS denied environments, by learning transfer rules of internal signals in the filter. In [28], recurrent neural networks are employed to learn the vehicle’s geometrical and kinematic features to regress the process noise covariance in a linear Kalman filer framework. In [29], DNN based multi models (triggered by traffic conditions classification) together with an EKF was proposed to cope with GNSS outages.

Recently, a DNN model was proposed to solve the GNSS outages, mainly for unmanned air vehicles (UAV). In [30], a convolutional neural network model was used for noise-free gyro measurements in open loop attitude estimation. They obtained state-of-the-art navigation performance in terms of attitude estimation where they compensated for gyro measurement errors as part of a strapdown integration approach. This trend of integrating learning approaches in various fields and applications also raises the motivation to adopt such approaches in the described problem.

In this paper, we propose a hybrid model and learning-based adaptive navigation filter. We rely on the model-based; yet, instead of using model-based adaptive process noise tuning, we design a DNN to tune the momentary system noise covariance matrix, based only on the inertial sensor readings. Once, the process noise covariance is learned, it is plugged into the well-established, model-based es-EKF. To that end, we simulated many vehicle trajectories based on six baseline trajectories to create a rich dataset. This dataset contains rich dynamic scenarios with many motion patterns and different IMU noise covariances. This rich dataset was created to enable robustness to motion dynamics and IMU noise characteristics. Based on this dataset, we adopt a supervised learning (SL) approach to regress the process noise covariance.

The main contributions of this paper are:

1. 1.

   Derivation of an adaptive hybrid learning algorithm to momentary determine the process noise covariance matrix as a function of the IMU measurements. That is, instead of using model-based approaches (as been done in the last 50 years), to the best of our knowledge, we are the first to propose an adaptive hybrid learning algorithm.

2. 2.

   Online integration of the proposed hybrid learning algorithm with es-EKF implementation of the navigation filter. In that manner, the proposed approach can be used with any external sensor aiding the INS.

3. 3.

   The proposed approach principles can be further applied to any estimation problem that requires adaptive tuning of the process noise covariance.

The advantages of the proposed approach lies in its hybrid fusion of the well celebrated es-EKF and applying learning approaches on the es-EKF soft spot, the adaptive determination of the process noise covariance. By using learning approaches we leverage from their ability to generalization of intrinsic properties appearing in sequential datasets and, therefore, better cope with varying conditions affecting the process noise values.

After deriving and validating the proposed framework using the stimulative dataset, field experiments using a quadrotor are done to evaluate the robustness and performance of the proposed approach testing phase and compare it to other adaptive model-based approaches. The recordings from the field experiment are used as a test dataset and also to examine our network capability for generalization. In our setup, we used GNSS position measurements to update the INS; however, the proposed approach is suitable for any aiding sensor and for any platform.

The rest of the paper is organized as follows: Section II presents the problem formulation for the INS/GNSS fusion in an es-EKF implementation with constant and adaptive process noise covariance. Section III presents our proposed hybrid adaptive es-EKF. Section IV gives the results, and Section V presents the conclusions of this study.

The nonlinear nature of the INS equations requires a nonlinear filter. The most common filter for fusing INS with external aiding sensors is the es-EKF [1], and in particular, with a $15$ error states implementation:

where $\delta{{\bf{p}}^{n}}\in{{\mathbb{R}}^{3\times 1}}$ is the position vector error states expressed in the navigation frame, $\delta{{\bf{v}}^{n}}\in{{\mathbb{R}}^{3\times 1}}$ is the velocity vector error states expressed in the navigation frame, ${{\bf{\delta\varepsilon}}^{n}}\in{{\mathbb{R}}^{3\times 1}}$ is the misalignment vector state expressed in the navigation frame, ${{\bf{b}}_{a}}\in{{\mathbb{R}}^{3\times 1}}$ is the accelerometer bias residuals vector state expressed in the body frame, and ${{\bf{b}}_{g}}\in{{\mathbb{R}}^{3\times 1}}$ is the gyro bias residuals vector state expressed in the body frame. The navigation frame is denoted by $n$ and the body frame is denoted by $b$. The navigation frame center is located at the body center of mass, where the $x$-axis points to the geodetic north, $z$-axis points down parallel to local vertical, and $y$-axis completes a right-handed orthogonal frame. The body frame center is located at the center of mass, $x$-axis is parallel to the longitudinal axis of symmetry of the vehicle pointing forwards, the $y$-axis points right, and the $z$-axis points down such that it forms a right-handed orthogonal frame. The linearized, error-state, continuous-time model is

where ${\bf{F}}\in{{\mathbb{R}}^{15\times 15}}$ is the system matrix, ${\bf{G}}\in{{\mathbb{R}}^{15\times 12}}$ is the shaping matrix, and $\delta{\bf{w}}={\left[{\begin{array}[]{*{20}{c}}{{{\bf{w}}_{a}}}&{{{\bf{w}}_{g}}}&{{{\bf{w}}_{ab}}}&{{{\bf{w}}_{gb}}}\end{array}}\right]^{T}}\in{{\mathbb{R}}^{12\times 1}}$ is the system noise vector consisting of the accelerometer, gyro, and their biases random walk noises, respectively [8]. The system matrix, $\bf F$, is given by

where ${{\bf{T}}_{b}^{n}}$ is the transformation matrix between body and navigation frame and ${{\bf{F}}_{ij}}\in{{\mathbb{R}}^{3\times 3}}$ can be found explicitly in the literature (see [1, 31, 32]). The shaping matrix is given by

where ${\bf I}_{3\times 3}$ is an identity matrix.
The corresponding discrete version of the navigation model (for small step sizes), as given in (2), is

The transition matrix, ${{\bf\Phi}_{k}}$, is defined by a first order approximation as

where $k$ is a time index, $\Delta t$ is the time step size, ${\bf{F}}$ is the system matrix (obtained using $(3)$ with the estimated state vector at time $k$), and $\delta{\bf{w}}_{k}$ is a zero mean white Gaussian noise vector. The discretized process noise is given by

where $\bf G$ is defined in $(4)$ (using the current estimated state vector at time $k$), and ${\bf Q}^{c}$ is the continuous process noise matrix.
The discrete es-EKF, as described here, is used in the fusion process between the INS and the external measurements. Equations $(8)-(12)$ summarize the navigation filter equations[1, 31]:

where $\delta{\bf{\hat{x}}}^{-}_{k}$ is the prior estimate of the error state and $\delta{\bf{\hat{x}}}_{k}$ is the posterior estimate, with their corresponding covariance matrices ${\bf P}_{k}^{-}$ $(9)$ and ${\bf P}_{k}$ $(12)$, respectively, ${\bf K}_{k}$ $(10)$ is the Kalman gain, $\delta{\bf z}_{k}$ is the $k$ measurement residual, ${\bf H}_{k}$ is the measurement matrix, and ${\bf R}_{k}$ is the measurement noise covariance matrix, assumed known and constant.

We rely on the well established, model-based es-EKF, yet instead of a model-based adaptive approach, a data-driven approach is used to regress the momentary process noise covariance, and, as a result, creates a hybrid adaptive es-EKF. To this end, the navigation filter equations (8)-(12) are used, but instead of using a constant process noise (7) or a model-based adaptive process noise (18), we derive a data-driven approach to estimate the time-varying continuous process noise covariance matrix ${\bf Q}^{c}$. Our proposed hybrid adaptive es-EKF framework, suitable for any aided INS scenario, is presented in Fig. 1.

We employ the following two metrics to evaluate the position accuracy of the proposed approach:

• •

  Position root mean squared error ($2^{nd}$ norm) for all three axes:

  $$PRMSE=\sqrt{\frac{1}{c}\sum\limits_{k=1}^{c}{\sum\limits_{j\in\left\{{x,y,z}\right\}}{\delta{{\hat{p}}_{jk}}^{2}}}}.$$

  (25)

• •

  Position mean absolute error for all six IMU channels:

  $$PMAE=\frac{1}{c}\sum\limits_{k=1}^{c}{\sum\limits_{j\in\left\{{x,y,z}\right\}}{\left|{\delta{{\hat{p}}_{jk}}}\right|}}.$$

  (26)

where $c$ is the number of samples, $k$ is a running index for the samples, $j$ is the running index for the Cartesian coordinate system, and $\delta{\hat{p}}_{jk}$ is the position error term.

The proper choice for the system noise covariance matrix is critical for accurate INS/GNSS fusion. In this paper, a hybrid learning and model based approach was suggested in an adaptive filter framework. To that end, a novel DNN-based approach to learn and online tune the system noise covariance was developed. The input to the DNN is only the IMU readings. This learning approach was then combined with the model based es-EKF, resulting in a hybrid adaptive filter.

The DNN model has been trained only once on a simulated database consisting of six different baseline trajectories. Analysis on this dataset showed a trade-off between accuracy and the regression solution latency, where as the latency increases the accuracy decreases. Relying on this analysis, an input length of $200$ samples was chosen.

To validate the performance of the proposed approach, it was compared to six model-based approaches representing both constant and adaptive process noise covariance selection. Quadrotor field experiments were conducted and used as an additional test dataset on the trained network. We obtained state-of-the-art results for each noise value compared to classical adaptive KF approaches; a PRMSE lower than $1.2[m]$ and a PMAE lower than $1.7[m]$ were obtained using our suggested approach, compared to a PRMSE of $1.6[m]$ and a PMAE of a $2.3[m]$ that were obtained using the innovation-based adaptive approach and classical tuning. Results in a total PRMSE improvement of $24\%$ and PMAE improvement of $27\%$.

This improvement also demonstrates the robustness and generalization properties of the proposed DNN architecture, enabling it to cope with unseen data from different IMU sensors. In addition, and together with the train dataset results, it proves our hypothesis of regressing only the first six elements in the diagonal of the continuous process noise covariance matrix.

Although demonstrated for quadrotor INS/GNSS fusion, the proposed approach can be elaborated for any external sensor aiding the INS and for any type of platform.

Although the present study demonstrated a small improvement over model-based methods using the test dataset of a quadrotor INS/GNSS fusion, it introduces a hybrid framework to combine deep learning in an adaptive navigation filter. This approach can be applied to other platforms, dynamics and external sensor aiding the INS and serve as a foundation approach for future work in the field.