TLIO: Tight Learned Inertial Odometry

I Introduction

Visual-Inertial Navigation Systems (VINS) in recent years have seen tremendous success, enabling a wide range of applications from mobile devices to autonomous systems. Using only light-weight cameras and Inertial Measurement Units (IMUs), VINS achieve high accuracy in tracking at low cost and provide one of the best solutions for localization and navigation on constrained platforms. With the unique combination of these advantages, VINS have been the de facto standard for demanding applications such as Virtual/Augmented Reality (VR/AR) on mobile phones or headsets.

Despite the impressive performance of state-of-the-art VINS algorithms, demands from consumer AR/VR products are posing new challenges on state estimation, pushing the research frontier. Visual-Inertial Odometry (VIO) largely relies on consistent image tracking, leading to failure cases under extreme lighting or camera blockage conditions, such as in a dark room or inside a pocket. High frequency image processing makes power consumption a bottleneck for sustainable long-term operations. In addition, widespread camera usage carries privacy implications. Targeting an alternative to the state-of-the-art VINS algorithms for pedestrian applications, this paper focuses on consumer grade IMU-only state estimation problem known as strap-down Inertial Navigation System (INS) or Dead Reckoning .

Inertial Pedestrian Dead Reckoning (PDR) has gained increasing interest with the price decline and widespread usage of MEMS sensors in the past two decades . Strap-down pedestrian IMU navigation faces the challenge of the accumulation of sensor errors since the IMU kinematic model provides only relative state estimates. To compensate for the errors and reduce drift without the aid of other sensors or floor maps, the existing approaches rely on the prior knowledge of human walking motion, in particular, steps. One way to make use of steps is Zero-velocity UPdaTe (ZUPT) . It detects when the foot touches the ground to generate pseudo-measurement updates in an Extended Kalman Filter (EKF) framework to calibrate IMU biases. However, it only works well when the sensor is attached to the foot where step detection is obvious. Another category is step counting which does not require the sensor to be attached to the foot. Such systems consist of multiple submodules: the identification and classification of steps, data segmentation and step length prediction, all of which require heavy tuning of hand-designed heuristics or machine learning.

In parallel, recent research advances (IONet and RoNIN ) have shown that integrating average velocity estimates from a trained neural network results in highly accurate 2D trajectory reconstruction using only IMU data from pedestrian hand-carried devices. These results showed the ability of networks to learn a translation motion model from pedestrian data. In this work, we draw inspiration from these new findings to build an IMU-only dead reckoning system trained with data collected from a head-mounted device, similar to a VR headset. This paper has two major contributions:

We propose a network design to regress both the 3D displacement and the corresponding covariance, and show the network’s capability of providing both accurate and statistically consistent outputs trained on our pedestrian dataset.

We propose a complete state estimation system combining the neural network with an EKF in a tightly-coupled formulation that jointly estimates position, orientation, velocity, and IMU biases with only pedestrian IMU data.

Our approach presents significant advantages over other IMU-only state estimation approaches. Comparing to traditional PDR methods, it avoids the limitations and complexities of step-counting/stride or gait detection by learning a displacement motion model valid for any data segments, simplifying the process and improving accuracy and robustness. Comparing to common deep learning approaches like RoNIN, it elevates the problem onto 3D domain and does not require an external ZUPT-based orientation estimator. This tight fusion approach reduces average yaw and position drift by 27% and 33% respectively on our test dataset comparing to the best performing RoNIN velocity concatenation baseline approach. Fig. 1 shows an example of the estimated trajectory. We name our method Tight Learned Inertial Odometry (tlio).

II Related Works

VIO. Visual-Inertial Odometry has been well studied in the literature . Without power and privacy issues and when static visual features are clearly trackable, VIO is the standard method for state estimation.

PDR. Solutions to Pedestrian Dead Reckoning problems can be divided into two categories: Bayesian filtering and step counting . With IMU as the only source of information for both propagation and update steps of an EKF, the measurements need to be carefully constructed. Different types of pseudo measurement approaches include Zero-velocity UPdaTe (ZUPT) and Zero Angular Rate Update (ZARU) which detects when the system is static, and heuristic heading reduction (HDR) which identifies walking in a straight line to calibrate gyroscope bias. Foot-mounted IMU sensor enables reliable detection of contact from which accurate velocity constraints can be derived , however such information is not present for hand carried devices, and various signal processing and machine learning algorithms such as peak detection in time/frequency domain and activity classification have been explored . These algorithms are complex and involve lots of tuning to accurately estimate a trajectory. It is also an active research direction as deep learning approaches solving for subproblems such as gait classification and stride detection are investigated.

Deep Learning. The emergence of deep learning provides new possibilities to extract information from IMU data. Estimating average velocity from IMU data segments has seen great success in position estimation. IONet first proposed an LSTM structure to output relative displacement in 2D polar coordinates and concatenate to obtain position. RIDI and RoNIN both assume orientation is known from an Android device to rotate IMU data into a gravity-aligned frame. While RIDI regresses velocity to optimize bias but still uses double integration from the corrected IMU data for position, RoNIN directly integrates the regressed velocity and showed better results. This shows that a statistical IMU motion model can outperform the traditional kinematic IMU model in scenarios that can be captured with training data.

In addition to using networks alone for pose estimates, Backprop KF proposes an end-to-end differentiable Kalman filter framework. Because the loss function is on the accuracy of the filter outputs, the noise parameters are trained to produce the best state estimate, and do not necessarily best capture the measurement error model. AI-IMU uses this approach to estimate IMU noise parameters and measurement uncertainties for car applications. In this work, we also combine deep learning with a Kalman filter. However, instead of training an end-to-end system, we leverage a state-of-the-art visual-inertial fusion algorithm as ground-truth for supervised learning of the measurement function itself. We use a likelihood loss function to jointly train for 3D displacement and uncertainty, which are directly used as input to the EKF in the measurement update.

III System Design

Our estimation system takes IMU data as input and has two major components: the network and the filter. Figure 2 shows a high-level diagram of the system.

The first component is a convolutional neural network trained to regress 3D relative displacement and uncertainty between two time instants given the segment of IMU data in between. The network is required to infer positional displacement over a short timespan from acceleration and angular velocity measurements alone, without access to the initial velocity. This is intentional: It forces the network to learn only a prior expressing ”typical human motion” and leaves model-based state propagation, i.e. acceleration integration to propagate velocity, and respective uncertainty propagation, to the second system component, the EKF. In order for this to work well, we found that it is important to include the inferred prior’s uncertainty to the network output, allowing the network to encode how much motion model prior it obtained from the input measurements.

The EKF estimates the current state: 3D position, velocity, orientation and IMU biases, and a sparse set of past poses. The EKF propagates with raw IMU samples and uses network outputs for measurement updates. We define the measurement in a local gravity-aligned frame to decouple global yaw information from the relative state measurement (see Sec. V-D). The propagation from raw IMU data provides a model-based kinematic motion model, and the neural network provides a statistical motion model. The filter tightly couples these two sources of information.

At runtime, the raw IMU samples are interpolated to the network input frequency and rotated to a local gravity-aligned frame using rotations estimated from the filter state and gyroscope data. A gravity-aligned frame always has gravity pointing downward. Placing data in this frame implicitly gives the gravity direction information to the network.

Note that in the proposed approach, IMU data is being used twice, both as direct input for state propagation and indirectly through the network as the measurement. This violates the independence assumptions the EKF is based on: the errors in the IMU network input (initial orientation, sensor bias, and sensor noise) would propagate to its output which is used to correct the propagation results polluted with the same noise, which could lead to estimation inconsistency. We address this issue with training techniques (see Sec. IV) to reduce this error propagation by adding random perturbations to IMU bias and gravity direction during training. We show in Section VII that these techniques successfully improved the robustness of the network to sensor bias and rotation inaccuracies.

IV Neural Statistical Motion Model

Our network uses a 1D version of ResNet18 architecture proposed in . The input dimension to the network is $N\times 6$, consisting of $N$ IMU samples in the gravity-aligned frame. The output of the network contains two 3D vectors: the displacement estimates $\hat{\bm{d}}$ and their uncertainties $\hat{\bm{u}}$ which parametrize the diagonal entries of the covariance. The two vectors have independent fully-connected blocks extending the main ResNet architecture.

We make use of two different loss functions during training: the Mean Square Error (MSE) and the Gaussian Maximum Likelihood loss. The MSE loss on the trained dataset is defined as:

where $\hat{\bm{d}}=\{\hat{\bm{d}}_{i}\}_{i\leq n}$ are the 3D displacement output of the network and $\bm{d}=\{\bm{d}_{i}\}_{i\leq n}$ are the ground truth displacement. $n$ is the number of data in the training set.

We define the Maximum Likelihood loss as the negative log-likelihood of the displacement according to the regressed Gaussian distribution:

where $\hat{\bm{\Sigma}}=\{\hat{\bm{\Sigma}}_{i}\}_{i\leq n}$ are the $3\times 3$ covariance matrices for $i^{th}$ data as a function of the network uncertainty output vector $\hat{\bm{u}}_{i}$. $\hat{\bm{\Sigma}}_{i}$ has 6 degrees of freedom, and there are various covariance parametrizations for neural network uncertainty estimation . In this paper, we simply assume a diagonal covariance output, parametrized by 3 coefficients written as:

The diagonal assumption decouples each axis, while regressing the logarithm of the standard deviations removes the singularity around zero in the loss function, adding numerical stabilization and helping the convergence in the optimization process. This choice constrains the principal axis of the uncertainty ellipses to be along the gravity-aligned frame axis.

IV-B Data Collection and Implementation Details

We use a dataset collected by a custom rig where an IMU (Bosch BMI055) is mounted on a headset rigidly attached to the cameras. The full dataset contains more than 400 sequences totaling 60 hours of pedestrian data that pictures a variety of activities including walking, standing still, organizing the kitchen, playing pool, going up and down the stairs etc. It was captured with multiple different physical devices by more than 5 people to depict a wide range of individual motion patterns and IMU systematic errors. A state-of-the-art visual-inertial filter based on provides position estimates at 1000 Hz on the entire dataset. We use these results both as supervision data in the training set and as ground truth in the test set. The dataset is split into $80\%$ training, $10\%$ validation and $10\%$ test subsets randomly.

During training, because we assume the headset can be worn at an arbitrary heading angle with respect to the walking direction, we augment the input data for the network to be yaw invariant by giving a random horizontal rotation to each sample following RoNIN . In our final estimator, the network is fed with potentially inaccurate input from the filter, especially at the initialization stage. We simulate this at training time by random perturbations on the sensor bias and the gravity direction to reduce network sensitivity to these input errors. To simulate bias, we generate additive bias vectors with each component independently sampled from uniform distribution in $[-0.2,0.2]\,m/s^{2}$ or $[-0.05,0.05]\,rad/s$ for each input sample. Gravity direction is perturbed by rotating those samples along a random horizontal rotation axis with magnitude sampled from $[0,5^{\rm o}]$.

Optimization is done through the Adam optimizer. We used an initial learning rate of $0.0001$, zero weight decay, and dropouts with a probability of $0.5$ for the fully connected layers. We observe that training for $\mathcal{L}_{\text{ML}}$ directly does not converge. Therefore we first train with $\mathcal{L}_{\text{MSE}}$ for 10 epochs until the network stabilizes, then switch to $\mathcal{L}_{\text{ML}}$ until the network fully converges. It takes around another 10 epochs to converge and a total of 4 hours of training time is needed on an NVIDIA DGX computer.

V Stochastic Cloning Extended Kalman Filter

The EKF in our system tightly integrates the displacements predicted by the network with a statistical IMU model as used in other inertial navigation systems. As the displacement estimates from the network express constraints on pairs of past states, we adopt a stochastic cloning framework . Similar to , we maintain a sliding window of $m$ poses in the filter state. In contrast to , however, we only apply constraints between pairs of poses, and these constraints are derived from the network described in the preceding section, rather than camera data.

At each instant, the full state of the EKF is defined as:

where $\bm{\xi}_{i}$, $i=1,\dots,m$ are past (cloned) states, and $\bm{s}$ is the current state. More specifically,

As commonly done in such setups, we apply the error-based filtering methodology to linearize locally on the manifold of the minimal parametrization of the rotation. More specifically, the filter covariance is defined as the covariance of the following error-state:

V-B IMU model

We assume that the IMU sensor provides direct measurements of the non-gravitational acceleration $\bm{a}$ and angular velocity $\bm{\omega}$, expressed in the IMU frame. We assume as it is common for this class of sensor that the measurements are polluted with noise ${\bm{n}}_{g;a}$ and bias $\bm{b}_{g:a}$.

${\bm{n}}_{g}$ and ${\bm{n}}_{a}$ are random noise variables following a zero-centered Gaussian distribution. Evolution of biases is modeled as a random walk process with discrete parameters $\bm{\eta}_{gd}$ and $\bm{\eta}_{ad}$ over the IMU sampling period $\Delta t$.

V-C State Propagation and Augmentation

The filter propagates the current state $\bm{s}$ with IMU data using a kinematic motion model. If the current timestamp is associated to a measurement update, stochastic cloning is performed together with propagation in one step. During cloning, a new state $\bm{\xi}$ is appended to the past state.

We use the strapdown inertial kinematic equation assuming a uniform gravity field $\prescript{\text{w}}{}{\bm{g}}$ and ignoring Coriolis forces and the earth’s curvature:

Here, $\exp_{SO3}$ denotes the SO(3) exponential map, the inverse function of $\log_{SO3}$. The discrete-time noise $\bm{\eta}_{gdk}$, $\bm{\eta}_{adk}$ have been defined above and follow a Gaussian distribution.

The linearized error propagation can be written as:

with ${\bm{W}}_{(12,12)}$ the covariance matrices of sensor noise and bias random walk noise. The state covariance ${\bm{P}}$ is of the dimension $(6m+15)$ of the full state $\bm{X}$. In the implementation, multiple propagation steps can be combined together to reduce the computational cost .

V-C2 State Augmentation

In our system, state augmentations are performed at the measurement update frequency. During an augmentation step, the state dimension is incremented through propagation with cloning:

$\bar{\bm{A}}_{k}$ is now a copy operation plus the augmented and current state propagation operations. $\bm{A}^{\xi}_{k}$ and $\bm{B}^{\xi}_{k}$ are partial propagation matrices for rotation and position only. After the increment, the dimension of the state vector increases by 6. Old past states will be pruned in the marginalization step, see Sec.V-E.

V-D Measurement Update

It would be natural to define the measurement function as the displacement $\bm{d}$ expressed in the world frame, however such measurement function would imply heading observability at the filter level. Heading is theoretically unobservable as the IMU propagation model and the learned prior are invariant to the change of yaw angle. In order to prevent injecting spurious information into the filter, we carefully define the measurement function $h$ as the 3D displacement in a local gravity-aligned frame. This frame is anchored to a clone state $i$. Vectors in this frame can be obtained by rotating the corresponding world frame vectors by the yaw rotation matrix ${\mathtt{R}}_{\gamma}$ of the state rotation matrix ${\mathtt{R}}_{i}$. We decompose ${\mathtt{R}}_{i}$ using extrinsic ”XYZ” Euler angle convention: ${\mathtt{R}}_{i}={\mathtt{R}}_{\gamma}{\mathtt{R}}_{\beta}{\mathtt{R}}_{\alpha}$, where $\alpha$, $\beta$, $\gamma$ correspond to roll, pitch and yaw respectively. $h$ then writes:

$\hat{\bm{d}}_{ij}$ is the network output displacement between state $i$ and $j$, using IMU samples rotated to the local gravity-aligned frame anchored at pose $i$ as input. $\bm{\eta}_{{d}_{ij}}$ is a random variable that, we assume, follows the normal distribution $\mathcal{N}(0,\hat{\bm{\Sigma}}_{ij})$ given by the network.

In the above expression, $\lfloor{\bm{x}}\rfloor_{\times}$ is the skew-symmetric matrix built from vector ${\bm{x}}$. The singularity $\cos{\beta}=0$ occurs when the person wearing the headset is looking straight down or straight up: we simply discard the update for these cases. Note that these situations are unlikely and never occur in our dataset.

Finally, ${\bm{H}}$ and $\hat{\bm{\Sigma}}_{ij}$ are used to compute the Kalman gain to update the state $\bm{X}$ and the covariance ${\bm{P}}$ as follows:

We use a $\chi_{2}$ test to protect the filter estimate against occasional wrong network output: we discard the update when the normalized innovation error $\|\bm{d}_{ij}-\hat{\bm{d}}_{ij}\|_{{\bm{H}}{\bm{P}}{\bm{H}}^{T}+\hat{\bm{\Sigma}}_{ij}}$ is greater than a threshold. We choose here the threshold value of 11.345 which corresponds to the $99^{th}$ percentile of the $\chi_{2}$ distribution with 3 degrees of freedom.

In practice, when using the measurement covariance $\hat{\bm{\Sigma}}_{ij}$ from the network, we scale the covariance by a factor of 10 to compensate for the temporal correlation of measurements as noted in Sec. VII-A1.

V-E State size, Marginalization and Initialization

Our EKF needs a good initialization to converge. In this work we assumed the initial state is given by an external procedure. In our experiments we initialize the speed, roll, and pitch with the ground-truth estimate assigning a large covariance, while the yaw and the position are initialized with a strong prior in order to fix the gauge. Biases are initialized as with the values of an initial factory calibration, and we refine the biases online to account for its evolution due to turn-on change, temperature effect, or aging.

In practice, we use the following values to initialize the state covariance: $\sigma_{\prescript{\text{w}}{}{\bm{v}}}=0.1$ m/s, $\sigma_{\bm{b}_{a}}=0.2$ m/s2, $\sigma_{\bm{b}_{g}}=1.10^{-4}$ rad/s, $\sigma_{\theta}=\text{diag}(10,10,0.1)$ deg. We also compensate all the input IMU samples for non-orthogonality scale factor and g-sensitivity from the factory calibration.

VI Experimental Results

We evaluate our system on the test split of our dataset. Each of these 37 trajectories contains between 3 to 7 minutes of human activities. We compare the performance of two pose estimators against a state-of-the-art VIO implementation based on and considered as ground-truth (GT):

tlio: the tightly-coupled EKF with displacement update from the trained network presented in this work.

3d-ronin: an estimator where the displacements from the same trained network are concatenated in the direction given by an engineered AHRS attitude filter, resembling what smartphones have. We borrow the name from , but we extended the method to 3D and we trained the network on our dataset to get a fair comparison. Note that our dataset contains trajectories involving climbing staircases, sitting motions, and walking outdoors on uneven terrain, where the 2D PDR method of is not directly applicable.

In order to assess our system performance, for each dataset of length $n$, we define the following metrics:

ATE (m): $\sqrt{\frac{1}{n}\sum_{i}^{n}\|\prescript{\text{w}}{}{\bm{p}}_{i}-\prescript{\text{w}}{}{\hat{\bm{p}}}_{i}\|^{2}}$

The Absolute Translation Error indicates the spatial closeness of position estimate to the GT over the whole trajectory, computed as root-mean square error (RMSE) between these sets of values.

RTE-$\Delta t$ (m): $\sqrt{\frac{1}{n}\sum_{i}^{n}\|\prescript{\text{w}}{}{\bm{p}}_{i+\Delta t}-\prescript{\text{w}}{}{\bm{p}}_{i}-\mathtt{R}_{\gamma}\hat{\mathtt{R}}_{\gamma}^{T}(\prescript{\text{w}}{}{\hat{\bm{p}}}_{i+\Delta t}-\prescript{\text{w}}{}{\hat{\bm{p}}}_{i})\|^{2}}$

DR (%) : $(\|\prescript{\text{w}}{}{\bm{p}}_{n}-\prescript{\text{w}}{}{\hat{\bm{p}}}_{n}\|)/(\text{trajectory-length})$

The final translation drift over the distance traveled.

We compute similar metrics for yaw angle $\gamma$, measuring the quality of the unobservable orientation around gravity:

VI-B System Performance

Figure 3 shows the distribution of the metrics across the entire test set. It demonstrates that our system consistently performs better than the decoupled orientation and position estimator 3d-ronin on all metrics.

On 3D position, tlio performs better than 3d-ronin which integrates average velocities. Integration approach has the advantage of being robust to outliers since the measurements at each instant are decoupled. The result shows not only the benefit of integrating IMU kinematic model, but also the overall robustness of the filter, which comes from the quality of the covariance output from the network and the effectiveness of outlier rejection with $\chi_{2}$ test.

Our system also has a smaller yaw drift than 3d-ronin, indicating that even without any hand engineered heuristics or step detection, using displacement estimates from a trained statistical model outperforms a smartphone AHRS attitude filter. This shows that the EKF can accurately estimate not only position and velocity, but also gyroscope biases.

Figure 4 shows a hand-picked collection of 7 trajectories containing the best, typical and worst cases of tlio and 3d-ronin, while Figure 1 shows a 3D visualization of one additional sequence. See the corresponding captions for more details about the trajectories and failure cases.

VII Components and Variation Studies

In this section, we evaluate the deep learning component of our system in isolation.

VII-A2 Consistency of learned covariance

VII-A3 Sensitivity Analysis

Figure 7 shows the network output robustness to input perturbation on bias and gravity direction. We observe that the models trained with bias perturbation are more robust to IMU bias, in particular accelerometer bias. The model trained with gravity perturbation is significantly more robust to gravity direction errors. These network training techniques effectively help reduce the propagation of errors from input to the output, protecting the independence assumption required in a Kalman Filter framework. Therefore we choose 200hz-1s-1s trained with bias and gravity perturbation as our system model.

VII-B EKF System

We compare system variants using a hand-tuned constant covariance and networks trained without likelihood loss to show the benefit of using the regressed uncertainty for tlio. To find the best constant covariance parameters, we use the standard deviation of the measurement errors on the test set - $\text{diag}(0.051,0.051,0.013)$m - multiplied by a factor yielding the best median ATE tuned by grid-search. Fig. 8 shows the Cumulative Density Function of ATE, Drift and AYE over the test set of various system configurations. We observe that using 3d-ronin, training the network with only MSE loss gives a more accurate estimate. However, using a fixed covariance, such network variants in a filter system achieve similar performance. What makes a difference is the regressed covariance from the network, which significantly improves the filter in ATE and Drift. We also notice a dataset where using a fixed covariance loses track due to initialization failure, showing the adaptive property of the regressed uncertainty for different inputs, improving system robustness. Comparing to 3d-ronin-mse, tlio has an improvement of $27\%$ and $31\%$ on the average yaw and position drift respectively.

VII-B2 Timing parameters

Fig. 9 shows the performance comparison between variations on network IMU frequency, $\Delta t_{ij}$, and filter measurement-update frequency. ATE and drift are reduced when using high frequency measurements. Once again, we observe minor differences between network parameter variations on the monitored metrics. This shows that our entire pipeline is not sensitive to these choices of parameters, and tlio outperforms 3d-ronin in all cases.

VIII Conclusion

In this paper we propose a tightly-coupled inertial odometry algorithm introducing a learned component in an EKF framework. We present the network regressing 3D displacement and uncertainty, and the EKF fusing the displacement measurement to estimate the full state. We train a network that learns a prior on the displacement distributions given IMU data from statistical motion patterns. We show through experimental results and variation studies that the network outputs are statistically consistent, and the filter outperforms the state-of-the-art trajectory estimator using velocity integration on position estimates, as well as a model-based AHRS attitude filter on orientation. This demonstrates that with a learned prior, an IMU sensor alone can provide enough information to do low drift pose estimation and calibration for pedestrian dead-reckoning. As common with learning approaches, the system is limited by the scope of training data. Unusual motions cause system failure as discussed with examples. Whether similar approach can be generalized to wider use cases such as legged robots is still an unexplored but promising field of research.

References