MAV Localization in Large-Scale Environments: A Decoupled Optimization/Filtering Approach

Abstract

Developing new sensor fusion algorithms has become indispensable to tackle the daunting problem of GPS-aided micro aerial vehicle (MAV) localization in large-scale landscapes. Sensor fusion should guarantee high-accuracy estimation with the least amount of system delay. Towards this goal, we propose a linear optimal state estimation approach for the MAV to avoid complicated and high-latency calculations and an immediate metric-scale recovery paradigm that uses low-rate noisy GPS measurements when available. Our proposed strategy shows how the vision sensor can quickly bootstrap a pose that has been arbitrarily scaled and recovered from various drifts that affect vision-based algorithms. We can consider the camera as a “black-box” pose estimator thanks to our proposed optimization/filtering-based methodology. This maintains the sensor fusion algorithm’s computational complexity and makes it suitable for MAV’s long-term operations in expansive areas. Due to the limited global tracking and localization data from the GPS sensors, our proposal on MAV’s localization solution considers the sensor measurement uncertainty constraints under such circumstances. Extensive quantitative and qualitative analyses utilizing real-world and large-scale MAV sequences demonstrate the higher performance of our technique in comparison to most recent state-of-the-art algorithms in terms of trajectory estimation accuracy and system latency.

1. Introduction

Robust localization of micro aerial vehicles (MAVs) in uncharted large-scale areas can rely on complementary data gathered by many sensor modalities. The study of simultaneous localization and mapping (SLAM), primarily used for MAV navigation in expansive and dynamic settings, may be enriched and expanded by using multi-modal datasets [1]. These settings have certain traits, such as the dynamic range of the scene’s object intensities. For instance, mapping a small interior space with adequate illumination might be of more outstanding quality than mapping a rural area at night with heavy rain, wind, and fog (outdoors dynamic environment). The benefits of multimodal approaches become apparent when systems rely on sensors with a high dynamic range and strong sensing capabilities, such as event cameras, LiDARs, or radars, or typical inexpensive cameras fused with other sensor modalities such as the inertial measurement units (IMUs) and GPS sensors. These multimodal approaches can indeed fill some lack of data during scene mapping and MAV localization.

Towards this aim, we develop a trustworthy (quick and precise) localization solution that utilizes information from three sensor modalities: camera frame data, IMU measurements, and GPS readings. Nevertheless, the GPS sensor readings are consistently slower and noisier than those from the IMU or camera modules, and they frequently experience signal loss in GPS-restricted locations. Therefore, a localization system that depends on GPS data must perform effectively when GPS readings are lost.

Visual-inertial odometry (VIO) is one of the most mature and well-established approaches in the localization field [2,3,4]. Efficient visual odometry can be achieved using a high-quality perception of the surroundings. Sensors performing this perception task can differ in their nature of data collection. On the one hand, the most common visual odometry sensors are cameras such as RGB cameras [5], event cameras [6], and RGB-D cameras [7]. On the other hand, using the LiDAR sensor [8] can provide point clouds and a GPS sensor [9,10] can locate the MAV using satellite signals triangulation, as represented in Figure 1.

The accuracy of the state estimation process relies on an error-state extended Kalman filter (ES-EKF) and the bootstrapping quality of its states. A well-established IMU-based state estimator initialization technique was discussed in [5]. In this bootstrapping method, the global metric scale of the trajectory and the IMU-camera gravity alignment is optimized using a specific amount of IMU readings preintegration combined with an initial up-to-scale trajectory estimated using the camera only. This bootstrapping process is prone to failure due to insufficient IMU excitation, especially when the MAV navigates in a planar terrain.

The MAV should contain a localization system that continually calculates the pose with high accuracy and low latency during search and rescue missions, for instance. The MAV is equipped with restricted resources regarding the data processing unit and the limited power source capacity for long-term navigation operations in large-scale situations. In light of this, the state estimate approach should consistently have low computational complexity and resist sensor readings that deviate from the norm.

Our work’s main contribution to tackle the aforementioned challenges is three-fold:

-

In the case of state estimator initialization failure, we propose a unique instant bootstrapping technique based on continuous-time manifold optimization via pose graph optimization (PGO) and range factors, which depends on low-rate GPS signals.

-

A closed-form estimation method without nonlinear optimization during IMU/CAM fusion produces a reduced system latency with constant CPU computing complexity. The mathematical modeling of a linear ES-EKF with a precise and quick gyroscope integration strategy accounts for the simplicity of our proposed localization solution.

-

The EuRoC benchmark [12], for MAV localization assessment in indoor environments, and the Fast Flight dataset [11], for large-scale outdoor environments, are two real-world publicly available benchmarks on which our IMU/GPS-CAM fusion system has been thoroughly tested. With thorough ablation investigations into the role of each sensor modality in the overall accuracy of the state estimation process, the assessment is conducted using the most recent state-of-the-art visual-inertial odometry methodologies.

2. Related Work

2.1. Sensor Fusion

Figure 2 presents a global overview of the current state-of-the-art approaches for localization. The ability to continually estimate the robot’s ego-motion (position and orientation) over time is a significant difficulty in autonomous navigation, path planning, object tracking, and collision avoidance platforms [13]. The Global Positioning System (GPS) is a well-known localization method applied to several autonomous system domains. One kind of global navigation satellite system (GNSS) is GPS [10]. GPS is used as a self-localization source, such as for MAVs security applications, and gives any user with a GPS receiver positional information with meter-level precision. The satellite signal blockage, high noise levels, multipath effects, and other issues with GPS, on the other hand, make it a less trustworthy alternative sensor for self-localization modules. However, real-time kinematic (RTK) and precise point positioning (PPP) [9], two GPS technologies that are rapidly developing, can provide locations with decimeter- or centimeter-level precision.

The effectiveness of GPS satellite signals heavily depends on the surrounding environment; it works best in locations with clear skies and is ineffective for inside navigation since walls and other obstacles impede it [14]. This makes the GPS module an unsuitable primary sensor for reliable autonomous vehicle localization under adverse weather and environmental conditions. Hence, the fusion of GPS signals with other inertial and/or visual sensors is indispensable for a reliable localization solution, especially in such environments. The state-of-the-art sensor fusion systems are differentiated into two prominent families: loosely- [15], and tightly-coupled [16] fusion strategies. In loosely-coupled fusion, the camera frames for pose estimation are processed as a black-box. A filter or an optimization model is developed to fuse the arbitrary-scaled poses from the visual sensor with the noisy metric-scaled re-integrated IMU readings [17].

On the contrary, in the tightly coupled approach, scene information from the visual sensor is fused with the IMU measurements (linear accelerations and angular velocities) using a fusion filter or an optimization model that estimates the metric-scaled pose, visual odometry scale factor, IMU biases, and visual drift between the IMU-camera inertial frames. One of the prominent advantages of a tightly-coupled fusion scheme is that it can estimate accurate scene information to reconstruct a precise scene map, along with providing the SLAM system with high confidence in loop closure during re-localization situations.

2.2. Fusion Strategies

The two sensor fusion strategies (loosely and tightly coupled) have two main execution techniques: filter-based and optimization-based execution. Some filter-based state-of-the-art approaches are deterministic, such as MSCKF [18], S-MSCKF [11], S-UKF-LG/S-IEKF [19], and ROVIO [20]. At the same time, alternative strategies can be based on nondeterministic filters such as particle filters [21].

Optimization-based methods such as VINS-Mono [22], OKVIS [23], ORB-SLAM [24], and BASALT [25], can be deterministic or non-deterministic based on the optimization strategy and the convergence constraints. The estimation and robustness of visual localization frameworks have advanced significantly in recent decades, and this development may be furthered by tightly integrating visual and inertial data. Most methods integrate data utilizing optimization methods or filtering-based procedures.

Filtering approaches are ideally suited to real-time applications [26,27], which is the main emphasis of this study. In contrast, optimization-based methods are more precise but often have a more extensive processing complexity. The observability-constrained technique addresses the consistency issue, a shortcoming of traditional VIO filter-based algorithms [28]. The EKF/MSCKF and its cutting-edge variations are among the most widely used solutions because they effectively balance accuracy and computational complexity.

A recent study [29] shows that if the air mass’s random character is considered, the EKF system states of an MAV are observable. The drag and lift forces on the MAV will directly impact the projected pose and velocity due to the nature of air mass randomization. To make an online update for the uncertainties brought on by these random effects on the precise position of the sensors’ reference frames, we contribute with a visual drift augmentation technique to our EKF measurement model. The EKF’s ability to tolerate significant disturbances in the MAV’s velocity state variable and still converge to the undisturbed estimates is what we target.

2.3. Visual Odometry

The main objective of a visual odometry solution is to perform an accurate and precise localization of the robot (ground or aerial vehicle) to estimate its pose during the navigation task. Estimated poses can be on either discrete- or continuous-time manifolds. Cioffi et al. [30] studied the reliability of the estimated poses on both manifolds using IMU/Visual/GPS sensors. They came to an important conclusion: similar results are produced by the two representations when the camera and IMU are time-synchronized.

In [13], the sliding window pose-graph optimization of the most recent robot states uses global position data with poses predicted by a VIO method. Like [15], pose-graph optimization employs an independent VIO technique to generate pose estimations fused with GPS data. In contrast to [13], the pose-graph in [15] includes an extra node representing the local coordinate frame’s origin to confine the absolute orientation. However, these methods are loosely connected, meaning that a separate VIO algorithm generates the relative pose estimations. Inspired by [13,15], we present a loosely coupled strategy that considers the correlations between all measures by including them in a hybrid optimization and filtering problem.

It is demonstrated in [23] that considering all measurement correlations is essential for high-precision estimations in the visual-inertial situation. A tightly coupled sliding window optimization for visual and inertial data with loosely connected GPS refinement is presented in [14]. The GPS readings are given the same timestamp as the temporally nearest image to be included in the sliding window because it is believed that they would only be accessible at low rates. As opposed to [14], we efficiently compute the global positional factors by closely coupling the global position measurements using the Runge–Kutta 4th-order gyroscope preintegration scheme [31]. This enables the sliding window to incorporate numerous global parameters and each keyframe with barely any additional processing load.

2.4. Methodology Background

We highlight the methodology that inspires our study in blue-dashed rectangles in Figure 2. Where the loosely coupled fusion strategy [32] is adopted to keep constant computational complexity for real-time performance, along with adding a reset mode for the framework, as discussed in [33] as well as an online IMU-camera extrinsic calibration paradigm [4]. Integrating the IMU/GPS readings with the global shutter visual sensor monocular frames raises our localization solution’s accuracy level, leveraging the MAV’s inertial and global localization information.

Pushing the limits of the extended Kalman filter to raise the robustness of our localization solution towards a resilient system, we leverage the high accuracy of the optimization to initialize the filter pose states using a novel instant approach utilizing the low-rate noisy GPS readings when available. Sensor fusion on continuous-time (CT) manifolds, such as B-splines [34], suffers from a high execution complexity, especially with the time derivatives of high-order manifolds for integrating the IMU measurements in the estimation process. Hence, in our novel method, we avoid this dilemma with a simple spline-fitting approach for the GPS readings during the data pre-processing stage.

3. System Architecture

Our core sensor setup consists of an inertial navigation sensor (IMU), a global positioning sensor (GPS), and a monocular camera, as illustrated in Figure 3. The pipeline starts with the data acquisition and pre-processing for the initialization process, as discussed in Section 3.1. The initialization is an optimization-based phase (see Algorithm 1) with a considerably low complexity and processing time whose output is an instant metric-scaled pose estimated from the camera, GPS, and gyroscope readings. Then, an ES-EKF (see Algorithm 2) whose dynamic model is given in Section 3.2, is applied to estimate all the system states, including the MAV’s trajectory, velocity, and a scale factor to recover the initially estimated trajectory in the case of GPS readings loss. Finally, we present the measurement model in Section 3.3 with a novel false pose augmentation paradigm to ensure the observability of all the filter states, as analyzed in Appendix A.

Algorithm 1 Bootstrapping: Pose Graph Optimization and Range Factors

Input: RGB frames (c), camera matrix (${\mathcal{K}}_c$), GPS readings (DT-GPS), IMU readings ($\mathcal{I}$)  Output: Metric-scaled trajectory (${\mathcal{T}}_{vc}[p_v^c,q_v^c]\in SE\left(3\right)$) 1: ${\mathcal{T}}_{vc}^0\Leftarrow \mathrm{KLT}\text{-}\mathrm{VO}(\mathrm{c},{\mathcal{K}}_c)$                 ▹ Arbitrary-scaled pose 2: $p\left(u\right)\Leftarrow \mathrm{spline}\_\mathrm{fit} \left(\mathrm{DT}\text{-}\mathrm{GPS}\right)$               ▹ CT-GPS by Equation (4) 3: $[\varphi ,\theta ,\psi ]\Leftarrow \mathrm{RK}4\left({\mathcal{I}}_{gyro}\left(\omega \right)\right)$                 ▹ Initial orientations 4: whilenotconvergeddo                 ▹ Initial trajectory optimization 5:     ${\mathcal{T}}_{vc}\Leftarrow \mathrm{optimize}({\mathcal{T}}_{vc}^0,p\left(u\right),[\varphi ,\theta ,\psi ])$               ▹ Equation (6) 6: end while

The state representation is a 31-element state vector $\mathcal{X}$:

$$\mathcal{X}={\left[{p_w^i}^{\top }{v_w^i}^{\top }{q_w^i}^{\top }{b_{\omega }}^{\top }{b_a}^{\top }\lambda {p_i^c}^{\top }{q_i^c}^{\top }{p_v^w}^{\top }{q_v^w}^{\top }\right]}^{\top },$$

(1)

where $p_w^i$ is the position of the IMU in the world frame (world frame is a gravity-aligned frame.) $\left(w\right)$, its velocity $v_w^i$, and its attitude rotation quaternion $q_w^i$ describing a rotation from the IMU frame $\left(i\right)$ into the world frame $\left(w\right)$. $b_{\omega }$ and $b_a$ are the gyro and acceleration biases along with the visual odometry scale factor $\lambda$. $R_{\left(q\right)}$ is the quaternion q rotational matrix, g is the gravity vector aligned with the world frame $\left(w\right)$, and $\Omega \left(\omega \right)$ is the quaternion-multiplication matrix of $\omega$.

The IMU/camera calibration states are the rotation from the camera frame into the IMU frame $q_i^c$, and the position of the camera center with regard to the IMU frame $p_i^c$.

Finally, the visual attitude drifts between the black-boxed visual frame (vision frame is the frame to which the camera pose is estimated in the black-box vision framework) $\left(v\right)$ and the world inertial frame $\left(w\right)$ are reflected in $q_v^w$ and the translational ones in $p_v^w$. We assume that all the visual drifts are spatial without any temporal drifts, i.e., the IMU and the camera have synchronized timestamps.

Algorithm 2 End-to-End State Estimation Scheme

Input: IMU readings, initial optimized trajectory ${\mathcal{T}}_{vc}$      Output: FilterStates $\mathcal{X}=\left\{\lambda ,{\mathcal{K}}_i[b_a,b_{\omega }],{\mathcal{T}}_{ic},{\mathcal{T}}_{wv},{\mathcal{T}}_{wi},v_{wi}\right\},\forall T[p,q]\in SE\left(3\right)$ 1: $P,Q_c,\mathcal{R}$_initialization, FilterStates_initialization 2: ErrorStates_initialization=0 3: while imuRead do 4:     Read LastStep (k) P, FilterStates, ErrorStates 5:     Read LastStep (k) IMU (Accel, Gyro) values 6:     Read current (k+1) IMU (Accel, Gyro) values 7:     Step 1: Propagate IMU states                    ▹ Equation (12) 8:     Step 2: Calculate $F_d$ and $Q_d$               ▹ Equations (14) and (16) 9:     Step 3: Compute P state covariance matrix              ▹ Equation (18) 10:    if camRead then 11:         Read current (k+1) CAM ${\mathcal{T}}_{vc}$ values               ▹ Metric-scaled pose 12:         Step 4: Estimate false pose                     ▹ Equation (19) 13:         Step 5: Calculate $\tilde{z}$, H                       ▹ Equation (20) 14:         Step 6: Calculate S, K, ErrorStates $\widehat{\tilde{x}}$, P            ▹ Equations (26) and (27) 15:         Step 7: Update: FilterStates += ErrorStates 16:         Step 8: RESET $\widehat{\tilde{x}}=0$, P                      ▹ Equation (29) 17:     end if 18: end while

The corresponding 28-elements error state vector is defined by:

$$\tilde{x}={\left[{\Delta p_w^i}^{\top }{\Delta v_w^i}^{\top }{\delta {\theta }_w^i}^{\top }{\Delta b_{\omega }}^{\top }{\Delta b_a}^{\top }\Delta \lambda {\Delta p_i^c}^{\top }{\delta {\theta }_i^c}^{\top }{\Delta p_v^w}^{\top }{\delta {\theta }_v^w}^{\top }\right]}^{\top },$$

(2)

as the difference of an estimate $\widehat{x}$ to its quantity x, i.e., $\tilde{x}=x-\widehat{x}$. We apply this to all state variables except the error quaternions, which are defined by:

$$\delta q_y^x=q_y^x\otimes {\widehat{q}}_y^x\approx {\left[\begin{array}{cc}\frac{1}{2}\delta {\theta }_y^x& 1\end{array}\right]}^{\top }.$$

(3)

This error quaternion representation increases the numerical stability of the estimation process and handles the quaternion in its minimal representation [35].

3.1. State Estimator Initialization

An incremental structure from motion (SfM) algorithm [36] is applied to the acquired image frames, whose goal is to retrieve the camera poses and the 3D structure of the scene, based on the five-point algorithm proposed in [37]. ORB features are detected, and the highest quality points are tracked between 10 consecutive frames using the KLT method [38].

To solve the arbitrary-scale problem of the camera trajectory only, we apply an on-manifold cumulative B-spline (https://github.com/AbanobSoliman/B-splines (accessed on 1 October 2022)) interpolation [34] to synthesize a very smooth continuous-time (CT) trajectory in ${\mathbb{R}}^3$ from the low-rate noisy GPS readings.

The matrix form for the cumulative B-spline manifold of order $k=n+1$, where n is the spline degree, is modeled at $t\in [t_i,t_{i+k-1}]$ as:

$$p\left(u\right)=p_i+\sum _{j=1}^{k-1}{\tilde{B}}_j^{\left(k\right)}.{\overline{u}}_j^{\left(k\right)}.d_j^i,$$

(4)

where $p\left(u\right)\in {\mathbb{R}}^3$ is the continuous-time B-spline increment that interpolates k GPS measurements on the normalized unit of time $u\left(t\right):=(t-t_i)/\Delta t_s-P_n$ with $1/\Delta t_s$ denoting the spline generation frequency and $P_n$ being the pose number that contributes to the current spline segment $P_n\in [0,\cdots ,k-1]$. $p_i$ is the initial discrete-time (DT) GPS location measurement at time $t_i$. The term $d_j^i=p_{i+j}-p_{i+j-1}$ is the difference vector between two consecutive DT-GPS readings. The matrix ${\tilde{B}}_j^{\left(k\right)}$ is the cumulative basis blending and ${\overline{u}}_j^{\left(k\right)}$ is the normalized time vector, both of which are defined as:

$$\begin{array}{c}{\tilde{B}}_j^{\left(k\right)}={\tilde{b}}_{j,n}^{\left(k\right)}=\sum _{s=j}^{k-1}{b}_{s,n}^{\left(k\right)},\hfill \\ b_{s,n}^{\left(k\right)}=\frac{C_{k-1}^n}{(k-1)!}\sum _{l=s}^{k-1}{(-1)}^{l-s}{C}_k^{l-s}{(k-1-l)}^{k-1-n},\hfill \\ {\overline{u}}_j^{\left(k\right)}={[u^0,\cdots ,u^{k-1},u^k]}^{\top },u\in [0,\cdots ,1].\hfill \end{array}$$

(5)

Our GPS-IMU aided initialization system comprises two optimization factors: the first is a pose graph optimization (PGO) factor $r^p$ that optimizes the 6-DoF of every pose, whereas the second is a range factor $r^s$ that constraints the translation limits between every two KLT-VO poses. Hence, the metric scale of the visual odometry pose is recovered using the gyroscope and GPS readings, leveraging the high accuracy of the optimization process. An illustrative scheme for the initialization process factor graph is shown in Figure 4.

Level 1’s objective function $L^{p,s}$ is modeled as:

$$L^{p,s}=\underset{{\mathcal{T}}_{wi}}{arg\; min}\left[\sum _{(i,j)}^N\left({\left|\left|r^p(i,j)\right|\right|}_{{\Sigma }_{i,j}^p}^2+{\left|\left|r^s(i,j)\right|\right|}_{{\Sigma }_{i,j}^s}^2\right)\right].$$

(6)

${\Sigma }_{i,j}^p,{\Sigma }_{i,j}^s$ are the information matrices associated with the GPS readings covariance, reflecting the PGO and Range factors noises on the global metric scale estimation process between two RGB-D aligned frames.

Pose Graph Optimization (PGO) factor. The PGO is a 6-DoF factor that controls the relative pose error between two consecutive edges $i,j$ and is formulated as:

$$r^p={\left|\left|\left({\widehat{\mathit{T}}}_i^{-1}{\widehat{\mathit{T}}}_j\right)\ominus \Delta {\mathit{T}}_{ij}^{\omega ,GPS}\right|\right|}_2$$

(7)

where ${\left||.|\right|}_2$ is the L2-norm, ${\widehat{\mathit{T}}}_{i,j}\in SE\left(3\right)$ is the ${\mathcal{T}}_{wi}^0$ estimated from the front-end pipeline at frames $i,j$. The operator ⊖ is the $SE\left(3\right)$ logarithmic map as defined in [39]. The error transformation $\Delta T_{ij}^{\omega ,GPS}[\delta R_{ij}^{\omega },\delta p_{ij}^{GPS}]\in \mathfrak{se}\left(3\right)$, where $\delta p_{ij}^{GPS}=p_j-p_i$ is the CT-GPS measurement increment and $\delta R_{ij}^{\omega }={[\delta \varphi ,\delta \theta ,\delta \psi ]}^{\top }\in \mathfrak{so}\left(3\right)$ is the gyroscope integrated increment $\delta R_{ij}^{\omega }={\int }_{k=i}^j\left({\omega }_k\right).dk$ using the Runge–Kutta 4th order (RK4) integration method [31] between the keyframes i and j.

Range factor. The range factor limits the front-end visual drift and keeps the global metric scale under control within a sensible range defined by the GPS signal and is formulated as:

$$r^s={\left|\left|{||{\widehat{\mathit{t}}}_j-{\widehat{\mathit{t}}}_i||}_2-{||p_j^{GPS}-p_i^{GPS}||}_2\right|\right|}_2$$

(8)

where ${\widehat{\mathit{t}}}_{i,j},p_{i,j}^{GPS}\in {\mathbb{R}}^3$ are the translation vectors of two consecutive front-end (KLT-VO) poses and CT-GPS signals, respectively.

3.2. Dynamic Model

The core state estimation is performed by fusing the RGB camera frames and the IMU reading using an error states extended Kalman filter (ES-EKF). Figure 5 illustrates the inter-sensor extrinsic relation between the IMU/GPS sensors and a monocular camera.

To use the linear states estimator, we assume that the IMU measurements contain a particular bias $b_a\in \mathcal{N}(0,{\sigma }_{ba})$, $b_{\omega }\in \mathcal{N}(0,{\sigma }_{b\omega })$ and a white Gaussian noise $n_a\in \mathcal{N}(0,{\sigma }_a)$, $n_{\omega }\in \mathcal{N}(0,{\sigma }_{\omega })$.

Thus, the real angular velocities $\omega$ and accelerations a in the IMU body frame $\left(i\right)$ can be written as:

$$\begin{array}{ccc}\omega ={\omega }_m-b_{\omega }-n_{\omega }& and& a=a_m-b_a-n_a,\end{array}$$

(9)

where the subscript m denotes the measured value. The dynamics of the non-static biases are modeled as a random process:

$$\begin{array}{cc}\dot{b_{\omega }}=n_{b_{\omega }},& \dot{b_a}=n_{b_a}.\end{array}$$

(10)

The standard deviation ${\sigma }_{b_{\omega }},{\sigma }_{b_a},{\sigma }_w,{\sigma }_a$ values are generally given by the IMU manufacturer’s data in Allan deviation plots. For discrete time steps, it will be applied in the filter. We need to convert these values according to their units:

$$\begin{array}{cc}d{\sigma }_{\omega ,a}^2=\frac{{\sigma }_{\omega ,a}^2}{\nabla t},& d{\sigma }_{b_{\omega ,a}}^2={\sigma }_{b_{\omega ,a}}^2\ast \nabla t.\end{array}$$

(11)

The following differential equations govern IMU state propagation:

$$\begin{array}{c}\dot{p_w^i}=v_w^i,\hfill \\ \dot{v_w^i}=R_{\left(q_w^i\right)}^{\top }(a_m-b_a-n_a)-g,\hfill \\ \dot{q_w^i}=\frac{1}{2}\Omega ({\omega }_m-b_{\omega }-n_{\omega })q_w^i,\hfill \\ \dot{b_{\omega }}=n_{b_{\omega }},\dot{b_a}=n_{b_a},\dot{\lambda }=0,\hfill \\ \dot{p_i^c}=0,\dot{q_i^c}=0,\dot{p_v^w}=0,\dot{q_v^w}=0,\hfill \end{array}$$

(12)

For the quaternion integration inside the ES-EKF, we use the first-order integrator defined in [35] as:

$$\begin{array}{c}\overline{w}=\frac{{\omega }_{k+1}+{\omega }_k}{2},\kappa =\frac{1}{2}.\Omega \left(\overline{\omega }\right).\Delta t,\\ {\widehat{q_w^i}}_{k+1}=[e^{\kappa }+\frac{\Delta t^2}{48}(\Omega \left({\omega }_{k+1}\right).\Omega \left({\omega }_k\right)-\Omega \left({\omega }_k\right).\Omega \left({\omega }_{k+1}\right))].{\widehat{q_w^i}}_k.\end{array}$$

(13)

where the hat term $\widehat{}$ means the estimated value. The exponential term $e^{\kappa }$ is expanded by the Maclaurin series.

The states transition matrix $F_d$ is modeled as:

$$F_d=\left[\begin{array}{cccccc}{I}_{d_3}& \Delta t& A& B& -R_{\left(\widehat{q_w^i}\right)}^{\top }\frac{\Delta t^2}{2}& 0_{3\times 13}\\ 0_3& I_{d_3}& C& D& -R_{\left(\widehat{q_w^i}\right)}^{\top }\Delta t& 0_{3\times 13}\\ 0_3& 0_3& E& F& 0_3& 0_{3\times 13}\\ 0_3& 0_3& 0_3& I_{d_3}& 0_3& 0_{3\times 13}\\ 0_3& 0_3& 0_3& 0_3& I_{d_3}& 0_{3\times 13}\\ 0_{13\times 3}& 0_{13\times 3}& 0_{13\times 3}& 0_{13\times 3}& 0_{13\times 3}& I_{d_{13}}\end{array}\right].$$

(14)

Then, we apply the small-angle approximation for which $\left|\omega \right|\to 0$ apply the de l’Hopital rule and obtain a compact solution for the six matrix blocks $A,B,C,D,E,F$ [35]:

$$\begin{array}{c}A=-R_{\left(\widehat{q_w^i}\right)}^{\top }{⌊\widehat{a}⌋}_{\times }(\frac{\Delta t^2}{2!}-\frac{\Delta t^3}{3!}{⌊\widehat{\omega }⌋}_{\times }+\frac{\Delta t^4}{4!}{⌊\widehat{\omega }⌋}_{\times }^2),\hfill \\ B=-R_{\left(\widehat{q_w^i}\right)}^{\top }{⌊\widehat{a}⌋}_{\times }(\frac{-\Delta t^3}{3!}+\frac{\Delta t^4}{4!}{⌊\widehat{\omega }⌋}_{\times }-\frac{\Delta t^5}{5!}{⌊\widehat{\omega }⌋}_{\times }^2),\hfill \\ C=-R_{\left(\widehat{q_w^i}\right)}^{\top }{⌊\widehat{a}⌋}_{\times }(\Delta t-\frac{\Delta t^2}{2!}{⌊\widehat{\omega }⌋}_{\times }+\frac{\Delta t^3}{3!}{⌊\widehat{\omega }⌋}_{\times }^2),\hfill \\ D=-A,\hfill \\ E=I_{d_3}-\Delta t{⌊\widehat{\omega }⌋}_{\times }+\frac{\Delta t^2}{2!}{⌊\widehat{\omega }⌋}_{\times }^2,\hfill \\ F=-\Delta t+\frac{\Delta t^2}{2!}{⌊\widehat{\omega }⌋}_{\times }-\frac{\Delta t^3}{3!}{⌊\widehat{\omega }⌋}_{\times }^2,\hfill \end{array}$$

(15)

with $\widehat{\omega }={\omega }_m-\widehat{b_{\omega }}$, $\widehat{a}=a_m-\widehat{b_a}$ and ${⌊\widehat{\omega }⌋}_{\times }$, ${⌊\widehat{a}⌋}_{\times }$ the skew-symmetric matrices for IMU readings.

We can now derive the discrete-time input noise covariance matrix $Q_d$ as:

$$Q_d=\underset{\Delta t}{ʃ}{F}_d\left(\tau \right)G_c{Q}_c{G}_c^{\top }{F}_d{\left(\tau \right)}^{\top }d\tau ,$$

(16)

where $Q_c$ is the CT process noise covariance, and $G_c$ is calculated in the form:

$$G_c=\left[\begin{array}{cccc}{0}_3& 0_3& 0_3& 0_3\\ -R_{\left(\widehat{q_w^i}\right)}^{\top }& 0_3& 0_3& 0_3\\ 0_3& 0_3& I_{d_3}& 0_3\\ 0_3& 0_3& 0_3& I_{d_3}\\ 0_3& -I_{d_3}& 0_3& 0_3\\ 0_{13\times 3}& 0_{13\times 3}& 0_{13\times 3}& 0_{13\times 3}\end{array}\right].$$

(17)

The closed-form solution of the complete derivation of the $Q_d$ covariance matrix is given in detail in Appendix B.

Finally, the propagated state covariance matrix computation is defined as:

$$P_{k+1|k}=F_d{P}_{k|k}{F}_d^{\top }+Q_d.$$

(18)

3.3. Measurement Model

The main contribution of our measurement model for an observable ES-EKF is the false relative pose augmentation methodology of the visual drift quaternion state at the previous time step (k) updated with the current camera measurement at a time (k+1) and modeled as:

$$q_v^w\left(k\right)={\widehat{q}}_w^i{\left(k\right)}^{-1}\otimes {\widehat{q}}_i^c{\left(k\right)}^{-1}\otimes q_v^c(k+1).$$

(19)

The camera position measurement model yields the position of the camera with respect to the vision frame $p_v^c$. The error in measurement modeled as ${\tilde{z}}_p$ and linearized as ${\tilde{z}}_{pL}$:

$${\tilde{z}}_p=z_p-{\widehat{z}}_p=p_v^c-R_{\left({\widehat{q}}_v^w\right)}^{\top }({\widehat{p}}_w^i+R_{\left({\widehat{q}}_w^i\right)}^{\top }{\widehat{p}}_i^c)\widehat{\lambda }\dot{=}{\tilde{z}}_{pL}=H_p\tilde{x},$$

(20)

with

$$H_p^{\top }=\left[\begin{array}{c}{R}_{\left({\widehat{q}}_v^w\right)}^{\top }\widehat{\lambda }\\ 0_{3\times 3}\\ -R_{\left({\widehat{q}}_v^w\right)}^{\top }{R}_{\left({\widehat{q}}_w^i\right)}^{\top }{⌊{\widehat{p}}_i^c⌋}_{\times }\widehat{\lambda }\\ 0_{6x3}\\ R_{\left({\widehat{q}}_v^w\right)}^{\top }{R}_{\left({\widehat{q}}_w^i\right)}^{\top }{\widehat{p}}_i^c+R_{\left({\widehat{q}}_v^w\right)}^{\top }{\widehat{p}}_w^i\\ R_{\left({\widehat{q}}_v^w\right)}^{\top }{R}_{\left({\widehat{q}}_w^i\right)}^{\top }\widehat{\lambda }\\ 0_{6x3}\\ -R_{\left({\widehat{q}}_v^w\right)}^{\top }{⌊({\widehat{p}}_w^i+R_{\left({\widehat{q}}_w^i\right)}^{\top }{\widehat{p}}_i^c)\widehat{\lambda }⌋}_{\times }\end{array}\right],$$

(21)

using the definition of the error-quaternion

$$\begin{array}{c}{q}_w^i=\delta q_w^i\otimes {\widehat{q}}_w^i,\\ R_{\left(q_w^i\right)}\approx (I_{d_3}-{⌊\delta {\theta }_w^i⌋}_{\times }).R_{\left(q_w^{\widehat{i}}\right)}.\end{array}$$

(22)

The vision algorithm yields the rotation from the camera frame into the vision frame $q_v^c$. We can model the error measurement as

$${\tilde{z}}_q=z_q-{\widehat{z}}_q=q_i^c\otimes q_w^i\otimes q_v^w\otimes {(q_i^c\otimes {\widehat{q}}_w^i\otimes {\widehat{q}}_v^w)}^{-1}.$$

(23)

Finally, the measurements Jacobian H in $\tilde{z}=H.\tilde{x}$ is calculated based on the method in [33] and can be stacked together in the form

$$\left[\begin{array}{c}{\tilde{z}}_p\\ {\tilde{z}}_q\end{array}\right]=\left[\begin{array}{c}{H}_p\\ \begin{array}{cccccc}{0}_{3x6}& {\tilde{H}}_q^{wi}& 0_{3x10}& {\tilde{H}}_q^{ic}& 0_{3\times 3}& {\tilde{H}}_q^{vw}\end{array}\end{array}\right]\tilde{x}.$$

(24)

with the Jacobian matrices ${\tilde{H}}_q^{xy}$, known as the right Jacobian of $SO\left(3\right)$, and are defined as:

$$\begin{array}{c}{\tilde{H}}_q^{xy}=J_r\left({\theta }_x^y\right)=\underset{\delta \theta \to 0}{lim}\frac{Log(Exp{\left(\theta \right)}^{\ast }\otimes Exp(\theta +\delta \theta ))}{\delta \theta },\\ J_r\left({\theta }_x^y\right)=I_{d_3}-\left(\frac{1-cos∥\delta \theta ∥}{{∥\delta \theta ∥}^2}\right).{⌊\delta {\theta }_x^y⌋}_{\times }+\left(\frac{∥\delta \theta ∥-sin∥\delta \theta ∥}{{∥\delta \theta ∥}^3}\right).{⌊\delta {\theta }_x^y⌋}_{\times }^2.\end{array}$$

(25)

3.4. States Update

To update the framework for the current time step (k+1), we compute the innovation term S, Kalman gain K, and the states correction vector $\widehat{\tilde{x}}$ defined as:

$$\begin{array}{ccc}S=HP{H}^{\top }+\mathcal{R},& K=P{H}^{\top }{S}^{-1},& \widehat{\tilde{x}}=K\tilde{z}\end{array}.$$

(26)

The error state covariance is updated as follows:

$$P_{k+1|k+1}=(I_{d_{28}}-KH)P_{k+1|k}{(I_{d_{28}}-KH)}^{\top }+K\mathcal{R}{K}^{\top },$$

(27)

where ${\mathcal{R}}_{\left[6x6\right]}=diag({\mathcal{R}}_{position},{\mathcal{R}}_{orientation})$ is the measurement noise covariance matrix.

The error quaternion is calculated by (3) to ensure its unit length, and then update the states vector: ${\mathcal{X}}_{k+1}={\mathcal{X}}_k+\widehat{\tilde{x}}$.

For the quaternions state update:

$${\widehat{q}}_{k+1}=\frac{\left[\begin{array}{cccc}1& \frac{1}{2}\delta {\theta }_{k+1}^1& \frac{1}{2}\delta {\theta }_{k+1}^2& \frac{1}{2}\delta {\theta }_{k+1}^3\end{array}\right]\otimes {\widehat{q}}_k}{∥\left[\begin{array}{cccc}1& \frac{1}{2}\delta {\theta }_{k+1}^1& \frac{1}{2}\delta {\theta }_{k+1}^2& \frac{1}{2}\delta {\theta }_{k+1}^3\end{array}\right]\otimes {\widehat{q}}_k∥},$$

(28)

where $\delta {\theta }_{k+1}^i$ is the ith error state of this quaternion.

3.5. Reset Mode

The ES-EKF reset mode is performed by setting $\widehat{\tilde{x}}\leftarrow 0$ and $P\leftarrow G.P.G^{\top }$, where G is the Jacobian matrix defined by

$$\begin{array}{c}G=diag(I_{d_6},J_{r_{wi}},I_{d_{10}},J_{r_{ic}},I_{d_3},J_{r_{vw}}),\\ J_{r_{xy}}=\frac{\partial \delta {\theta }_x^{y^{+}}}{\partial \delta {\theta }_x^y}=I_{d_3}-\frac{1}{2}{⌊\widehat{\delta {\theta }_x^y}⌋}_{{}_{\times }}.\end{array}$$

(29)

4. Experiments

4.1. Setup

An extensive quantitative and qualitative evaluation is carried out to validate all the state estimation process aspects. This thorough performance analysis is run on the EuRoC benchmark [12] for an indoor system global positioning evaluation in low-speed flights and on the Fast Flight dataset [11] for outdoor experimentation at relatively high-speed flights. For a fair comparison, all the pipeline processing stages in both Algorithms 1 and 2 are performed on a 16 GB RAM laptop computer running 64-bit Ubuntu 20.04.3 LTS with AMD(R) Ryzen 7 4800 h × 16 cores 2.9 GHz processor and a Radeon RTX NV166 Renoir graphics card. In

Table 1. Insights into our experiments’ statistical information and sensor settings.

Parameter		EuRoC Benchmark [12]				Fast Flight Dataset [11]
Stats	Total processed sequences	6 (Vicon room)				4 (airport runway)
	Total sequences duration	11.6111 min				8.8867 min
	Total sequences length	411.5425 m				2539.0599 1 m
	Maximum speed	2.3 (m/s)				17.5 (m/s)
Camera	Total processed frames	13,736				21,312
	Frame resolution	752 × 480 pixels				960 × 800 pixels
	Intrinsics $(f_x,f_y,c_x,c_y)$	458.65	457.30	367.22	248.38	606.58	606.73	474.93	402.28
	Distortion $(k_1,k_2,p_1,p_2)$	−0.2834	0.0739	0.0001	0.000018	−0.0147	−0.0058	0.0072	−0.0046
	Camera-IMU $p_i^c$(x,y,z,1) (m)	−0.0216	−0.0647	0.0098	1.0000	0.1058	−0.0177	−0.0089	1.0000
	Camera-IMU $q_i^c$(x,y,z,w) [-]	−0.0077	0.0105	0.7018	0.7123	−1.0000	0.0042	−0.0039	0.0015
	Frame rate	20 (Hz)				40 (Hz)
IMU	Gyroscope noise density (${\sigma }_{n_{\omega }}$)	$1.6968\times {10}^{-4}$ [rad/s/$\sqrt{\mathrm{Hz}}$]				$6.1087\times {10}^{-5}$ [rad/s/$\sqrt{\mathrm{Hz}}$]
	Gyroscope random walk (${\sigma }_{n_{b_{\omega }}}$)	$1.9393\times {10}^{-5}$ [rad/s${}^2$/$\sqrt{\mathrm{Hz}}$]				$9.1548\times {10}^{-5}$ [rad/s${}^2$/$\sqrt{\mathrm{Hz}}$]
	Accelerometer noise density (${\sigma }_{n_a}$)	$2.0000\times {10}^{-3}$ [m/s${}^2$/$\sqrt{\mathrm{Hz}}$]				$1.3734\times {10}^{-3}$ [m/s${}^2$/$\sqrt{\mathrm{Hz}}$]
	Accelerometer random walk (${\sigma }_{n_{b_a}}$)	$3.0000\times {10}^{-3}$ [m/s${}^3$/$\sqrt{\mathrm{Hz}}$]				$2.7468\times {10}^{-3}$ [m/s${}^3$/$\sqrt{\mathrm{Hz}}$]
	Data rate ($1/\Delta t$)	200 (Hz)				200 (Hz)
GPS	Type/operation	Indoors/Vicon system				Outdoors/satellite Triangulation
	Readings	X (m), Y (m), Z (m)				Long. (deg), Lat. (deg), Alt. (m)
	Data rate	1 (Hz) (down-sampled)				5 (Hz)

¹ Denotes the exact value of the total trajectories lengths for all of the sequences of Fast Flight dataset shown on the x axis of Figure 1 (≈2.5 (km)).

, we represent the quantitative insights of our experiment settings regarding the benchmarks statistical data and the sensors parameters in-detail.

The front-end of the pipeline, including both the data acquisition and pre-processing steps, is developed as a Python API that sends the optimization variables to the factor graph implemented in C++ using the Ceres solver [40] to achieve the lowest possible system latency before the state estimation process. The Sparse Normal Cholesky linear solver by the Ceres solver is employed to solve the least-squares convex optimization problem formulated in Equation (6) along with the Levenberg–Marquardt trust region strategy with the automatic differentiation tool for Jacobian calculations. The sparse Schur linear method is applied to utilize the Schur complement for a more robust and fast optimization process. The pipeline’s back-end for the state estimation process is developed entirely in MATLAB (https://github.com/AbanobSoliman/VIO_RGB_IMU (accessed on 30 October 2022)) and all the initialization parameters are given explicitly in

Table 2. The ES-EKF initialization parameters for both the EuRoC and Fast Flight sequences.

Parameter Initialization	EuRoC Benchmark [12]	Fast Flight Dataset [11]
28-element error state vector ($\widehat{\tilde{x}}$)	$0_{28\times 1}$	$0_{28\times 1}$
31-element state vector 1 ($\mathcal{X}$)	${\left(0_{3\times 1}{0}_{3\times 1}{\overline{q}}^{\top }{0}_{3\times 1}{0}_{3\times 1}1{p_i^c}^{\top }{q_i^c}^{\top }{0}_{3\times 1}{\overline{q}}^{\top }\right)}^{\top }$
States propagation covariance (P)	${10}^{-7}\times I_{d_{28}}$	${10}^{-12}\times I_{d_{28}}$
CT process noise covariance 2 ($Q_c$)	$diag(d{\sigma }_{n_a}^2.I_{d_3},d{\sigma }_{n_{b_a}}^2.I_{d_3},d{\sigma }_{n_{\omega }}^2.I_{d_3},d{\sigma }_{n_{b_{\omega }}}^2.I_{d_3})$
Measurement noise covariance ($\mathcal{R}$)	$diag(0.01,0.01,0.03,{10}^{-4},{10}^{-4},{10}^{-4})$

¹$\overline{q}$ denotes the unity quaternion [0,0,0,1]. ² IMU noise density values for each dataset are from Table 1 and discretized using Equation (11).

.

The performance analysis is performed using the two trajectory evaluation metrics: root mean square error (RMSE) for the Fast Flight dataset compared to the GPS trajectory $p_{gps}$, and the RMS absolute trajectory error (ATE) for the EuRoC benchmark compared to the ground truth trajectory $T_{gt}$ provided with Vicon room sequences. The positional RMSE metric for the Fast Flight sequences is chosen because the ground truth GPS trajectories exist with unknown ground truth orientations. However, for EuRoC sequences, we select the RMS ATE metric for two reasons: 1. the Vicon system provides ground truth poses (positions and orientations); and 2. to ensure a fair comparison with the latest state-of-the-art methods based on the same error metric. The two trajectory evaluation metrics are formulated as follows:

$$\mathrm{RMSE}=\sqrt{\frac{1}{n}{\displaystyle \sum _{i=1}^n}{\left|\left|\widehat{p}\left(i\right)-p_{gps}\left(i\right)\right|\right|}^2},\mathrm{ATE}=\sqrt{\frac{1}{n}\sum _{i=1}^n{\left|\right|p(T_{gt}^{-1}\left(i\right).T_{rel}.\widehat{T}\left(i\right))\left|\right|}^2}\left[m\right],$$

(30)

where $\widehat{p}$ is the estimated translation vector of the $\widehat{T}\in SE\left(3\right)$ trajectory; $p(.)$ is the translation vector of the $T\in SE\left(3\right)$ pose; and $T_{rel}$ is rigid-body transformation corresponding to the least-squares solution that maps the $\widehat{T}$ trajectory onto the $T_{gt}$ trajectory calculated by optimization. We set it constant for all sequences that belong to the same benchmark.

4.2. The EuRoC MAV Benchmark

The two main characteristics of the EuRoC MAV sequences are the complex combined 6-DoF motions and the relatively low speeds compared to the Fast Flight sequences. These prominent characteristics allow an accurate evaluation of the ES-EKF marginally stable states, such as the velocity and the visual drift. In

Table 3. Ablation study on the contribution of the GPS sensor on the system accuracy. The latest state-of-the-art (monocular/stereo) VI-SLAM systems are compared to our proposed trajectory initialization (PGO factors) and ES-EKF state estimation methods. Bold denotes the most accurate.

Method		EuRoC Benchmark [12] (RMS ATE [m])						Avg.
		V1-01	V1-02	V1-03	V2-01	V2-02	V2-03
Mono-VI	OKVIS [23]	0.090	0.200	0.240	0.130	0.160	0.290	0.185
	ROVIO [20]	0.100	0.100	0.140	0.120	0.140	0.140	0.123
	VINS-Mono [22]	0.047	0.066	0.180	0.056	0.090	0.244	0.114
	OpenVINS [41]	0.056	0.072	0.069	0.098	0.061	0.286	0.107
	CodeVIO 1 [42]	0.054	0.071	0.068	0.097	0.061	0.275	0.104
	Cioffi et al. 2 [16]	0.034	0.035	0.042	0.026	0.033	0.057	0.038
Stereo-VI	VINS-Fusion [13]	0.076	0.069	0.114	0.066	0.091	0.096	0.085
	BASALT [25]	0.040	0.020	0.030	0.030	0.020	0.050	0.032
	Kimera [43]	0.050	0.110	0.120	0.070	0.100	0.190	0.107
	ORB-SLAM3 [24]	0.038	0.014	0.024	0.032	0.014	0.024	0.024
Mono-(V/I/G) 3	CT (V+I+G) [30]	0.024	0.014	0.011	0.012	0.010	0.010	0.014
	CT (V+G) [30]	0.011	0.013	0.012	0.009	0.008	0.012	0.011
	CT (I+G) [30]	0.062	0.102	0.117	0.112	0.164	0.363	0.153
	DT (V+I+G) [30]	0.016	0.024	0.018	0.009	0.018	0.033	0.020
	DT (V+G) [30]	0.010	0.025	0.024	0.010	0.012	0.029	0.018
	DT (I+G) [30]	0.139	0.137	0.138	0.138	0.138	0.139	0.138
	Ours (PGO)	0.008	0.017 4	0.023 4	0.008	0.022	0.025 4	0.017
	Ours (ES-EKF)	0.009	0.012	0.011	0.010	0.011	0.010	0.011

¹ Denotes the only learning-based baseline in the table and incorporates point clouds using LiDAR. ² Denotes values from the original work with four GPS readings connected to each optimization state. ³ V,I,G: Vision, IMU, and GPS (generated from the Vicon system readings). ⁴ Denotes KLT-VO tracks features in 5 consecutive frames instead of 10 due to the rapid movement of the MAV.

, we report the ATE values as an evaluation parameter for the trajectory estimation accuracy compared to the ground truth. Moreover, Table 3 shows an ablation study that investigates the contribution of the GPS sensor to the overall estimation accuracy, especially for the monocular vision-based optimization methods: ours (PGO) and the recent work of Cioffi et al. [30]. The selection of the six Vicon room sequences from the EuRoC benchmark is because a comparison with an alternative method such as [30] incorporating GPS signals simulated from the Vicon system readings better emphasizes the findings of this ablation study.

A prominent finding of this ablation study is that vision is the most significant type of sensor. In most sequences, the lowest ATE is obtained by fusing the camera trajectory from the vision KLT-based SfM algorithm to a gravity-aligned frame using the noisy simulated GPS data, and adding inertial measurements does not provide a measurable benefit in this case. However, adding the gyroscope measurements to the visual-GPS fusion has led to the least ATE achieved by our PGO model compared to all other discrete-time (DT) methods. Figure 6 and Figure 7 show our trajectory and velocity estimations after incorporating the accelerometer readings in the ES-EKF model, resulting in the lowest achievable errors that can compete with the continuous-time optimization model in [30].

4.3. The Fast Flight Dataset

The main observation, which is validated upon both the EuRoC and Fast Flight sequences (see

Table 4. Ablation study on the effect of the high MAV speed on the accuracy of the filtering approaches compared to optimization approaches. The first sub-section compares monocular (VINS-Mono and Ours) to stereo (OKVIS) optimization-based VI systems. The second sub-section compares stereo filtering-based approaches to our proposed method. Bold denotes the most accurate in each sub-section.

Method	Fast Flight [11] (RMSE (m))				Avg.
	gps5	gps10	gps15	gps175
OKVIS [23]	3.224	4.987	3.985	4.535	4.183
VINS-Mono [22]	5.542	8.753	2.875	3.452	5.156
Ours (PGO)	0.417	0.759	0.180	0.927	0.571
S-MSCKF [11]	4.985	2.751	4.752	7.852	5.085
S-UKF-LG [19]	4.875	2.589	5.128	7.865	5.114
S-IEKF [19]	4.986	2.544	5.124	8.152	5.201
Ours (ES-EKF)	4.751	7.924	7.221	9.488	7.346

and Figure 8), is that for velocities less than 5 (m/s), the monocular loosely coupled ES-EKF can achieve considerably lower estimation errors concerning the other filter- or optimization-based methods. For velocities more than 5 (m/s), our proposed optimization-based initialization scores the lowest RMSE compared to all other methods in comparison in Table 4. On the contrary, the monocular ES-EKF scores the lowest RMSE, especially for velocities more than 10 (m/s), compared to the best-performing Kalman filter stereo model of the S-MSCKF.

Since the maximum achieved velocity of the EuRoC MAV is nearly 2.3 (m/s), the quantitative results in Table 3 further support this conclusion, where our ES-EKF scores the best performance compared to the other state-of-the-art methods. In-depth reasoning for this degraded performance at high speeds (more than 5 (m/s)) can be clarified based on the hardware characteristics of the MAV sensors’ properties, such as the data rate, latency, and noise effects at high speeds. Our optimization-based (PGO) initialization outperforms all other optimization- or filtering-based methods with high-rate visual-inertial sensors.

An insightful overview of the velocity profiles estimated by our ES-EKF is represented in Figure 9. The main conclusion is that the estimated velocity profile during the planar motion of the MAV in the X–Z plane optimally fits the upper and lower bounds of the top speed for each sequence. Towards an in-depth investigation to understand the high perturbations in the estimated velocity when approaching the maximum limit, we plot the velocity error states in the ES-EKF showing a high error at the instances when approaching top speeds due to the strong vibrations in the MAV structure affecting the IMU readings.

The high estimation accuracy of our ES-EKF model compared to GPS readings and the PGO optimization-based initialization process is further verified by the Y axis trajectory estimation in Figure 8. The maximum estimated altitude for all sequences by the ES-EKF is nearly 60 (m), whereas both the GPS readings and the initialization optimizer estimate a maximum altitude of nearly 100 (m). To physically validate which is a more accurate altitude estimation, we took snippets of the scene at a time instance in the exact halfway of all trajectories as shown in Figure 1. We can observe that the MAV is nearly on the same level as the roof of a commercial aircraft hangar, which is in the range of 30 (m) to 66 (m). This observation validates the high estimation accuracy of the altitude using our ES-EKF.

4.4. Real-Time Performance Analysis

The filter-based approaches are more advantageous for real-time onboard applications because they use the CPU more efficiently than the monocular and stereo optimization-based methods. Due to its computationally intensive front-end pipeline for both temporal and stereo matching, OKVIS uses more CPU than VINS-Mono. Additionally, OKVIS’s back-end operates at a speed that is much faster than the set 10 (Hz) rate of VINS-Mono. Approximately 90% of the work in our back-end, ES-EKF, is brought on by the front-end, which includes ORB feature detection, KLT-based tracking, and matching. At 200 (Hz), the filter uses approximately 10% of a core. Our suggested technique offers the maximum estimation frequency, which provides the optimal balance between the precision and computing cost.

Figure 10 contrasts how much CPU time various VIO solutions used on the EuRoC benchmark and the Fast Flight dataset. Since V2-03 has considerable scale drift with S-IEKF and S-UKF-LG techniques and hence has significantly worse accuracy when compared to other methods, the CPU consumption of V2-03 is excluded from the comparison. According to the testing, the ES-EKF achieves the lowest CPU consumption while retaining a similar level of accuracy in comparison with other methods. We notice that the proposed method puts more computing work into the image processing front-end than the tests using the EuRoC dataset. Higher imaging frequency and resolution are one explanation, while Fast Flight results in a shorter feature lifetime, necessitating frequent new feature identification, is another reason.

5. Conclusions

Our work aimed to provide an accurate and computationally inexpensive localization solution during MAVs’ long-term navigation in large-scale environments. We represented a loosely coupled IMU/GPS camera fusion framework with pose failure detection methodology toward this goal. Moreover, we proposed a novel decoupled optimization- and filtering-based sensor fusion technique that achieves a high estimation accuracy and minimum system complexity compared to the other methods in the literature. We used real-world indoor and outdoor settings for the MAV localization studies to validate and test the findings of our proposed method.

The vision-based black-box pose estimation accuracy is first examined in a controlled laboratory Vicon room of the EuRoC benchmark. The outcomes confirmed the system’s reliance on monocular vision. The experiments on EuRoC and Fast Flight sequences have shown remarkable accuracy in the trajectory estimation studies. We also evaluated the proposed scheme in terms of computational complexity, measured by CPU usage, where our monocular-vision optimization/filtering solution outperformed all the competing techniques.

This conclusion enforces our work’s contributions to a reliable (fast and accurate) sensor fusion solution for challenging and large-scale environments. From a future perspective, it will be necessary to comprise situations where GPS sensor constraints, such as the multipath effects on the optimizer. Finally, further generalizing the optimization problem will be necessary to extend the algorithm’s pose estimation capability to include multiple vision sensors (stereo RGB, for instance).