A Robust Indoor/Outdoor Navigation Filter Fusing Data from Vision and Magneto-Inertial Measurement Unit

Abstract

Visual-inertial Navigation Systems (VINS) are nowadays used for robotic or augmented reality applications. They aim to compute the motion of the robot or the pedestrian in an environment that is unknown and does not have specific localization infrastructure. Because of the low quality of inertial sensors that can be used reasonably for these two applications, state of the art VINS rely heavily on the visual information to correct at high frequency the drift of inertial sensors integration. These methods struggle when environment does not provide usable visual features, such than in low-light of texture-less areas. In the last few years, some work have been focused on using an array of magnetometers to exploit opportunistic stationary magnetic disturbances available indoor in order to deduce a velocity. This led to Magneto-inertial Dead-reckoning (MI-DR) systems that show interesting performance in their nominal conditions, even if they can be defeated when the local magnetic gradient is too low, for example outdoor. We propose in this work to fuse the information from a monocular camera with the MI-DR technique to increase the robustness of both traditional VINS and MI-DR itself. We use an inverse square root filter inspired by the MSCKF algorithm and describe its structure thoroughly in this paper. We show navigation results on a real dataset captured by a sensor fusing a commercial-grade camera with our custom MIMU (Magneto-inertial Measurment Unit) sensor. The fused estimate demonstrates higher robustness compared to pure VINS estimate, specially in areas where vision is non informative. These results could ultimately increase the working domain of mobile augmented reality systems.

1. Introduction

1.1. Motivation

Infrastructure-less navigation and positioning in indoor location is a technical prerequisite for numerous industrial and consumer applications: ranging from lone worker safety in industrial facilities, to augmented reality. Still, it remains an open challenge to efficiently and reliably combine embedded sensors to reconstruct a position or a trajectory. In the present work, we address this challenge restricting ourselves to the following sensors: MEMS gyroscopes, accelerometers, magnetometers and a standard industrial vision camera. We motivate this choice by the fact these sensors are cheap and can easily be embedded in a wearable form factor, which makes this combination appealing for pedestrian application. Moreover, VINS (Visual-Inertial Navigation Systems) literature, showed recently tremendous progress in the past few years.

1.2. State of the Art and Contribution

Indeed, if a wide range of embedded visual sensors were previously presented to solve the problem, such as rotating LIDARs [1] or depth sensors [2], much of the recent efforts focused on conventional cameras, as they are cheap, and already present in a wide range of lightweight devices, such as smart-phones. While authors of [3,4,5] rely on multiple embedded cameras, single cameras solutions have been shown to provide good results. For instance, authors of [6,7] present efficient filtering methods for monocular VINS. Nonetheless, monocular VINS remains highly sensitive to degenerate motions scenarios. For instance, the scale factor is weakly constrained in case of a steady motion and pure rotation motions must be tackled with special care in the estimation process. Moreover, current VINS implementations rely heavily on high frequency visual corrections (10–20 Hz), and break when visual environment is not adequate for more than a few second (bad illumination, presence of smoke, motion blur, etc.).

Compared to the previous literature, we explore a sensor suite alternative to VINS and able to provide a precise and robust navigation information. We combine the vision sensor with a MIMU (Magneto-Inertial Measurement Unit), i.e., an IMU sensor augmented with an array of magnetometers. Within a stationary and non-uniform magnetic environment, the magnetic measurements render the body speed observable, which has been shown to significantly improve motion prediction compared to IMU alone [8,9]. This technique, named Magneto-Inertial Dead-Reckoning (MI-DR) hereafter, is perfectly fitted for indoor navigation as human made environment, wall, floor, and furnitures all perturb the magnetic field in a significant way. As a result, the open-loop position error of this method is often around a few percent of the trajectory length in indoor environment (see [10]). MI-DR fails however in places where the gradient of the magnetic field vanishes, (commonly outdoor) and lacks robustness when the magnetic environment is not stationary.

We have already presented various approaches to fuse the information from MIMU sensor with vision sensors for dead-reckoning estimation. In [11], we proposed a semi-tight fusion scheme combining a depth sensor with a MIMU to increase robustness and availability of the position/orientation informations. Yet, we finally turned towards conventional monocular camera, mainly because the limited range of depth sensors makes them unable to improve the MI-DR estimation in large rooms or outdoor. In order to still be able to estimate accurately the scale, we also turned towards a fully tight fusion scheme, that includes estimation of camera pose, current speed, magnetic field and inertial sensor biases. We investigated an optimization-based solution in [12] and a filter-based solution in [13]. Here we present an extension of the work presented in the conference paper [13] with new results and a slightly different implementation of the filter. The used estimator is still inspired by the pure VINS method presented in [14] for the visual measurements and by the magnetic prediction and measurement process of [9] for the MIMU handling process. We will call it MI-MSCKF for Magneto-inertial Multi State Constraint Kalman Filter. As in [13], we choose a square-root implementation which leads to a better overall conditioning of matrices operations involved in the filtering process. The inverse form is also computationally interesting for high-dimensionality measurements [15] (p. 141).

We show on experimental data that the MIMU provides robustness in situations where vision fails, and that, reciprocally, vision allows the system to handle situations where the magnetic gradient is to low for the MIMU to work properly, typically in outdoor situations.

1.3. Paper Organization

After introducing general notations and conventions in Section 2, we describe the dynamic model our filter is built on in Section 3, with a focus on the magnetic prediction equation, less known in visual-inertial literature. This section also presents the model discretization and error model of the sensors. The Section 4 describes the filter design: chosen state and equations for propagation and measurement update steps. The last section (Section 5) presents comparative results of trajectory estimation obtained on real datasets.

2. Notations

2.1. General Conventions

Bold capital letters $\mathbf{X}$ denote matrices or elements of manifold. Parenthesis are used to denote the Cartesian product of two elements $a\in \mathcal{A},b\in \mathcal{B}\mapsto (a,b)\in \mathcal{A}\times \mathcal{B}$ and brackets for the concatenation of two row vectors. For a vector $\mathbf{x}={[x_1,x_2,x_3]}^{\mathsf{T}}$, ${\mathbf{x}}_2$ denotes its second component $x_2$ and ${\mathbf{x}}_{2:3}$ is the sub-vector ${[x_2,x_3]}^{\mathsf{T}}$. For matrices, we define the vectorization operation $\mathrm{Vec}\left(\right)$ so that:

$$\mathrm{Vec}\left(\left[\begin{array}{cc}{x}_{11}& x_{12}\\ x_{21}& x_{22}\end{array}\right]\right)={\left[\begin{array}{c}{x}_{11},x_{21},x_{12},x_{22}\end{array}\right]}^{\mathsf{T}}.$$

(1)

$\mathrm{A}\otimes \mathrm{B}$ will denotes the Kronecker product of matrices $\mathrm{A}$ and $\mathrm{B}$. ${\partial }_{\mathrm{A}}$ will be a shorthand for the derivative with respect to the coefficients of $\mathrm{Vec}\left(\mathrm{A}\right)$. The notation ${\parallel \mathbf{x}\parallel }_{\mathsf{\Sigma }}$ is the Mahalanobis norm of invertible covariance $\mathsf{\Sigma }$: ${\parallel \mathbf{x}\parallel }_{\mathsf{\Sigma }}={\mathbf{x}}^{\mathsf{T}}{\mathsf{\Sigma }}^{-1}\mathbf{x}$. ${\mathbf{I}}_n$ is the identity matrix of size n and ${\mathbf{0}}_{n\times m}$ the zero matrix of size $n\times m$. ${\mathbf{I}}_n$ is the identity matrix of size $n\times n$ or corresponding application. $\mathcal{O}\left(n\right)$ is the orthogonal matrix group of size n.

2.2. Reserved Symbols

Generally and except stated otherwise we use the following symbols: $\mathbf{p}$ for the translational part of the body pose, $\mathtt{R}$ for its rotational part. $\mathbf{v}$ for the velocity, ${\mathbf{b}}_a$ and ${\mathbf{b}}_g$ for inertial sensor bias, $\mathbf{B}$ for the magnetic field, $\nabla \mathbf{B}$ for its $3\times 3$ gradient matrix. $\omega$ is used for the rotational speed and $\mathbf{a}$ for the specific acceleration. 3D landmark position are noted with the letter $\mathbf{l}$ and these observation into an image with the letter $\mathit{o}$. We use a tilde symbol for measured quantities or generally quantities that can be derived from sensor reading. We use an hat for estimated version of a physical quantity. $\mathbf{g}$ symbol is kept for the gravity vector in inertial coordinates. The world coordinates are defined such that the gravity vector writes $\mathbf{g}\simeq {[0,0,-9.81]}^{\mathsf{T}}$. When ambiguous, the reference frame in which a quantity is expressed will be noted in exponent: $w$ stands for the world gravity-aligned reference frame, $b$ for the current body reference frame and $c$ for the camera frame. The Figure 1 summarizes the chosen notations.

2.3. Rotation Parametrization

For rotations, we use the convention that ${\mathtt{R}}^w$ transforms a vector from body frame to world frame by left multiplying it. For the sake of clarity of the developments, we represent the attitude of the sensor as a rotation matrix. The Special Orthogonal Group is denoted $\mathrm{SO}\left(3\right)$ and its associated Lie algebra $\mathfrak{so}\left(3\right)$—the set of skew symmetric matrices. Any element of $\mathfrak{so}\left(3\right)$ can be identified with a vector of ${\mathbb{R}}^3$: ${\left[x\right]}_{\times }\in \mathfrak{so}\left(3\right)$ with $x\in {\mathbb{R}}^3$ and $vex$ so that $vex\left({\left[x\right]}_{\times }\right)=x$. $\exp$ and $\log$ are the standard exponential map and logarithm on $\mathrm{SO}\left(3\right)$. As in [16], we use vectorized versions of $\exp$ and $\log$:

$$\begin{array}{c}\mathrm{Exp}:\begin{array}{ccc}{\mathbb{R}}^3\hfill & \to & \mathrm{SO}\left(3\right)\hfill \\ \delta \theta \hfill & \mapsto & \exp \left({\left[\delta \theta \right]}_{\times }\right)\hfill \end{array}\hfill \end{array}$$

(2)

and $\mathrm{Log}:\mathrm{SO}\left(3\right)\to {\mathbb{R}}^3$ the inverse function. With these conventions, $\mathrm{Log}\left(\mathrm{Exp}\right(x\left)\right)=x$.

3. On-Board Sensors and Evolution Model

3.1. Sensing Hardware

The MIMU sensors (see Figure 2) provide raw measurements of biased proper acceleration ${\tilde{\mathbf{a}}}_{}^b$, biased angular velocity ${\tilde{\omega }}_{}^b$, magnetic field ${\tilde{\mathbf{B}}}_{}^b$ and its gradients $\nabla {\tilde{\mathbf{B}}}_{}^b$. The latter is a $3\times 3$ matrix which elements are estimated by finite differences between signals recorded on an array of magnetometers. These sensors are carefully calibrated offline and harmonised in the same spatial coordinate frame with a method similar to [17]. The resulting MIMU coordinate frame, centered around the magnetometer and accelerometer (which are assumed colocalized here), will be used as the body frame in all subsequent derivations. We use a global shutter camera modeled as a pinhole camera with instantaneous exposure. In practice, recorded real images are undistorted using intrinsics calibration parameters. Intrinsic parameters (focal length in pixels $(f_x,f_y)$, principal point coordinates $(c_x,c_y)$ and distortion coefficients) are assumed to be known from a preliminary calibration. The pinhole camera projection function $\pi$ maps a landmark 3D coordinates ${\mathbf{l}}^c$ expressed in the camera frame to the pixel coordinates of its projection onto the image:

$$\begin{array}{c}\pi :\begin{array}{ccc}{\mathbb{R}}^3\hfill & \to & {\mathbb{R}}^2\hfill \\ {\mathbf{l}}^c\hfill & \mapsto & \left[\begin{array}{c}{f}_x\frac{{\mathbf{l}}_1^c}{{\mathbf{l}}_3^c}+c_x\\ f_y\frac{{\mathbf{l}}_2^c}{{\mathbf{l}}_3^c}+c_y\end{array}\right]\hfill \end{array}.\hfill \end{array}$$

(3)

The camera is rigidly attached to the MIMU sensor board. The transformation $({\mathtt{R}}_{\mathrm{c}}^{\mathrm{b}},{\mathbf{p}}_{\mathrm{c}}^{\mathrm{b}})$ between the camera frame and the body frame is assumed to be known. We could alternatively include this transform into the state filter as done in [18] for instance.

Since hardware synchronization was not possible with the components used here, we use a datation approach. In the online estimation process we make the simplifying assumption that images are captured simultaneously with the MIMU sample closest in time. More precisely, the image information at time $t_{{image}_p}$ is processed using the state estimate at time $t_{{mimu}_k}$ with k such as:

$$k={\arg \min }_{k,t_{{mimu}_k}<t_{{image}_p}}|t_{{mimu}_k}-t_{{image}_p}|.$$

The temporal error done with this approach is always smaller than the sampling period of the MIMU sensors, which is often below the exposure time of the camera. Besides, this approximation significantly simplifies time management as all measurements are indexed by the MIMU time sampling.

3.2. Evolution Model

In order to estimate the position and orientation of the body coordinate frame in the world frame, one has to track an estimate of its speed and of the magnetic field at the current position of its center. We model the evolution of these quantities with the following differential equations:

$$\begin{array}{cc}{\dot{\mathtt{R}}}^w\left(t\right)\hfill & ={\mathtt{R}}^w\left(t\right){\left[{\omega }^b\left(t\right)\right]}_{\times },\hfill \end{array}$$

(4)

$$\begin{array}{cc}{\dot{\mathbf{p}}}^w\left(t\right)\hfill & ={\mathtt{R}}^w\left(t\right){\mathbf{v}}^b\left(t\right),\hfill \end{array}$$

(5)

$$\begin{array}{cc}{\dot{\mathbf{v}}}^b\left(t\right)\hfill & =-{\left[{\omega }^b\left(t\right)\right]}_{\times }{\mathbf{v}}^b\left(t\right)+{\mathtt{R}}^{w\mathsf{T}}\left(t\right){\mathbf{g}}^w+{\mathbf{a}}^b\left(t\right),\hfill \end{array}$$

(6)

$$\begin{array}{cc}{\dot{\mathbf{B}}}^b\left(t\right)\hfill & =-{\left[{\omega }^b\left(t\right)\right]}_{\times }{\mathbf{B}}^b\left(t\right)+\nabla {\mathbf{B}}^b\left(t\right){\mathbf{v}}^b\left(t\right).\hfill \end{array}$$

(7)

This model relies on the following assumptions on the environment:

• Flat-earth approximation. We assume that the Est-North-Up (ENU) earth frame at filter initialization is an inertial frame.
• Stationary magnetic field in the world frame—although possibly spatially non-uniform, leading to the spatial gradient in (7).

The first assumption is, in practice, often used in VINS literature, in which gyroscopes are not precise enough to measure earth rotational speed (roughly $7\times {10}^{-5}\mathrm{rad}/\mathrm{s}$). However if high-end gyroscopes had been used, it is likely that an estimate based on the simple model (4)–(7) would introduce some error, confusing earth rotational velocity with biases estimates. In our opinion though, even in the case of high-end hardware, it is not clear that the visual pipeline used in this work would be able to provide information reliable enough to estimate biases with sufficient accuracy to be influenced by earth rotational speed. This would be a great challenge to address.

Note also that the world frame differs from the ENU frame defined at filter initialization: they both have their origin at the position of the center of the body frame at initial time, but they can differ by a rotation around the gravity direction, as heading is not observable at initialization.

Equation (7) is the key equation for MI-DR. It relates the evolution of the magnetic field with kinematics quantities and local magnetic gradient. It actually renders the body velocity observable provided the matrix $\nabla {\mathbf{B}}^b$ is invertible. However, it fails giving useful translational information if the magnetic field is uniform. This happens outdoor, where the magnetic field is uniformly equal to the earth magnetic field or in very large empty rooms. In the latter situation, magnetic gradients can vanish a few meters away from a wall, ceiling or floor.

Also, the stationarity assumption can be challenged in some environments, or punctually if pieces of metal are moving in the vicinity of the magnetometers. While some instationarities can be modeled—as the case of power-line interference [9]—in practice, any useful estimator should also be robust to non modeled effects.

3.3. Model Discretization

The model will be used in a discrete extended Kalman filtering framework described in Section 4 below. We thus discretize it the following way:

$$\begin{array}{cc}{\mathtt{R}}_{k+1}^w\hfill & ={\mathtt{R}}_k^w{\widetilde{\Delta \mathtt{R}}}_{kk+1},\hfill \end{array}$$

(8)

$$\begin{array}{cc}{\mathbf{p}}_{k+1}^w\hfill & ={\mathbf{p}}_k^w+{\mathtt{R}}_k^w{\mathbf{v}}_k^b\Delta t+\frac{1}{2}{\mathbf{g}}^w\Delta t_{ij}^2+{\mathtt{R}}_k^w{\widetilde{\Delta \mathbf{p}}}_{kk+1},\hfill \end{array}$$

(9)

$$\begin{array}{cc}{\mathbf{v}}_{k+1}^b\hfill & ={\widetilde{\Delta \mathtt{R}}}_{kk+1}^{\mathsf{T}}\left({\mathbf{v}}_k^b+{{\mathtt{R}}_k^w}^{\mathsf{T}}{\mathbf{g}}^w\Delta t+{\widetilde{\Delta \mathbf{v}}}_{kk+1}\right),\hfill \end{array}$$

(10)

$$\begin{array}{cc}{\mathbf{B}}_{k+1}^b\hfill & ={\widetilde{\Delta \mathtt{R}}}_{kk+1}^{\mathsf{T}}\left({\mathbf{B}}_k^b+{\tilde{\mathit{D}}}_{\mathbf{v};\mathit{k}\mathit{k}+\mathbf{1}}{\mathbf{v}}_k^b+{\tilde{\mathit{D}}}_{\mathtt{R};\mathit{k}\mathit{k}+\mathbf{1}}{{\mathtt{R}}_k^w}^{\mathsf{T}}{\mathbf{g}}^w+{\tilde{\mathit{M}}}_{\mathit{k}\mathit{k}+\mathbf{1}}\right).\hfill \end{array}$$

(11)

where we introduced the following notation corresponding to continuous integrals:

$$\begin{array}{cc}{\widetilde{\Delta \mathtt{R}}}_{kk+1}\hfill & \stackrel{\mathsf{def}}{=}\Delta {\mathtt{R}}_k\left(t_{k+1}\right),\hfill \end{array}$$

(12)

$$\begin{array}{cc}{\widetilde{\Delta \mathbf{v}}}_{kk+1}\hfill & \stackrel{\mathsf{def}}{=}{\widetilde{\Delta \mathbf{v}}}_k\left(t_{k+1}\right),\hfill \end{array}$$

(13)

$$\begin{array}{cc}{\widetilde{\Delta \mathbf{p}}}_{kk+1}\hfill & \stackrel{\mathsf{def}}{=}{\int }_{t_k}^{t_{k+1}}{\widetilde{\Delta \mathbf{v}}}_k\left(\tau \right)\mathtt{d}\tau ,\hfill \end{array}$$

(14)

$$\begin{array}{cc}{\tilde{\mathit{D}}}_{\mathbf{v};\mathit{k}\mathit{k}+\mathbf{1}}\hfill & \stackrel{\mathsf{def}}{=}{\int }_{t_k}^{t_{k+1}}\Delta {\mathtt{R}}_k\left(\tau \right)\nabla {\mathbf{B}}^b\left(\tau \right)\Delta {\mathtt{R}}_k{\left(\tau \right)}^{\mathsf{T}}\mathtt{d}\tau ,\hfill \end{array}$$

(15)

$$\begin{array}{cc}{\tilde{\mathit{D}}}_{\mathtt{R};\mathit{k}\mathit{k}+\mathbf{1}}\hfill & \stackrel{\mathsf{def}}{=}{\int }_{t_k}^{t_{k+1}}\Delta {\mathtt{R}}_k\left(\tau \right)\nabla {\mathbf{B}}^b\left(\tau \right)\Delta {\mathtt{R}}_k{\left(\tau \right)}^{\mathsf{T}}\left[\tau -t_k\right]\mathtt{d}\tau ,\hfill \end{array}$$

(16)

$$\begin{array}{cc}{\tilde{\mathit{M}}}_{\mathit{k}\mathit{k}+\mathbf{1}}\hfill & \stackrel{\mathsf{def}}{=}{\int }_{t_k}^{t_{k+1}}\Delta {\mathtt{R}}_k\left(\tau \right)\nabla {\mathbf{B}}^b\left(\tau \right)\Delta {\mathtt{R}}_k{\left(\tau \right)}^{\mathsf{T}}\Delta {\tilde{\mathbf{v}}}_k\left(\tau \right)\mathtt{d}\tau ,\hfill \end{array}$$

(17)

$$\begin{array}{ccccc}\mathrm{with}\Delta {\mathtt{R}}_k\left(\tau \right)\hfill & \stackrel{\mathsf{def}}{=}{\mathbf{I}}_3+{\int }_{t_k}^{\tau }\Delta {\mathtt{R}}_k\left(s\right){\left[{\omega }^b\left(s\right)\right]}_{\times }\mathtt{d}s\hfill & \mathrm{and}\hfill & {\widetilde{\Delta \mathbf{v}}}_k\left(\tau \right)\hfill & \stackrel{\mathsf{def}}{=}{\int }_{t_k}^{\tau }\Delta {\mathtt{R}}_k\left(s\right){\mathbf{a}}^b\left(s\right)\mathtt{d}s.\hfill \end{array}$$

(18)

The notation $\Delta {\mathtt{R}}_k\left(\tau \right)$ is the rotation matrix that transforms point in body frame at time $\tau$ to corresponding point in body frame at time $t_k$, that can be deduced from gyroscopes integration.

These integrals can be computed from unbiased MIMU measurements. We thus estimate the biases of the accelerometers and gyrometers along with previously quantities. These are assumed to follow a first-order Gauss-Markov stochastic evolution:

$$\begin{array}{cc}\dot{{\mathbf{b}}_g}\left(t\right)\hfill & =-\frac{1}{{\tau }_g}{\mathbf{b}}_g+{\mathbf{J}}_{bg},\hfill \end{array}$$

(19)

$$\begin{array}{cc}\dot{{\mathbf{b}}_a}\left(t\right)\hfill & =-\frac{1}{{\tau }_a}{\mathbf{b}}_a+{\mathbf{J}}_{ba}.\hfill \end{array}$$

(20)

where generating noises ${\mathbf{J}}_{bg}$ and ${\mathbf{J}}_{ba}$ satisfy:

$$\begin{array}{cc}\mathbb{E}\left([{\mathbf{J}}_{bg}\left(t\right),{\mathbf{J}}_{ba}\left(t\right)]\right)\hfill & ={\mathbf{0}}_{6\times 1},\hfill \end{array}$$

(21)

$$\begin{array}{cc}\mathbb{E}\left([{\mathbf{J}}_{bg}\left(t_1\right),{\mathbf{J}}_{ba}\left(t_1\right)].{[{\mathbf{J}}_{bg}\left(t_2\right),{\mathbf{J}}_{ba}\left(t_2\right)]}^{\mathsf{T}}\right)\hfill & ={\mathcal{W}}_c\delta (t_2-t_1),\hfill \end{array}$$

(22)

$${\mathcal{W}}_c=\mathrm{diag}({\sigma }_{bg;c}^2{\mathbf{I}}_3,{\sigma }_{ba;c}^2{\mathbf{I}}_3).$$

(23)

${\tau }_b$, ${\tau }_a$, ${\sigma }_{bg;c}$ and ${\sigma }_{ba;c}$ are expressed in s, s, $\frac{\mathrm{rad}}{s^2}\frac{1}{\sqrt{Hz}}$ and $\frac{\mathrm{m}}{s^3}\frac{1}{\sqrt{Hz}}$ respectively and are characteristics of the IMU. Discretization of the evolution of biases leads to:

$$\begin{array}{cc}{{\mathbf{b}}_g}_{k+1}\hfill & =-\exp \left(\frac{\Delta t_{ij}}{{\tau }_g}\right){{\mathbf{b}}_g}_k+{\mathit{\eta }}_{bg}\hfill \end{array}$$

(24)

$$\begin{array}{cc}{{\mathbf{b}}_a}_{k+1}\hfill & =-\exp \left(\frac{\Delta t_{ij}}{{\tau }_a}\right){{\mathbf{b}}_a}_k+{\mathit{\eta }}_{ba}\hfill \end{array}$$

(25)

The ${\mathit{\eta }}_{\mathtt{x}}$ appearing in (24) and (25) will be then modelled as discrete random variables with Gaussian density $\mathcal{N}(0,\mathit{W})$, $\mathit{W}$ being computed as $(t_{k+1}-t_k){\mathcal{W}}_c$ [19] (p. 231).

The presented models discretization exhibits integrals that all have to be computed numerically in order to build the filter on. Normally, the choice of the numerical integration method depends on the required accuracy. In the current implementation though, we take a very simple approach: we assume that the biases, acceleration and magnetic field gradient—the last two are in world frame—are constant between two MIMU sample and use the following identity:

$$\begin{array}{cc}{\widetilde{\Delta \mathtt{R}}}_{kk+1}\hfill & \simeq \mathrm{Exp}\left(\left({\tilde{\omega }}_k^b-{{\mathbf{b}}_g}_k\right)\Delta t_k\right),\hfill \end{array}$$

(26)

$$\begin{array}{cc}{\widetilde{\Delta \mathbf{v}}}_{kk+1}\hfill & \simeq \left({\tilde{\mathbf{a}}}_k^b-{{\mathbf{b}}_a}_k\right)\Delta t_k,\hfill \end{array}$$

(27)

$$\begin{array}{cc}{\widetilde{\Delta \mathbf{p}}}_{kk+1}\hfill & \simeq \frac{1}{2}\left({\tilde{\mathbf{a}}}_k^b-{{\mathbf{b}}_a}_k\right)\Delta t_k^2,\hfill \end{array}$$

(28)

$$\begin{array}{cc}{\tilde{\mathit{D}}}_{\mathbf{v};\mathit{k}\mathit{k}+\mathbf{1}}\hfill & \simeq \nabla {\tilde{\mathbf{B}}}_k^b\Delta t_k,\hfill \end{array}$$

(29)

$$\begin{array}{cc}{\tilde{\mathit{D}}}_{\mathtt{R};\mathit{k}\mathit{k}+\mathbf{1}}\hfill & \simeq \frac{1}{2}\nabla {\tilde{\mathbf{B}}}_k^b\Delta t_k^2,\hfill \end{array}$$

(30)

$$\begin{array}{cc}{\tilde{\mathit{M}}}_{\mathit{k}\mathit{k}+\mathbf{1}}\hfill & \simeq \nabla {\tilde{\mathbf{B}}}_k^b\frac{1}{2}\left({\tilde{\mathbf{a}}}_k^b-{{\mathbf{b}}_a}_k\right)\Delta t_k^2.\hfill \end{array}$$

(31)

The error induced by this simple integration scheme is limited by the relatively high frequency of MIMU sample (325 Hz).

3.4. Sensors Error Model

Sensors errors propagate through the discrete model when used for filtering. We assume that the noisy sensor reading at time k (the input vector of the filter) can be written:

$${\tilde{\mathbf{u}}}_k=\underset{\mathrm{measurement}}{\underbrace{\left[\begin{array}{c}{\tilde{\mathbf{a}}}_k^b\\ {\tilde{\omega }}_k^b\\ \mathrm{Vec}\left(\nabla {\tilde{\mathbf{B}}}_k^b\right)\end{array}\right]}}=\underset{\mathrm{real}\mathrm{values}}{\underbrace{\left[\begin{array}{c}{\mathbf{a}}_k\\ {\omega }_k\\ \mathrm{Vec}\left(\nabla {\mathbf{B}}_k\right)\end{array}\right]}}+\left[\begin{array}{ccc}{\mathbf{I}}_3& {\mathbf{0}}_3& {\mathbf{0}}_{3\times 5}\\ {\mathbf{0}}_3& {\mathbf{I}}_3& {\mathbf{0}}_{3\times 5}\\ {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathcal{P}}_{\nabla \mathbf{B}}\end{array}\right]\underset{\mathrm{input}\mathrm{noise}\delta {\mathbf{u}}_k\in {\mathbb{R}}^{11}}{\underbrace{\left[\begin{array}{c}{\mathit{\eta }}_{{\mathbf{a}}_k^b}\\ {\mathit{\eta }}_{{\omega }_k^b}\\ {\mathit{\eta }}_{\nabla {\mathbf{B}}_k^b}\end{array}\right]}}$$

(32)

The noise $\delta {\mathbf{u}}_k$ is assumed to be gaussian $\delta {\mathbf{u}}_k\propto \mathcal{N}(0,{\mathsf{\Sigma }}_{\mathbf{u}})$ and is a sensor characteristic. We use here:

$${\mathsf{\Sigma }}_{\mathbf{u}}=\left[\begin{array}{ccc}{\sigma }_{\omega }^2{\mathbf{I}}_3& 0_{3\times 3}& 0_{3\times 5}\\ 0_{3\times 3}& {\sigma }_{\mathbf{a}}^2{\mathbf{I}}_3& 0_{3\times 5}\\ 0_{5\times 3}& 0_{5\times 3}& {\mathsf{\Sigma }}_{\nabla \mathbf{B}}\end{array}\right]\mathrm{with}{\sigma }_{\omega }\mathrm{in}\frac{rad}{s},{\sigma }_{\mathbf{a}}\mathrm{in}\frac{m}{s^2}\mathrm{and}{\mathsf{\Sigma }}_{\nabla \mathbf{B}}\mathrm{in}\frac{\mathrm{Gauss}}{m}\in {\mathbb{R}}^{5\times 5}.$$

(33)

Note that ${\mathcal{P}}_{\nabla \mathbf{B}}$ reflects that we explicitly exploit the symmetry in the magnetic field from Maxwell equation. They implies that the gradient should be a symmetric matrix and of zero trace and thus has 5 degrees of freedom, instead of the nine coefficient of the full matrix. ${\mathcal{P}}_{\nabla \mathbf{B}}\in {\mathbb{R}}^{9\times 5}$ is thus the matrix of the application generating the vectorized gradient matrix from minimal gradient coordinates:

$$\begin{array}{ccc}{\mathbb{R}}^5\hfill & \to & {\mathbb{R}}^9\hfill \\ \left(\begin{array}{c}{g}_1\\ g_2\\ g_3\\ g_4\\ g_5\end{array}\right)\hfill & \mapsto & \mathrm{Vec}\left(\left(\begin{array}{ccc}{g}_1& g_2& g_3\\ g_2& g_4& g_5\\ g_3& g_5& -g_1-g_4\end{array}\right)\right)\hfill \end{array}$$

(34)

4. Tight Fusion Filter

This section describe thoroughly the MI-MSCKF filter that is used in the experimental Section 5. Section 4.1 describes the parametrization of the state of the filter. Section 4.2 is dedicated to the propagation step while Section 4.3 details the magnetic and visual measurement equations.

4.1. State and Error State

We define the state space at time index k, ${\mathcal{X}}_k$ as a manifold which is compound of :

• the keyframe poses state space ${\mathcal{K}}_k$, which elements are the poses of a set of N past frames at time indexes $\{i_1,\dots i_N\}$ not necessarily temporally successive but close in time. With poses written ${\xi }_i=({\mathtt{R}}_i^w,{\mathbf{p}}_i^w)$, this part of the state have the following form:

  $$\left({\xi }_{i_1},\cdots ,{\xi }_{i_N}\right)\in {\mathcal{K}}_k={\left(\mathrm{SO}\left(3\right)\times {\mathbb{R}}^3\right)}^N$$

  (35)
• the current mimu state $\mathcal{S}$ space which elements have the following form:

  $${\mathit{s}}_k={\left({\xi }_k,{\mathbf{v}}_k^b,{\mathbf{B}}_k^b,{{\mathbf{b}}_a}_k,{{\mathbf{b}}_g}_k\right)}^{\mathsf{T}}\in \mathrm{SO}\left(3\right)\times {\mathbb{R}}^3\times {\mathbb{R}}^{12}$$

  (36)

A complete state space element at time index k is thus noted:

$${\mathbf{X}}_k=\left({\xi }_{i_1},\cdots ,{\xi }_{i_N},{\mathit{s}}_k\right)\in {\mathcal{X}}_k={\mathcal{K}}_k\times \mathcal{S}$$

(37)

We use the Lie group structure of this manifold and its tangent space to (i) define the error tracked by the filter and (ii) define the Jacobian of the measurement process. This derives from the fact that a perturbations around an element can be expressed as an element of its Lie algebra. We use the ⊞ operator symbol, so that ${\mathbf{X}}_k⊞\delta {\mathit{X}}_k$ computes a new state in ${\mathcal{X}}_k$ from a tangent perturbation $\delta {\mathit{X}}_k$ around ${\mathbf{X}}_k$. We define it as regular addition operation for all components of the state except for pose states where we use:

$$\xi ⊞\delta \xi =\left(\mathrm{Exp}\left(\delta {\xi }_{4:6}\right)\mathtt{R},\mathbf{p}+\delta {\xi }_{1:3}\right)$$

(38)

Similarly we define the reciprocal operator ⊟ as the binary operator giving the perturbation element between two states of $\mathcal{X}$. It is defined as regular minus operation except for pose states for which it is:

$${\xi }_2\boxminus {\xi }_1={\left[\mathrm{Log}{\left({\mathtt{R}}_2{\mathtt{R}}_1^{-1}\right)}^T,{\mathbf{p}}_2^{\mathsf{T}}-{\mathbf{p}}_1^{\mathsf{T}}\right]}^{\mathsf{T}}$$

(39)

We here define the error state as the application ⊟ operator between the true state and the estimated state, noted hereafter with an hat. It is thus an element of the tangent space at the current estimate.

$$\begin{array}{c}{ϵ}_k\stackrel{\mathsf{def}}{=}{\mathbf{X}}_k\boxminus {\widehat{\mathbf{X}}}_k\iff {\mathbf{X}}_k={\widehat{\mathbf{X}}}_k⊞{ϵ}_k\hfill \end{array}$$

(40)

Note that this implies a parametrization of the rotation error in world frame, and is different from our previous work [13] where rotation error was parametrized in the body frame.

The filtering process propagates the estimated mean, along with an estimate of uncertainty. This uncertainty is represented as a Gaussian density on the error state ${ϵ}_k\propto \mathcal{N}(0,{\mathbf{P}}_k)$ in order to take advantage of the Lie group structure defined previously, i.e., a minimal parametrization and a locally euclidean structure in tangent space.

For numerical reasons, the covariance ${\mathbf{P}}_k$ will be tracked by the filter in an square-root information form, such that we have the relationship ${\mathbf{P}}_k={\left({\widehat{\mathcal{S}}}_k^{\mathsf{T}}{\widehat{\mathcal{S}}}_k\right)}^{-1}$ with ${\widehat{\mathcal{S}}}_k$ an upper triangular matrix. The next section describes how this quantity evolves through the different steps of the filter: propagation and update.

4.2. Propagation/Augmentation/Marginalization

At the time of arrival of a new IMU data $k+1$, ${\mathbf{X}}_{k|k}$ and ${\widehat{\mathcal{S}}}_{k|k}$ are propagated. This is summarized by Figure 3 and splitted in three steps. First, the mimu state ${\mathit{s}}_k$ is propagated with discretized model (Section 4.2.1), then the full state is augmented with the resulting new mimu state ${\mathit{s}}_{k+1}$ (Section 4.2.2). Finally, some part of this augmented state are marginalized before the update step (Section 4.2.3).

4.2.1. Propagation

Keyframe poses states are estimation of physical quantities blocked at fixed instant in time, their error do not evolve with time and thus they are propagated with an identity function. Besides, the current mimu state error is propagated according to

$${\mathit{s}}_{k+1}={\mathbf{f}}_{\mathrm{mimu}}({\mathit{s}}_k,{\tilde{\mathbf{u}}}_k,{\mathit{\eta }}_k)$$

(41)

where the discrete mimu process function ${\mathbf{f}}_{\mathrm{mimu}}$ summarizes (8)–(11) and (24)–(25).

The mimu state error is increased from the three sources of uncertainty: the stochastic model of biases, the measurement noise $\delta {\mathbf{u}}_k$ on the input vector and the uncertainty of the previous estimate:

$$\begin{array}{cc}{ϵ}_{\mathrm{mimu},k+1}\hfill & ={\mathbf{f}}_{\mathrm{mimu}}({\widehat{\mathit{s}}}_k⊞{ϵ}_k^{\mathrm{mimu}},{\tilde{\mathbf{u}}}_k-\delta {\mathbf{u}}_k,{\mathit{\eta }}_k)\boxminus {\mathbf{f}}_{\mathrm{mimu}}({\widehat{\mathit{s}}}_k,{\tilde{\mathbf{u}}}_k,0)\hfill \end{array}$$

(42)

$$\begin{array}{c}\simeq {\mathsf{\Phi }}_{k+1,k}^{\mathrm{mimu}}{ϵ}_k^{\mathrm{mimu}}+{\mathbf{G}}_{k+1,k}^{\mathrm{mimu}}\delta {\mathbf{u}}_k+{\mathbf{C}}_{k+1,k}^{\mathrm{mimu}}{\mathit{\eta }}_k+O\left({ϵ}_k^{\mathrm{mimu}},\delta {\mathbf{u}}_k,{\mathit{\eta }}_k\right)\hfill \end{array}$$

(43)

The expression of matrices ${\mathsf{\Phi }}_{k+1,k}^{\mathrm{mimu}}$, ${\mathbf{G}}_{k+1,k}^{\mathrm{mimu}}$, ${\mathbf{C}}_{k+1,k}^{\mathrm{mimu}}$ are derived by 1st order development of (42):

$${\mathsf{\Phi }}_{k+1,k}^{\mathrm{mimu}}=\left[\begin{array}{cccccc}{\mathbf{I}}_3& {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{0}}_3& {{\mathsf{\Phi }}_{k+1,k}^{\mathrm{mimu}}}_{\mathtt{R}{\mathbf{b}}_g}& {\mathbf{0}}_3\\ {\left[-{\mathtt{R}}_k({\mathbf{v}}_k^b\Delta t+{\widetilde{\Delta \mathbf{p}}}_{kk+1})\right]}_{\times }& {\mathbf{I}}_3& {\mathtt{R}}_k\Delta t& {\mathbf{0}}_3& {{\mathsf{\Phi }}_{k+1,k}^{\mathrm{mimu}}}_{\mathbf{p}{\mathbf{b}}_g}& {{\mathsf{\Phi }}_{k+1,k}^{\mathrm{mimu}}}_{\mathbf{p}{\mathbf{b}}_g}\\ {\mathtt{R}}_{k+1}^{\mathsf{T}}{\left[{\mathbf{g}}^w\right]}_{\times }\Delta t& {\mathbf{0}}_3& \Delta \mathtt{R}& {\mathbf{0}}_3& {{\mathsf{\Phi }}_{k+1,k}^{\mathrm{mimu}}}_{\mathbf{v}{\mathbf{b}}_g}& {{\mathsf{\Phi }}_{k+1,k}^{\mathrm{mimu}}}_{\mathbf{v}{\mathbf{b}}_a}\\ \Delta {\mathtt{R}}^{\mathsf{T}}{\tilde{\mathit{D}}}_{\mathtt{R};\mathit{k}\mathit{k}+\mathbf{1}}{\left[{\mathbf{g}}^w\right]}_{\times }& {\mathbf{0}}_3& \Delta {\mathtt{R}}^{\mathsf{T}}{\tilde{\mathit{D}}}_{\mathit{v};\mathit{k}\mathit{k}+\mathbf{1}}& \Delta {\mathtt{R}}^{\mathsf{T}}& {{\mathsf{\Phi }}_{k+1,k}^{\mathrm{mimu}}}_{\mathbf{B}{\mathbf{b}}_g}& {{\mathsf{\Phi }}_{k+1,k}^{\mathrm{mimu}}}_{\mathbf{B}{\mathbf{b}}_a}\\ {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{0}}_3& -\exp \left(\frac{\Delta t}{{\tau }_g}\right)& {\mathbf{0}}_3\\ {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{0}}_3& -\exp \left(\frac{\Delta t}{{\tau }_a}\right)\end{array}\right]$$

(44)

$$\begin{array}{cc}{\mathbf{G}}_{k+1,k}^{\mathrm{mimu}}=\left[\begin{array}{ccc}-{{\mathsf{\Phi }}_{k+1,k}^{\mathrm{mimu}}}_{\mathtt{R}{\mathbf{b}}_g}& 0& 0\\ -{{\mathsf{\Phi }}_{k+1,k}^{\mathrm{mimu}}}_{\mathbf{p}{\mathbf{b}}_g}& -{{\mathsf{\Phi }}_{k+1,k}^{\mathrm{mimu}}}_{\mathbf{p}{\mathbf{b}}_a}& 0\\ -{{\mathsf{\Phi }}_{k+1,k}^{\mathrm{mimu}}}_{\mathbf{v}{\mathbf{b}}_g}& -{{\mathsf{\Phi }}_{k+1,k}^{\mathrm{mimu}}}_{\mathbf{v}{\mathbf{b}}_a}& 0\\ -{{\mathsf{\Phi }}_{k+1,k}^{\mathrm{mimu}}}_{\mathbf{B}{\mathbf{b}}_g}& -{{\mathsf{\Phi }}_{k+1,k}^{\mathrm{mimu}}}_{\mathbf{B}{\mathbf{b}}_a}& {{\mathbf{G}}_{k+1,k}^{\mathrm{mimu}}}_{\mathbf{B}\nabla \mathbf{B}}\\ 0& 0& 0\\ 0& 0& 0\end{array}\right]\hfill & {\mathbf{C}}_{k+1,k}^{\mathrm{mimu}}=\left[\begin{array}{cc}0& 0\\ 0& 0\\ 0& 0\\ 0& 0\\ {\mathbf{I}}_3& 0\\ 0& {\mathbf{I}}_3\end{array}\right]\hfill \end{array}$$

(45)

$$\begin{array}{cc}{{\mathsf{\Phi }}_{k+1,k}^{\mathrm{mimu}}}_{\mathtt{R}{\mathbf{b}}_g}\hfill & =-{\mathtt{R}}_k\Delta t\hfill \end{array}$$

(46)

$$\begin{array}{cc}{{\mathsf{\Phi }}_{k+1,k}^{\mathrm{mimu}}}_{\mathbf{v}{\mathbf{b}}_g}\hfill & =-\Delta t{\left[{\mathbf{v}}_{k+1}\right]}_{\times }+{\partial }_{{{\mathbf{b}}_g}_k}\left({\widetilde{\Delta \mathbf{v}}}_{kk+1}\right)\hfill \end{array}$$

(47)

$$\begin{array}{cc}{{\mathsf{\Phi }}_{k+1,k}^{\mathrm{mimu}}}_{\mathbf{B}{\mathbf{b}}_g}=\hfill & -\Delta t{\left[{\mathbf{B}}_{k+1}\right]}_{\times }\hfill \\ & +\Delta {\mathtt{R}}^{\mathsf{T}}\left({\mathbf{v}}_k^{\mathsf{T}}\otimes {\mathbf{I}}_3\right){\partial }_{{\mathbf{b}}_g}\left(\mathrm{Vec}\left({\tilde{\mathit{D}}}_{\mathbf{v};\mathit{k}\mathit{k}+\mathbf{1}}\right)\right)\hfill \\ & +\Delta {\mathtt{R}}^{\mathsf{T}}\left({\mathbf{g}}^{\mathsf{T}}{\mathtt{R}}_k\otimes {\mathbf{I}}_3\right){\partial }_{{\mathbf{b}}_g}\left(\mathrm{Vec}\left({\tilde{\mathit{D}}}_{\mathtt{R};\mathit{k}\mathit{k}+\mathbf{1}}\right)\right)\hfill \end{array}$$

(48)

$$\begin{array}{c}+\Delta {\mathtt{R}}^{\mathsf{T}}{\partial }_{{\mathbf{b}}_g}\left({\tilde{\mathit{M}}}_{\mathit{k}\mathit{k}+\mathbf{1}}\right)\hfill \end{array}$$

(49)

$$\begin{array}{cc}{{\mathsf{\Phi }}_{k+1,k}^{\mathrm{mimu}}}_{\mathbf{p}{\mathbf{b}}_g}\hfill & ={\mathtt{R}}_k{\partial }_{{{\mathbf{b}}_g}_k}\left({\widetilde{\Delta \mathbf{p}}}_{kk+1}\right)\hfill \end{array}$$

(50)

$$\begin{array}{cc}{{\mathsf{\Phi }}_{k+1,k}^{\mathrm{mimu}}}_{\mathbf{v}{\mathbf{b}}_a}\hfill & =\Delta {\mathtt{R}}^{\mathsf{T}}{\partial }_{{{\mathbf{b}}_a}_k}\left({\widetilde{\Delta \mathbf{v}}}_{kk+1}\right)\hfill \end{array}$$

(51)

$$\begin{array}{cc}{{\mathsf{\Phi }}_{k+1,k}^{\mathrm{mimu}}}_{\mathbf{p}{\mathbf{b}}_a}\hfill & ={\mathtt{R}}_k{\partial }_{{{\mathbf{b}}_a}_k}\left({\widetilde{\Delta \mathbf{p}}}_{kk+1}\right)\hfill \end{array}$$

(52)

$$\begin{array}{cc}{{\mathsf{\Phi }}_{k+1,k}^{\mathrm{mimu}}}_{\mathbf{B}{\mathbf{b}}_a}\hfill & =\Delta {\mathtt{R}}^{\mathsf{T}}{\partial }_{{{\mathbf{b}}_a}_k}\left({\tilde{\mathit{M}}}_{\mathit{k}\mathit{k}+\mathbf{1}}\right)\hfill \\ {{\mathbf{G}}_{k+1,k}^{\mathrm{mimu}}}_{\mathbf{B}\nabla \mathbf{B}}\hfill & =\Delta {\mathtt{R}}^{\mathsf{T}}\left({\mathbf{v}}_k^{\mathsf{T}}\otimes {\mathbf{I}}_3\right){\partial }_{\nabla \mathbf{B}}\left(\mathrm{Vec}\left({\tilde{\mathit{D}}}_{\mathbf{v};\mathit{k}\mathit{k}+\mathbf{1}}\right)\right)\hfill \end{array}$$

(53)

$$\begin{array}{c}+\Delta {\mathtt{R}}^{\mathsf{T}}\left({\mathbf{g}}^{\mathsf{T}}{\mathtt{R}}_k\otimes {\mathbf{I}}_3\right){\partial }_{\nabla \mathbf{B}}\left(\mathrm{Vec}\left({\tilde{\mathit{D}}}_{\mathtt{R};\mathit{k}\mathit{k}+\mathbf{1}}\right)\right){\mathcal{P}}_{\nabla \mathbf{B}}+\Delta {\mathtt{R}}^{\mathsf{T}}{\partial }_{\nabla \mathbf{B}}\left({\tilde{\mathit{M}}}_{\mathit{k}\mathit{k}+\mathbf{1}}\right){\mathcal{P}}_{\nabla \mathbf{B}}\hfill \end{array}$$

(54)

where ${\mathcal{P}}_{\nabla \mathbf{B}}$ is defined as in (34). Note that we wrote here the transition and noise matrices as a function of the integrals (12)–(17), independently of the way the are computed, so that these expressions are still valid if one choose to compute the integrals with a more sophisticated scheme. Derivative of these integrals with respect to the biases are also required: these quantities should be computed simultaneously with the integrals. In our implementation, they are computed easily with the approximations made in (26)–(31).

4.2.2. State Augmentation

When MIMU sample $k+1$ arrives, the mimu propagation function is used to augment the state with the new current mimu state ${\widehat{\mathit{s}}}_{k+1}={\mathbf{f}}_{\mathrm{mimu}}({\widehat{\mathit{s}}}_k,{\tilde{\mathbf{u}}}_k,0)$, leading to the augmented state ${\widehat{\mathbf{X}}}_{k+1|k}^{\oplus }$. The error state square root information matrix is augmented accordingly to [14], ${\widehat{\mathcal{S}}}_{k+1|k}^{\oplus }$ using the Jacobian derived in previous subsection.

$$\begin{array}{c}{\widehat{\mathbf{X}}}_{k+1|k}^{\oplus }\stackrel{\mathsf{def}}{=}\left({\mathbf{X}}_{k|k},{\mathit{s}}_{k+1}\right)\hfill \end{array}$$

(55)

$$\begin{array}{c}{\widehat{\mathcal{S}}}_{k+1|k}^{\oplus }\stackrel{\mathsf{def}}{=}\left[\begin{array}{cc}{\widehat{\mathcal{S}}}_{k|k}& 0\\ {\mathbf{V}}_{\mathbf{1}}& {\mathbf{V}}_{\mathbf{2}}\end{array}\right]\hfill \end{array}$$

(56)

with

$$\begin{array}{cc}{\mathbf{V}}_{\mathbf{1}}\hfill & =[{\mathbf{0}}_{18\times 6},\cdots ,{\mathbf{0}}_{18\times 6},{\mathbf{Q}}_{k+1}^{-\frac{1}{2}}{{\mathsf{\Phi }}_{k+1,k}^{\mathrm{mimu}}}_{k+1,k}]\hfill \end{array}$$

(57)

$$\begin{array}{cc}{\mathbf{V}}_{\mathbf{2}}\hfill & ={\mathbf{Q}}_{k+1}^{-\frac{1}{2}}\hfill \end{array}$$

(58)

and the discrete model noise at ${\mathbf{Q}}_k$ time k :

$${\mathbf{Q}}_{k+1}=\left(\begin{array}{c}{\mathbf{G}}_{k+1,k}^{\mathrm{mimu}}{\mathbf{C}}_{k+1,k}^{\mathrm{mimu}}\end{array}\right)\left(\begin{array}{cc}{\mathsf{\Sigma }}_{\mathbf{u}}& 0\\ 0& \mathit{W}\end{array}\right)\left(\begin{array}{c}{{\mathbf{G}}_{k+1,k}^{\mathrm{mimu}}}^T\\ {{\mathbf{C}}_{k+1,k}^{\mathrm{mimu}}}^T\end{array}\right).$$

(59)

4.2.3. Marginalization of Old State

Then, in order to bound the size of ${\widehat{\mathbf{X}}}_{k+1|k}$, some part of it are marginalized. The state elements to be marginalized depend on the type of data available at the current timestamps as depicted in Figure 3. If only MIMU data (without an new keyframe image attached) are arriving at time k, ${\mathit{s}}_k$ is marginalized. If MIMU data and a new keyframe image are available at time k, the oldest keyframe pose is marginalized together with the non pose element of ${\mathit{s}}_k$. Note that since the image frame rate is well below MIMU frequency, the first case happens more often than the second case.

Within the square root information form, marginalization is done similarly to [14]. With ${\Pi }_k$ being the matrix permutation putting the to marginalize error states at the beginning, a QR decomposition of a square root information matrix of the permuted augmented error state vector writes

$$\begin{array}{cc}{\widehat{\mathcal{S}}}_{k+1|k}^{\oplus }{\Pi }_k\hfill & ={\mathbf{O}}_p{\mathit{R}}_p,\begin{array}{c}{\mathbf{O}}_p\in \mathcal{O}\left(n\right)\hfill \\ {\mathit{R}}_p\in {\mathbb{R}}^n\hfill \end{array}\hfill \end{array}$$

(60)

$$\begin{array}{c}={\mathbf{O}}_p\left[\begin{array}{cc}*& *\\ 0& {\widehat{\mathcal{S}}}_{k+1|k}\end{array}\right]\hfill \end{array}$$

(61)

We obtain the predicted state ${\widehat{\mathbf{X}}}_{k+1|k}$ removing marginalized states, and its—upper triangular—square-root information matrix ${\widehat{\mathcal{S}}}_{k+1|k}$. If no measurement update has to be performed at current time step they can be used directly as ${\mathbf{X}}_{k+1|k+1}$ and ${\widehat{\mathcal{S}}}_{k+1|k+1}$ for the next propagation.

Note: The marginalization of a joint Gaussian distribution with a square-root information form is not often demonstrated in book or lecture but can be deduced from the full information form ${\Lambda }_{\mathrm{joint}}$:

$$\begin{array}{cccc}\mathrm{if}{\Lambda }_{\mathrm{joint}}\hfill & =\left[\begin{array}{cc}{\Lambda }_M& {\Lambda }_{MR}\\ {\Lambda }_{MR}^{\mathsf{T}}& {\Lambda }_R\end{array}\right]={\widehat{\mathcal{S}}}_{\mathrm{joint}}^{\mathsf{T}}{\widehat{\mathcal{S}}}_{\mathrm{joint}}=\left[\begin{array}{cc}{\widehat{\mathcal{S}}}_M^{\mathsf{T}}{\widehat{\mathcal{S}}}_M& {\widehat{\mathcal{S}}}_M^{\mathsf{T}}{\widehat{\mathcal{S}}}_{MR}\\ {\widehat{\mathcal{S}}}_{MR}^{\mathsf{T}}{\widehat{\mathcal{S}}}_M& {\widehat{\mathcal{S}}}_R^{\mathsf{T}}{\widehat{\mathcal{S}}}_R+{\widehat{\mathcal{S}}}_{MR}^{\mathsf{T}}{\widehat{\mathcal{S}}}_{MR}\end{array}\right]\hfill & \mathrm{with}{\widehat{\mathcal{S}}}_{\mathrm{joint}}\hfill & =\left[\begin{array}{cc}{\widehat{\mathcal{S}}}_M& {\widehat{\mathcal{S}}}_{MR}\\ 0& {\widehat{\mathcal{S}}}_R\end{array}\right]\hfill \end{array}$$

(62)

then the square root information matrix resulting from marginalization of the the M variables is: ${\widehat{\mathcal{S}}}_R^{\mathrm{marg}}={\widehat{\mathcal{S}}}_{R.}$ This is deduced by calculus from the usually demonstrated fact that: ${\Lambda }_R^{\mathrm{marg}}={\Lambda }_R-{\Lambda }_{MR}{\Lambda }_M^{-1}{\Lambda }_{RM.}^{\mathsf{T}}$

4.3. Measurement Update

The filter processes two kinds of measurement: (i) the magnetic one that compares the magnetic field measured at the current timestamp with the magnetic field predicted by the filter; (ii) the visual measurement equation for features for which tracking has just ended.

We first briefly recall the update process of the inverse square root filter on a manifold. Let us suppose that some measurement occurs that can be modeled as:

$$\begin{array}{c}\mathbf{h}\left({\mathbf{X}}_{k+1}\right)={\tilde{\mathbf{h}}}_{k+1}+{\mathit{\eta }}_{\mathbf{h}}\in {\mathbb{R}}^{n_m},{\mathit{\eta }}_{\mathbf{h}}\sim \mathcal{N}(0,{\mathsf{\Sigma }}_{\mathbf{h}})\hfill \end{array}$$

(63)

with $n_m$ the dimension of the measurement vector. Writing the dimension of the predicted state as $n_s$, the measurement error ${\mathbf{z}}_{k+1}=\mathbf{h}\left({\widehat{\mathbf{X}}}_{k+1|k}\right)-{\tilde{\mathbf{h}}}_{k+1}$ and $\mathit{H}$ the jacobian of the application:

$$\begin{array}{c}\begin{array}{ccc}{\mathbb{R}}^{n_s}\hfill & \to & {\mathbb{R}}^{n_m}\hfill \\ \delta \mathit{X}\hfill & \mapsto & \mathbf{h}({\widehat{\mathbf{X}}}_{k+1|k}⊞\delta \mathit{X})\hfill \end{array},\hfill \end{array}$$

(64)

the update step finds the tangent correction $\delta {\mathbf{X}}^{*}$ that minimizes the following linearized cost:

$$\begin{array}{cc}\mathcal{C}\left(\delta \mathbf{X}\right)\hfill & ={\parallel \delta \mathbf{X}\parallel }_{{\mathbf{P}}_{\mathit{k}+\mathbf{1}|\mathit{k}}}^2+{\parallel \mathit{H}\delta \mathbf{X}-{\mathbf{z}}_{k+1}\parallel }_{{\mathsf{\Sigma }}_h}^2\hfill \\ & ={∥{\widehat{\mathcal{S}}}_{k+1|k}.\delta \mathit{X}∥}^2+{∥{\mathsf{\Sigma }}_{\mathbf{h}}^{-\frac{1}{2}}\left(\mathit{H}\delta \mathbf{X}-{\mathbf{z}}_{k+1}\right)∥}^2\hfill \\ & ={∥\left[\begin{array}{c}{\widehat{\mathcal{S}}}_{k+1|k}\\ {\mathsf{\Sigma }}_{\mathbf{h}}^{-\frac{1}{2}}\mathit{H}\end{array}\right]\delta \mathit{X}-\left[\begin{array}{c}0\\ {\mathsf{\Sigma }}_{\mathbf{h}}^{-\frac{1}{2}}{\mathbf{z}}_{k+1}\end{array}\right]∥}^2.\hfill \end{array}$$

(65)

The optimum point can be obtained by of a thin QR decomposition:

$$\left[\begin{array}{c}{\widehat{\mathcal{S}}}_{k+1|k}\\ {\mathsf{\Sigma }}_h^{-\frac{1}{2}}\mathit{H}\end{array}\right]={\mathbf{O}}_u{\mathit{R}}_{\mathbf{u}},\begin{array}{c}{\mathbf{O}}_u\in \mathcal{O}(n_s+n_m)\hfill \\ {\mathit{R}}_{\mathbf{u}}\in {\mathbb{R}}^{n_s+n_m}\hfill \end{array}$$

(66)

$$\mathcal{C}\left(\delta \mathit{X}\right)={∥{\mathit{R}}_{\mathbf{u}}\delta \mathit{X}-{\mathbf{O}}_u^{\mathsf{T}}\left[\begin{array}{c}0\\ {\mathsf{\Sigma }}_{\mathbf{h}}^{-\frac{1}{2}}{\mathbf{z}}_{k+1}\end{array}\right]∥}^2,$$

(67)

${\mathit{R}}_{\mathit{u}}$ being upper triangular, $\delta {\mathbf{X}}^{*}$ is efficiently computed by back-substitution. This optimal correction is finally applied to the predicted state with the retraction operator :

$$\begin{array}{cc}{\mathbf{X}}_{k+1|k+1}\hfill & ={\widehat{\mathbf{X}}}_{k+1|k}⊞\delta {\mathbf{X}}^{*}\hfill \end{array}$$

(68)

$$\begin{array}{cc}{\widehat{\mathcal{S}}}_{k+1|k}\hfill & ={\mathit{R}}_{\mathbf{u}}{\mathtt{J}}_r^{-1}.\hfill \end{array}$$

(69)

With ${\mathtt{J}}_r$ being the Jacobian at e = 0 of:

$$\begin{array}{cc}{\mathbb{R}}^{n_s}\hfill & \to {\mathbb{R}}^{n_s}\hfill \\ \hfill e& \mapsto ({\widehat{\mathbf{X}}}_{k+1|k}⊞(\delta {\mathbf{X}}^{*}+e))\boxminus ({\widehat{\mathbf{X}}}_{k+1|k}⊞\delta {\mathbf{X}}^{*}).\hfill \end{array}$$

(70)

Intuitively, this Jacobian transforms the square-root information matrix from the tangent space of predicted state to the tangent space of the updated state. Note that, in the current parametrization choice (cf. (38)), this Jacobian is the identity matrix; this was not the case in our previous work [13] where the rotation error was expressed in body frame.

4.3.1. Magnetic Measurement Update

The magnetic measurement update is the simplest and uses at MIMU frequency the direct magnetic field measurement which writes:

$$\begin{array}{cc}{h}_{\mathbf{B}}\left({\mathbf{X}}_k\right)\hfill & ={\mathit{P}}_{{\mathbf{B}}_b}{\mathbf{X}}_k\hfill \end{array}$$

(71)

$$\begin{array}{cc}{\mathsf{\Sigma }}_{h_{\mathbf{B}}}\hfill & ={\sigma }_{\mathbf{B}}^2{\mathbf{I}}_3\hfill \end{array}$$

(72)

with ${\mathit{P}}_{{\mathbf{B}}_b}$ the projection operator from the state space to the coordinates of the state corresponding to ${\mathbf{B}}_b$. ${\sigma }_{\mathbf{B}}$ is the noise of the magnetometers reading.

4.3.2. Opportunistic Feature Tracks Measurement Update

We use the feature tracks in the same way as proposed by [20]. We derive here the equation for completeness.

When a feature track ends or when the frame in which the feature was detected is about to be marginalized, we process the entire feature track as a measurement constraining the pose of each in-state frame where the feature was detected.

The predicted reprojection of feature i in frame at time t is written:

$$\begin{array}{c}{\mathbf{pr}}_t^i({\xi }_t,{\mathbf{l}}_i^w)=\pi \left({\mathtt{R}}_{\mathrm{c}}^{\mathrm{b}}\mathsf{T}\left({{\mathtt{R}}_t^w}^{\mathsf{T}}\left({\mathbf{l}}_i^w-{\mathbf{p}}_t^w\right)-{\mathbf{p}}_{\mathrm{c}}^{\mathrm{b}}\right)\right)\in {\mathbb{R}}^2\hfill \end{array}$$

(73)

with ${\mathbf{l}}_i^w\in {\mathbb{R}}^3$ the 3D-position of the features i in inertial coordinates and $\pi :{\mathbb{R}}^3\to {\mathbb{R}}^2$ the projection function of the camera. We recall that $({\mathtt{R}}_{\mathrm{c}}^{\mathrm{b}},{\mathbf{p}}_{\mathrm{c}}^{\mathrm{b}})$ is the known transform between the body frame and the camera frame. ${\mathbf{l}}_i^w$ is computed by a fast triangulation function from known in-state poses and the measured observation, noted ${\mathit{o}}_{it}$.

By stacking all these reprojections, we can write the non linear measurement function:

$$\begin{array}{cc}{\mathbf{h}}_{fi}(\mathbf{X},{\mathbf{l}}_i^w)\hfill & =\left[\begin{array}{c}·\\ {\mathbf{pr}}_t^i({\xi }_t,{\mathbf{l}}_i^w)\\ ·\end{array}\right]=\left[\begin{array}{c}·\\ {\mathit{o}}_{it}\\ ·\end{array}\right]+{\mathit{\eta }}_{fi},\hfill \end{array}$$

(74)

with ${\mathit{\eta }}_{fi}$ is assumed to be an additive Gaussian white noise : ${\mathit{\eta }}_{fi}\sim \mathcal{N}(0,{\mathsf{\Sigma }}_C)$, ${\mathsf{\Sigma }}_C={\sigma }_c{I}_{2m}$. We note subsequently ${\mathit{o}}_i$ the vector resulting from stacking of 2-vector observation ${\mathit{o}}_{it}$.

The attentive reader may note that this measurement equation is not of the form of (63), because ${\mathbf{l}}_i^w$ is not part of our state: for this reason we can not directly use (65) and (67). Worse, the computed ${\mathbf{l}}^w$ estimate is correlated with the current state error, so that we can not just use it as a fixed constant. The solution used here aims at expressing a projection of the residual that depends only on the poses, up to the first order.

We start by linearizing the residual then proceeds the minimization in two steps to extract the used measurement equation. Linearization of ${\mathbf{r}}_i:\mathbf{X},{\mathbf{l}}_i^w\mapsto \left({\mathbf{h}}_{fi}(\mathbf{X},{\mathbf{l}}_i^w)-{\mathit{o}}_{\mathbf{i}}\right)$ yields:

$$\begin{array}{c}{\mathbf{r}}_i({\mathbf{X}}_{k+1|k}⊞\delta \mathit{X},{\mathbf{l}}_i^w+\delta {\mathbf{l}}_i^w)\hfill \\ \hspace{1em}\simeq {\mathit{F}}_i\delta \mathit{X}+{\mathit{E}}_f\delta {\mathbf{l}}_i^w+\left({\mathbf{h}}_{fi}({\mathbf{X}}_{k+1|k},{\mathbf{l}}_i^w)-{\mathit{o}}_i\right)\hfill \\ \hspace{1em}={\mathit{F}}_{\mathit{k}}\delta \mathit{X}+{\mathit{E}}_{\mathbf{f}}\delta {\mathbf{l}}_i^w-\delta {\mathit{o}}_i\hfill \end{array}$$

(75)

$\delta {\mathit{o}}_i={\mathbf{h}}_{fi}({\mathbf{X}}_{k+1|k},{\mathbf{l}}_i^w)-{\mathit{o}}_i$ is the $2m$-vector of predicted residual error. ${\mathit{E}}_{\mathbf{f}}$ is a $2m\times 3$ matrix of rank 3 ; m denoting the number of observations for the feature. The rank of ${\mathit{E}}_{\mathbf{f}}$ is guaranteed during the triangulation. Its QR decomposition writes:

$$\begin{array}{cc}{\mathit{E}}_{\mathbf{f}}\hfill & =\left[{\mathbf{O}}_{\mathit{E}1},{\mathbf{O}}_{\mathit{E}0}\right]\left[\begin{array}{c}{\mathit{R}}_{\mathit{E}1}\\ {\mathbf{0}}_{2m-3\times 2m}\end{array}\right]\hfill \end{array}$$

(76)

$$\begin{array}{cc}{\mathbf{O}}_{\mathit{E}1}\in {\mathbb{R}}^{2m\times 3},\hfill & {\mathbf{O}}_{\mathit{E}0}\in {\mathbb{R}}^{2m\times 2m-3},{\mathit{R}}_{\mathit{E}1}\in G{L}_3\left(\mathbb{R}\right);\hfill \end{array}$$

(77)

Properties of square orthogonal matrices on the $L_2$ norm of vectors allow to split the cost function into two terms, one depending only of the current predicted state vector.

$$\begin{array}{c}\underset{\delta \mathit{X},\delta {\mathbf{l}}_i^w}{\min }{\parallel {\mathbf{r}}_i{\parallel }^2}_{{\mathsf{\Sigma }}_C}=\underset{\delta \mathit{X},\delta {\mathbf{l}}_i^w}{\min }{{∥\left[\begin{array}{c}{\mathbf{O}}_{\mathit{E}1}^{\mathsf{T}}\\ {\mathbf{O}}_{\mathit{E}0}^{\mathsf{T}}\end{array}\right]{\mathbf{r}}_{\mathbf{i}}∥}^2}_{{\mathsf{\Sigma }}_C}\\ =\underset{\delta \mathit{X},\delta {\mathbf{l}}_i^w}{\min }{∥{\mathbf{O}}_{\mathit{E}1}^{\mathsf{T}}{\mathit{F}}_i\delta \mathit{X}+{\mathit{R}}_{\mathit{E}1}\delta {\mathbf{l}}_i^w-{\mathbf{O}}_{\mathit{E}1}^{\mathsf{T}}\delta {\mathit{o}}_i∥}_{{\mathsf{\Sigma }}_C}^2\\ +\underset{\delta \mathit{X}}{\min }{∥{\mathbf{Q}}_{\mathit{E}0}^{\mathsf{T}}{\mathit{F}}_i\delta \mathit{X}-{\mathbf{O}}_{\mathit{E}0}^{\mathsf{T}}\delta {\mathit{o}}_i∥}_{{\mathsf{\Sigma }}_C}^2\end{array}$$

(78)

As ${\mathit{R}}_{\mathit{E}1}$ is invertible, the first term of the quantity to be minimized can be reduced to zero for all $\delta \mathit{X}$. Minimization reduces thus to:

$$\begin{array}{c}\underset{\delta \mathit{X}}{\min }{∥{\mathbf{O}}_{\mathit{E}0}^{\mathsf{T}}{\mathit{F}}_i\delta \mathit{X}-{\mathbf{O}}_{\mathit{E}0}^{\mathsf{T}}\delta {\mathit{o}}_i∥}_{{\mathsf{\Sigma }}_C}^2\hfill \end{array}$$

(79)

Finally, the previous linearized residual is used in the cost function (65), i.e., we use for the $\mathit{H}$ matrix, the $\mathbf{z}$ error vector and the covariance of measurement ${\mathsf{\Sigma }}_{\mathbf{h}}$ the quantities:

$$\begin{array}{cccccc}{\mathit{H}}_f\hfill & ={\mathbf{O}}_{\mathit{E}0}^{\mathsf{T}}{\mathit{F}}_i,\hfill & {\mathbf{z}}_f\hfill & ={\mathbf{O}}_{\mathit{E}0}^{\mathsf{T}}\delta {\mathit{o}}_i,\hfill & {\mathsf{\Sigma }}_{{\mathbf{h}}_f}\hfill & ={\sigma }_C{I}_{2m-3}.\hfill \end{array}$$

(80)

Note that everything happens here as if we introduced the features position into the state, but instantly marginalized it.

4.4. Filter Initialization

One sensitive issue in a VINS filter is initialization. It usually involves some specific algorithm as described in [21,22] for instance. In our implementation, we proceed as follows: after switch on, the current state is initialized at origin with an attitude matching the current acceleration direction and a zero speed, both with high variance. The filter is then ran using the high frequency magnetic update equation in order to get rapidly a stabilized trajectory from the first few seconds. This trajectory is used to bootstrap the first keyframe poses state and then to start using features information. This initialization process relies on the empirical observation that the MI-DR filter convergence basin is large. Admittedly, it would degrade severely the filter if the system’s switch on occurs in an area where magneto-inertial dead-reckoning is not reliable.

5. Experimental Study

5.1. Hardware Prototype Description and Data Syncing

The sensor system is pictured on Figure 4. The camera is rigidly attached 47 cm away of the MIMU system to avoid any potential magnetic perturbation. Such a large distance is necessary because the off-the-shelf camera has not been specifically designed for reducing its magnetic footprint. Reducing the hardware to a wearable size would involve sensors co-design that was not in the scope of this work. For the vision part, we use an IDS uEye 3241-LE equipped with a Lensagon BM4018S118 lens. It provides around 100 degrees of field of view. Camera intrinsics and extrinsic parameters are calibrated with the Kalibr toolbox [23]. The MIMU provides data at 325 Hz, the camera at 20 Hz. Magnetometers, accelerometers and gyro-meters are all MEMS sensors digitized with sigma-delta ADCs which were carefully calibrated.

The camera and MIMU provide timestamps computed from different clocks. We synchronize them offline: timestamps shifts are estimated both at start and at the end of the records by the checkerboard-based Kalibr calibration toolbox; clock drift is then deduced and corrected for.

The camera exposure time is fixed at 10 milliseconds maximum, reducing worst case motion blur and allowing to timestamp accordingly each image observation. However, this choice limits the camera’s ability to adapt to a very low-light environment.

5.2. Filter Parameters Tuning

Most parameters of the filter are chosen in a deterministic and consistent fashion. MIMU noise standard deviation ${\sigma }_{\mathbf{a}},{\sigma }_{\omega },{\sigma }_{\mathbf{B}}$ and biases evolution parameters ${\tau }_{bg},{\tau }_{ba},{\sigma }_{bg;c},{\sigma }_{ba;c}$ are derived from sensors characteristics measured empirically with an Allan standard deviation. The pixel reprojection noise ${\sigma }_C$ is set to $\sqrt{2}$, which is the diagonal size of a pixel. Only the magnetic part of covariance ${\mathsf{\Sigma }}_{\nabla \mathbf{B}}$ is tuned empirically. It is set greater than what would derive from the covariance of magnetometers white noise, so as to absorb some modeling errors of the magnetic field, such as small non-stationarities or high values of the 2nd order spatial term. Note that parameter tuning is unchanged for all presented datasets in order to draw fair conclusions.

5.3. Visual Processing Implementation

The visual processing pipeline aims at constantly track 200 interest points well spread in the image. In order to enforce a good repartition of corners across the entire image, we use a bucket strategy. Harris-corner response is computed over the entire image and we retain only the strongest features in buckets for which the number of already tracked points is below a threshold. We use here a partition of the image in $6\times 8$ buckets. Detected corners are tracked from frame to frame with OpenCV pyramidal KLT algorithm until either:

• they go out of the field of view;
• the tracking fails;
• they are classified as outliers;
• the frame where they were firstly detected is to be marginalized at next propagation step.

We ran a 2-point RANSAC algorithm between subsequent frames for outliers detection and rejection, using the relative orientation from the integrated gyroscope as rotation between the two frames.

An ended feature track is used as measurement, only if:

• it spans at least three poses;
• its initial triangulation did not exhibit any degeneracy;
• its re-projection error is below a threshold.

Contrarily to our previous implementation [13], this threshold is dynamically set with a ${\chi }^2$ threshold. As a result, the criterion becomes looser when the estimated uncertainty of relative poses increases.

In order to make the visual pipeline more robust to dark areas, we normalize each input image by its averaged intensity before corner detection and tracking. Some very dark images then become usable for corner detection despite a significant increase of the photometric noise which affect them. Noise amplification leads to spurious features track, but these are most often correctly rejected by our outlier rejection strategy. Overall, we found that normalization significantly improves the performance of pure MSCKF VINS algorithm on our dataset. Some raw/normalized images are presented in Figure 5.

Note that, in contrast with the tuning of the filter, the parameters of the vision pipeline (KLT window size, number of octaves in the pyramid of KLT, RANSAC threshold and minimum Harris score for corner detection) were chosen empirically.

5.4. Trajectory Evaluation

5.4.1. Dataset Presentation

We evaluate our algorithm on a dataset of five test trajectories. A pedestrian is carrying the system depicted in Figure 4 and walks through an industrial facility. Trajectories are specifically designed to be challenging for MI-DR and VINS: they are partly done outdoor, with very low magnetic field gradient and they also contains portions visually non informative made in the non lit basement of the building. Detailed results on Traj2 and Traj5 are presented on Figure 6, Figure 7 and Figure 8, the others being more briefly depicted in Figure 9. In all cases, a dedicated plot indicates with a color code the parts of the trajectory where the magnetic field gradient vanishes and parts of the trajectories where the mean intensity is low.

5.4.2. Overall Comparison

Three estimators derived from the presented filter have been compared. The MI-DR is the presented filter without any visual update. MSCKF is the presented filter without any magnetic update. MI-MSCKF is the proposed filter fusing both information. Moreover, we also compare our results with the state of the art VINS filter of [24].

Unfortunately, we do not have access to a ground truth trajectory for our datasets. We then consider three evaluation criteria:

• the estimated trajectories are superimposed to a georeferenced orthoimage in which one pixel represents precisely 0.5 m. We compute an alignment of the trajectory when the checkerboard detected for the first time in the sequence. This alignement results from setting manually the position and heading of the checkerboard frame relative to the coordinates system of the satellite image. Note that no manual scale alignment has been made, hence this visualization allows to evaluate roughly the correctness of the global scale of the estimate, for instance on Figure 6a;
• the z profile is globally known as the pedestrian walks along flat corridors—except when he takes stairs to change levels;
• a translational error is computed each time the system comes back to its initial position, thanks to a static checkerboard placed at starting point. This criterion can be visualized in Figure 6c for Traj2 where it is clear the MI-DR estimate is less stable vertically over the entire trajectory.

The last criterion can be quantitavely evaluated: results are displayed in

Table 1. Summary of final drift error on full dataset (% of trajectory length).

	Traj1	Traj2	Traj3	Traj4	Traj5
MI-DR	1.11	1.98	1.81	1.54	2.87
MSCKF (VINS)	0.33	0.63	0.59	1.05	0.21
MI-MSCKF	0.20	0.31	0.49	0.71	0.15
State of the art VINS [24]	0.26	0.52	0.79	0.62	0.20

with an error given in percentage of the trajectory length. Next sections emphasize some differences in the behaviors of the tested methods.

5.4.3. The Fused Estimate Improves MI-DR in Outdoor Trajectories

The Figure 6 shows the three versions of our filter on Traj2. The trajectory estimated by MI-DR is very close to the others until some point in the outdoor part—note that the outdoor part corresponds to the weak gradient part of the trajectory as depicted on Figure 6b. During this outdoor part, as expected, the MI-DR drifts away compared to the two vision-based estimates which directly leads to a higher final translation error. The same effect is also clearly visible on Traj3, see Figure 9b.

5.4.4. Data Fusion Improves Local Consistency

By reducing the drift in dark areas or low gradient areas, the fused estimates improve the local position estimate consistency, an effect which is not always visible on the metrics of Table 1.

The benefit of magnetometry information in this sense is demonstrated in the details of results on the Traj2 displayed in Figure 7. The two left plots show a similar situation: the VINS estimate (red) is for a few seconds strongly corrected by the filter, leading to non continuous estimates (see green circles). The MI-MSCKF filter stays smoother during the entire trajectories, thanks to the speed observability provided by Equation (7). Interestingly, the pure VINS estimate joins the MI-MSCKF estimate later in the sequence, which makes the temporary drift mostly invisible in the final loop error metric of Table 1. This correction happens when visual information becomes available again. It means that the VINS filter is still able to correct itself after reasonable drift through the information stored in the prior.

The same effect occurs on all trajectories. Consider for instance the trajectory Traj5 depicted on Figure 8, where the lowest part goes also through the dark basement of the building. The MSCKF drifts vertically before being corrected when the pedestrian takes the stair up again. This effect is depicted on details in Figure 8c, which clarifies the evolution of the estimated height with respect to time.

In turn, visual information also helps trajectory consistency. It is clearly visible on the strong vertical drift of MI-DR displayd on Figure 6c around 250 s. Again, this drift is corrected as soon as magnetic gradients becomes sufficiently high to make speed observable again. Note the horizontal drift was never corrected though, leading to the larger final drift shown in Table 1.

The reader may have noticed at that point that Figure 8c shows that MI-DR fails badly on Traj5. Yet, we would like to stress that the fully fused estimate is still able to outperform the VINS estimate, leveraging magnetic information correctly beyond the breaking point of MI-DR. The reason of the failure of MI-DR is an unstationary local magnetic field in the first few seconds of Traj5. It perturbes the MI-DR initialization, which has dramatic consequence on all the rest of the trajectory.

The fact that the fused estimate prevents local drift could be, in our opinion, highly beneficial to the long-term performance in a Extended Kalman Filter strategy. Indeed, it could reduce overall linearization errors and the maximum magnitude of corrections, which are recognized, in VINS community, as an important drawback of filtering approaches compared to bundle adjustment or optimization-based methods.

5.4.5. Comparison with a State of the Art Filter

We also ran the released binary version (Available on https://github.com/MARSLab-UMN/MARS-VINS, we used the commit 8531daf.) of [24] on our dataset. Indeed, we think it is the state of the art in VINS filters for pedestrian navigation. As [24] takes only stereo images (even if their method works well with monocular setup also, as said in the paper) we had to trick slightly their software to use a monocular input. Note that, to be as fair as possible, we have entered in their code the same normalized monocular images we use. In doing so, we observed that normalization has also drastically improved the performance of their filter on our data.

An in-depth and comprehensive comparison between the two implementation is difficult as the code of [24] is not open. If inertial handling in both implementation should be close, the visual pipeline is very sensitive to parameters value and implementation details that are not known by us. Nevertheless, Table 1 demonstrates that both filters compare reasonably and are clearly below 1% of trajectory length error.

We also observed in [24] implementation the presence of strong filters correction after dark areas, as in described in Section 5.4.4. It indicates that this local consistency problem, which is essentially solved by the proposed fused estimate, is indeed a general issue of all VINS filters.

6. Conclusions

This work presented a filter to fuse information from a magnetometer array with other sensors traditionally used in VINS. We described in detail, the method we used and discussed its results on real datasets. Comparing the results of three estimators—one using only magnetic-inertial information, one using only visual-inertial information and one fusing both informations—we showed that the fused estimate leads to a more robust trajectory estimate. First, our fused estimate is able to reconstruct the trajectory outdoor where MI-DR techniques breaks because of the lack of gradient; secondly, our system avoids the unrealistic trajectory correction of VINS after significant duration without a good illumination. One trajectory also highlighted the need for either robust estimation technique or outlier rejection scheme for magnetic information. This should deserve more work in the future.

We think that the proposed approach could help to improve localization systems for augmented reality (AR) that are currently using VINS with consumer grade cameras and IMUs. Not only this combination extends the applicability domain of traditional VINS to degraded environments, but we also foresee opportunities to reduce power consumption. Indeed, taking advantages of the good trajectories given by the MI-DR in various conditions, it might be possible to reduce the computational load of the visual pipeline, which could be of major interest in practical applications.