VIMO: A Visual-Inertial-Magnetic Navigation System Based on Non-Linear Optimization

Abstract

Visual-inertial navigation systems are credited with superiority over both pure visual approaches and filtering ones. In spite of the high precision many state-of-the-art schemes have attained, yaw remains unobservable in those systems all the same. More accurate yaw estimation not only means more accurate attitude calculation but also leads to better position estimation. This paper presents a novel scheme that combines visual and inertial measurements as well as magnetic information for suppressing deviation in yaw. A novel method for initializing visual-inertial-magnetic odometers, which recovers the directions of magnetic north and gravity, the visual scalar factor, inertial measurement unit (IMU) biases etc., has been conceived, implemented, and validated. Based on non-linear optimization, a magnetometer cost function is incorporated into the overall optimization objective function as a yawing constraint among others. We have done extensive research and collected several datasets recorded in large-scale outdoor environments to certify the proposed system’s viability, robustness, and performance. Cogent experiments and quantitative comparisons corroborate the merits of the proposed scheme and the desired effect of the involvement of magnetic information on the overall performance.

1. Introduction

As befits one of the most crucial problems in robotics, SLAM has received much attention for the past two decades and opened up many new vistas for autonomous robots, alongside numerous proposed approaches and schemes for implementing it, which fall into three categories by sensing modality: Laser SLAM, visual SLAM, and visual-inertial SLAM. Laser SLAM preponderated in the early stages of the development of SLAM by virtue of its high precision, long range, and capacity for obstacle avoidance. Notwithstanding its advantages, its scale in size and weight means its application is confined to platforms allowing a limited load and a capacity enough for carrying a laser scanner, which therefore circumscribes the application range and agility of the platform. Being cost-effective, lightweight, and efficient in energy consumption, cameras, or vision-based sensors in general, are in the ascendant, which is also attributed to its informative representation of geometry in a single image caught on camera. A desirable consequence of images being abundant in information is the capacity to retrieve previously registered scenes, known as scene recognition, so as to perform loop closure to curb drift in estimates.

For a vision-based SLAM system or those reliant mainly on visual perception, four components are deemed indispensable, including visual tracking, filtering/optimization, loop closure, and mapping, each of which holds systemic effects on the overall performance of a system and to which various solutions have been conceived, implemented, and experimented. Approaches to tracking visual cues can be categorized into such two classes as feature-based and direct methods according to how they implement data association. Feature-based methods rely on the descriptors of features extracted in images to associate image points appearing across several images, and points existing in more than one images can then be used to estimate poses with stability due to the invariance of descriptors. Apart from the relatively high computational cost of extracting features and descriptors, one of the penalties to feature-based methods is their lack of resistance to texture-less views. The descriptor, dependent on features being identifiable, does not contain the amount of information enough to perform data association. Direct methods, however, eschew this problem by direct exploitation of changes in illumination and by minimization of photometric errors rather than geometric ones, thereby being more impervious to scarcity of texture as well as driving down computational consumption since they don’t have to describe features. The concept behind every formulations of estimation is probabilistic modelling with noisy measurements as input to estimate certain parameters. Typical models are based on maximum a posterior (MAP) in which the final estimates maximize the a posteriori probability given existent measurements. For pure visual SLAM, the probabilistic model is usually based on Maximum Likelihood (ML) in the absence of prediction models. Earlier SLAM schemes estimate states mainly in a filtering way in which the prior parameters are always marginalized out to focus merely on current states for containment of computation, i.e., all the previous states are fixed, which is bound to affect precision. Recently nonlinear optimization-based methodology has begun to gain ground among scholars alongside increases in the performance of hardware and decreases in its cost. Due to the sparsity inherent in the Hessian matrix of BA (Bundle Adjustment), the computational complexity of estimation in SLAM is lighter than would be conceived. By benefiting from the sparsity and properly assigning tasks among several parallels and with existent optimization modules including Ceres, g2o, iSAM, and gtsam which have been leveraged across many platforms of navigation, optimization-based systems can run smoothly within reasonable time but still need to marginalize out older states to restrict the number of keyframes. The factor graph based on fast incremental matrix factorization, on the other hand, allows for a more accurate and efficient solution by virtue of recalculating only the matrix entries in need of change, whereby the system can considerably slash computation while holding the whole trajectory.

The inertial measurement unit (IMU), light in weight, size, and price, and often in the form of a MEMS (Micro-Electro-Mechanical System), has been gaining in popularity as a source of inertial information complementary to vision since it recovers poses from motion itself and permits rendering observable the metric scale of monocular vision and the direction of gravity whereas visual constraints, in reverse, given accurate data association, can check error accumulation in the integration of IMU measurements (angular velocity, acceleration). The fusion of visual and inertial information began with loosely-coupled mechanisms and has now transitioned into tightly-coupled ones. The procedure of propagating and predicting states with inertial measurements between two consecutive frames and later using a visual image (or several) as an observation to update estimates is characteristic of loosely-coupled mechanisms which are straightforward but fall short of gratifying decision because they fail to take into account the correlation between the two types of data, as opposed to which, tightly-coupled ones fare better by incorporating variables specific to vision (the coordinates of landmarks) and those to inertia (gyroscope and accelerometer biases) into the whole set of optimization states, which in essence utilizes the complementary attribute to a greater extent. To ameliorate the additional computational expense incurred by the introduction of inertial constraints into the optimization graph, in which case the Hessian matrix is no longer as sparse as that of purely visual models, pre-integration on manifold may be adopted to reduce calculation for optimization and bias correction.

Merging inertial and visual information leads to the angles of pitch and roll being observable with the last dimension of attitude, namely yaw, still unobservable and bound to drift over time. It is conceivable that high accuracy in yawing estimation can bolster the overall precision as every pose is estimated partly based on previous estimates and is therefore affected by their yaw angles. The magnetometer has long been employed in the field of navigation, often in combination with other types of apparatus such as an IMU. The magnetometer, as its name reveals, is a type of sensors that measure magnetic density in a magnetic field, especially the Earth’s magnetic field (EMF), one of the Geophysical Fields of the Earth (GFE) including the Earth’s Gravitational Field (EGF). It has been used in a wide range of commercial and military applications, mostly for directional information [1], whereas another way of using it for motion estimation is through measuring magnetic field gradients so as to acquire velocity information as in [2] which formulated a sensing suite consisting of a vision sensor and a MIMU (Magneto-Inertial Measurement Unit) that, on the presupposition of stationary and non-uniform magnetic field surroundings (particularly indoor environments), can render the body speed observable. The said two manners of employing the magnetometer imply that for this sensor there is a dichotomy in how to process its readings between outdoor and indoor scenarios, and it will not be straightforward to reconcile them.

However, little research has been done about integrating visual-inertial frameworks with magnetic observation that could ably suppress the cumulative azimuth error and further facilitate navigation. It seems that the magnetometer is not so much appealing for scholars in the area of SLAM as it ought to be. What keeps magnetometers from being adopted might be its liability to magnetic disturbance. Ubiquitous sources magnetic interference from ferrous metals and even from the platform itself would entail the system both growing in size and demanding more intricate handling. Since applications of SLAM are always towards compactness and generality, the magnetometer has remained out of favour.

We gather that despite the magnetometer’s weaknesses, outdoor environments might still be in accord with its characteristics with appropriate measures taken, and therein lies the main motivation of the paper.

In this paper, we present a visual-inertial-magnetic system of navigation based on graph optimization that, apart from visual and inertial measurements, utilizes the geomagnetic field to restrain drift in yaw. From devising the scheme to verifying its superiority, the following tasks have been followed through along the way:

• Exhaustive mathematic deductions have been made to support the proposed system theoretically and mathematically, embodying observation models, least squares problems concerned with initialization, the novel optimization framework that fuses visual-inertial-magnetic information, and a novel way of loop-closure formulation aimed at enhancing the robustness to false loops.
• A complete and reliable procedure of initialization for visual-inertial-magnetic navigation systems is presented.
• An effective and efficient optimization using visual-inertial-magnetic measurements as observation is established.
• A suite of sensors and a CPU with other hardware, capable of data acquisition and real-time operation, has been assembled.
• The system has been tried and tested on several datasets collected in large-scale outdoor environments. Analysis and comparison of the experiment results attest to the feasibility, efficacy, and excellence of the proposed system.

2. Related Work

Since [3] came out, large numbers of well-designed systems [4,5,6,7,8] have swollen the ranks of SLAM schemes. Copious studies have been done on visual-inertial SLAM along with various applications that have since been on a continuum to full maturity [9,10,11]. Until today, scholars are still endeavoring to bring it to perfection with novel ideas and approaches [12,13,14,15,16]. Incipient methods fuse inertial and visual measurements loosely where the two types of information are processed, filtered, and used for estimation all separately [17]. In overcoming the notorious inconsistency of loosely-coupled approaches, researchers have come to appreciate that the advantages of fusing tightly, such as improved consistency [18,19]. Whether sources of information are amalgamated loosely or tightly, filtering mechanisms hold more likelihood to arrive at a suboptimal solution in the wake of linearizing all previous states. In break with traditional filter-based methods, non-linear optimization employed in SLAM has been deemed more desirable with its high precision and tolerable computational requirements. Reference [20] copes with holding on to and optimizing an entire trajectory by what the authors call ‘full smoothing’, but the ever increasing complexity in line with the incremental map and trajectory largely limits its applicability. Reference [21] is generally recognized as one of most early mature systems using non-linear optimization based on sliding window and marginalization for containment of problem size. To make a virtue of escalated computation due to the addition of inertial measurements, reference [22] proposes what is called the IMU preintegration technique that integrates on manifold a segment of measurements between two time points with the Jacobian matrices maintained for correction. In that way, changes in linearization will not need complete re-integration but a few minor adjustments with respect to the Jacobian matrices as long as they are not too large.

Another property of visual-inertial SLAM that comes to scholar’s attention is its initialization and what its does is to work out several parameters describing the system’s initial states and sensors’ relation. Sfm calls for enough motion to be reliable while estimation of gravity’s direction is better off under stationary condition. This contradiction suggests that visual-inertial initialization can not be trifled with. Reference [23] proposes a deterministic closed-form method that can recover gravity’s direction and scalar factor, but fails to take cognizance of IMU biases, making the system less stable than it would otherwise be. Reference [17] estimates not only IMU biases but velocity using an EFK but the convergence takes long. Reference [24] initializes system on the assumption that MAVs take off flat with as small inclination as possible, which is, of necessity, unlikely to be the case in practice. Reference [25], much like [24], relies on alignment with gravity at the beginning to initialize. In [26,27], initialization does not calculate gyroscope biases, derogating from the precision.

As it develops, SLAM has begun to go beyond passively observing surroundings to actively exploring environments so as to gain coverage, hence the name ‘active SLAM’ [28]. So-called active SLAM integrates SLAM itself with path planing. Using this technique, a system covers an area autonomously while performing plain SLAM.

Reference [29] is aimed at solving active SLAM problems where coverage is required and certain constraints are imposed. With that end in view, reference [29] proposes a solution to it that focuses on minimization and area coverage within an MPC (Model Predictive Control) framework. It uses a sub-map joining method to improve both effectiveness and efficiency. The D-opt MPC problem is resolved with recourse to a graph topology and convex optimization and the SQP method is employed to address the coverage problem.The main contribution of [29] is it presents a new method capable of generating a sound collision-free trajectory so as to better perform coverage tasks than many other systems do.

Reference [30] presents an effective indoor navigation system for the Fetch robot. The main idea is founded on sub-mapping and DeepLCD and the system is implemented using Cartographer and AMCL (adaptive Monte Carlo localization). The main method comprises mapping and on-line localization modules and sequential sub-maps are generated by fusing data from a 2D laser scanner and a RGBD camera. AMCL is used to perform accurate localization according to image matching results. Not only the localization system itself but novel evaluation methods for it are presented in the paper and by using it the robustness and accuracy of the system is demonstrated.

Reference [31] presents a navigation method whereby a flying robot is able to explore and map an underground mine without collision. Simulations have verified that the system performs as well as the authors expected whether the robot is circling above flat or sloping ground. The authors also claim in the paper that the system is as simple as it is reliable.

3. Preliminaries

3.1. Notation

We employ the following symbols throughout this paper. ${(.)}_W$ denotes the world frame, ${(.)}_{B_k}$ the kth body frame which doubles as the kth IMU frame as aligned with the body frame, ${(.)}_{C_k}$ the kth camera frame, and ${(.)}_{M_k}$ the kth magnetometer frame. As for measurements, $w_k$ represents the gyroscope’s readings, $a_k$ the accelerometer’s, and $h_k$ the magnetometer’s, at kth frame. ${(u,v)}_k$ the coordinates of a feature point in the kth image.

The overall states to be estimated are expressed in such a vector as $x={\left[\begin{array}{c}P,q,V,bg,ba,q_{BC}\end{array}\right]}_{B_k}$ with P the position, q the body orientation quaternion, V the velocity, $b_g$ and $b_a$ the gyroscope’s and accelerometer’s biases respectively at the kth frame, $q_{BC}$ the transformation from frame C to frame B intrinsic to the installation and thus considered to be invariant. $L_W$ symbolizes a landmark expressed homogeneously with $L_W=\left[\begin{array}{c}x,y,z,1\end{array}\right]$.

${\theta }^{\times }$ represents the askew matrix of the vector $\theta$:

$$\begin{array}{c}\hfill {\theta }^{\times }=\left[\begin{array}{c}{\theta }_1\\ {\theta }_2\\ {\theta }_3\end{array}\right]=\left[\begin{array}{ccc}0& -{\theta }_3& {\theta }_2\\ {\theta }_3& 0& -{\theta }_1\\ -{\theta }_2& {\theta }_1& 0\end{array}\right]\end{array}$$

(1)

Note that ${{\theta }^{\times }}^T=-{\theta }^{\times }$ by its very definition.

3.2. Useful Properties of SO(3)

$SO\left(3\right)$, the special orthogonal group in 3 dimensions, describes rotations in 3D space, with its corresponding vector $so\left(3\right)$ in parameter. $SO\left(3\right)$ and $so\left(3\right)$ are related to each other through the exponential and logarithmic maps:

$$R\left(\theta \right)=\mathit{Exp}\left(\theta \right)=I+\frac{sin\left|\right|\theta \left|\right|}{\left|\right|\theta \left|\right|}{\theta }^{\times }+\frac{1-cos\left|\right|\theta \left|\right|}{{\left|\right|\theta \left|\right|}^2}{{\theta }^{\times }}^2$$

(2)

$$\theta =\mathit{Log}\left(R\left(\theta \right)\right)=\frac{\left|\right|\theta \left|\right|}{2sin\left|\right|\theta \left|\right|}{(R-R^T)}^{\vee }$$

(3)

$$||\theta ||=arccos(\frac{trace\left(R\right)-1}{2})$$

(4)

Four properties of $SO\left(3\right)$ are essential in IMU pre-integration and Jacobian computation for optimization and thus merit a mention:

• commutative

  $${\theta }^{\times }\varphi =-{\varphi }^{\times }\theta$$

  (5)
• approximation

  $$Exp\left(\theta \right)\approx I+{\theta }^{\times }$$

  (6)
• adjoint

  $$\begin{array}{cc}\hfill R·Exp\left(\theta \right)& =Exp(Ad{j}_{\left(R\right)}·\theta )·R\hfill \end{array}$$

  $$\begin{array}{cc}\hfill & =Exp(R·\theta )·R\hfill \end{array}$$

  (7)

  $$\begin{array}{cc}\hfill Exp\left(\theta \right)·R& =R·Exp(Ad{j}_{\left(R\right)}^T·\theta )\hfill \end{array}$$

  $$\begin{array}{ccc}\hfill & & =R·Exp(R^T·\theta )\hfill \end{array}$$

  (8)
• the righ-hand jacobian

  $$\begin{array}{cc}\hfill Exp(\theta +\delta \theta )& \approx Exp\left(\delta \right)Exp\left(J_r\left(\theta \right)\delta \theta \right)\hfill \end{array}$$

  (9)

  $$\begin{array}{cc}\hfill Exp\left(\theta \right)Exp(\theta +\delta \theta )& \approx Exp(\theta +J_r^{-1}\left(\theta \right)\delta )\hfill \end{array}$$

  (10)

  $$\begin{array}{cc}\hfill Log\left(Exp\right(\theta \left)Exp\right(\theta +\delta \theta \left)\right)& \approx \theta +J_r^{-1}\left(\theta \right)\hfill \end{array}$$

  (11)

  $$\begin{array}{cc}\hfill J_r\left(\theta \right)& =I-{\displaystyle \frac{1-cos\left|\right|\theta \left|\right|}{{\left|\right|\theta \left|\right|}^2}}{\left[\theta \right]}^{\times }+{\displaystyle \frac{\left|\right|\theta -sin\left|\right|\theta \left|\right|}{{\left|\right|\theta \left|\right|}^3}}{{\left[\theta \right]}^{\times }}^2\hfill \end{array}$$

  (12)

  $$\begin{array}{cc}\hfill J_r^{-1}\left(\theta \right)& =I+{\displaystyle \frac{1}{2}}{\left[\theta \right]}^2-({\displaystyle \frac{1}{{\left|\right|\theta \left|\right|}^2}}-{\displaystyle \frac{1+cos\left|\right|\theta \left|\right|}{2\left|\right|\theta \left|\right|sin\left|\right|\theta \left|\right|}}){{\left[\theta \right]}^{\times }}^2\hfill \end{array}$$

  (13)

Note that there is also $J_l$ called the left-hand jacobian as opposed to $J_r$. We shall refer to them as $J_r$ and $J_l$ for the following content.

4. VIMO Measurement Models

4.1. Vision

We use a pinhole camera as a source of visual measurement. Under the pinhole camera projection model, a 3D point is projected onto a plane with Z normalized and then projected onto the image plane with projection parameters intrinsic to the camera:

$$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\frac{1}{Z}\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]$$

(14)

where u, v denote the coordinates of the point projected onto the plane. In most cases, coordinates go through radial and tangential distortion on the normalized plane with parameters called distortion coefficients before scaled and displaced to form the final pixel coordinates. The distortion coefficients vary in value and number according to the lenses of cameras. Lenses with a great degree of distortion should be treated with up to 3 coefficients to express the distortion properly. The lens of the camera used in the proposed scheme is an ordinary one and only two distortion coefficients are adopted.

$$\begin{array}{c}\hfill x_{dis}=x+2p_{1}xy+p_2(r^2+2x^2)\\ \hfill y_{dis}=y+p_1(r^2+2y_2)+2p_{2}xy\end{array}$$

(15)

The Jacobian matrices of pixel coordinates with respect to distorted ones, distorted ones to normalized ones, and normalized ones to 3D coordinates are present as follows:

$$J_{img}=\left[\begin{array}{cc}{f}_x& 0\\ 0& f_y\end{array}\right]$$

(16)

$$J_{dis}=\left[\begin{array}{cc}1+k_1{r}^2+k_2{r}^4+2k_1{k}_2{u}^2+4k_2{u}^2{r}^2+2p_{1}v+6p_{2}u& 2k_{1}uv+4k_2{r}^{2}uv+2p_{1}u+2p_{2}v\\ 2k_{1}uv+4k_2{r}^{2}uv+2p_{1}u+2p_{2}v& 1+k_1{r}^2+k_2{r}^4+2k_1{u}^2+4k_2{r}^2{u}^2+6p_{1}v+2p_{2}u\end{array}\right]$$

(17)

$$J_{nor}=\left[\begin{array}{ccc}1/Z& 0& -X/Z^2\\ 0& 1/Z& -Y/Z^2\end{array}\right]$$

(18)

The overall Jacobian matrix can then be obtained by applying the chain rule:

$$J_{pr{o}_{2\mathcal{D}\to 3\mathcal{D}}}=J_{img}·J_{dis}·J_{nor}$$

(19)

4.2. Inertia

Inertial data are obtain from an IMU at successive time instants at a frequency of 200 Hz:

$$\begin{array}{cc}\hfill w_m& =w_t+b_w+n_w\hfill \end{array}$$

(20)

$$\begin{array}{cc}\hfill a_m& =R_{BW}(a_t+g_W)+b_a+n_a\hfill \end{array}$$

(21)

where $w_m$ and $a_m$ are acceleration and angular readings, $n_w$ and $n_a$ conceived of as Gaussian white noise, $b_w$ and $b_a$ modelled as random walk, and $w_t$ and $a_t$ the angular rate and acceleration in the world frame.

The error-state kinematics in continuous time are

$$\begin{array}{cc}\hfill \delta \dot{p}& =\delta v\hfill \end{array}$$

(22)

$$\begin{array}{cc}\hfill \delta \dot{v}& =-R{[a_m-b_a]}^{\times }\delta \theta -R\delta a_b+\delta g-R{w}_n\hfill \end{array}$$

(23)

$$\begin{array}{cc}\hfill \delta \dot{\theta }& =-{[w_m-w_b]}^{\times }\delta \theta -\delta b_w-n_w\hfill \end{array}$$

(24)

$$\begin{array}{cc}\hfill \delta \dot{b_a}& =n_a\hfill \end{array}$$

(25)

$$\begin{array}{cc}\hfill \delta \dot{b_g}& =n_w\hfill \end{array}$$

(26)

In traditional navigation schemes where filters are often used, states are predicted by integration of inertial measurements based on the prior states, indicating that accurate estimates of the initial states are crucial since even slightly skewed gravity direction would translate into enormously egregious errors in position and velocity estimates, giving rise to complete divergence. For positioning systems based on state optimization applied in more generic environments, not only is the unknown initial orientation problematic but the necessity of the system being re-linearized after each optimization step acts as a further drag. With a view to tackling this issue, the concept of IMU pre-integration has emerged and been well applied in visual-inertial navigation. Through multiplying both sides of the kinematics by a rotation matrix intended to transform the reference frame from the world frame W to the beginning frame $B_k$ in question, relative integration terms independent of state variables and gravity can be separated from other terms, obviating the need for re-propagation.

We reckon that mid-point integration is accurate enough as well as being efficient in computation. As IMU readings arrive at regular intervals, the integration goes on step by step as follows:

$$\begin{array}{cc}\hfill \Delta R_{k+1}& =\Delta R_k·Exp((\frac{w_k+w_{k+1}}{2}-b_w)·\Delta t)\hfill \end{array}$$

(27)

$$\begin{array}{cc}\hfill \Delta V_{k+1}& =\Delta V_k+\frac{\Delta R_k+\Delta R_{k+1}}{2}·(\frac{a_k+a_{k+1}}{2}-b_a)\Delta t\hfill \end{array}$$

(28)

$$\begin{array}{cc}\hfill \Delta P_{k+1}& =\Delta P_k+\Delta V_k\Delta t+\frac{1}{2}·\frac{\Delta R_k+\Delta R_{k+1}}{2}(\frac{a_k+a_{k+1}}{2}-b_a)\Delta t^2\hfill \end{array}$$

(29)

where $\Delta R$, $\Delta V$, and $\Delta P$ are the pre-integration terms with the subscript ${(.)}_k$ denoting IMU frames. Note that the pre-integration terms are independent of state variables except biases. No re-propagation will be required after the position $P_k$, the velocity $V_k$, and the rotation $R_k$ change and for small alterations in biases the pre-integration will be adjusted according to the jacobians, caculated iteratively alongside pre-integration, of errors in $\Delta R$, $\Delta V$, $\Delta R$ with respect to the biases $b_w$ and $b_a$.

For error propagation, the error-state kinematics differ from those corresponding to Euler integration since they involve measurements at both the previous and the next time instant. The error updates are as follows:

$$\begin{array}{cc}\hfill \mathit{\delta }{\mathit{\theta }}_{\mathit{k}+\mathbf{1}}\leftarrow & \mathit{\delta }{\mathit{\theta }}_{\mathit{k}}-R_k{J}_L({\overline{w}}_m\Delta t)\mathit{\delta }{\mathit{b}}_{\mathit{w}}+R_k{J}_L({\overline{w}}_m\Delta t){\mathit{n}}_{\mathit{w}}\hfill \end{array}$$

(30)

$$\begin{array}{cc}\hfill \mathit{\delta }{\mathit{V}}_{\mathit{k}+\mathbf{1}}\leftarrow & \mathit{\delta }{\mathit{V}}_{\mathit{k}}-{(\overline{R}{\overline{a}}_m\Delta t)}^{\times }\mathit{\delta }{\mathit{\theta }}_{\mathit{k}}+\frac{1}{2}\Delta t^2{R}_{k+1}{\overline{a}}_m^{\times }{J}_R({\overline{w}}_m\Delta t)\mathit{\delta }{\mathit{b}}_{\mathit{w}}-\overline{R}\Delta t\mathit{\delta }{\mathit{b}}_{\mathit{a}}\hfill \\ \hfill & -\frac{1}{2}\Delta t^2{R}_{k+1}{\overline{a}}_m^{\times }{J}_R({\overline{w}}_m\Delta t){\mathit{n}}_{\mathit{w}}+\overline{R}\Delta t{\mathit{n}}_{\mathit{a}}\hfill \end{array}$$

(31)

$$\begin{array}{cc}\hfill \mathit{\delta }{\mathit{P}}_{\mathit{k}+\mathbf{1}}\leftarrow & \mathit{\delta }{\mathit{P}}_{\mathit{k}}+\Delta t\mathit{\delta }{\mathit{V}}_{\mathit{k}}-\frac{1}{2}{(\overline{R}{\overline{a}}_m\Delta t^2)}^{\times }\mathit{\delta }{\mathit{\theta }}_{\mathit{k}}+\frac{1}{4}\Delta t^3{R}_{k+1}{\overline{a}}_m^{\times }{J}_R({\overline{w}}_m\Delta t)\mathit{\delta }{\mathit{b}}_{\mathit{w}}-\frac{1}{2}\overline{R}\Delta t^2\mathit{\delta }{\mathit{b}}_{\mathit{a}}\hfill \\ \hfill & -\frac{1}{4}\Delta t^3{R}_{k+1}{\overline{a}}_m^{\times }{J}_R({\overline{w}}_m\Delta t){\mathit{n}}_{\mathit{w}}+\frac{1}{2}\overline{R}\Delta t^2{\mathit{n}}_{\mathit{a}}\hfill \end{array}$$

(32)

$$\begin{array}{cc}\hfill \delta {\mathit{bw}}_{\mathit{k}+\mathbf{1}}\leftarrow & \mathit{\delta }{\mathit{bw}}_{\mathit{k}}+{\mathit{n}}_{\mathit{bw}}\Delta t\hfill \end{array}$$

(33)

$$\begin{array}{cc}\hfill \delta {\mathit{ba}}_{\mathit{k}+\mathbf{1}}\leftarrow & \mathit{\delta }{\mathit{ba}}_{\mathit{k}}+{\mathit{n}}_{\mathit{ba}}\Delta t\hfill \end{array}$$

(34)

where $\delta (.)$ indicates error states, $\Delta t$ the discrete time interval between $t_k$ and $t_{k+1}$. Note that in the above equations ${\overline{w}}_m$ and ${\overline{a}}_m$ take the place of $\frac{w_k+w_{k+1}}{2}-b_w$ and $\frac{a_k+a_{k+1}}{2}-b_a$ and $\overline{R}$ of $\frac{R_k+R_{k+1}}{2}$ for notation simplicity. For the Equation (30) of rotation error propagation, the angular error is defined on the right side, i.e., $R_{true}=\delta RR$, probably opposite to most other definitions.

The jacobians $\frac{\delta \theta }{\delta b_w}$, $\frac{\delta V}{\delta b_w}$, $\frac{\delta V}{\delta b_a}$, $\frac{\delta P}{\delta b_w}$, $\frac{\delta P}{\delta b_a}$ calculated by (30), (31), and (32) are used for correction of pre-integration values in response to variations in biases. If the norm of the bias vector ${\left[\begin{array}{cc}{b}_w& b_a\end{array}\right]}^T$ (rad/s${}^2$ for $b_w$ and m/s${}^2$ for $b_a$) reaches above a threshold of ${10}^{-4}$, re-propagation over this period is warranted and executed as the linearization point has changed too much.

According to (30)–(34), the error-state transition matrix is

$$F_{\delta x}=\left[\begin{array}{ccccc}I& 0& 0& -R_k{J}_L({\overline{w}}_m\Delta t)& 0\\ -{(\overline{R}{\overline{a}}_m\Delta t)}^{\times }& 0& \frac{1}{2}\Delta t^2{R}_{k+1}{\overline{a}}_m^{\times }{J}_R({\overline{w}}_m\Delta t)& -\overline{R}\Delta t\\ -\frac{1}{2}{(\overline{R}{\overline{a}}_m\Delta t^2)}^{\times }& \Delta t& I& \frac{1}{4}\Delta t^3{R}_{k+1}{\overline{a}}_m^{\times }{J}_R({\overline{w}}_m\Delta t)& -\frac{1}{2}\overline{R}\Delta t^2\\ 0& 0& 0& I& 0\\ 0& 0& 0& 0& I\end{array}\right],$$

(35)

and the Jabocian matrix of pre-integration with respect to the noise vector.

$$F_i=\left[\begin{array}{cccc}{R}_k{J}_L({\overline{w}}_m\Delta t)& 0& 0& 0\\ -\frac{1}{2}\Delta t^2{R}_{k+1}{\overline{a}}_m^{\times }{J}_R({\overline{w}}_m\Delta t)& \overline{R}\Delta t& 0& 0\\ -\frac{1}{4}\Delta t^3{R}_{k+1}{\overline{a}}_m^{\times }{J}_R({\overline{w}}_m\Delta t)& \frac{1}{2}\overline{R}\Delta t^2& 0& 0\\ 0& 0& \Delta t& 0\\ 0& 0& 0& \Delta t\end{array}\right]$$

(36)

The propagation of errors and covariances for pre-integration is summarized as

$$\begin{array}{cc}\hfill \delta x_{k+1}& \leftarrow F_x(x_k,{\overline{w}}_m\Delta t,{\overline{a}}_m\Delta t)\delta x_k\hfill \end{array}$$

(37)

$$\begin{array}{cc}\hfill P_{k+1}& \leftarrow F_x{P}_k{F_x}^T+F_i{Q}_i{F_i}^T\hfill \end{array}$$

(38)

where $Q_i$ is the covariance matrix of Gaussian white noise determined from the IMU datasheet or through calibration experiments. $P_{B_k{B}_{k+1}}$ propagated by (38) during the pre-integration from ${\mathit{f}}_{B_k}$ to ${\mathit{f}}_{B_{k+1}}$ accounts for uncertainty, or observation noise covariances, and is used to weight inertial residuals.

4.3. Magnetism

Besides vision and inertia, geomagnetism is also involved to relieve localization of the accumulative yawing error that would otherwise be ever mounting up.

As with the accelerator that can measure the gravitational field on a static platform, the magnetometer purveys readings of the projection of its surrounding magnetic field onto its body frame. While what it gauges is not the projection purely of the EMF, traditionally for centuries it has been used for bearing information. Figure 1 illustrates the vector of the total density EMF T and its projections on the geomagnetic and geographic coordinate axes.

In Figure 1:

• $\mathit{T}$ – the vector of the total density EMF;
• $\mathit{X}$, $\mathit{Y}$, and $\mathit{Z}$ – the Geographic System Coordinates;
• $\mathit{I}$ – the angle of magnetic inclination;
• $\mathit{D}$ – the angle of magnetic declination;

Such parameters as $\mathit{T}$, $\mathit{I}$, $\mathit{D}$ can be determined by referencing geomagnetic maps that describe the geomagnetic features of various locations around the globe.

The average magnetometer’s calibration model is

$$\begin{array}{c}\hfill \left[\begin{array}{c}{M_x}_t\\ {M_y}_t\\ {M_z}_t\end{array}\right]=\left[\begin{array}{ccc}{k}_{xx}& k_{yx}& k_{zx}\\ k_{yy}& k_{xy}& k_{zy}\\ k_{zz}& k_{xz}& k_{yz}\end{array}\right]\left[\begin{array}{c}{M}_x\\ M_y\\ M_z\end{array}\right]-\left[\begin{array}{c}{b}_{m_x}\\ b_{m_y}\\ b_{m_z}\end{array}\right]\end{array}$$

(39)

where: ${\left[\begin{array}{ccc}{M}_x& M_y& M_z\end{array}\right]}^T$ is the magnetometer’s raw readings that are a coefficient matrix and a bias vector away from the true magnetic projection on the sensor’s body frame ${\left[\begin{array}{ccc}{M_x}_t& {M_y}_t& {M_z}_t\end{array}\right]}^T$; $k_{xx}$, $k_{yy}$, and $k_{zz}$ are scale factor coefficients; $k_{xy}$, $k_{yz}$, and $k_{zx}$ are the transverse coefficients caused by the magnetometer’s axes non-orthogonality; $b_{m_x}$, $b_{m_y}$, and $b_{m_z}$ are correction coefficients for biases created by local magnetic field.

According to Figure 1, the observation model of the magnetometer is

$$M_M=R_{MB}{R}_{WB}^T{H}_W$$

(40)

where $R_{MB}$ is the relative rotation matrix from the body frame ${\mathit{f}}_B$ to the magnetometer’s frame ${\mathit{f}}_M$ and is determined though calibration.

Following from the magnetometer’s observation model, the Jacobians of the measurement function with respect to the variable to optimize $R_{WB}$ are

$$J_{\frac{M}{R}}=\frac{\partial M_M}{\partial R_{WB}}=R_{MB}{R}_{WB}^T{H}_W^{\times }$$

(41)

where the perturbation is defined on the right side of $R_{WB}$ (globally defined).

5. Overall System Structure

Figure 2 depicts the system’s structure plainly.

6. Visual-Inertial-Magnetic Initialization

The initialization of monocular visual inertial odometry is as crucial as it is intricate, on account of its precarious structure. Futile or incomplete initialization spell trouble for the entire system. On the one hand, monocular vision calls for a certain length of translation long enough to reflect the depths of key points, on the other hand, the projection of gravity onto the body frame can only be calculated when there’s no extra acceleration other than gravity.

This section intends to present and lay out a novel and efficacious procedure of initialization. As drawn out in Section 2, successful initialization is a prerequisite for the system to start off properly. How accurate the initial parameters are estimated will play a huge part in how stably and smoothly the system operates. The involvement of magnetometers introduces extra parameters to be determined, namely the initial magnetic bearing. The proposed method delivers visual-inertial-magnetic initialization with efficacy and credibility assured in some measure by deciding whether or not it is initialized successfully through a set of specific criteria. Figure 3 illustrates the procedure of initialization.

As opposed to VI-SLAM, the system uses magnetic information as rotation constraint during visual recovery with the intention of expediting the initialization process and enhancing its precision.

6.1. Visual Recovery and Rotation Calculation through Magnetism

The task the visual module undertakes is estimating the relative transformation with respect to the first frame by vision itself. Using a monocular camera without depth information indicates the first step is to extract from two selected images the essential or homography matrix which can be decomposed to recover the transformation between the images. The two images for recovering poses are selected if there’s enough parallax between them. The essential matrix is better at computing poses if the camera’s moved, whereas if it is only rotated without translation the homography matrix fare better. The problem is there’s no way of establishing whether or not the camera’s moved because either pure rotation or translation causes parallax between two images. For better robustness, we compute and decompose both of them and check through whose transformation the reprojection is smaller to decide to use which matrix. Key points tracked in the two images are triangulated to determine depths if there’s sufficient translation between them. Those 3D key points are then utilized to execute PnP (Perspective-n-Point) on all the intervening key frames under the auspices of BA (Bundle Adjustment).

Magnetic information is also exploited for rotation estimation by incorporating its measurements into the BA problem as extra cost functions in Equation (42).

$$E_M=M_{M_{k+1}}-R_{W{B}_{k+1}}^T{R}_{W{B}_k}{R}_{BM}{M}_{M_k}$$

(42)

where $M_k$ denotes magnetometer’s kth frame and $R_{BM}$ is the rotation from magnetometer’s frame to body’s frame.

6.2. Visual-Inertial-Magnetic Alignment

Before alignment, gyroscope biases need to be worked out. The reason why only gyroscope biases are computed is because attitude holds much more influence on pose estimation as its estimates dictate whether gravity can be rightly projected onto the body frame. By solving Equation (43) through least squares, gyroscope biases $b_{w_{B_k}}$ can be obtained:

$$\Delta R_{B_k{B}_{k+1}}R(\frac{\partial \Delta R_{B_k{B}_{k+1}}}{\partial b_{w_{B_k}}}{b}_{w_{B_k}})=R_{W{C}_k}^T{R}_{BC}^T{R}_{W{C}_{k+1}}{R}_{BC}^T$$

(43)

where $\Delta R_{B_k{B}_{k+1}}$ is the rotational preintegration and $\frac{\partial \Delta R_{B_k{B}_{k+1}}}{\partial b_{w_{B_k}}}$ is its partial derivative with respect to gyroscope biases. Section 4.2 lays out how to maintain this partial derivative.

$$\Delta P^{B_k{B}_{k+1}}=R_{W{B}_k}^T\mathbf{s}{R}_C^B{P}_W^{W{C}_{k+1}}-R_{W{B}_k}^T(\mathbf{s}{R}_C^B{P}_W^{W{C}_k}+\Delta t{\mathbf{V}}_{\mathbf{W}}^{\mathbf{W}{\mathbf{B}}_{\mathbf{k}}}-\frac{1}{2}\Delta t^2{\mathbf{G}}_{\mathbf{W}})$$

(44)

$$\Delta V^{B_k{B}_{k+1}}=R_{W{B}_k}^T{\mathbf{V}}_{\mathbf{W}}^{\mathbf{W}{\mathbf{B}}_{\mathbf{k}+\mathbf{1}}}-R_{W{B}_k}^T({\mathbf{V}}_{\mathbf{W}}^{\mathbf{W}{\mathbf{B}}_{\mathbf{k}}}-\Delta t{\mathbf{G}}_{\mathbf{W}})$$

(45)

where $\mathbf{s}$, ${\mathbf{V}}_{\mathbf{W}}^{\mathbf{W}{\mathbf{B}}_{\mathbf{k}}}$, ${\mathbf{V}}_{\mathbf{W}}^{\mathbf{W}{\mathbf{B}}_{\mathbf{k}+\mathbf{1}}}$, and ${\mathbf{G}}_{\mathbf{W}}$ are the scalar factor, velocity, and the vector of gravity to be determined. Obviously, $P_W^{W{C}_k}$ and $P_W^{W{C}_{k+1}}$ come from the visual recovery module. Every combination of adjacent images and the preintegration in-between forms a set of equations like (44), stacking up into a least squares problem.

The process described above is merely visual-inertial alignment after which the z-axis of the world frame is aligned with the vector of gravity, velocity states projected onto the world frame, and key points’ depths scaled to proper size. The next step is to align the magnetometer with the world frame by making its projection on the $xy$ plane of the world frame parallel with the x-axis.

After the complete alignment, the world frame’s z-axis is parallel with the direction of gravity and x-axis with magnetic north.

6.3. Initialization Completion Verdict

It is not beyond the bounds of possibility that parameters initialized could be dubious. A common case explaining this phenomenon is when the system keeps moving in one direction at a constant speed, not administering sufficient excitation to IMU.

Inspired by [32,33], we examines initialization’s efficacy by reviewing estimation error that links in to a degree with the uncertainty of the initialization system.

With the estimation error satisfactorily low, the system will be notified and go to the optimization stage.

7. Joint Nonlinear Optimization

As regards calculation of Jacobians, it warrants mention that the perturbation is defined on the right side where rotation variables are involved, as evidenced in [34] to have better properties.

$$T\leftarrow \delta T\otimes T\left\{\begin{array}{c}R\leftarrow \mathit{\delta }\mathit{R}·R\hfill \\ P\leftarrow P+\delta P\hfill \end{array}\right.$$

(46)

Different definitions of the perturbation for differentiation certainly lead to different Jacobians and inconsistency of which side the perturbation is operated on and how the Jacobians are calculated is heading for failure in the process of optimization.

7.1. Visual Constraint

Feature points for tracking are extracted from every image [35] and data association across image frames is realized by tracking them through optical flow [36], which, as exhibited in real-time operation, relieves the visual front-end of heavy computation because it does not have to describe features and subsequently match them as do standard feature-based methods. The number of successfully tracked points will surely diminish as images arrive and pass frame after frame either because of points moving out of the image region or simply of tracking failure. Additional features are extracted in every frame where the number of successfully tracked points drops to a certain amount. Non-maximum suppression is applied to the extraction of key points to obtain a more even distribution of points that would otherwise cluster around a few areas little more than single features.

As key points have to be dispersed to better represent the whole image, so do frames need to be selected as key frames and the number of them be curtailed to avoid redundancy. Other than representing a solitary frame and indicating a static state through small errors of re-projection, frames with little parallax in-between or even identical due to a stationary state hold little significance for the whole, probably quite long and large, trajectory and map retained in the system for loop closure detection and other uses. A key frame ought to hold enough connections with its previous and next key frames through co-visibility [37] for pose coverage and efficient estimation in regard to the local graph while being distinct enough from its adjacent ones for the conciseness of the whole pose-graph. In our formulation, whether or not a frame is “key” is conditioned by its key points’ association with the previous and next frames, or specifically, the ratio of the number in the next frame to that in the current frame of successfully tracked key-points that originate from the previous or older frames. A high value (say 0.8) of this ratio suggests that the majority of key points tracked from previous frames to the current one are well observed again in the next frame and thus this frame may be considered to be redundant in the presence of its neighbours, whereas the lower the ratio is, the more contributory the frame is to the observation and retention of landmarks. A practice is to pre-set a ratio threshold above which a frame is deemed ‘non-key’ and is consequently going to be marginalized out after the current round of optimization.

We use a plain pinhole camera as the visual sensing module with an ordinary projection model that has been presented in Section 4.1. The re-projection error is

$$E(i,k)=Z_k^i-\pi \left({{\mathit{T}}_{\mathit{BC}}}^T{{\mathit{T}}_{\mathit{WB}}}^T{{\mathit{L}}_{\mathit{W}}^{\mathit{WL}}}^{\mathbf{i}}\right)$$

(47)

where i indexes landmarks and k denotes frames, variables to be optimized in bold type. $\pi (.)$ is the projection function whose Jacobian matrix ($J_{pr{o}_{2\mathcal{D}\to 3\mathcal{D}}}$) with respect to the original $3D$ point vector has been demonstrated in Section 4.1. $T_{BC}$ is the transformation from ${\mathit{f}}_C$ to ${\mathit{f}}_B$ that is regarded as being constant provided the sensors are rigidly fixed on the platform, and the optimization of which is thus optional with it calibrated in advance.

The Jacobians of the re-projection error with respect to each variable are as follows:

$$\frac{\partial E(i,k)}{\partial R_{BC}}=-J_{pr{o}_{2\mathcal{D}\to 3\mathcal{D}}}·R_{BC}^T{(T_{WB}^T{L_W^{WL}}^i-t_{BC})}^{\times }$$

(48)

$$\frac{\partial E(i,k)}{\partial t_{BC}}=J_{pr{o}_{2\mathcal{D}\to 3\mathcal{D}}}·R_{BC}^T$$

(49)

$$\frac{\partial E(i,k)}{\partial R_{WB}}=-J_{pr{o}_{2\mathcal{D}\to 3\mathcal{D}}}·R_{BC}^T{R}_{WB}^T{({L_W^{WL}}^i-t_{WB})}^{\times }$$

(50)

$$\frac{\partial E(i,k)}{\partial t_{WB}}=J_{pr{o}_{2\mathcal{D}\to 3\mathcal{D}}}·R_{BC}^T{R}_{WB}^T$$

(51)

$$\frac{\partial E(i,k)}{\partial {L_W^{WL}}^i}=-J_{pr{o}_{2\mathcal{D}\to 3\mathcal{D}}}·T_{BC}^T{T}_{WB}^T$$

(52)

where $t_{BC}$ and $t_{WB}$ are the translation parts of $T_{BC}$ and $T_{WB}$. $J_{pr{o}_{2\mathcal{D}\to 3\mathcal{D}}}$ are the Jacobian matrices of pixel coordinates with respect to normalized $3D$ points as duduced in Section 4.1. The above jacobians are derived with the rotation and translation variables treated separately as they are when being updated, rather than computed as a whole as in [38].

7.2. Inertial Constraint

As inertial observations take the form of the integration of measurements between two adjacent frames aligned in time with camera frames, they somewhat resemble a measurement of the relative transformation between each two frames, save that gravity is incorporated in the pre-integration. The equations below express the observation model of pre-integration.

$$\begin{array}{cc}\hfill \Delta P^{B_k{B}_{k+1}}& ={{\mathit{R}}_{\mathit{W}{\mathit{B}}_{\mathit{k}}}}^T{\mathit{P}}_{\mathit{W}}^{\mathit{W}{\mathit{B}}_{\mathit{k}+\mathbf{1}}}-{{\mathit{R}}_{\mathit{W}{\mathit{B}}_{\mathit{k}}}}^T({\mathit{P}}_{\mathit{W}}^{\mathit{W}{\mathit{B}}_{\mathit{k}}}+\Delta t{\mathit{V}}_{\mathit{W}}^{\mathit{W}{\mathit{B}}_{\mathit{k}}}-\frac{1}{2}\Delta t^2{G}_W)\hfill \end{array}$$

(53)

$$\begin{array}{cc}\hfill \Delta V^{B_k{B}_{k+1}}& ={{\mathit{R}}_{\mathit{W}{\mathit{B}}_{\mathit{k}}}}^T{\mathit{V}}_{\mathit{W}}^{\mathit{W}{\mathit{B}}_{\mathit{k}+\mathbf{1}}}-{{\mathit{R}}_{\mathit{W}{\mathit{B}}_{\mathit{k}}}}^T({\mathit{V}}_{\mathit{W}}^{\mathit{W}{\mathit{B}}_{\mathit{k}}}-\Delta t{G}_W)\hfill \end{array}$$

(54)

$$\begin{array}{cc}\hfill \Delta R_{B_k{B}_{k+1}}& ={{\mathit{R}}_{\mathit{W}{\mathit{B}}_{\mathit{k}}}}^T{\mathit{R}}_{\mathit{W}{\mathit{B}}_{\mathit{k}+\mathbf{1}}}\hfill \end{array}$$

(55)

where ${(.)}^{B_k}$ and ${(.)}^{B_{k+1}}$ denote the first and last frames of the pre-integration and the variables to optimize are highlighted in bold type.

The whole state vector to be optimized according to the pre-integration observation from ${\mathit{f}}_{B_k}$ to ${\mathit{f}}_{B_{k+1}}$ is given below:

$$\begin{array}{c}\hfill \left[\begin{array}{cccccccccc}{\mathit{P}}_{\mathit{W}}^{\mathit{W}{\mathit{B}}_{\mathit{k}}}& {\mathit{V}}_{\mathit{W}}^{\mathit{W}{\mathit{B}}_{\mathit{k}}}& {\mathit{\theta }}_{\mathit{W}{\mathit{B}}_{\mathit{k}}}& {\mathit{b}}_{{\mathit{w}}_{{\mathit{B}}_{\mathit{k}}}}& {\mathit{b}}_{{\mathit{a}}_{{\mathit{B}}_{\mathit{k}}}}& {\mathit{P}}_{\mathit{W}}^{\mathit{W}{\mathit{B}}_{\mathit{k}+\mathbf{1}}}& {\mathit{V}}_{\mathit{W}}^{\mathit{W}{\mathit{B}}_{\mathit{k}+\mathbf{1}}}& {\mathit{\theta }}_{\mathit{W}{\mathit{B}}_{\mathit{k}+\mathbf{1}}}& {\mathit{b}}_{{\mathit{w}}_{{\mathit{B}}_{\mathit{k}+\mathbf{1}}}}& {\mathit{b}}_{{\mathit{a}}_{{\mathit{B}}_{\mathit{k}+\mathbf{1}}}}\end{array}\right]\end{array}$$

(56)

with the corresponding residuals:

$$\begin{array}{c}\hfill \left[\begin{array}{ccccc}{\mathfrak{R}}_{B_k{B}_{k+1}}\left(P\right)& {\mathfrak{R}}_{B_k{B}_{k+1}}\left(V\right)& {\mathfrak{R}}_{B_k{B}_{k+1}}\left(\theta \right)& {\mathfrak{R}}_{B_k{B}_{k+1}}\left(b_w\right)& {\mathfrak{R}}_{B_k{B}_{k+1}}\left(b_a\right)\end{array}\right]\end{array}$$

(57)

$$\begin{array}{c}\hfill {\mathfrak{R}}_{inertia}=\left[\begin{array}{c}{\mathfrak{R}}_{B_k{B}_{k+1}}\left(P\right)\hfill \\ {\mathfrak{R}}_{B_k{B}_{k+1}}\left(V\right)\hfill \\ {\mathfrak{R}}_{B_k{B}_{k+1}}\left(\theta \right)\hfill \\ {\mathfrak{R}}_{B_k{B}_{k+1}}\left(b_w\right)\hfill \\ {\mathfrak{R}}_{B_k{B}_{k+1}}\left(a_w\right)\hfill \end{array}\right]=\left[\begin{array}{c}\Delta P^{B_k{B}_{k+1}}-{{\mathit{R}}_{\mathit{W}{\mathit{B}}_{\mathit{k}}}}^T({\mathit{P}}_{\mathit{W}}^{\mathit{W}{\mathit{B}}_{\mathit{k}+\mathbf{1}}}-{\mathit{P}}_{\mathit{W}}^{\mathit{W}{\mathit{B}}_{\mathit{k}}}-\Delta t{\mathit{V}}_{\mathit{W}}^{\mathit{W}{\mathit{B}}_{\mathit{k}}}+\frac{1}{2}\Delta t^2{G}_W)\\ \hfill \Delta V^{B_k{B}_{k+1}}-{{\mathit{R}}_{\mathit{W}{\mathit{B}}_{\mathit{k}}}}^T({\mathit{V}}_{\mathit{W}}^{\mathit{W}{\mathit{B}}_{\mathit{k}+\mathbf{1}}}-{\mathit{V}}_{\mathit{W}}^{\mathit{W}{\mathit{B}}_{\mathit{k}}}+\Delta t{G}_W)\hfill \\ \hfill \Delta R_{B_k{B}_{k+1}}^T\oplus {{\mathit{R}}_{\mathit{W}{\mathit{B}}_{\mathit{k}}}}^T{\mathit{R}}_{\mathit{W}{\mathit{B}}_{\mathit{k}+\mathbf{1}}}\hfill \\ \hfill {\mathit{b}}_{{\mathit{w}}_{{\mathit{B}}_{\mathit{k}+\mathbf{1}}}}-{\mathit{b}}_{{\mathit{w}}_{{\mathit{B}}_{\mathit{k}}}}\hfill \\ \hfill {\mathit{b}}_{{\mathit{a}}_{{\mathit{B}}_{\mathit{k}+\mathbf{1}}}}-{\mathit{b}}_{{\mathit{a}}_{{\mathit{B}}_{\mathit{k}}}}\hfill \end{array}\right]\end{array}$$

(58)

where the variables to be optimized are in bold type as before. Note that the representations of ${\mathfrak{R}}_{B_k{B}_{k+1}}\left(b_w\right)$ and ${\mathfrak{R}}_{B_k{B}_{k+1}}\left(a_w\right)$ indicate that by nature the two biases would not change over a period of pre-integration.

The optimization of the variables for inertial residuals needs Jacobians, as do other optimization problems, around various linearized points to nudge variables towards at least a suboptimal solution. While the analytical form of Jacobians may not necessarily be required as there are available optimization modules capable of automatic differentiation, the analytical form is favourable nonetheless in its optimality and efficiency. The equations below present the Jacobians of pre-integration residuals with respect to variables

• Jacobians of ${\mathfrak{R}}_{B_k{B}_{k+1}}\left(P\right)$

  $$\begin{array}{cc}\hfill \frac{\partial \mathfrak{R}\left(P\right)}{\partial {\mathit{P}}_{\mathit{W}}^{\mathit{W}{\mathit{B}}_{\mathit{k}}}}& =R_{B_{k}W}\hfill \end{array}$$

  (59)

  $$\begin{array}{cc}\hfill \frac{\partial \mathfrak{R}\left(P\right)}{\partial {\mathit{V}}_{\mathit{W}}^{\mathit{W}{\mathit{B}}_{\mathit{k}}}}& =R_{B_{k}W}\Delta t\hfill \end{array}$$

  (60)

  $$\begin{array}{cc}\hfill \frac{\partial \mathfrak{R}\left(P\right)}{\partial {\mathit{P}}_{\mathit{W}}^{\mathit{W}{\mathit{B}}_{\mathit{k}+\mathbf{1}}}}& =-R_{B_{k}W}\hfill \end{array}$$

  (61)

  $$\begin{array}{cc}\hfill \frac{\partial \mathfrak{R}\left(P\right)}{\partial {\mathit{R}}_{{\mathit{B}}_{\mathit{k}}\mathit{W}}}& =-R_{B_{k}W}{(P_W^{W{B}_{k+1}}-P_W^{W{B}_k}-\Delta t{V}_W^{W{B}_k}+\frac{1}{2}\Delta t^2{G}_W)}^{\times }\hfill \end{array}$$

  (62)

  $$\begin{array}{cc}\hfill \frac{\partial \mathfrak{R}\left(P\right)}{\partial {\mathit{b}}_{{\mathit{w}}_{{\mathit{B}}_{\mathit{k}}}}}& =\frac{\partial \Delta P^{B_k{B}_{k+1}}}{\partial b_{w_{B_k}}}\hfill \end{array}$$

  (63)

  $$\begin{array}{cc}\hfill \frac{\partial \mathfrak{R}\left(P\right)}{\partial {\mathit{b}}_{{\mathit{a}}_{{\mathit{B}}_{\mathit{k}}}}}& =\frac{\partial \Delta P^{B_k{B}_{k+1}}}{\partial b_{a_{B_k}}}\hfill \end{array}$$

  (64)
• Jacobians of ${\mathfrak{R}}_{B_k{B}_{k+1}}\left(V\right)$

  $$\begin{array}{cc}\hfill \frac{\partial \mathfrak{R}\left(V\right)}{\partial {\mathit{V}}_{\mathit{W}}^{\mathit{W}{\mathit{B}}_{\mathit{k}}}}& =R_{B_{k}W}\hfill \end{array}$$

  (65)

  $$\begin{array}{cc}\hfill \frac{\partial \mathfrak{R}\left(V\right)}{\partial {\mathit{V}}_{\mathit{W}}^{\mathit{W}{\mathit{B}}_{\mathit{k}+\mathbf{1}}}}& =-R_{B_{k}W}\hfill \end{array}$$

  (66)

  $$\begin{array}{cc}\hfill \frac{\partial \mathfrak{R}\left(V\right)}{\partial {\mathit{R}}_{{\mathit{B}}_{\mathit{k}}\mathit{W}}}& =-R_{B_{k}W}{(V_W^{W{B}_{k+1}}-V_W^{W{B}_k}+\Delta t{G}_W)}^{\times }\hfill \end{array}$$

  (67)

  $$\begin{array}{cc}\hfill \frac{\partial \mathfrak{R}\left(V\right)}{\partial {\mathit{b}}_{{\mathit{w}}_{{\mathit{B}}_{\mathit{k}}}}}& =\frac{\partial \Delta V^{B_k{B}_{k+1}}}{b_{w_{B_k}}}\hfill \end{array}$$

  (68)

  $$\begin{array}{cc}\hfill \frac{\partial \mathfrak{R}\left(V\right)}{\partial {\mathit{b}}_{{\mathit{a}}_{{\mathit{B}}_{\mathit{k}}}}}& =\frac{\partial \Delta V^{B_k{B}_{k+1}}}{b_{a_{B_k}}}\hfill \end{array}$$

  (69)
• Jacobians of ${\mathfrak{R}}_{B_k{B}_{k+1}}\left(\theta \right)$

  $$\begin{array}{cc}\hfill \frac{\partial \mathfrak{R}\left(\theta \right)}{\partial {\mathit{R}}_{\mathit{W}{\mathit{B}}_{\mathit{k}}}}& =-\Delta R_{B_k{B}_{k+1}}^T·R_{B_{k}W}\hfill \end{array}$$

  (70)

  $$\begin{array}{cc}\hfill \frac{\partial \mathfrak{R}\left(\theta \right)}{\partial {\mathit{R}}_{\mathit{W}{\mathit{B}}_{\mathit{k}+\mathbf{1}}}}& =\Delta R_{B_k{B}_{k+1}}^T·R_{B_{k}W}\hfill \end{array}$$

  (71)

  $$\begin{array}{cc}\hfill \frac{\partial \mathfrak{R}\left(\theta \right)}{\partial {\mathit{b}}_{{\mathit{w}}_{{\mathit{B}}_{\mathit{k}}}}}& =\frac{\partial \Delta R_{B_k{B}_{k+1}}}{b_{w_{B_k}}}\hfill \end{array}$$

  (72)

  $$\begin{array}{cc}\hfill \frac{\partial \mathfrak{R}\left(\theta \right)}{\partial {\mathit{b}}_{{\mathit{a}}_{{\mathit{B}}_{\mathit{k}}}}}& =\frac{\partial \Delta R_{B_k{B}_{k+1}}}{b_{a_{B_k}}}\hfill \end{array}$$

  (73)
• Jacobians of ${\mathfrak{R}}_{B_k{B}_{k+1}}\left(b_w\right)$ and ${\mathfrak{R}}_{B_k{B}_{k+1}}\left(b_a\right)$

  $$\frac{\partial \mathfrak{R}\left(b_w\right)}{\partial {\mathit{b}}_{{\mathit{w}}_{{\mathit{B}}_{\mathit{k}}}}}=-I\frac{\partial \mathfrak{R}\left(b_a\right)}{\partial {\mathit{b}}_{{\mathit{a}}_{{\mathit{B}}_{\mathit{k}}}}}=-I\frac{\partial \mathfrak{R}\left(b_w\right)}{\partial {\mathit{b}}_{{\mathit{w}}_{{\mathit{B}}_{\mathit{k}+\mathbf{1}}}}}=I\frac{\partial \mathfrak{R}\left(b_a\right)}{\partial {\mathit{b}}_{{\mathit{a}}_{{\mathit{B}}_{\mathit{k}+\mathbf{1}}}}}=I$$

  (74)

7.3. Magnetic Constraint

Magnetic observation, as laid out in Section 4.3, is the projection of the vector of the EMF onto the body frame:

$$M_M=R_{MB}{R}_{WB}^T{H}_W$$

(75)

where $R_{MB}$ is the relative rotation matrix from the body frame ${\mathit{f}}_B$ to the magnetometer’s frame ${\mathit{f}}_M$ and is determined though calibration.

With the observation model, it is fairly straightforward to establish the magnetic residual:

$$\mathfrak{R}\left(M\right)=M_M-{\mathit{R}}_{\mathit{MB}}{{\mathit{R}}_{\mathit{WB}}}^T{H}_W$$

(76)

where ${\mathit{R}}_{\mathit{MB}}$ and ${\mathit{R}}_{\mathit{WB}}$ are to be optimized.

The corresponding Jacobians:

$$\frac{\partial \mathfrak{R}\left(M\right)}{\partial {\mathit{R}}_{\mathit{MB}}}={\left(R_{MB}{R}_{WB}^T{H}_W\right)}^{\times }\frac{\partial \mathfrak{R}\left(M\right)}{\partial {\mathit{R}}_{\mathit{WB}}}=-R_{MB}{R}_{WB}^T{H}_W^{\times }$$

(77)

Note that $M_M$ and $H_W$ are always normalized with norm equal to 1. $H_W$ is the vector of the total intensity of the EMF whose projection on the X-Y plane is in the direction of magnetic north cross true north at an angle called magnetic declination which is not concerned in our system since we use magnetometers only for suppressing yaw estimation shift.

7.4. Loop Closure Constraint

Loop closure constraints come from the front-end’s detection of loop closures when the robot arrives where it has roamed and thus its trajectory loops. Loop closure constraints take the form of

$$E_{loop}(i,j)=Z_j-\pi \left({{{\mathit{T}}_{\mathit{BC}}}_{\mathit{j}}}^T{{{\mathit{T}}_{\mathit{WB}}}_{\mathit{j}}}^T{L_W^{WL}}^i\right)$$

(78)

where i and j are the indexes of frames between which a loop closure occurs. ${\mathit{f}}_i$ is the older one whose pose together with the landmarks it holds are fixed on the grounds that the older a frame is the less its estimate has drifted. Equation (78) is almost identical to the visual constraint, save that the landmarks of the older frame are in regular type, signaling that they are not to be optimized since their would not be any other variable more precise than the start of the loop.

Apparently, only frames involved in loop closures are to be adjusted, leaving the rest unchanged, if optimization is executed merely on loop closures. That brings about inconsistency in the pose graph. One putative way is to combine odometry constraints and loop closures. Odometry constraints are relative pose transformations obtained from the current pose graph.

$$T_{ii+1}=T_{wi}^{-1}·T_{wi+1}$$

(79)

where $T_{ij}$ is a relative transformation taken from the existing pose graph as a measurement. The standard pose graph optimization problem is then presented as

$$E_{posegraph}=\sum _{i,j}{E}_{loopclosure}(i,j)+\sum _i{E}_{odometry}(i,i+1)$$

(80)

As [39] suggests, optimization of a pose graph is no more than a plain nonlinear least squares problem. What makes it call for extra cautious treatment is its susceptibility to false positive loop closures. Even a single outlier made by the front-end has the potential to bring the optimization into complete divergence or make the pose graph wrongly deform. One way to tackle this is to obviate outliers as early as in the detection stage. By applying RANSAC or other similar strategies under geometric models, outliers can be fairly picked out and eliminated, making false loops less likely to be carried over into the following optimization. Another relies on what is called the robust cost function which bears much resemblance to the Huber function [40] and can mitigate the impact of outliers by downgrading the cost functions to linear functions rather than quadratic ones, where the overall cost is high enough to be deemed harhouring false loops. Neither of these is foolproof. Systems using these methods are liable to suffer from wrongly detected closures all the same. False positive loop constraints are as much problematic as they are difficult to combat. Erroneous edges in the pose graph following detection of false loops can either lead the optimization to diverge or converge to a entirely egregious solution.

Inspired by [39], we adopt the idea that the topology of the pose graph is subject to the optimization with outliers being identified and removed automatically. To actualize this idea, a weighting factor is put on every loop closure constraint as a way of distinguishing normal loop closures from erroneous ones.

$$E_{posegraph}=\sum _{i,j}{w}_{ij}·E_{loopclosure}(i,j)+\sum _i{E}_{odometry}(i,i+1)$$

(81)

In the above equation, $w_{ij}$ is a weighting factor ranging from 0 to 1 corresponding to whether the constraint is active or deactivated, or rather, removed. As the weighting factor holds influence on the cost value, it indeed makes the topology itself subject to the optimization. The weighting factor is then given by a sigmoid function:

$$w_{ij}=sig\left(s_{ij}\right)=\frac{1}{1+e^{-s_{ij}}}$$

(82)

where $s_{ij}$ is a switch variable switches on and off a loop closure constraint during the optimization process, hence the name switch variable. These switch variables are to be optimized together with others and are set to 10 initially, meaning all loop closure constraints are initially activated.

Switch functions alone are apparently not enough to attain what is intended. The optimization module will simply drive all switch function costs to nearly 0 as a result of attempting to minimizing the overall cost. Introducing a penalty cost for each switch variables could counter this phenomenon. Penalty costs are to keep switch variables to the initial value and in the form of prior constraints:

$$\left|\right|{\xi }_{ij}-s_{ij}{\left|\right|}_{\mathsf{\Xi }}$$

(83)

where $\mathsf{\Xi }$, the covariance matrix for the penalty cost, is empirically set to ${20}^2$.

Switch functions and prior constraints together enable the optimization unit to winnow out false loops, and the principle behind it is to tap into the knowledge that there’s a certain amount of inconsistency between false and true loops and between false ones themselves in all likelihood while true loops tend to be in agreement with each other, and so what the proposed mechanism does essentially is to always slash the impact of loop closures that are incongruous with other loops.

8. Implementation Details and Experiments

8.1. Implementation Details

The navigation system’s software is programmed in C++ with recourse to ceres-solver on ROS (Robot Operating System).

The system runs on a standard central processing unit (Intel${}^{\circledR }$ NUC Kit NUC7i7BNH, Intel${}^{\circledR }$ Core™ i7-7567U Processor, 16 GB RAM). The suite of sensors is rigidly installed on top of the vehicle and collects data as it is moving.

Figure 4, Figure 5 and Figure 6 are pictures of the sensors employed in the system.

Figure 7 is the sensor suite with the CPU.

Figure 8 is pictures of the vehicle used.

8.2. Experiments

We conducted three large-scale outdoor experiments each of which covers over 1 kilometer by collecting and saving data into a bag file for later analysis. We examined the performance of VI-SLAM (visual-inertial SLAM) and VIMO (the proposed visual-inertial-magnetic navigation system) by running them on recorded datasets to glean quantitative results.

Figure 9, Figure 10 and Figure 11 are three experiments on EUROC datasets “MH 01 easy”, “MH 03 medium” and “MH 05 difficult” for VI-SLAM and OKVIS. VIMO is not compared with them, since the public datasets do not contain magnetic measurements.

Figure 9, Figure 10 and Figure 11 display estimated trajectories and absolute pose errors by VI-SLAM and OKVIS for comparison.

Figure 12, Figure 13 and Figure 14 are comparisons of 3 experiments together with error analysis. Manifest improvement shown by VIMO on VI-SLAM in positioning accuracy is validated thought these comparisons.

As is illustrated by the pose error map in Figure 9, the absolute pose error by VI-SLAM is ($-0.046$ (m) to $-0.606$ (m)) compared to that by OKVIS ($-0.051$ (m) to $-1.184$ (m)).

In Figure 10, for the dataset "MH 03 medium", absolute pose error is dragged down from $-1.714$ (m) by OKVIS to $-1.452$ (m) by VI-SLAM.

In Figure 11, since the dataset is labeled ’difficult’, the accuracy has fallen for either system, with $-2.102$ (m) by OKVIS and $-1.686$ (m) by VI-SLAM. An absolute decrease of $0.416$ (m) in error is shown.

Table 1. Error description of the experiment on EUROC dataset MH 01 easy.

Error Terms (m)	Max	Mean	Median	RMSE	StD
OKVIS [21]	1.1844	0.632254	0.623834	0.666927	0.212242
VI-SLAM	0.606102	0.28583	0.260383	0.314019	0.130034

,

Table 2. Error description of the experiment on EUROC dataset MH 03 medium.

Error Terms (m)	Max	Mean	Median	RMSE	StD
OKVIS [21]	1.71401	0.687552	0.623768	0.756585	0.315741
VI-SLAM	1.45192	0.597207	0.534837	0.656985	0.273813

and

Table 3. Error description of the experiment on EUROC dataset MH 05 difficult.

Error Terms (m)	Max	Mean	Median	RMSE	StD
OKVIS [21]	2.10193	0.704724	0.661593	0.786519	0.34925
VI-SLAM	1.68566	0.498092	0.345175	0.599893	0.33433

show differences in accuracy between OKVIS [21] and VI-SLAM for EUROC datasets. It is shown that VI-SLAM generally outperforms OKVIS.

In Table 1, the root square mean error by VI-SLAM (0.314019 m) and that error by OKVIS is (0.623834 m).

In Table 2, all error terms except for root mean square error are smaller by VI-SLAM (Max: 1.45192 m, Mean: 0.597207 m, Median: 0.534837 m, RMSE: 0.656985 m, StD: 0.273813 m ) than by OKVIS (Max: 1.71401 m, Mean: 0.687552 m, Median: 0.623768 m, RMSE: 0.756585 m, StD: 0.315741 m).

In Table 3, VI-SLAM (Max: 1.68566 m, Mean: 0.498092 m, Median: 0.345175 m, RMSE: 0.599893 m, StD: 0.33433 m) outperforms OKVIS (Max: 2.10193 m, Mean:0.704724 m, Median: 0.661593 m, RMSE: 0.786519 m, StD: 0.34925 m) for all error terms.

In Figure 12a, the trajectory estimated by VI-SLAM (Max: 14.7041 m, Mean: 7.0558 m, Median: 7.2115 m, RMSE: 7.6135 m, StD: 2.8604 m) deviates greatly from the groundtruth thanks to a sudden change in the vehicle’s direction, and the error in yawing has since remained, causing it to extend away from the groundtruth, whereas for VIMO (Max: 7.5324 m, Mean: 3.4541 m, Median: 2.8337 m, RMSE: 3.8741 m, StD: 1.7543 m) that issue is so effectively ameliorated that not only the heading estimation is corrected but the overall absolute pose error is better constrained.

In Figure 13a, the contrast becomes even more manifest. VI-SLAM’s trajectory (Max: 21.8827 m, Mean: 11.0507 m, Median: 11.8128 m, RMSE: 12.4916 m, StD: 5.8243 m) goes wildly away from the groundtruth in the wake of the vehicle turning around time after time. By comparison, the efficacy of fusing magnetic information is accentuated. VIMO (Max: 5.0943 m, Mean: 2.3737 m, Median: 1.9370 m, RMSE: 2.7394 m, StD: 1.3673 m) seems impervious to veering, generating much smaller absolute pose error than it would otherwise do.

Figure 14a implies a similar phenomenon. The difference in accuracy between the two begins to build up after the vehicle corners, and the error in VI-SLAM’s estimation (Max: 12.1847 m, Mean: 6.7346 m, Median: 6.7127 m, RMSE: 7.3614 m, StD: 2.9723 m) continues growing larger while VIMO’s (Max: 10.6441 m, Mean: 6.2678 m, Median: 6.5150 m, RMSE: 6.6390 m, StD: 2.1889 m) keeps nearly on groudtruth.

Figure 15 compares VIMO with OKVIS [21], a state-of-art method. Improvement in performance is demonstrated.

Another phenomenon warranting consideration is that for VI-SLAM the absolute pose error is larger (Max: 21.8827 m, Mean: 11.0507 m, Median: 11.8128 m, RMSE: 12.4916 m, StD: 5.8243 m) in experiment 2 (Figure 13) than in other experiments, while for VIMO it is quite the contrary. We conjures that it is because the shorter the vehicle travels along a straight line, the lesser error should have been incurred if it were not for accretion in yawing estimation, and since VIMO is immune to yawing deviation it is able to achieve much higher performance, which, again, brings to the fore the significance of magnetic information and the superiority of VIMO.

Various error terms, including max absolute pose error, mean, median, min, root mean squared error, standard deviation of errors, are arranged in

Table 4. Error description of Experiment 1.

Error Terms (m)	Max	Mean	Median	RMSE	StD
OKVIS [21]	24.7357	12.1936	11.3733	16.3864	5.3875
VI-SLAM	14.7041	7.0558	7.2115	7.6135	2.8604
VIMO	7.5324	3.4541	2.8337	3.8741	1.7543

,

Table 5. Error description of Experiment 2.

Error Terms(m)	Max	Mean	Median	RMSE	StD
OKVIS [21]	26.4495	15.8417	16.7346	19.9318	8.7485
VI-SLAM	21.8827	11.0507	11.8128	12.4916	5.8243
VIMO	5.0943	2.3737	1.9370	2.7394	1.3673

and

Table 6. Error description of Experiment 3.

Error Terms (m)	Max	Mean	Median	RMSE	StD
OKVIS [21]	20.8938	10.6794	9.2471		5.2168
VI-SLAM	12.1847	6.7346	6.7127	7.3614	2.9723
VIMO	10.6441	6.2678	6.5150	6.6390	2.1889

. Quantitative comparisons between VI-SLAM and VIMO drawn from the tables reveal the potency of fusing magnetic information into the system.

Table 4, Table 5 and Table 6 shows differences in accuracy among OKVIS [21], VI-SLAM and VIMO. As they demonstrate, both VI-SLAM and VIMO are able to achieve better results than OKVIS do for every error item. In Table 4, the root square mean error for VI-SLAM (7.6135 m) is about half that for OKVIS (16.3864 m), and VIMO makes it even lower (3.8741 m). In Table 5, the difference between VI-SLAM (12.4916 m) and OKVIS (19.9318 m) is not so pronounced as in Table 4, but for VIMO the error is nearly 20 times smaller (2.7394).

9. Conclusions

This paper presents a visual-inertial-magnetic navigation system with the originality of exploiting EMF as yaw observation. Respective measurement models are presented in Section 4. Section 5 provides an overview of the system’s structure. We present mathematic fundamentals and theories concerned with visual-inertial-magnetic initialization and non-linear optimization in Section 6 and Section 7, together with other algorithmic details. Lastly, we demonstrate the validity and superiority of our system over visual-inertial-only ones through 3 outdoor large-scale experiments with their error analysis.

According to Section 8.2, VIMO performs localization more accurately than both VI-SLAM and state-of-the-art methods such as OKVIS, slashing errors by half or more (from 16.3864 m to 1.7543 m in Table 4; 19.9318 m to 2.7394 m Table 5; 11.8855 m to 6.6390 m Table 6 in terms of root mean square error).

The most significant implication of the proposed system is that it opens up new vistas for the development of navigation systems with a new combination of sensors and new ways of information fusion and state estimation.

Future research shall follow the line of designing installation mechanisms that can blot out as much magnetic interference as possible, making better use of magnetometers for initialization (for example applying magnetic measurements to the least squares problem in initialization for more accurate estimation), further improving the system’s overall performance.