Robocentric visual–inertial odometry

Abstract

In this paper, we propose a novel robocentric formulation of the visual–inertial navigation system (VINS) within a sliding-window filtering framework and design an efficient, lightweight, robocentric visual–inertial odometry (R-VIO) algorithm for consistent motion tracking even in challenging environments using only a monocular camera and a six-axis inertial measurement unit (IMU). The key idea is to deliberately reformulate the VINS with respect to a moving local frame, rather than a fixed global frame of reference as in the standard world-centric VINS, in order to obtain relative motion estimates of higher accuracy for updating global pose. As an immediate advantage of this robocentric formulation, the proposed R-VIO can start from an arbitrary pose, without the need to align the initial orientation with the global gravitational direction. More importantly, we analytically show that the linearized robocentric VINS does not undergo the observability mismatch issue as in the standard world-centric counterparts that has been identified in the literature as the main cause of estimation inconsistency. Furthermore, we investigate in depth the special motions that degrade the performance in the world-centric formulation and show that such degenerate cases can be easily compensated for by the proposed robocentric formulation, without resorting to additional sensors as in the world-centric formulation, thus leading to better robustness. The proposed R-VIO algorithm has been extensively validated through both Monte Carlo simulation and real-world experiments with different sensing platforms navigating in different environments, and shown to achieve better (or competitive at least) performance than the state-of-the-art VINS, in terms of consistency, accuracy, and efficiency.

Keywords

visual–inertial odometry robocentric formulation extended Kalman filter observability analysis estimation consistency

1. Introduction

Enabling high-precision, energy-efficient, and robust motion tracking in 3D space on mobile platforms (e.g., robots) with minimal sensing holds potentially huge implications in many real applications ranging from mobile augmented reality to autonomous driving. To this end, inertial navigation offers a classical 3D localization solution which utilizes an inertial measurement unit (IMU) measuring the three-degree-of-freedom (DOF) angular velocity and 3-DOF linear acceleration of the sensing platform upon which it is rigidly attached. Typically, IMU works at high frequency (e.g., 100–1,000 Hz) which makes it suitable to sense highly dynamic motion, while due to the existence of sensor noise and bias purely integrating IMU measurements may easily result in unusable motion estimates. This necessitates the use of aiding information from at least a single camera to reduce the accumulated inertial navigation drifts, which results in the well-known visual–inertial navigation system (VINS).

Over the past decade, significant progress has been witnessed in the research domain of VINS, including the visual–inertial simultaneous localization and mapping (VI-SLAM) and the visual–inertial odometry (VIO), accordingly many different algorithms have been proposed to date (e.g., Mourikis and Roumeliotis, 2007; Jones and Soatto, 2011; Kelly and Sukhatme, 2011; Li and Mourikis, 2013a; Leutenegger et al., 2015; Usenko et al., 2016; Mur-Artal and Tardós, 2017; Qin et al., 2018, and references therein). However, almost all these algorithms are developed based on the standard world-centric VINS formulation; that is, to directly estimate absolute motion of the sensing platform with respect to a fixed, global frame of reference, such as the most widely used, Earth-centered Earth-fixed (ECEF) or north–east–down (NED) frame. In order to achieve accurate 3D localization such world-centric VINS algorithms usually require a particular initialization procedure to estimate the sensor’s starting pose in the fixed global frame of reference before the navigation, which, however, is hard to guarantee the accuracy in some cases (e.g., quick start, poor vision, or high latency of sensors). In addition, even the extended Kalman filter (EKF)-based VINS algorithms have the advantage of lower computational cost (e.g., Mourikis and Roumeliotis, 2007; Mourikis et al., 2009; Li and Mourikis, 2013a) in comparison with the batch optimization-based approaches whose relinearization incurs higher amount of computation (e.g., Leutenegger et al., 2015; Qin et al., 2018), they may become inconsistent, primarily due to the fact that the EKF linearized system has different observability properties from the corresponding original (nonlinear) system (Huang et al., 2009, 2010; Li and Mourikis, 2013a; Hesch et al., 2014b). To address this issue, the remedies include enforcing the correct observability constraints (Li and Mourikis, 2013a; Huang et al., 2014; Hesch et al., 2014b) or employing an invariant error representation (Zhang et al., 2017). However, one may ask: Do we have to formulate the VINS in a world-centric form? The answer is no. Intuitively, consider how humans navigate: we might not remember the starting pose after traveling a long distance, while knowing well the relative motion within a recent, short time interval. Bearing this in mind, we may relax this fixed global frame of the VINS, instead, choosing a moving local frame as the reference to better estimate the relative motion which can be used for global pose update.

Historically, the usage of sensor-centered formulation for mobile robot localization can be traced back to sonar/laser-based 2D SLAM in indoor environments (Tardós et al., 2002; Castellanos et al., 2007) where the global frame was included as one of the map points being observed from the robot. The outputs of odometry were fused with the sonar/laser-scanner observations via an EKF to estimate robot’s relative motion which was then used to update the global pose of robot and change the local frame of reference for map joining through a composition step, as moving forward. With a similar idea, Civera et al. (2009) used a camera-centered formulation to show the potential of fusing the visual information with the proprioceptive information, such as given the rotational and linear velocity measurements. By performing mapping with respect to the (local) robot frames, these robocentric EKF-based SLAM algorithms were shown to be able to improve estimation consistency. It should be noticed that a different robocentric formulation was recently introduced in an EKF-based VINS (Bloesch et al., 2015, 2017). In this system, by representing the state in the current IMU (or camera) frame, consisting of the IMU navigation states, a set of observed landmarks’ positions, as well as the camera-to-IMU spatial calibration parameters, the visual and inertial measurements were fused in a direct fashion by employing a photometric residual in the EKF update. In contrast to Castellanos et al. (2007) and Civera et al. (2009), this method stuck to estimating the (inverted) transformation between a fixed global frame and the current IMU frame via a standard iterated EKF, due to simply reversing the reference frame of the filter’s states referring to the world-centric formulation. As a result, it did not perform the composition step to shift the frame of reference, however, incurring higher computational cost as it performs iterative linearization on both landmarks (features) and IMU states at each time step when doing EKF update.

In this paper, we introduce a new robocentric formulation of the VINS with respect to a local IMU frame of reference, as illustrated in Figure 1. Specifically, in contrast to Castellanos et al. (2007), Civera et al. (2009), and Bloesch et al. (2015, 2017) which keep the features (or map points) in the state vector and would inevitably encounter the issue of ever-increasing computational cost as more features are observed and included, we devote to an efficient robocentric filtering-based framework, akin to the multi-state constraint Kalman filter (MSCKF) (Mourikis and Roumeliotis, 2007). In the proposed estimator, stochastic cloning (Roumeliotis and Burdick, 2002) is employed so that hundreds of features can be processed simultaneously, instead only a small number of relative poses, that are between consecutive robot locations from which the features are observed, are kept in the state vector, hence significantly reducing the computational cost. More importantly, as the proposed robocentric VIO does not suffer from the observability mismatch issue that exists in the world-centric counterparts, it possesses better consistency. In particular, the main contributions of this paper include the following:

We propose a novel robocentric EKF-VINS formulation by reformulating the VINS problem with respect to a local IMU frame, which treats the global frame as the only “feature” and includes the local gravity (i.e., with respect to the local frame of reference) into the state vector. The local frame of reference is shifted at every image time through a composition step, and the relative motion estimate between the consecutive local frames is used for updating the global pose estimate.

We develop an efficient and robust R-VIO algorithm within a sliding-window filtering framework, where a constant-size window of relative poses, instead of the observed features (or the global poses) by noting that the relative pose is of zero kinematics which avoids the linearization errors introduced by the change of frame of reference, is included in the filter’s state vector and jointly estimated by tightly fusing the camera and IMU measurements in a local frame of reference. As such, a tailored inverse depth-based measurement model is proposed to fully utilize this state configuration, such that a dense connection is established between feature measurements and the state of the R-VIO considering the geometric relation between feature and the poses from which it has been observed. It should be pointed out that even in the degenerate cases (e.g., motionless) this model is still able to fuse the bearing information from the features, which is particularly useful in real applications.

We study in-depth the observability properties of the proposed R-VIO’s system model, which analytically shows that it has an invariant unobservable subspace, that is independent of the R-VIO linearization points, under generic motions. Thus, the resulting linearized robocentric system does not undergo the observability mismatch which has been identified as the main cause of inconsistency (Huang et al., 2010; Li and Mourikis, 2013a; Hesch et al., 2014b). More importantly, from the analysis, the proposed R-VIO is shown to have not only correct unobservable dimension but also constant unobservable directions, thus significantly improving the consistency of estimation. Furthermore, we investigate the unobservable directions under degenerate motions and show that the potential performance degradations existing in the world-centric formulation can easily be compensated for by the proposed R-VIO without using the information from any additional sensor.

We perform extensive tests through both Monte Carlo simulation and real-world experiments running sensor data collected with different mobile platforms from the micro aerial vehicles (MAVs) flying indoor to ground vehicle driving in dynamic traffic environment. To further benefit our community, we open source our code at http://github.com/rpng/R-VIO.

Fig. 1.

World-centric versus robocentric formulation. The fixed global frame, ${G}$ , is replaced by a local robot frame (e.g., the IMU frame), ${R}$ , as the frame of reference of the VINS. Therefore, instead of estimating absolute motion in ${G}$ (blue), the relative motion of higher accuracy (yellow) is estimated with respect to ${R}$ and used for predicting and updating the global pose of robot.

The reminder of the paper is organized as follows: after a brief overview of related work in the next section, we present the proposed R-VIO algorithm in detail in Section 3, which is followed by a detailed observability analysis in Section 4. In Sections 5 and 6, we demonstrate the superior performance of the proposed algorithm through both simulations and real experiments. Finally, Section 7 concludes the work in this paper, as well as suggesting possible future research directions.

2. Related work

As mentioned earlier, the VINS algorithms generally include VI-SLAM (e.g., Lupton and Sukkarieh, 2012; Leutenegger et al., 2015; Qin et al., 2018) and VIO (e.g., Mourikis and Roumeliotis, 2007; Li and Mourikis, 2013a; Forster et al., 2015). The former jointly estimates the feature positions and camera/IMU pose that together form the system state vector, whereas the latter does not include the features in the state vector but utilizes the visual measurements to impose motion constraints for the camera/IMU pose estimation. In general, by performing mapping the VI-SLAM is able to achieve better accuracy from the feature map and/or possible loop closures while incurring higher computational cost compared with the VIO, although different approaches were proposed to address this issue (e.g., Leutenegger et al., 2015; Usenko et al., 2016; Mur-Artal and Tardós, 2017; Qin et al., 2018). While there were also efforts to integrate VIO and SLAM (e.g., Mourikis et al., 2009; Li and Mourikis, 2013b), in this paper we focus on the design of lightweight VIO that can either serve as a stand-alone motion estimator, or as an essential building block for large-scale navigation systems.

There are different schemes available for the VINS to fuse the visual and inertial measurements, which can be broadly categorized into the loosely-coupled and the tightly-coupled. The former processes the visual and inertial measurements separately to infer their own motion constraints which are to be fused later (e.g., Kneip et al., 2011; Weiss and Siegwart, 2011; Indelman et al., 2013). While this kind of method is computationally efficient, the decoupling of the visual and inertial constraints results in information loss. By contrast, the tightly-coupled approaches directly fuse the visual and inertial measurements within a single process and achieve higher accuracy (e.g., Mourikis and Roumeliotis, 2007; Li and Mourikis, 2013a; Forster et al., 2015; Leutenegger et al., 2015; Qin et al., 2018). As the embedded digital computing and sensing technology advance, the tightly-coupled VINS may even be able to run in real time on resource-constrained platforms, such as drones and smart phones, and the tightly-coupled method proposed in this paper is also shown to have such capability.

In particular, there exist two main approaches for tightly-coupled VINS estimation, the batch optimization-based and the EKF-based. Typically, bundle adjustment (BA) (Triggs et al., 1999) is employed by the former to estimate the states involved in the constrains of all the available measurements by solving a nonlinear least-squares problem (e.g., Forster et al., 2015; Leutenegger et al., 2015; Qin et al., 2018). As the iterative linearization of nonlinear measurement models is carried out at each time step, such methods would incur higher computational cost as compared with the EKF-based counterparts (e.g., Mourikis and Roumeliotis, 2007; Li and Mourikis, 2013a). However, as mentioned previously, the standard world-centric EKF-based VINS would suffer from estimation inconsistency which is mainly due to the issue of observability mismatch with the EKF linearization (Huang et al., 2010; Hesch et al., 2014b); while based on world-centric formulation, the observability-constrained (OC)-VINS has been proposed to address this issue (Li and Mourikis, 2013a; Huang et al., 2014; Hesch et al., 2014b). In addition, for the EKF-based method Bloesch et al. (2015, 2017) recently proposed a VINS using a robocentric representation of the filter state including also the features, which followed the traditional EKF-SLAM approach to estimate absolute sensor motion while the evolution of the features in local frame introduced extra linearization errors. By contrast, we, inspired by the robocentric mapping that improved the EKF consistency in 2D SLAM (Castellanos et al., 2007), propose a new robocentric formulation within a sliding-window filtering-based VIO framework and based on that develop the R-VIO algorithm which, in what follows, is shown to have correct observability properties, and therefore improve estimation consistency and accuracy.

3. Estimator design

Consider a mobile platform equipped with an IMU and a single camera navigating in a 3D environment. In contrast to the standard world-centric VINS using a fixed global frame of reference ${G}$ , in the proposed robocentric formulation, the frame ${I}$ affixed to IMU is set to be an immediate, local frame of reference for navigation, termed ${R}$ . As a result, the global frame ${G}$ (or the first local frame of reference, ${R_{0}}$ ) turns into a stationary object from the perspective of ${R}$ ; and during the navigation, ${R}$ can be transferred from one IMU frame to another. In this section, we deliberately reformulate the VINS problem with respect to such a moving local, rather than the fixed global, frame of reference, and present in detail the proposed R-VIO algorithm with a sliding-window filtering framework.

3.1. State vector

The state vector of the proposed robocentric VINS consists of two parts: (i) the global state that maintains the motion information of the starting frame ${G}$ (e.g., ${R_{0}}$ ), and (ii) the IMU state that characterizes the motion from the local frame of reference to the current IMU frame. In particular, at time $t_{τ} \in [t_{k}, t_{k + 1}]$ the state expressed in the local frame of reference, ${R_{k}}$ , is given by¹

\begin{matrix} {{}^{R_{k}}x}_{τ} = {[\begin{matrix} {{}^{R_{k}}x}_{G}^{⊤} & {{}^{R_{k}}x}_{I_{τ}}^{⊤} \end{matrix}]}^{⊤} \\ {{}^{R_{k}}x}_{G} = {[\begin{matrix} {}_{G}^{k}{\bar{q}}^{⊤} & {{}^{R_{k}}p}_{G}^{⊤} & {{}^{R_{k}}g}^{⊤} \end{matrix}]}^{⊤} \\ {{}^{R_{k}}x}_{I_{τ}} = {[\begin{matrix} {}_{k}^{τ}{\bar{q}}^{⊤} & {{}^{R_{k}}p}_{I_{τ}}^{⊤} & v_{I_{τ}}^{⊤} & b_{g_{τ}}^{⊤} & b_{a_{τ}}^{⊤} \end{matrix}]}^{⊤} \end{matrix}

(1)

where ${}_{G}^{k}{\bar{q}}$ is the $4 \times 1$ unit quaternion (Breckenridge, 1979) representing the orientation of ${G}$ in ${R_{k}}$ , and ${{}^{R_{k}}p}_{G}$ is the position of ${G}$ in ${R_{k}}$ ; ${}_{k}^{τ}{\bar{q}}$ and ${{}^{R_{k}}p}_{I_{τ}}$ are the relative rotation and translation from ${R_{k}}$ to the current IMU frame ${I_{τ}}$ ; $v_{I_{τ}}$ is the local velocity expressed in ${I_{τ}}$ , and $b_{g_{τ}}$ and $b_{a_{τ}}$ denote the IMU’s gyroscope and accelerometer biases, respectively. It should be noted that the local gravity, ${}^{R_{k}}g$ , is also included as part of freedom of the proposed system and modeled as a $3 \times 1$ vector of known magnitude (e.g., 9.81). The corresponding error state is then given by

\begin{matrix} {}^{R_{k}}{\tilde{x}}_{τ} = {[\begin{matrix} {}^{R_{k}}{\tilde{x}}_{G}^{⊤} & {}^{R_{k}}{\tilde{x}}_{I_{τ}}^{⊤} \end{matrix}]}^{⊤} \\ {}^{R_{k}}{\tilde{x}}_{G} = {[\begin{matrix} δ θ_{G}^{⊤} & {}^{R_{k}}{\tilde{p}}_{G}^{⊤} & {}^{R_{k}}{\tilde{g}}^{⊤} \end{matrix}]}^{⊤} \\ {}^{R_{k}}{\tilde{x}}_{I_{τ}} = {[\begin{matrix} δ θ_{τ}^{⊤} & {}^{R_{k}}{\tilde{p}}_{I_{τ}}^{⊤} & {\tilde{v}}_{I_{τ}}^{⊤} & {\tilde{b}}_{g_{τ}}^{⊤} & {\tilde{b}}_{a_{τ}}^{⊤} \end{matrix}]}^{⊤} \end{matrix}

(2)

In particular, the error quaternion is defined by $\bar{q} = δ \bar{q} \otimes \hat{\bar{q}}$ :

δ \bar{q} ≃ {[\begin{matrix} \frac{1}{2} δ θ^{⊤} & 1 \end{matrix}]}^{⊤}, C (δ \bar{q}) = I_{3} - ⌊ δ θ \times ⌋

(3)

where ⊗ denotes the quaternion multiplication, $δ \bar{q}$ is the error quaternion associated with the 3-DOF error angle $δ θ$ , and $C (\cdot)$ denotes a $3 \times 3$ rotation matrix with ⌊·×⌋ being the skew-symmetric operator (Trawny and Roumeliotis, 2005).

At time-step k when the corresponding IMU frame, ${I_{k}}$ , becomes the frame of reference (i.e., ${R_{k}}$ ), a window of the relative poses between the last N local frames of reference is included in the state vector, as

\begin{matrix} {\hat{x}}_{k} = {[\begin{matrix} {}^{R_{k}}{\tilde{x}}_{k}^{⊤} & {\hat{w}}_{k}^{⊤} \end{matrix}]}^{⊤} \\ {\hat{w}}_{k} = {[\begin{matrix} {}_{1}^{2}{\hat{\bar{q}}} & {}^{R_{2}}{\hat{p}}_{R_{1}}^{⊤} & \dots & {}_{N - 1}^{N}{\hat{\bar{q}}}^{⊤} & {}^{R_{N - 1}}{\hat{p}}_{R_{N}}^{⊤} \end{matrix}]}^{⊤} \end{matrix}

(4)

where ${}_{i - 1}^{i}{\hat{\bar{q}}}$ and ${}^{R_{i - 1}}{\hat{p}} R_{i}$ represent the relative rotation and translation from ${R_{i - 1}}$ to ${R_{i}}$ , $i = 2, \dots, N$ . To keep a state vector of constant size over time, we manage $w$ in a sliding-window fashion; that is, to marginalize out the oldest relative pose once a new one is included in the window. Accordingly, the augmented error state is given by

\begin{matrix} {\tilde{x}}_{k} = {[\begin{matrix} {}^{R_{k}}{\tilde{x}}_{k}^{⊤} & {\tilde{w}}_{k}^{⊤} \end{matrix}]}^{⊤} \\ {\tilde{w}}_{k} = {[\begin{matrix} δ θ_{2}^{⊤} & {}^{R_{1}}{\tilde{p}}_{R_{2}}^{⊤} & \dots & δ θ_{N}^{⊤} & {}^{R_{N - 1}}{\tilde{p}}_{R_{N}}^{⊤} \end{matrix}]}^{⊤} \end{matrix}

(5)

3.2. Propagation

We first present the motion model for the robocentric state ${{}^{R_{k}}x}_{τ}$ (see (1)), then extend it to the augmented state $x_{τ}$ (see (4)). Note that during the time interval $[t_{k}, t_{k + 1}]$ the global frame is static with respect to the local frame of reference, then ${}^{R_{k}}{\overset{\cdot}{\tilde{x}}}_{G} = 0_{9 \times 1}$ . For the IMU state ${{}^{R_{k}}x}_{I_{τ}}$ , we introduce a locally parameterized kinematic model:

\begin{matrix} {}_{k}^{τ}{\overset{\cdot}{\bar{q}}} = \frac{1}{2} Ω (ω)_{k}^{τ} \bar{q},^{R_{k}} {\overset{\cdot}{p}}_{I_{τ}} = C {(_{k}^{τ} \bar{q})}^{⊤} v_{I_{τ}}, \\ {\overset{\cdot}{v}}_{I_{τ}} =^{τ} a - ⌊ ω \times ⌋ v_{I_{τ}}, {\overset{\cdot}{b}}_{g} = n_{wg}, {\overset{\cdot}{b}}_{a} = n_{wa} \end{matrix}

(6)

where $n_{wg} ~ N (0, σ_{wg}^{2} I_{3})$ and $n_{wa} ~ N (0, σ_{wa}^{2} I_{3})$ are the zero-mean white Gaussian noises that drive the IMU biases, and $ω$ and $a^{τ}$ are the angular velocity and linear acceleration expressed in ${I_{τ}}$ , respectively. In addition, for $ω = [ω_{x}, ω_{y}, ω_{z}]^{⊤}$ , we have

Ω (ω) = [\begin{matrix} - ⌊ ω \times ⌋ & ω \\ - ω^{⊤} & 1 \end{matrix}], ⌊ ω \times ⌋ = [\begin{matrix} 0 & - ω_{z} & ω_{y} \\ ω_{z} & 0 & - ω_{x} \\ - ω_{y} & ω_{x} & 0 \end{matrix}]

Typically, IMU provides the gyroscope and accelerometer measurements, $ω_{m}$ and $a_{m}$ , expressed in the sensor frame:

ω_{m} = ω + b_{g} + n_{g}

(7)

a_{m} =^{I} a +^{I} g + b_{a} + n_{a}

(8)

where $n_{g} ~ N (0, σ_{g}^{2} I_{3})$ and $n_{a} ~ N (0, σ_{a}^{2} I_{3})$ are the zero-mean white Gaussian sensor noises, and $g^{I}$ characterizes the gravity effects on the sensor frame.

Evaluating (6) at the current state estimate, ${}^{R_{k}}{\hat{x}}_{I_{τ}}$ , yields the following continuous-time IMU state propagation:

\begin{matrix} {}_{k}^{τ}{\overset{\cdot}{\hat{\bar{q}}}} = \frac{1}{2} Ω (\hat{ω}) {}_{k}^{τ}{\hat{\bar{q}}},^{R_{k}} {\overset{\cdot}{\hat{p}}}_{I_{τ}} = {}_{k}^{τ}{C_{\hat{\bar{q}}}^{⊤}} {\hat{v}}_{I_{τ}}, \\ {\overset{\cdot}{\hat{v}}}_{I_{τ}} = \hat{a} -^{τ} \hat{g} - ⌊ \hat{ω} \times ⌋ {\hat{v}}_{I_{τ}}, {\overset{\cdot}{\hat{b}}}_{g} = 0_{3 \times 1}, {\overset{\cdot}{\hat{b}}}_{a} = 0_{3 \times 1} \end{matrix}

(9)

where for brevity we have defined $\hat{ω} = ω_{m} - {\hat{b}}_{g}$ and $\hat{a} = a_{m} - {\hat{b}}_{a}$ , ${}_{k}^{τ}{C_{\hat{\bar{q}}}} = C (_{k}^{τ} \hat{\bar{q}})$ and ${}^{τ}{\hat{g}} =_{k}^{τ} {C_{\hat{\bar{q}}}}^{R_{k}} \hat{g}$ . Accordingly, based on (6) and (9), we have continuous-time robocentric error-state model in the form of

{}^{R_{k}}{\overset{\cdot}{\tilde{x}}}_{τ} = F^{R_{k}} {\tilde{x}}_{τ} + Gn

(10)

where $n = [n_{g}^{⊤} n_{wg}^{⊤} n_{a}^{⊤} n_{wa}^{⊤}]^{⊤}$ denotes the input noise vector, $F$ is the robocentric error-state transition matrix, and $G$ is the input noise Jacobian, respectively:

F = [\begin{matrix} 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} \\ 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} \\ 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} \\ 0_{3} & 0_{3} & 0_{3} & - ⌊ \hat{ω} \times ⌋ & 0_{3} & 0_{3} & - I_{3} & 0_{3} \\ 0_{3} & 0_{3} & 0_{3} & - {}_{k}^{τ}{C_{\hat{\bar{q}}}^{⊤}} ⌊ {\hat{v}}_{I_{τ}} \times ⌋ & 0_{3} & {}_{k}^{τ}{C_{\hat{\bar{q}}}^{⊤}} & 0_{3} & 0_{3} \\ 0_{3} & 0_{3} & -_{k}^{τ} C_{\hat{\bar{q}}} & - ⌊ {}^{τ}{\hat{g}} \times ⌋ & 0_{3} & - ⌊ \hat{ω} \times ⌋ & - ⌊ {\hat{v}}_{I_{τ}} \times ⌋ & - I_{3} \\ 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} \\ 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} \end{matrix}], G = [\begin{matrix} 0_{3} & 0_{3} & 0_{3} & 0_{3} \\ 0_{3} & 0_{3} & 0_{3} & 0_{3} \\ 0_{3} & 0_{3} & 0_{3} & 0_{3} \\ - I_{3} & 0_{3} & 0_{3} & 0_{3} \\ 0_{3} & 0_{3} & 0_{3} & 0_{3} \\ - ⌊ {\hat{v}}_{I_{τ}} \times ⌋ & 0_{3} & - I_{3} & 0_{3} \\ 0_{3} & I_{3} & 0_{3} & 0_{3} \\ 0_{3} & 0_{3} & 0_{3} & I_{3} \end{matrix}]

(11)

For implementing the robocentric EKF, the discrete-time propagation model is needed. First, the IMU state estimate, ${}^{R_{k}}{\hat{x}}_{I_{τ}}$ , is obtained as follows:

(i) ${}_{k}^{τ}{\hat{\bar{q}}}$ is obtained by performing zeroth-order quaternion integration (Trawny and Roumeliotis, 2005) with

\begin{matrix} {}_{k}^{τ}{\hat{\bar{q}}} = \int_{t_{k}}^{t_{τ}} {}_{k}^{s}{\overset{\cdot}{\hat{\bar{q}}}} ds \\ = \int_{t_{k}}^{t_{τ}} \frac{1}{2} Ω (\hat{ω})_{k}^{s} \hat{\bar{q}} ds \\ = \int_{t_{k}}^{t_{τ}} \frac{1}{2} Ω (ω_{m} - {\hat{b}}_{g})_{k}^{s} \hat{\bar{q}} ds \end{matrix}

(12)

(ii) ${}^{R_{k}}{\hat{p}}_{I_{τ}}$ and ${}^{R_{k}}{\hat{v}}_{I_{τ}}$ can be computed respectively using the following integrations with respect to ${R_{k}}$ , as

\begin{matrix} {}^{R_{k}}{\hat{p}}_{I_{τ}} = {\hat{v}}_{I_{k}} Δ t + \int_{t_{k}}^{t_{τ}} \int_{t_{k}}^{s} {}_{k}^{μ}{C_{\hat{\bar{q}}}^{⊤}}^{μ} \hat{a} d μ ds \\ = {\hat{v}}_{I_{k}} Δ t \\ + \int_{t_{k}}^{t_{τ}} \int_{t_{k}}^{s} {}_{k}^{μ}{C_{\hat{\bar{q}}}^{⊤}} (^{μ} a_{m} - {\hat{b}}_{a} -^{μ} \hat{g}) d μ ds \\ = {\hat{v}}_{I_{k}} Δ t - \frac{1}{2} {}^{R_{k}}{\hat{g}} Δ t^{2} \\ + \underset{Δ p_{k, τ}}{\underset{︸}{\int_{t_{k}}^{t_{τ}} \int_{t_{k}}^{s} {}_{k}^{μ}{C_{\hat{\bar{q}}}^{⊤}} (^{μ} a_{m} - {\hat{b}}_{a}) d μ ds}} \end{matrix}

(13)

\begin{matrix} {}^{R_{k}}{\hat{v}}_{I_{τ}} = {\hat{v}}_{I_{k}} + \int_{t_{k}}^{t_{τ}} {}_{k}^{s}{C_{\hat{\bar{q}}}^{⊤}} {}^{s}{\hat{a}} ds \\ = {\hat{v}}_{I_{k}} + \int_{t_{k}}^{t_{τ}} {}_{k}^{s}{C_{\hat{\bar{q}}}^{⊤}} (^{s} a_{m} - {\hat{b}}_{a} -^{s} \hat{g}) ds \\ = {\hat{v}}_{I_{k}} -^{R_{k}} \hat{g} Δ t \\ + \underset{Δ v_{k, τ}}{\underset{︸}{\int_{t_{k}}^{t_{τ}} {}_{k}^{s}{C_{\hat{\bar{q}}}^{⊤}} (^{s} a_{m} - {\hat{b}}_{a}) ds}} \end{matrix}

(14)

where $Δ t = t_{τ} - t_{k}$ ; in particular, the preintegrated inertial terms, $Δ p_{k, τ}$ and $Δ v_{k, τ}$ , can be recursively computed with all the incoming IMU measurements in $[t_{k}, t_{τ}]$ (Eckenhoff et al., 2016); therefore, the estimate of velocity in the current IMU frame is obtained as

{\hat{v}}_{I_{τ}} =_{k}^{τ} {C_{\hat{\bar{q}}}}^{R_{k}} {\hat{v}}_{I_{τ}}

(15)

(iii) bias estimates are assumed constant over $[t_{k}, t_{τ}]$ ; that is, ${\hat{b}}_{g} = {\hat{b}}_{g_{k}}$ and ${\hat{b}}_{a} = {\hat{b}}_{a_{k}}$ for both (i) and (ii).

Then, for covariance propagation, the discrete-time error-state transition matrix $Φ (t_{τ}, t_{τ - 1})$ can be obtained using the forward Euler method over the time interval $[t_{τ - 1}, t_{τ}]$ :

Φ (t_{τ}, t_{τ - 1}) = \exp (F δ t) ≃ I_{24} + F δ t = : Φ_{τ, τ - 1}

(16)

where $δ t = t_{τ} - t_{τ - 1}$ . It results in the covariance propagation from time-step $τ - 1$ to $τ$ :

P_{τ | k} = Φ_{τ, τ - 1} P_{τ - 1 | k} Φ_{τ, τ - 1}^{⊤} + GQ G^{⊤}

(17)

noting that $Q = Diag [δ t σ_{g}^{2} I_{3} δ t σ_{wg}^{2} I_{3} δ t σ_{a}^{2} I_{3} δ t σ_{wa}^{2} I_{3}]$ is the discrete-time input noise covariance matrix, while the detailed derivations can be found in our companion technical report (Huai and Huang, 2018).

For the augmented state, ${\hat{x}}_{k}$ , we consider that the relative pose states in the sliding window are static so that ${\hat{w}}_{τ} = {\hat{w}}_{k}$ . The corresponding augmented covariance matrix, $P_{k}$ , can be partitioned according to the robocentric state and the sliding-window state (see (4)), as

P_{k} = [\begin{matrix} P_{x x_{k}} & P_{x w_{k}} \\ P_{x w_{k}}^{⊤} & P_{w w_{k}} \end{matrix}]

(18)

Thus, the augmented covariance matrix at time $t_{τ}$ is given by

P_{τ | k} = [\begin{matrix} P_{x x_{τ | k}} & Φ_{τ, k} P_{x w_{k}} \\ P_{x w_{k}}^{⊤} Φ_{τ, k}^{⊤} & P_{w w_{k}} \end{matrix}]

(19)

where $P_{x x_{τ | k}}$ can be recursively computed using (17) while the compound error-state transition matrix is obtained as

Φ_{τ, k} = Φ_{τ, τ - 1} \underset{t_{s} \in [t_{k}, t_{τ - 1}]}{Π} Φ_{s, s - 1}

(20)

with initial condition $Φ_{k, k} = I_{24}$ .

3.3. Update

3.3.1. Inverse-depth measurement model

We adopt the inverse depth parameterization (Civera et al., 2008) for the landmarks observed by a monocular camera, while being tailored for the proposed R-VIO. Assuming that a single landmark, $L_{j}$ , has been observed from a set of $n_{j}$ robocentric frames, $R_{j}$ , the measurements of $L_{j}$ expressed in the corresponding $n_{j}$ camera frames, $C_{j}$ , are given by the following perspective projection model in terms of xyz coordinates ( $i \in C_{j}$ ):

z_{j, i} = \frac{1}{z_{i}} [\begin{matrix} x_{i} \\ y_{i} \end{matrix}] + n_{j, i},^{C_{i}} p_{L_{j}} = {[\begin{matrix} x_{i} & y_{i} & z_{i} \end{matrix}]}^{⊤}

(21)

where $n_{j, i} ~ N (0, σ_{im}^{2} I_{2})$ is the additive image noise, and ${{}^{C_{i}}p}_{L_{j}}$ denotes the position of $L_{j}$ in the camera frame ${C_{i}}$ .

Considering the relative poses between the camera frames in $C_{j}$ , ${{}^{C_{i}}p}_{L_{j}}$ can also be expressed as

{{}^{C_{i}}p}_{L_{j}} =_{1}^{i} {\bar{C}}_{\bar{q}}^{C_{1}} p_{L_{j}} +^{i} {\bar{p}}_{1}

(22)

in terms of the following inverse depth parameters:

{{}^{C_{1}}p}_{L_{j}} = \frac{1}{ρ} e (ϕ, ψ), e = [\begin{matrix} \cos ϕ \sin ψ \\ \sin ϕ \\ \cos ϕ \cos ψ \end{matrix}]

(23)

where ${{}^{C_{1}}p}_{L_{j}}$ is the position of $L_{j}$ in the first camera frame of $C_{j}$ formed by $e$ (i.e., the directional vector of ${{}^{C_{1}}p}_{L_{j}}$ with $ϕ$ and $ψ$ the elevation and azimuth expressed in ${C_{1}}$ ) and $ρ$ the inverse depth along $e$ . In particular, the relative poses between ${C_{1}}$ and ${C_{i}}$ , $i = 2, \dots, n_{j}$ , are expressed using the camera-to-IMU calibration parameters, ${_{I}^{C} \bar{q},^{C} p_{I}}$ , as

{}_{1}^{i}{\bar{C}}_{\bar{q}} = {}_{I}^{C}{C_{\bar{q}}} {}_{1}^{i}{C_{\bar{q}}} {}_{C}^{I}{C_{\bar{q}}}

(24)

{}^{i}{\bar{p}}_{1} =_{I}^{C} {C_{\bar{q}}}_{1}^{i} {C_{\bar{q}}}^{I} p_{C} +_{I}^{C} {C_{\bar{q}}}^{R_{i}} p_{R_{1}} +^{C} p_{I}

(25)

where we have used the following identities constructed with the sliding-window state, $w$ :

{{}_{1}^{i}C}_{\bar{q}} = {{}_{i - 1}^{i}C}_{\bar{q}} {{}_{i - 2}^{i - 1}C}_{\bar{q}} \dots_{1}^{2} C_{\bar{q}} = \prod_{n = 2}^{i} {{}_{n - 1}^{n}C}_{\bar{q}}

(26)

{{}^{R_{i}}p}_{R_{1}} = - ({{}_{i - 1}^{i}C}_{\bar{q}} {{}^{R_{i - 1}}p}_{R_{i}} + {{}_{i - 2}^{i}C}_{\bar{q}} {{}^{R_{i - 2}}p}_{R_{i - 1}} + \dots + {{}_{1}^{i}C}_{\bar{q}} {{}^{R_{1}}p}_{R_{2}}) = - \sum_{n = 2}^{i} {{}_{1}^{i}C}_{\bar{q} 1} {{}^{n - 1}C}_{\bar{q}}^{⊤} {{}^{R_{n - 1}}p}_{R_{n}}

(27)

Interestingly, if the landmark is in the distance (i.e., $ρ \to 0$ ), we can normalize (22) by premultiplying $ρ$ to both sides to avoid the potential numerical issues, as

\begin{matrix} ρ^{C_{i}} p_{L_{j}} =_{1}^{i} {\bar{C}}_{\bar{q}} e (ϕ, ψ) + ρ^{i} {\bar{p}}_{1} \\ = : h_{i} (w, ϕ, ψ, ρ) = [\begin{matrix} h_{i, 1} (w, ϕ, ψ, ρ) \\ h_{i, 2} (w, ϕ, ψ, ρ) \\ h_{i, 3} (w, ϕ, ψ, ρ) \end{matrix}] \end{matrix}

(28)

Note that, this equation reserves the perspective geometry of (22) while encompassing two degenerate cases: (i) observing the landmarks at infinity (i.e., $ρ \to 0$ ), and (ii) having small parallax between the camera poses (i.e., $| {}^{i}{\bar{p}}_{1} | \to 0$ ). For both cases, (28) can be approximated by $h_{i} ≃_{1}^{i} {\bar{C}}_{\bar{q}} e (ϕ, ψ)$ , and hence the corresponding measurements can still provide the constraints about the camera’s orientation.

Therefore, we introduce the following inverse depth-based measurement model for the proposed R-VIO:

z_{j, i} = \frac{1}{h_{i, 3}} [\begin{matrix} h_{i, 1} \\ h_{i, 2} \end{matrix}] + n_{j, i}

(29)

Denoting $λ : = [ϕ, ψ, ρ]^{⊤}$ and linearizing (29) at the current state estimate, $\hat{x}$ , and $\hat{λ}$ , we have the linearized measurement residual equation:

r_{j, i} = z_{j, i} - {\hat{z}}_{j, i} ≃ H_{x_{j, i}} \tilde{x} + H_{λ_{j, i}} \tilde{λ} + n_{j, i}

(30)

where the measurement Jacobians are given by

\begin{matrix} H_{x_{j, i}} = H_{p_{j, i}} [\begin{matrix} 0_{3 \times 24} & H_{w_{j, i}} \end{matrix}], H_{λ_{j, i}} = H_{p_{j, i}} H_{in v_{j, i}} \\ H_{p_{j, i}} = \frac{1}{{\hat{h}}_{i, 3}} [\begin{matrix} 1 & 0 & - \frac{{\hat{h}}_{i, 1}}{{\hat{h}}_{i, 3}} \\ 0 & 1 & - \frac{{\hat{h}}_{i, 2}}{{\hat{h}}_{i, 3}} \end{matrix}], \\ H_{in v_{j, i}} = \frac{\partial h_{i}}{\partial \tilde{λ}} \\ = [\begin{matrix} \frac{\partial h_{i}}{\partial {[\tilde{ϕ}, \tilde{ψ}]}^{⊤}} & \frac{\partial h_{i}}{\partial \tilde{ρ}} \end{matrix}] \\ = [\begin{matrix} {}_{1}^{i}{\bar{C}}_{\hat{\bar{q}}} [\begin{matrix} - \sin \hat{ϕ} \sin \hat{ψ} & \cos \hat{ϕ} \cos \hat{ψ} \\ \cos \hat{ϕ} & 0 \\ - \sin \hat{ϕ} \cos \hat{ψ} & - \cos \hat{ϕ} \sin \hat{ψ} \end{matrix}] & {}^{i}{\hat{\bar{p}}}_{1} \end{matrix}], \\ H_{w_{j, i}} = \frac{\partial h_{i}}{\partial \tilde{w}} \\ = [\begin{matrix} \frac{\partial h_{i}}{\partial δ θ_{2}} & \frac{\partial h_{i}}{\partial^{R_{1}} {\tilde{p}}_{R_{2}}} & \dots & \dots & \frac{\partial h_{i}}{\partial δ θ_{N}} & \frac{\partial h_{i}}{\partial^{R_{N - 1}} {\tilde{p}}_{R_{N}}} \end{matrix}], \\ \frac{\partial h_{i}}{\partial δ θ_{n}} =_{I}^{C} {C_{\bar{q}}}_{1}^{i} C_{\hat{\bar{q}}} ⌊ (_{C}^{I} C_{\bar{q}} \hat{e} + {\hat{ρ}}^{I} p_{C} - {\hat{ρ}}^{R_{1}} {\hat{p}}_{R_{n}}) \times ⌋ {{}_{1}^{n}C}_{\hat{\bar{q}}}^{⊤}, \\ \frac{\partial h_{i}}{\partial^{R_{n - 1}} {\tilde{p}}_{R_{n}}} = - {\hat{ρ}}_{I}^{C} {C_{\bar{q}}}_{1}^{i} {C_{\hat{\bar{q}}}}_{1}^{n - 1} C_{\hat{\bar{q}}}^{⊤}, n = 2, \dots, i \leq N \end{matrix}

(31)

Specifically, $H_{x_{j, i}}$ and $H_{λ_{j, i}}$ are the Jacobians with respect to the vectors of state and inverse depth, respectively. Note that through the Jacobian $H_{w_{j, i}}$ each measurement of $L_{j}$ is correlated to a sequence of relative poses in $w$ , building up a dense connection between the measurement and the state, however, without increasing the computational complexity. This is also different from Mourikis and Roumeliotis (2007) where each measurement is only correlated to the global pose from which it is observed. Since an estimate of $λ$ is needed for computing $r_{j, i}$ and $H_{λ_{j, i}}$ , first a local BA is solved for $\hat{λ}$ with the measurements, ${z_{j, i}, i \in C_{j}}$ , and the relative pose estimates, $\hat{w}$ (see Appendix B). After stacking the residuals (see (30)) for all $i \in C_{j}$ , we obtain

r_{j} ≃ H_{x_{j}} \tilde{x} + H_{λ_{j}} \tilde{λ} + n_{j}

(32)

Assuming the measurements obtained from different camera poses are independent, the covariance matrix of $n_{j}$ is hence $R_{j} = σ_{im}^{2} I_{2 n_{j}}$ . As $\hat{x}$ (precisely, $\hat{w}$ ) is used to compute $\hat{λ}$ , the error of inverse depth is correlated with $\tilde{x}$ . In order to be a valid residual for EKF update, (32) is projected to the left nullspace of $H_{λ_{j}}$ (i.e., $O_{λ_{j}}^{⊤} H_{λ_{j}} = 0$ , with $O_{λ_{j}}^{⊤} O_{λ_{j}} = I$ ):

{\bar{r}}_{j} = O_{λ_{j}}^{⊤} r_{j} ≃ O_{λ_{j}}^{⊤} H_{x_{j}} \tilde{x} + O_{λ_{j}}^{⊤} n_{j} = {\bar{H}}_{x_{j}} \tilde{x} + {\bar{n}}_{j}

(33)

In general, $H_{λ_{j}}$ is a $2 n_{j} \times 3$ matrix of full column rank. The nullspace projection of dimension $2 n_{j} - 3$ can be efficiently computed, for example, using the Givens rotations (Golub and Van Loan, 2012), with $O (n_{j}^{2})$ complexity. Since $O_{λ_{j}}$ is unitary, the covariance matrix of ${\bar{n}}_{j}$ becomes

{\bar{R}}_{j} = O_{λ_{j}}^{⊤} R_{j} O_{λ_{j}} = σ_{im}^{2} I_{2 n_{j} - 3}

(34)

At this point, let us examine the special cases where $H_{λ_{j, i}}$ (more precisely, $H_{p_{j, i}}$ or $H_{in v_{j, i}}$ ) may become rank deficient (see (31)), which would affect computing the residual (33). First, if $H_{p_{j, i}}$ becomes rank deficient, then we can find two possible causes about ${\hat{h}}_{i}$ : (i) ${\hat{h}}_{i, 1} ≃ {\hat{h}}_{i, 2} ≃ {\hat{h}}_{i, 3}$ , which implies that the image size should be at least $2 f \times 2 f$ with f the camera focal length, or (ii) ${\hat{h}}_{i, 1} \to 0$ and ${\hat{h}}_{i, 2} \to 0$ , which means that the measurement of $L_{j}$ is close to the principal point of camera image. Second, if $H_{in v_{j, i}}$ is rank deficient, we can also find two possible causes: (iii) $\cos \hat{ϕ} \to 0$ , which implies that we have either infinitely small focal length or infinitely large image size of the camera so that $| \hat{ϕ} | \to π / 2$ can happen, or (iv) $| {}^{i}{\hat{\bar{p}}}_{1} | \to 0$ , which means a small parallax between ${C_{1}}$ and ${C_{i}}$ . Among these causes, (i) is about the selection of the lens, which must be restricted by the size of camera image, and (iii) is too ideal to be realized in the real world; while (ii) and (iv) are common in the vision-based navigation which can be effectively detected by checking the values of pixel measurements and relative pose estimates, respectively. Then, we might reject those measurements that meet (ii) when computing the Jacobians. However, in the case of (iv) (e.g., purely rotating or motionless) since the last column of $H_{λ_{j, i}}$ (and, hence, of $H_{λ_{j}}$ ) approaches zero, we perform the Givens rotations with only the first two columns of $H_{λ_{j}}$ to numerically guarantee a valid nullspace projection (see (33)) and thus the dimension of ${\bar{r}}_{j}$ is increased by 1 (see (34)).

In addition, the Mahalanobis distance is checked for each landmark using all its measurements before the EKF update, serving as the probabilistic outlier rejection:

D_{j} = {\bar{r}}_{j}^{⊤} ({\bar{H}}_{x_{j}} {P \bar{H}}_{x_{j}}^{⊤} + {\bar{R}}_{j})^{- 1} {\bar{r}}_{j} \leq χ_{r, 1 - α}^{2}

(35)

where $χ_{r, 1 - α}^{2}$ is a threshold obtained from the $χ^{2}$ distribution with $r = \dim ({\bar{r}}_{j})$ and $α$ the significance level (e.g., 0.05). If (35) holds, then landmark $L_{j}$ can be accepted as an inlier and used for EKF update. It should be noted that, in addition to the outlier rejection based on vision alone, the state estimate, ${\hat{x}, P}$ , is used as a prior to help identify the outliers. Thus, the measurements that render detrimental residuals given the current state estimate (e.g., may correspond to the features of objects that move slowly or in parallel with the camera in dynamic environments) can be more accurately detected and discarded to further ensure the health of the estimator.

3.3.2. EKF update

Assuming that at time-step $k + 1$ we have the measurements of M landmarks (inliers) to process, thus we can stack the resulting ${\bar{r}}_{j}$ , $j = 1, \dots, M$ , to have

\bar{r} = {\bar{H}}_{x} \tilde{x} + \bar{n}

(36)

of which the dimension $d = \sum_{j = 1}^{M} (2 n_{j} - 3)$ . However, in practice, d could be a large number even if M is small (e.g., $d = 170$ , for 10 landmarks observed from 10 camera poses). To reduce the computational complexity, QR decomposition is applied to (36) to compress the dimension of measurement residual. Note that ${\bar{H}}_{x}$ is rank deficient by the zero columns corresponding to the robocentric states, whereas the non-zero columns corresponding to the relative pose states within the sliding window are linearly independent. Therefore, to save the computational cost the QR decomposition can be applied only to the non-zero part of ${\bar{H}}_{x}$ , as

\begin{matrix} {\bar{H}}_{x} = [\begin{matrix} 0_{d \times 24} & {\bar{H}}_{w} \end{matrix}] \\ = [\begin{matrix} 0_{d \times 24} & [\begin{matrix} Q_{1} & Q_{2} \end{matrix}] [\begin{matrix} {\bar{T}}_{w} \\ 0_{(d - 6 (N - 1)) \times 6 (N - 1)} \end{matrix}] \end{matrix}] \\ = [\begin{matrix} Q_{1} & Q_{2} \end{matrix}] [\begin{matrix} 0_{d \times 24} & [\begin{matrix} {\bar{T}}_{w} \\ 0_{(d - 6 (N - 1)) \times 6 (N - 1)} \end{matrix}] \end{matrix}] \end{matrix}

where $Q_{1}$ and $Q_{2}$ are the unitary matrices of dimension $d \times 6 (N - 1)$ and $d \times (d - 6 (N - 1))$ , respectively, and ${\bar{T}}_{w}$ is an upper triangular matrix of dimension $6 (N - 1)$ . With this definition, (36) yields

\begin{matrix} \bar{r} = [\begin{matrix} Q_{1} & Q_{2} \end{matrix}] [\begin{matrix} 0 & {\bar{T}}_{w} \\ 0 & 0 \end{matrix}] \tilde{x} + \bar{n} \Rightarrow \\ [\begin{matrix} Q_{1}^{⊤} \\ Q_{2}^{⊤} \end{matrix}] \bar{r} = [\begin{matrix} 0 & {\bar{T}}_{w} \\ 0 & 0 \end{matrix}] \tilde{x} + [\begin{matrix} Q_{1}^{⊤} \\ Q_{2}^{⊤} \end{matrix}] \bar{n} \end{matrix}

(37)

for which we can discard the lower $d - 6 (N - 1)$ rows that are only about the measurement noise, but employ the upper $6 (N - 1)$ rows, instead of (36), as the compressed residual:

\overset{⌣}{r} = Q_{1}^{⊤} \bar{r} = [\begin{matrix} 0 & {\bar{T}}_{w} \end{matrix}] \tilde{x} + Q_{1}^{⊤} \bar{n} = {\overset{⌣}{H}}_{x} \tilde{x} + \overset{⌣}{n}

(38)

where $\overset{⌣}{n} = Q_{1}^{⊤} \bar{n}$ is the noise vector with covariance matrix $\overset{⌣}{R} = Q_{1}^{⊤} \bar{R} Q_{1} = σ_{im}^{2} I_{6 (N - 1)}$ . In particular, when we have $d >> 6 (N - 1)$ this can be done using the Givens rotations, with $O (N^{2} d)$ complexity. Based on the above, the standard EKF update is performed according to (Maybeck, 1979)

\begin{matrix} K = {P \overset{⌣}{H}}_{x}^{⊤} ({\overset{⌣}{H}}_{x} {P \overset{⌣}{H}}_{x}^{⊤} + \overset{⌣}{R})^{- 1} \\ {\hat{x}}_{k + 1 | k + 1} = {\hat{x}}_{k + 1 | k} + K \overset{⌣}{r} \\ P_{k + 1 | k + 1} = (I - K {\overset{⌣}{H}}_{x}) P_{k + 1 | k} (I - K {\overset{⌣}{H}}_{x})^{⊤} + K \overset{⌣}{R} K^{⊤} \end{matrix}

3.3.3. State augmentation

To utilize the most accurate, relative motion information for estimation, we employ stochastic state cloning (Roumeliotis and Burdick, 2002). In particular, the state augmentation is carried out once an EKF update is done, for which a copy of the current relative pose estimate, ${_{k}^{k + 1} {\hat{\bar{q}}}_{k + 1 | k + 1}$ , ${}^{R_{k}}{\hat{p}}_{I_{k + 1 | k + 1}}}$ , is appended to the end of the current sliding-window vector, ${\hat{w}}_{k + 1 | k + 1}$ . Accordingly, the covariance matrix is augmented as follows:

\begin{matrix} P_{k + 1 | k + 1} \leftarrow [\begin{matrix} I_{24 + 6 (N - 1)} \\ J \end{matrix}] P_{k + 1 | k + 1} {[\begin{matrix} I_{24 + 6 (N - 1)} \\ J \end{matrix}]}^{⊤}, \\ J = [\begin{matrix} 0_{3 \times 9} & I_{3} & 0_{3} & 0_{3 \times 9} & 0_{3 \times 6 (N - 1)} \\ 0_{3 \times 9} & 0_{3} & I_{3} & 0_{3 \times 9} & 0_{3 \times 6 (N - 1)} \end{matrix}] \end{matrix}

(39)

3.4. Composition

Note that in the proposed robocentric formulation every time when the update is finished, we shift the frame of reference of the VINS. At this point, the current IMU frame ${I_{k + 1}}$ is set as the new frame of reference, ${R_{k + 1}}$ , to replace ${R_{k}}$ . The state vector expressed in ${R_{k + 1}}$ is then obtained as

\begin{matrix} {\hat{x}}_{k + 1} = [\begin{matrix} {}^{R_{k + 1}}{\hat{x}}_{k + 1} \\ {\hat{w}}_{k + 1} \end{matrix}] = [\begin{matrix} {}^{R_{k}}{\hat{x}}_{k + 1} ⊞^{R_{k}} {\hat{x}}_{I_{k + 1}} \\ {\hat{w}}_{k + 1 | k + 1} \end{matrix}] \Rightarrow \\ [\begin{matrix} {}_{G}^{k + 1}{\hat{\bar{q}}} \\ {}^{R_{k + 1}}{\hat{p}}_{G} \\ {}^{R_{k + 1}}{\hat{g}} \\ {}_{k + 1}^{k + 1}{\hat{\bar{q}}} \\ {}^{R_{k + 1}}{\hat{p}}_{R_{k + 1}} \\ {\hat{v}}_{R_{k + 1}} \\ {\hat{b}}_{g_{k + 1}} \\ {\hat{b}}_{a_{k + 1}} \\ {\hat{w}}_{k + 1} \end{matrix}] = [\begin{matrix} {}_{k}^{k + 1}{\hat{\bar{q}}} \otimes_{G}^{k} \hat{\bar{q}} \\ {{}_{k}^{k + 1}C}_{\hat{\bar{q}}} (^{R_{k}} {\hat{p}}_{G} -^{R_{k}} {\hat{p}}_{I_{k + 1}}) \\ {{}_{k}^{k + 1}C}_{\hat{\bar{q}}} {}^{R_{k}}{\hat{g}} \\ {\bar{q}}_{0} \\ 0_{3 \times 1} \\ {\hat{v}}_{I_{k + 1}} \\ {\hat{b}}_{g_{k + 1}} \\ {\hat{b}}_{a_{k + 1}} \\ {\hat{w}}_{k + 1 | k + 1} \end{matrix}] \end{matrix}

(40)

where ${\bar{q}}_{0} = [0, 0, 0, 1]^{⊤}$ and ⊞ denotes the state composition operator. For brevity of presentation the full subscripts of the robocentric states have been omitted. Note that in the IMU state the relative pose is reset to the origin of ${R_{k + 1}}$ , while the velocity and biases evolving in the local IMU frame are not affected by the change of (local) frame of reference. The covariance composition is correspondingly performed using the transformation:

P_{k + 1} = U_{k + 1} P_{k + 1 | k + 1} U_{k + 1}^{⊤}

(41)

\begin{matrix} U_{k + 1} = \frac{\partial {\tilde{x}}_{k + 1}}{\partial {\tilde{x}}_{k + 1 | k + 1}} = [\begin{matrix} V_{k + 1} & 0_{24 \times 6 N} \\ 0_{6 N \times 24} & I_{6 N} \end{matrix}] \end{matrix}

(42)

where $V_{k + 1}$ is the Jacobian with respect to the robocentric state in ${R_{k}}$ :

\begin{matrix} V_{k + 1} = \frac{\partial^{R_{k + 1}} {\tilde{x}}_{k + 1}}{\partial^{R_{k}} {\tilde{x}}_{k + 1 | k + 1}} = \\ [\begin{matrix} {{}_{k}^{k + 1}C}_{\hat{\bar{q}}} & 0_{3} & 0_{3} & I_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} \\ 0_{3} & {{}_{k}^{k + 1}C}_{\hat{\bar{q}}} & 0_{3} & ⌊ {}^{R_{k + 1}}{\hat{p}}_{G} \times ⌋ & -_{k}^{k + 1} C_{\hat{\bar{q}}} & 0_{3} & 0_{3} & 0_{3} \\ 0_{3} & 0_{3} & {{}_{k}^{k + 1}C}_{\hat{\bar{q}}} & ⌊ {}^{R_{k + 1}}{\hat{g}} \times ⌋ & 0_{3} & 0_{3} & 0_{3} & 0_{3} \\ 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} \\ 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} \\ 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & I_{3} & 0_{3} & 0_{3} \\ 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & I_{3} & 0_{3} \\ 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & I_{3} \end{matrix}] \end{matrix}

(43)

The corresponding covariance of the relative pose in the IMU state is also reset to zero; that is, no uncertainty for the robocentric frame of reference itself.

3.5. Initialization

It is important to point out that with the proposed robocentric formulation, the filter initialization becomes simple, because the states are uniformly relative to a local frame of reference and typically start from zero, without the need to align the initial pose with a fixed global frame. In particular, in our implementation: (i) the initial global pose and IMU relative pose are both set to ${{\bar{q}}_{0}, 0_{3 \times 1}}$ , (ii) the initial local gravity is the average of first available accelerometer measurement(s), ${\underline{a}}_{m}$ , before the IMU moves, and (iii) the initial accelerometer bias is obtained by removing standard gravity effects from ${\underline{a}}_{m}$ , while the initial gyroscope bias uses the average of the corresponding stationary measurement(s), ${\underline{ω}}_{m}$ . In addition, the initial covariance subblock for global pose is set to $10^{- 6} I_{6}$ , while for the local gravity and the biases, the corresponding subblocks are set to $P_{0}^{g} = Δ T σ_{a}^{2} I_{3}$ , $P_{0}^{b_{g}} = Δ T σ_{wg}^{2} I_{3}$ , and $P_{0}^{b_{a}} = Δ T σ_{wa}^{2} I_{3}$ , respectively, where $Δ T$ denotes the time length of the initialization. In summary, the main steps of the proposed R-VIO are outlined in Algorithm 1.

Algorithm 1 Robocentric Visual–Inertial Odometry
Input: Camera images and IMU measurements
Output: 6-DOF real-time pose estimates
R-VIO: Initialize the state and covariance with respect to the first local frame of reference, ${R_{0}}$ (e.g., ${I_{0}}$ ), when first available IMU measurement(s) comes in. After that, every time when a camera image is available, do
• Visual tracking: extract features from the image, and perform Kanade–Lucas–Tomasi (KLT) tracking with outlier rejection; record the tracking history of the inliers within the current sliding window.
• Propagation: propagate state and covariance matrix using preintegration with all the IMU measurements starting from last image time.
• Update: for the features whose tracks are complete (i.e., losing track, or reaching the maximum tracking length) compute the inverse-depth measurement model matrices, then do
– EKF update: use the features that have passed the Mahalanobis distance test for EKF update.
– State augmentation: augment state and covariance matrix with the (updated) current relative pose (state and covariance) estimate.
• Composition: shift local frame of reference to the current IMU frame, and update global state and covariance matrix using (updated) current relative pose estimate; reset relative pose estimate of IMU state for next image.

4. Observability analysis

Observability of the system reveals whether the information provided by the sensor measurements is sufficient to estimate the states without ambiguities. In this section, we examine the observability properties of the proposed R-VIO’s system model with the case of a single landmark being observed by a mobile sensing platform which performs arbitrary motion, while the conclusion of analysis can easily be generalized to the case of multiple landmarks. Note that a direct analysis of the observability properties of R-VIO could be cumbersome mainly due to the feature marginalization (see (33)) and state cloning, thus we perform observability analysis by using an EKF-SLAM model having the same observability properties as an EKF-VIO model provided the same linearization points used, which has been shown to be a common practice (see Guo and Roumeliotis, 2013; Li and Mourikis, 2013a; Hesch et al., 2014a,b) in the VINS literature.

To this end, the state vector at time-step k including the position of a single landmark L:

x_{k} = {[\begin{matrix} {{}^{R_{k}}x}_{k}^{⊤} & {{}^{R_{k}}p}_{L}^{⊤} \end{matrix}]}^{⊤}

(44)

and the measurement model (21) (or its inverse-depth form (29)) are employed in what follows. The observability matrix is computed as (Chen et al., 1990)

M = [\begin{matrix} H_{k} \\ ⋮ \\ H_{ℓ} Ψ_{ℓ, k} \\ ⋮ \\ H_{k + m} Ψ_{k + m, k} \end{matrix}]

(45)

where $Ψ_{ℓ, k}$ is the error-state transition matrix from time-step k to $ℓ$ , and $H_{ℓ}$ is the measurement Jacobian corresponding to the observation(s) at time-step $ℓ$ . Each block row is evaluated at ${}^{R_{k}}{\hat{p}}_{L}$ and ${}^{R_{k}}{\hat{x}}_{i}$ , $i = k, \dots, ℓ, \dots, k + m$ . The nullspace of $M$ consists of the directions of the state space in which no information is provided by the measurements; that is, the unobservable subspace. It should be noted that since the proposed robocentric EKF includes three steps, propagation, update and composition, and the local frame of reference is changed by the composition step, we study the observability in a complete cycle of: (i) propagation and update, and then (ii) composition. We analytically prove that the proposed R-VIO’s system model has an invariant unobservable subspace and does not undergo the observability mismatch issue that has been identified to be the main cause of inconsistency (Huang et al., 2010; Li and Mourikis, 2013a; Hesch et al., 2014a,b), thus being able to improve the resulting VINS performance.

4.0.1. Error-state transition matrix

To ensure theoretical analysis, the analytic, closed-form error-state transition matrix is computed as follows:

Ψ (ℓ, k) = [\begin{matrix} Φ (ℓ, k) & 0_{24 \times 3} \\ 0_{3 \times 24} & I_{3} \end{matrix}]

(46)

where, instead of (20), $Φ (ℓ, k)$ is obtained by integrating the following differential equation over the time interval $[t_{k}, t_{ℓ}]$ :

\overset{\cdot}{Φ} (ℓ, k) = F Φ (ℓ, k)

(47)

with initial condition $Φ (k, k) = I_{24}$ . While the closed-form solution is given in the following, the interested reader is referred to our companion technical report (Huai and Huang, 2018) for the detailed derivations:

Φ (ℓ, k) = [\begin{matrix} I_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} \\ 0_{3} & I_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} \\ 0_{3} & 0_{3} & I_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} \\ 0_{3} & 0_{3} & 0_{3} & Φ_{44} & 0_{3} & 0_{3} & Φ_{47} & 0_{3} \\ 0_{3} & 0_{3} & Φ_{53} & Φ_{54} & I_{3} & Φ_{56} & Φ_{57} & Φ_{58} \\ 0_{3} & 0_{3} & Φ_{63} & Φ_{64} & 0_{3} & Φ_{66} & Φ_{67} & Φ_{68} \\ 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & I_{3} & 0_{3} \\ 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & I_{3} \end{matrix}]

Φ_{44} (ℓ, k) =_{k}^{ℓ} C_{\hat{\bar{q}}}

(48)

Φ_{47} (ℓ, k) = -_{k}^{ℓ} C_{\hat{\bar{q}}} \int_{t_{k}}^{t_{ℓ}} {{}_{k}^{τ}C}_{\hat{\bar{q}}}^{⊤} d τ

(49)

Φ_{53} (ℓ, k) = - \frac{1}{2} I_{3} Δ t_{k, ℓ}^{2}

(50)

Φ_{54} (ℓ, k) = - ⌊ (^{R_{k}} {\hat{p}}_{I_{ℓ}} + {\frac{1}{2}}^{R_{k}} \hat{g} Δ t_{k, ℓ}^{2}) \times ⌋

(51)

Φ_{56} (ℓ, k) = I_{3} Δ t_{k, ℓ}

(52)

\begin{matrix} Φ_{57} (ℓ, k) = \int_{t_{k}}^{t_{ℓ}} ⌊ {{}_{k}^{τ}C}_{\hat{\bar{q}}}^{⊤} {\hat{v}}_{I_{τ}} \times ⌋ \int_{t_{k}}^{τ} {}_{k}^{μ}{C_{\hat{\bar{q}}}^{⊤}} d μ d τ \\ + ⌊ {}^{R_{k}}{\hat{g}} \times ⌋ \int_{t_{k}}^{t_{ℓ}} \int_{t_{k}}^{τ} \int_{t_{k}}^{μ} {{}_{k}^{λ}C}_{\hat{\bar{q}}}^{⊤} d λ d μ d τ \\ - \int_{t_{k}}^{t_{ℓ}} \int_{t_{k}}^{τ} {}_{k}^{μ}{C_{\hat{\bar{q}}}^{⊤}} ⌊ {\hat{v}}_{I_{μ}} \times ⌋ d μ d τ \end{matrix}

(53)

Φ_{58} (ℓ, k) = - \int_{t_{k}}^{t_{ℓ}} \int_{t_{k}}^{τ} {}_{k}^{μ}{C_{\hat{\bar{q}}}^{⊤}} d μ d τ

(54)

Φ_{63} (ℓ, k) = -_{k}^{ℓ} C_{\hat{\bar{q}}} Δ t_{k, ℓ}

(55)

Φ_{64} (ℓ, k) = -_{k}^{ℓ} C_{\hat{\bar{q}}} ⌊ {}^{R_{k}}{\hat{g}} \times ⌋ Δ t_{k, ℓ}

(56)

Φ_{66} (ℓ, k) = Φ_{44} (ℓ, k)

(57)

\begin{matrix} Φ_{67} (ℓ, k) =_{k}^{ℓ} C_{\hat{\bar{q}}} ⌊ {}^{R_{k}}{\hat{g}} \times ⌋ \int_{t_{k}}^{t_{ℓ}} \int_{t_{k}}^{τ} {}_{k}^{μ}{C_{\hat{\bar{q}}}^{⊤}} d μ d τ \\ - \int_{t_{k}}^{t_{ℓ}} {{}_{τ}^{ℓ}C}_{\hat{\bar{q}}} ⌊ {\hat{v}}_{I_{τ}} \times ⌋ d τ \end{matrix}

(58)

Φ_{68} (ℓ, k) = -_{k}^{ℓ} C_{\hat{\bar{q}}} \int_{t_{k}}^{t_{ℓ}} {{}_{k}^{τ}C}_{\hat{\bar{q}}}^{⊤} d τ

(59)

where $Δ t_{k, ℓ} = t_{ℓ} - t_{k}$ .

4.0.2. Measurement Jacobian

At time $t_{ℓ} \in [t_{k}, t_{k + m}]$ , the position of landmark L in ${I_{ℓ}}$ can be expressed as

{{}^{I_{ℓ}}p}_{L} =_{k}^{ℓ} C_{\bar{q}} (^{R_{k}} p_{L} -^{R_{k}} p_{I_{ℓ}})

(60)

Based on (21), the bearing-only measurement is given by

z_{ℓ} = \frac{1}{z} [\begin{matrix} x \\ y \end{matrix}],^{I_{ℓ}} p_{L} = {[\begin{matrix} x & y & z \end{matrix}]}^{⊤}

(61)

We assume that the IMU and camera frames coincide for brevity of presentation, and the corresponding measurement Jacobian is thus in the form of

\begin{matrix} H_{ℓ} = {H_{p}}_{k}^{ℓ} C_{\hat{\bar{q}}} [\begin{matrix} 0_{3} & 0_{3} & 0_{3} & H_{θ_{ℓ}} & - I_{3} & 0_{3 \times 9} | I_{3} \end{matrix}] \\ H_{p} = \frac{1}{\hat{z}} [\begin{matrix} 1 & 0 & - \frac{\hat{x}}{\hat{z}} \\ 0 & 1 & - \frac{\hat{y}}{\hat{z}} \end{matrix}], H_{θ_{ℓ}} = ⌊ (^{R_{k}} {\hat{p}}_{L} -^{R_{k}} {\hat{p}}_{I_{ℓ}}) \times ⌋ {{}_{k}^{ℓ}C}_{\hat{\bar{q}}}^{⊤} \end{matrix}

(62)

4.1. Observability of propagation and update

Based on the above Jacobians, we obtain the $ℓ$ th block row, $M_{ℓ}$ , of $M$ as follows (see (46), (48)–(59), and (62)):

\begin{matrix} M_{ℓ} = H_{ℓ} Ψ_{ℓ, k} \\ = Π [\begin{matrix} 0_{3} & 0_{3} & Γ_{1} & Γ_{2} & - I_{3} & Γ_{3} & Γ_{4} & Γ_{5} | I_{3} \end{matrix}] \end{matrix}

where

Π = {H_{p}}_{k}^{ℓ} C_{\hat{\bar{q}}}

(63)

Γ_{1} = - Φ_{53} = \frac{1}{2} I_{3} Δ t_{k, ℓ}^{2}

(64)

\begin{matrix} Γ_{2} = ⌊ (^{R_{k}} {\hat{p}}_{L} -^{R_{k}} {\hat{p}}_{I_{ℓ}}) \times ⌋ {{}_{k}^{ℓ}C}_{\hat{\bar{q}}}^{⊤} Φ_{44} - Φ_{54} \\ = ⌊ {}^{R_{k}}{\hat{p}}_{L} \times ⌋ + \frac{1}{2} ⌊ {}^{R_{k}}{\hat{g}} \times ⌋ Δ t_{k, ℓ}^{2} \end{matrix}

(65)

Γ_{3} = - Φ_{56} = - I_{3} Δ t_{k, ℓ}

(66)

\begin{matrix} Γ_{4} = ⌊ (^{R_{k}} {\hat{p}}_{L} -^{R_{k}} {\hat{p}}_{I_{ℓ}}) \times ⌋ {{}_{k}^{ℓ}C}_{\hat{\bar{q}}}^{⊤} Φ_{47} - Φ_{57} \\ = - ⌊ (^{R_{k}} {\hat{p}}_{L} -^{R_{k}} {\hat{p}}_{I_{ℓ}}) \times ⌋ \int_{t_{k}}^{t_{ℓ}} {{}_{k}^{τ}C}_{\hat{\bar{q}}}^{⊤} d τ - Φ_{57} \end{matrix}

(67)

Γ_{5} = - Φ_{58}

(68)

Note that for generic motion (i.e., $ω \neq 0_{3 \times 1}$ and $a \neq 0_{3 \times 1}$ ), the values of $Φ_{57}$ and $Φ_{58}$ are time-varying, and thus $Γ_{4}$ and $Γ_{5}$ are linearly independent. Moreover, as the value of $Δ t_{k, ℓ}$ is varying with different time intervals, $Γ_{1}$ , $Γ_{2}$ , and $Γ_{3}$ are general vectors and thus linearly independent. As an immediate result, all these general vectors, $Γ_{1}$ , $Γ_{2}$ , $Γ_{3}$ , $Γ_{4}$ , and $Γ_{5}$ are linearly independent of $I_{3}$ . According to that, we do matrix elementary transformations on $M_{ℓ}$ to facilitate the search for the nullspace:

\begin{matrix} M_{ℓ} = Π [\begin{matrix} 0_{3} & 0_{3} & Γ_{1} & Γ_{2} & - I_{3} & Γ_{3} & Γ_{4} & Γ_{5} | I_{3} \end{matrix}] \\ ~ Π [\begin{matrix} 0_{3} & 0_{3} & Γ_{1} & Γ_{2} & - I_{3} & Γ_{3} & Γ_{4} & Γ_{5} | 0_{3} \end{matrix}] \end{matrix}

from which we can find that $M_{ℓ}$ is rank deficient by 9 (i.e., the dimension of its nullspace is 9). Specifically, $\forall ℓ \geq k$ , we can find that the nullspace of $M$ consists of the following nine directions, as

null (M) = \underset{col .}{span} [\begin{matrix} I_{3} & 0_{3} & 0_{3} \\ 0_{3} & I_{3} & 0_{3} \\ 0_{3} & 0_{3} & 0_{3} \\ 0_{3} & 0_{3} & 0_{3} \\ 0_{3} & 0_{3} & I_{3} \\ 0_{3} & 0_{3} & 0_{3} \\ 0_{3} & 0_{3} & 0_{3} \\ 0_{3} & 0_{3} & 0_{3} \\ 0_{3} & 0_{3} & I_{3} \end{matrix}]

(69)

which may be interpreted as follows:

Remark 1. The proposed robocentric model possesses (9) unobservable directions: the first 6 DOFs are correlated to the orientation (3) and position (3) of the global frame,² while the remaining 3 DOFs belong to the same translation (3) simultaneously applied to the sensing platform and the landmark(s), which agrees with our intuition that relative (camera and IMU) measurements do not provide any global information in the VINS estimation, in analogy to the SLAM case (Huang et al., 2010).

4.2. Observability with composition

After update at time-step $ℓ$ , the estimates of ${{}^{R_{k}}x}_{ℓ}$ and ${{}^{R_{k}}p}_{L}$ are obtained, we have the following linearized model from time-step k to $ℓ$ , including the composition step, as

{\tilde{x}}_{ℓ} = {\overset{⌣}{V}}_{ℓ} Ψ (ℓ, k) {\tilde{x}}_{k} = \overset{⌣}{Ψ} (ℓ, k) {\tilde{x}}_{k}

(70)

where

\begin{matrix} {\overset{⌣}{V}}_{ℓ} = [\begin{matrix} V_{ℓ} & 0_{24 \times 3} \\ L_{ℓ} & N_{ℓ} \end{matrix}] = [\begin{matrix} V_{ℓ} & 0_{24 \times 3} \\ \frac{\partial {\tilde{x}}_{ℓ}}{\partial^{R_{k}} {\tilde{x}}_{ℓ}} & \frac{\partial {\tilde{x}}_{ℓ}}{\partial^{R_{k}} {\tilde{p}}_{L}} \end{matrix}], \\ L_{ℓ} = [\begin{matrix} 0_{3} & 0_{3} & 0_{3} & ⌊ {}^{R_{ℓ}}{\hat{p}}_{L} \times ⌋ & -_{k}^{ℓ} C_{\hat{\bar{q}}} & 0_{3} & 0_{3} & 0_{3} \end{matrix}], \\ N_{ℓ} =_{k}^{ℓ} C_{\hat{\bar{q}}} \end{matrix}

(71)

For brevity of analysis, only the pertinent entries of $\overset{⌣}{Ψ} (ℓ, k)$ are shown in the following:

\begin{matrix} \overset{⌣}{Ψ} (ℓ, k) = {\overset{⌣}{V}}_{ℓ} Ψ (ℓ, k) = \\ [\begin{matrix} {\overset{⌣}{Ψ}}_{11} & 0_{3} & 0_{3} & {\overset{⌣}{Ψ}}_{14} & 0_{3} & 0_{3} & {\overset{⌣}{Ψ}}_{17} & 0_{3} & 0_{3} \\ 0_{3} & {\overset{⌣}{Ψ}}_{22} & {\overset{⌣}{Ψ}}_{23} & {\overset{⌣}{Ψ}}_{24} & {\overset{⌣}{Ψ}}_{25} & {\overset{⌣}{Ψ}}_{26} & {\overset{⌣}{Ψ}}_{27} & {\overset{⌣}{Ψ}}_{28} & 0_{3} \\ 0_{3} & 0_{3} & {\overset{⌣}{Ψ}}_{33} & {\overset{⌣}{Ψ}}_{34} & 0_{3} & 0_{3} & {\overset{⌣}{Ψ}}_{37} & 0_{3} & 0_{3} \\ 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} \\ 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} \\ 0_{3} & 0_{3} & {\overset{⌣}{Ψ}}_{63} & {\overset{⌣}{Ψ}}_{64} & 0_{3} & {\overset{⌣}{Ψ}}_{66} & {\overset{⌣}{Ψ}}_{67} & {\overset{⌣}{Ψ}}_{68} & 0_{3} \\ 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & I_{3} & 0_{3} & 0_{3} \\ 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & 0_{3} & I_{3} & 0_{3} \\ 0_{3} & 0_{3} & {\overset{⌣}{Ψ}}_{93} & {\overset{⌣}{Ψ}}_{94} & {\overset{⌣}{Ψ}}_{95} & {\overset{⌣}{Ψ}}_{96} & {\overset{⌣}{Ψ}}_{97} & {\overset{⌣}{Ψ}}_{98} & {\overset{⌣}{Ψ}}_{99} \end{matrix}] \end{matrix}

{\overset{⌣}{Ψ}}_{93} = -_{k}^{ℓ} C_{\hat{\bar{q}}} Φ_{53}

(72)

{\overset{⌣}{Ψ}}_{94} = ⌊ {}^{R_{ℓ}}{\hat{p}}_{L} \times ⌋ Φ_{44} -_{k}^{ℓ} C_{\hat{\bar{q}}} Φ_{54}

(73)

{\overset{⌣}{Ψ}}_{95} = -_{k}^{ℓ} C_{\hat{\bar{q}}}

(74)

{\overset{⌣}{Ψ}}_{96} = -_{k}^{ℓ} C_{\hat{\bar{q}}} Φ_{56}

(75)

{\overset{⌣}{Ψ}}_{97} = ⌊ {}^{R_{ℓ}}{\hat{p}}_{L} \times ⌋ Φ_{47} - {{}_{k}^{ℓ}C}_{\hat{\bar{q}}} Φ_{57}

(76)

{\overset{⌣}{Ψ}}_{98} = -_{k}^{ℓ} C_{\hat{\bar{q}}} Φ_{58}

(77)

{\overset{⌣}{Ψ}}_{99} =_{k}^{ℓ} C_{\hat{\bar{q}}}

(78)

Note that the measurement model of (61) becomes linear:

z_{ℓ} =^{R_{ℓ}} p_{L},^{R_{ℓ}} p_{L} =_{k}^{ℓ} C_{\bar{q}} (^{R_{k}} p_{L} -^{R_{k}} p_{I_{ℓ}})

(79)

and the measurement Jacobian with respect to ${\tilde{x}}_{ℓ}$ is

{\overset{⌣}{H}}_{ℓ} = [\begin{matrix} 0_{3 \times 24} | I_{3} \end{matrix}]

(80)

Therefore, after composition we have the block row, $M_{ℓ}$ , of $M$ in the form of

\begin{matrix} M_{ℓ} = {\overset{⌣}{H}}_{ℓ} {\overset{⌣}{Ψ}}_{ℓ, k} \\ = [\begin{matrix} 0_{3} & 0_{3} & {\overset{⌣}{Ψ}}_{93 : 94} & -_{k}^{ℓ} C_{\hat{\bar{q}}} & {\overset{⌣}{Ψ}}_{96 : 98} |_{k}^{ℓ} C_{\hat{\bar{q}}} \end{matrix}] \end{matrix}

Note that for generic motion (i.e., $ω \neq 0_{3 \times 1}$ and $a \neq 0_{3 \times 1}$ ), ${\overset{⌣}{Ψ}}_{93}$ , ${\overset{⌣}{Ψ}}_{94}$ , ${\overset{⌣}{Ψ}}_{96}$ , ${\overset{⌣}{Ψ}}_{97}$ , and ${\overset{⌣}{Ψ}}_{98}$ are linearly independent, and obviously the same nullspace as that of the propagation and update (see (69)) can be obtained.

Remark 2. For the proposed robocentric model, changing local frame of reference (by composition) does not alter the unobservable subspace.

Up to now, we have completely shown that the proposed robocentric VINS model has an invariant unobservable subspace that is independent of the linearization points under generic motions, which guarantees that the proposed R-VIO has not only correct unobservable dimension as in Huang et al. (2010), Li and Mourikis (2013a), Hesch et al. (2014a,b), and Huang et al. (2014), but also constant unobservable directions, thus being expected to improve estimation consistency.

4.3. Observability under special motions

Depending on the motion undertaken, the system observability properties might change in some degenerate cases. Identifying and understanding such special motions is essential for improving the VINS performance, especially in practice. For example, one of the most commonly seen cases is the planar motion (the translation is only excited in the x–y plane, and the rotation is only about the z-axis) and the recent analysis on the world-centric VINS (Wu et al., 2017) has pointed out that in this type of motion two more unobservable directions emerge: (i) the global orientation and (ii) the scale. It should be noted that for the proposed robocentric system model the global orientation has already been shown to be unobservable (see (69)). Therefore, in what follows, we study in-depth the observability under special motions by focusing on the scale (un)observability which may directly affect the performance.

4.3.1. Effect of scaling on VINS states

We are first to understand the implications of an underlying scale factor applied to the state vector of the proposed robocentric system, which is to form the basis for identifying the degenerate motions causing the special unobservable directions.

Lemma 1. For the proposed robocentric model, given the actual state, $x$ , and the underlying state, $x'$ , that are related through a scale factor, s, there exists the following relation between the corresponding error states (see (44)):

\begin{matrix} [\begin{matrix} δ θ_{G} \\ {}^{R_{k}}{\tilde{p}}_{G} \\ {}^{R_{k}}{\tilde{g}} \\ δ θ_{I} \\ {}^{R_{k}}{\tilde{p}}_{I} \\ {\tilde{v}}_{I} \\ {\tilde{b}}_{g} \\ {\tilde{b}}_{a} \\ {}^{R_{k}}{\tilde{p}}_{L} \end{matrix}] = [\begin{matrix} δ θ'_{G} \\ {}^{R_{k}}{\tilde{p}'}_{G} \\ {}^{R_{k}}{\tilde{g}}' \\ δ θ'_{I} \\ {}^{R_{k}}{\tilde{p}'}_{I} \\ \tilde{v}'_{I} \\ \tilde{b}'_{g} \\ \tilde{b}'_{a} \\ {}^{R_{k}}{\tilde{p}'}_{L} \end{matrix}] + (s - 1) [\begin{matrix} 0_{3 \times 1} \\ {}^{R_{k}}{\hat{p}'}_{G} \\ 0_{3 \times 1} \\ 0_{3 \times 1} \\ {}^{R_{k}}{\hat{p}'}_{I} \\ \hat{v}'_{I} \\ 0_{3 \times 1} \\ -^{I} \hat{a}' \\ {}^{R_{k}}{\hat{p}'}_{L} \end{matrix}] \Rightarrow \\ \tilde{x} = \tilde{x}' + (s - 1) u \end{matrix}

(81)

Proof. See Appendix C. □

4.3.2. Special motions for scale unobservability

It becomes clear from (81) that if the proposed robocentric VINS estimation is metrically scaled by a factor of s, then the error state (and, hence, the state) would be changed along the direction of u by a factor of $(s - 1)$ . However, as evident from the proof (see Appendix C) we might not distinguish this scale ambiguity from the camera and IMU measurements, which implies that the direction of scale is unobservable. The following analysis further identifies the special motions that can cause the scale unobservability for the proposed robocentric system.

Lemma 2. For the proposed robocentric model, there exist such special motions that can cause scale unobservable: (i) no rotation, with

Δ t_{k, ℓ} \hat{v}'_{I_{ℓ}} = - \frac{1}{2} Δ t_{k, ℓ}^{2} {}^{ℓ}{\hat{a}}, \forall ℓ \geq k

(82)

(ii) constant local acceleration, with $\hat{v}'_{I_{τ}} \equiv 0$ , $\forall τ \in [t_{k}, t_{ℓ}]$ ; that is, the system is either stationary or purely rotating.

Proof. See Appendix D. □

As a final remark, it is clear from the above lemma that the scale unobservable direction does exist when: (i) (82) holds true (e.g., may occur during the deceleration motion), or (ii) the sensing platform stays in the same place. However, these two cases can easily be mitigated in the estimation. Specifically, in the case of (i), as it holds when $Δ t_{k, ℓ} \to 0$ , we can simply increase $Δ t_{k, ℓ}$ in practice to avoid the scale change. While in the case of (ii), we will confront low parallax, but the inverse-depth measurement model used by the proposed R-VIO (see (29)) forces it mainly to exploit the orientation information from the measurements, thus holding the scale. It should be pointed out that in contrast to the world-centric remedy (Wu et al., 2017), which fused the wheel odometer measurements, the proposed R-VIO does not require any additional sensor to address this scale issue, thus revealing the better adaptability and robustness, as shown later in the real-world experiment.

5. Simulation results

In this section, we present the Monte Carlo simulation results that verify the analysis provided in the preceding sections, and demonstrate the performance of the proposed R-VIO in comparison with two world-centric EKF-based counterparts: (i) the standard (Std)-MSCKF (Mourikis and Roumeliotis, 2007), as well as the state-of-the-art (ii) state-transition and observability constrained (STOC)-MSCKF (Huang et al., 2014) that enforces correct system observablility to improve estimation consistency. In particular, two metrics are used for the evaluation: (a) the root-mean-squared error (RMSE), which offers a concise measure of the filter’s accuracy, and (b) the normalized estimation error squared (NEES), which provides a standard criterion for evaluating the given filter’s consistency (Bar-Shalom et al., 2001). In order to make a fair comparison, we implemented all three filters using the same parameters, such as the number of features used per update, and processing the same data of 50 Monte Carlo trails which were generated with real microelectromechanical system (MEMS) sensors’ quality (see Table 1 and Figure 2 for details).

Table 1.

Sensor parameters in simulation.

Parameter	Value
IMU rate	100 [Hz]
Gyroscope noise density ( $σ_{g}$ )	$1.1220 \times 10^{- 4}$ [ $rad / s / \sqrt{Hz}$ ]
Gyroscope random walk ( $σ_{wg}$ )	$5.6323 \times 10^{- 6}$ [ $rad / s^{2} / \sqrt{Hz}$ ]
Accelerometer noise density ( $σ_{a}$ )	$5.0119 \times 10^{- 4}$ [ $m / s^{2} / \sqrt{Hz}$ ]
Accelerometer random walk ( $σ_{wa}$ )	$3.9811 \times 10^{- 5}$ [ $m / s^{3} / \sqrt{Hz}$ ]
Camera rate	10 [Hz]
Image noise level ( $σ_{im}$ )	1.5 [pixels]
Magnitude of gravity	9.8038 [ $m / s^{2}$ ]

Fig. 2.

Simulation scenario: a camera/IMU pair moves along a circular path of radius 5 m (black) at an average speed 1.0 m/s. The camera with $45 °$ field of view observes point features (pink) randomly distributed on a circumscribing cylinder of radius 6 m.

The statistical results over 50 Monte Carlo trails are shown in Figure 3, while Table 2 provides the average RMSE and NEES results for all the filters, which clearly show that the proposed R-VIO as expected outperforms the Std-MSCKF, as well as the STOC-MSCKF in terms of accuracy (smaller RMSE) and consistency (NEES closer to three), attributed to the novel reformulation of the system.

Fig. 3.

Simulation results: the statistics of NEES and RMSE of orientation and position over 50 Monte Carlo trials.

Table 2.

Average RMSE and NEES corresponding to Figure 3.

	Orientation	Position	Orientation	Position
	[deg]	[m]	NEES	NEES
Std-MSCKF	3.470	0.477	7.048	5.810
STOC-MSCKF	2.523	0.430	4.096	3.793
R-VIO	0.694	0.253	2.615	3.552

6. Experimental results

We further experimentally validate the proposed R-VIO, in both indoor and outdoor environments, using both the public benchmark dataset on MAVs and the real urban driving data collected by our own sensing platform on a car. As described in Algorithm 1, we implemented that in C++ multithread framework.³ In the front end, the visual tracking handler extracts features from the image using the Shi-Tomasi corner detector (Shi and Tomasi, 1994), that are being tracked between consecutive images with the KLT algorithm (Baker and Matthews, 2004). In particular, to deal with the varying lighting conditions in practice, a combined operation of the adaptive thresholding and box blurring was applied for every image before doing the KLT tracking. This effectively mitigates the sharp change of illumination while outlining the environmental structures even in the dark area (see Figure 4), which is experimentally helpful for the feature detection. In addition, to remove the bad tracks from the tracking results, we realized the gyro-aided, two-point random sample consensus (RANSAC) algorithm (Troiani et al., 2014). In the end, the tracking history of all the inliers are stored by a first-in, first-out (FIFO) data structure, in order to be efficiently queried during the estimation.

Fig. 4.

Visual tracking: the post-processing results (left) and the respective raw images (right). Inliers (blue dots) are tracked between pairwise images with outliers (red dots) being rejected by the RANSAC. In particular, the tracking results have been visualized by the inliers’ tracks (blue lines). Note that, this front end is able to process: (i) dusky, (ii) blurred, as well as (iii) overexposed image streams of real-world environments.

Once the visual tracking is done, the back end processes all the visual and inertial measurements using the proposed robocentric EKF. In particular, for the features losing track in the current image, we use all their measurements within the current sliding window for update, while for those reaching the maximum tracking length (e.g., the sliding-window size) we use a subset (e.g., 1/2) of their respective measurements and keep the rest for next update. All the following tests run on an Intel Core i7-4710MQ @ 2.5 GHz laptop in real time.

6.1. EuRoC MAV dataset

We tested the proposed R-VIO first using all 11 sequences in the EuRoC MAV dataset (Burri et al., 2016) for which a FireFly helicopter equipped with VI-sensor (an ADIS16448 IMU @ 200 Hz, and dual cameras $752 \times 480$ pixels @ 20 Hz) was used for flying data collection. In this test, only the left camera images were used for vision inputs and 200 features were uniformly extracted based on each image. The sliding-window size was set up to 20 (i.e., about 1 s memory for the most recent relative motion). We compared the proposed R-VIO with two state-of-the-art world-centric sliding-window optimization-based VINS, the OKVIS⁴ (Leutenegger et al., 2015) and the VINS-Mono⁵ (Qin et al., 2018), both of which perform nonlinear batch optimization in the back ends using the Google Ceres optimizer (Agarwal et al., 2010). It should be noted that in order to be a fair comparison between the three systems, we turned off the loop closure of VINS-Mono in the tests. As the 6-DOF pose ground truths are provided for all the sequences, we employed the evaluation tool⁶ (Zhang and Scaramuzza, 2018) to generate the results of comparison. Specifically, the results of pose RMSE and the computation time in Table 3 show both the absolute pose accuracy and the computational efficiency of the systems. Figure 5 depicts the estimated trajectories of nine representative sequences, and the corresponding relative pose errors are shown in Figure 6. It is important to note that the proposed R-VIO does not utilize any kind of map, whereas both OKVIS and VINS-Mono do. Nevertheless, it is clear from Table 3 that on this dataset the proposed R-VIO performs very competitively, or even better in some sequences, with the OKVIS and VINS-Mono; this, however, is achieved at the least computational cost among all three systems considered.

Table 3.

Pose estimation accuracy (RMSE) and computation time (per image) on the EuRoC dataset.

		R-VIO			OKVIS			VINS-Mono
	Length	Orientation	Position	Time	Orientation	Position	Time	Orientation	Position	Time
	[m]	[deg]	[m]	[ms]	[deg]	[m]	[ms]	[deg]	[m]	[ms]
V1_01_easy	58.6	0.106	0.080	24.6	0.099	0.083	43.9	0.110	0.089	68.5
V1_02_medium	75.9	0.033	0.108	26.5	0.048	0.192	45.7	0.057	0.110	49.6
V1_03_difficult	79.0	0.021	0.121	25.5	0.069	0.199	46.5	0.106	0.188	35.2
V2_01_easy	36.5	0.038	0.158	24.0	0.013	0.154	46.7	0.036	0.088	35.2
V2_02_medium	83.2	0.030	0.162	26.3	0.023	0.216	44.5	0.074	0.160	46.6
V2_03_difficult	86.1	0.077	0.274	23.8	0.060	0.287	43.1	0.056	0.278	27.3
MH_01_easy	80.6	0.036	0.187	27.9	0.027	0.267	44.3	0.012	0.174	67.1
MH_02_easy	73.5	0.021	0.305	25.8	0.040	0.318	43.2	0.015	0.221	63.6
MH_03_medium	130.9	0.025	0.287	27.3	0.058	0.306	41.8	0.032	0.221	64.5
MH_04_difficult	91.7	0.072	0.761	25.7	0.078	0.297	43.4	0.014	0.371	58.0
MH_05_difficult	97.6	0.013	0.445	27.3	0.046	0.502	45.9	0.008	0.351	60.6
Mean (Sequence 1–6)	—	0.050	0.150	25.1	0.052	0.188	45.0	0.073	0.152	48.2
Mean (Sequence 7–11)	—	0.033	0.397	26.8	0.049	0.338	43.7	0.016	0.267	62.7

Fig. 5.

Trajectory estimates on EuRoC dataset: R-VIO (blue), OKVIS (yellow), and VINS-Mono (red) with the ground truth (black).

Fig. 6.

Relative pose errors corresponding to the trajectories shown in Figure 5(a)–(i). Each row presents the relative translation error (left), the percentage of the relative translation error (middle), and the relative yaw error (right) of R-VIO (blue), OKVIS (yellow), and VINS-Mono (red) with respect to different segments of trajectory of each sequence, respectively.

6.2. Vicon Loops dataset

We further tested the proposed R-VIO on a long-term indoor dataset presented in Leutenegger et al. (2015). This dataset was collected using a handheld VI-sensor, moving mostly in circles at the speed around 2 m/s, which lasted almost 14 minutes and traveled about 1,200 meters in total. Similarly, the images from left camera were used for vision inputs and 200 features were uniformly extracted per image, while this time a shorter sliding window of size 10 was used in order to handle the mostly short tracking lengths of features caused by constantly, highly dynamic motion for stimulating MAV flying. Again, we compared the performance of the proposed R-VIO with both OKVIS and VINS-Mono while using the metric introduced by Leutenegger et al. (2015); that is, to evaluate the absolute pose errors (between ground truth and the estimates) based on different distances traveled. To this end, for each travel interval the orientation and translation errors are calculated with respect to the starting ground-truth pose of each interval. Comprehensively, Figure 7 shows the estimated trajectories and the error statistics based on a set of distances, ${60, 180, 300, 420, 540, 660, 780, 900, 1, 020, 1, 140}$ , noting that the results of OKVIS were generated using the original data provided by Leutenegger et al. (2015). It should also be pointed out that as this dataset was starting from motion (i.e., there was no static phase at the beginning), the proposed R-VIO only used the first single IMU measurement for initialization, while the VINS-Mono was initialized based on a visual–inertial alignment procedure using multiple measurements (Qin et al., 2018). As is evident from Figure 7, all three systems drift in both the orientation and the translation as the distance traveled becomes longer, while without loop closure the VINS-Mono accumulates the obviously larger drifts, in both z-component and yaw angle, than the OKVIS and the proposed R-VIO.

Fig. 7.

Vicon Loops dataset evaluation: R-VIO (blue), OKVIS (yellow) and VINS-Mono (red). In particular, (a) and (b) illustrate the system performance in x-y plane and z-axis; (c) and (d) are the error statistics in terms of median, 5th and 95th percentiles.

6.3. Urban driving test

We have also performed a road test for R-VIO with our own sensing platform (an Xsens Mti-G-710 GNSS, and a FLIR Bumblebee2 stereo pair $1, 024 \times 768$ pixels @ 15 Hz) on a car by driving on the streets of Newark, DE. The IMU provided measurements at 400 Hz, while the GPS signal was received at 4 Hz as the (position) ground truth. Similarly, only the left camera images were used for vision inputs with 200 features being uniformly extracted from each image. It is important to note that this test is very challenging primarily due to: (i) several traffic lights where we must stop and wait for 15–25 seconds; (ii) frequent stop/yield signs before which we must stop or decelerate; (iii) dynamic scenes including running vehicles and pedestrians in the vicinity; (iv) strong lens flare when driving facing the sun; and (v) high speeds of vehicle when driving on some particular road sections (see Figure 9).

As we discussed earlier, both (i) and (ii) are the degenerate scenes that may cause the scale to be unobservable for the proposed VIO model. The usage of inverse depth-based measurement model partially solved the scale drift during the static phase, while for the deceleration phase we tested three update rates: high (15 Hz), low (7.5 Hz), and adaptive (switching between the high and low). For actual implementation, we employed a heuristic switching logic for the adaptive mode which is to be optimized in our future work; that is, the R-VIO changes the frequency of update from 15 to 7.5 Hz after detecting the speed descending ( $Δ v < 0$ ) for at least 2 seconds, and changes it back to 15 Hz once detecting the speed ascending ( $Δ v > 0$ ) for at least 2 seconds. The results are summarized in Table 4, while Figure 8 depicts the estimated trajectories of all three update rates. We should note that when using the high update rate, the R-VIO captured the motion of vehicle better than using the low update rate, given an instance that after the first turning our vehicle sped up to 86 km/h and for that the trajectory under high update rate fitted the ground truth better. At the second turning, a series of deceleration events occurred due to the busy traffic at the intersection, as a consequence, the drifts in scale biased trajectory estimation afterwards. In contrast, by lowering down the update rate the R-VIO could compensate for the scale drifts and made the estimated trajectory closer to the ground truth. Thus, as expected, the proposed adaptive mode was able to benefit from both of the aforementioned advantages. In addition to the inference, we further confirm it through a test which the differences of translation between consecutive poses of R-VIO’s estimates, $Δ_{est}$ , and of the ground truth, $Δ_{gt}$ , for each update rate are accumulated for every 10 seconds, accordingly the scale can be estimated by $\frac{Δ_{est}}{Δ_{gt}}$ . The results referring to the estimated speed are shown in Figure 10, from which we can find that the distinct scale variance (≥2) only emerged when either a sharp deceleration occurred (e.g., 600–610 s) or vehicle was stopped (e.g., 430–455 s); which agrees with our analysis provided in the preceding section and shows that the drifts in scale can be compensated for by using the proposed adaptive method. Among the three modes, the adaptive one performs the best in terms of median absolute deviation (MAD), a robust measure of the variability of a univariate sample, of scale 0.079, and 0.114 and 0.099 for high and low update rates, respectively, with respect to the ground truth.

Table 4.

Estimation accuracy (RMSE) in the urban driving test.

	Length / Duration	Maximum speed	Position RMSE
	Length / Duration	Maximum speed	x [m]	y [m]	z [m]
H	9.8 km / 15 min	86 km/h	30.934	68.561	8.418
L	— / —	—	33.984	15.883	10.426
A	— / —	—	24.222	18.901	7.689

Fig. 8.

Trajectory estimates plotted over the map of Newark, DE. The initial position of the vehicle is marked by a green triangle. The black solid line corresponds to the trajectory of ground truth, in addition to the red dashed line to the high update rate (H), the yellow dash-dotted line to the low update rate (L), and the blue solid line to the adaptive update rate (A), with the ends of trajectories being marked by the squares in their respective colors.

Fig. 9.

Snapshots during the urban driving test. From left to right and top to bottom: (i) at the start, (ii) at a turning with yield sign, (iii) at a pedestrian traffic light, (iv) at a regular traffic light, (v) at a free intersection with stop sign, and (vi) in the campus area.

Fig. 10.

Statistics of the scale of estimate versus vehicle speed for the three update rates.

Note also that, as the local gravity is jointly estimated, the z-axis drifts are much smaller than the x–y position errors. In the test a sliding window of size 20 was used and the average processing time of pipeline was 48.7 ms per image including 40.8 ms spent on the visual tracking and RANSAC outlier rejection and the other 7.9 ms on robocentric EKF (see Figure 11). In this challenging driving scenario, without utilizing any kind of map, the proposed R-VIO achieves the average position RMSEs of 0.77% (high update rate), 0.40% (low update rate), and 0.32% (adaptive update rate) of the total 9.8 km driving distance.

Fig. 11.

Time consumption per image (at high update rate): the average processing rate of R-VIO is 20.5 Hz as compared with the image rate of 15 Hz, which satisfies real-time requirement.

7. Conclusion and future work

In this paper, we have reformulated the VINS with respect to a moving local frame and developed a lightweight, high-precision, robocentric visual–inertial odometry algorithm, termed R-VIO. With this novel reformulation, we have analytically shown that the proposed R-VIO does not suffer from the observability mismatch issue under generic motions that is encountered by the world-centric counterparts, while even in the degenerate motion cases the observability issue can be easily compensated for without using additional sensor information, thus providing better consistency, accuracy, and robustness. Extensive Monte Carlo simulation as well as real-world experiments using different sensing platforms and navigating in different environments are performed to thoroughly validate our theoretical analysis and show that the proposed R-VIO is versatile and robust to different types of motions and environments, and is capable of providing long-term, high-precision 3D motion tracking in real time. In the future, we will integrate efficient loop closure and online mapping into the current robocentric system in order to bound localization errors, as well as performing online calibration of intrinsic and extrinsic sensor parameters to further improve performance.

Footnotes

Appendix A. Index to multimedia extensions

Archives of IJRR multimedia extensions published prior to 2014 can be found at http://www.ijrr.org, after 2014 all videos are available on the IJRR YouTube channel at http://www.youtube.com/user/ijrrmultimedia

SAGE-Journals-Accessible-Video-Player 10.1177/0278364919853361.M1 sj-vid-1-ijr-10.1177_0278364919853361 Extension 1

The demo of the urban driving test (R-VIO is in the adaptive mode).

Appendix B: BA using inverse-depth parameterized landmark

Assuming a single landmark, L, which has been observed from a set of consecutive robocentric frames in the sliding window, the set of corresponding camera frames is denoted by $C = {1, 2, \dots}$ . To compute an inverse-depth estimate of L, that is, $\hat{λ} = [\hat{ϕ}, \hat{ψ}, \hat{ρ}]^{⊤}$ , we use the proposed inverse-depth measurement model (see (29)) ( $i \in C$ ):

(83)

z_{i} = \frac{1}{h_{i, 3} (Λ)} [\begin{matrix} h_{i, 1} (Λ) \\ h_{i, 2} (Λ) \end{matrix}] + n_{i} : = z_{i} (Λ) + n_{i}

where $n_{i} ~ N (0, Λ_{i}^{- 1})$ is the image noise, while the relative pose, $w$ , is assumed known. Given the measurements of L, ${z_{i} = {[\frac{u_{i}}{f}, \frac{v_{i}}{f}]}^{⊤}, i \in C}$ where f is the focal length, $u_{i}$ and $v_{i}$ are the undistorted pixel coordinates originated from the center of image i, we can formulate a BA problem for $λ$ , as

(84)

\begin{matrix} λ^{*} = \arg min_{λ} \sum_{i \in C} ∥ z_{i} - z_{i} (λ) ∥_{Λ_{i}} \\ = \arg min_{λ} \sum_{i \in C} ∥ ϵ_{i} (λ) ∥_{Λ_{i}} \end{matrix}

where $∥ \cdot ∥_{Λ_{i}}$ denotes the $Λ_{i}$ -weighted energy norm, and we define $ϵ_{i}$ as the residual associated to $z_{i}$ . This problem can be solved iteratively via Gauss–Newton approximation about an initial estimate of $λ$ , as

(85)

\begin{matrix} δ λ^{*} = \arg min_{δ λ} \sum_{i \in C} ∥ ϵ_{i} (\hat{λ} + δ λ) ∥_{Λ_{i}} \\ ≃ \arg min_{δ λ} \sum_{i \in C} ∥ ϵ_{i} (\hat{λ}) + H_{i} δ λ ∥_{Λ_{i}} \end{matrix}

For the initial value of $\hat{λ}$ , we obtain $[\hat{ϕ}, \hat{ψ}]^{⊤}$ by directly using the first pixel measurement, $z_{1}$ , with the following equation:

(86)

[\begin{matrix} \hat{ϕ} \\ \hat{ψ} \end{matrix}] = [\begin{matrix} \arctan (\frac{v_{1}}{f}, \sqrt{{(\frac{u_{1}}{f})}^{2} + 1}) \\ \arctan (\frac{u_{1}}{f}, 1) \end{matrix}]

However, the initial value of $\hat{ρ}$ can be empirically chosen, for which we use 0 to first put the estimate at infinity, then let it converge by performing iterations. The Jacobian of residual, $H_{i} = \frac{\partial ϵ_{i} (\hat{λ} + δ λ)}{\partial δ λ}$ , evaluated at $\hat{λ}$ can be computed following the chain rule, as

H_{i} = \frac{\partial ϵ_{i}}{\partial h_{i}} \frac{\partial h_{i}}{\partial λ} \frac{\partial λ}{\partial δ λ} = \frac{\partial ϵ_{i}}{\partial h_{i}} \frac{\partial h_{i}}{\partial λ}

where

(87)

\begin{array}{l} \frac{\partial ϵ_{i}}{\partial h_{i}} = \frac{1}{{\hat{h}}_{i, 3}} [\begin{matrix} 1 & 0 & - \frac{{\hat{h}}_{i, 1}}{{\hat{h}}_{i, 3}} \\ 0 & 1 & - \frac{{\hat{h}}_{i, 2}}{{\hat{h}}_{i, 3}} \end{matrix}], \\ \frac{\partial h_{i}}{\partial λ} = [\begin{matrix} \frac{\partial h_{i}}{\partial {[ϕ, ψ]}^{T}} & \frac{\partial h_{i}}{\partial ρ} \end{matrix}] \\ = [{}_{1}^{i}{\bar{C}}_{\hat{\bar{q}}} [\begin{matrix} - \sin \hat{ϕ} \sin \hat{ψ} & \cos \hat{ϕ} \cos \hat{ψ} \\ \cos \hat{ϕ} & 0 \\ - \sin \hat{ϕ} \cos \hat{ψ} & - \cos \hat{ϕ} \sin \hat{ψ} \end{matrix}] {}^{i}{\hat{\bar{p}}}_{1}] \end{array}

Every iteration we have an optimal inverse-depth correction, $δ λ^{*}$ , to update current estimate, $\hat{λ}$ , in the form of

(88)

\begin{matrix} δ λ^{*} = {(\sum_{i \in C} H_{i}^{⊤} Λ_{i} H_{i})}^{- 1} (\sum_{i \in C} H_{i}^{⊤} Λ_{i} ϵ_{i}), \\ \hat{λ} \leftarrow \hat{λ} + δ λ^{*} \end{matrix}

Once $δ λ^{*}$ converges (e.g., less than a threshold, $10^{- 6}$ ), we find the optimal inverse-depth estimate: $λ^{*} = \hat{λ}$ .

Appendix C: Proof of Lemma 1

Consider the case that the VINS estimation process is up to a scale factor of s (i.e., to recover the actual state, $x$ , the underlying state, $x'$ , has to be “scaled up” metrically). This results in the following expressions of the VINS states, in which the relative translation and landmark’s position with respect to ${R_{k}}$ (see (44)) can be written as

(89)

{}^{R_{k}}{p_{G}} = s^{R_{k}} p'_{G}

(90)

{}^{R_{k}}{p_{I}} = s^{R_{k}} p'_{I}

(91)

{}^{R_{k}}{p_{L}} = s^{R_{k}} p'_{L}

where ${}^{R_{k}}{p'}_{G}$ , ${}^{R_{k}}{p'}_{I}$ , and ${}^{R_{k}}{p'}_{L}$ are the values of underlying states. Note that, the analysis presented in this proof holds true for any $t \in [t_{k}, t_{k + m}]$ , hence we omit the time indexes for brevity of presentation. As the scale s only corresponds to the translation, the scale change does not affect the rotation. Therefore, we have the angular velocity:

(92)

\begin{array}{l} {}_{G}^{k}{C_{\bar{q}}} {C^{'}}_{\bar{q}},_{k}^{I} C_{\bar{q}} =_{k}^{I} {C^{'}}_{\bar{q}} \Rightarrow \\ ω = ω^{'} \end{array}

With those equations, the IMU velocity and acceleration can be obtained by taking the time derivative of (90), as

(93)

\begin{array}{l} {}_{k}^{I}{C_{\bar{q}}} v_{I} = s_{k}^{I} {C^{'}}_{\bar{q}} {}^{R_{k}}{v'}_{I} \Rightarrow \\ v_{I} = s {v^{'}}_{I},^{I} a = s^{I} a^{'} \end{array}

In particular, ${}^{R_{k}}g$ is a state having known magnitude, thus is not affected by the scaling:

(94)

{}^{R_{k}}g =^{R_{k}} g'

Then, the gravity effects to the IMU frame can be derived as

(95)

\begin{array}{l} {}_{k}^{I}{C_{\bar{q}}} g =_{k}^{I} {C^{'}}_{\bar{q}} {}^{R_{k}}g \Rightarrow \\ {}^{I}g =^{I} g^{'} \end{array}

If such scale is unobservable, then the measurements from the camera and IMU should remain the same. First, for the camera measurement of L (see (61)), we have

(96)

\begin{matrix} [\begin{matrix} x \\ y \\ z \end{matrix}] =^{I} p_{L} =_{k}^{I} C_{\bar{q}} (^{R_{k}} p_{L} -^{R_{k}} p_{I}) \\ = s_{k}^{I} C'_{\bar{q}} (^{R_{k}} p'_{L} -^{R_{k}} p'_{I}) = s^{I} p'_{L} = s [\begin{matrix} x' \\ y' \\ z' \end{matrix}] \Rightarrow \\ z_{I} = \frac{1}{z} [\begin{matrix} x \\ y \end{matrix}] = \frac{1}{sz'} [\begin{matrix} sx' \\ sy' \end{matrix}] = \frac{1}{z'} [\begin{matrix} x' \\ y' \end{matrix}] = z'_{I} \end{matrix}

where the camera measurement does not change because the scale has no effect on such perspective projection. Then, for the IMU measurements we first examine the angular velocity measured by the gyroscope (see (7)), as

(97)

\begin{matrix} ω_{m} = ω + b_{g} = ω' + b'_{g} \Rightarrow \\ b_{g} = b'_{g} \end{matrix}

Similarly, for the linear acceleration measurements from the accelerometer (see (8)), we have

(98)

\begin{matrix} a_{m} =^{I} a +^{I} g + b_{a} =^{I} a' +^{I} g' + b'_{a} \Rightarrow \\ b_{a} = b'_{a} - (s - 1)^{I} a' \end{matrix}

Note that $b_{a}$ cannot be simply represented as an s multiple of $b'_{a}$ , because it is a random walk process (see (6)). Thus, based on (89), (90), (91), (92), (93), (94), (97), and (98), it is not difficult to validate the corresponding error-state relation as shown in (81).

Appendix D: Proof of Lemma 2

Based on the observability matrix (see (45)), the $ℓ$ th block row, $M'_{ℓ}$ , of observability matrix $M'$ evaluating at ${}^{R_{k}}{\hat{x'}}_{ℓ}$ and ${}^{R_{k}}{\hat{p'}}_{L}$ has the following structure:

M'_{ℓ} = Π' [\begin{matrix} 0_{3 \times 6} & Γ'_{1} & Γ'_{2} & - I_{3} & Γ'_{3} & Γ'_{4} & Γ'_{5} | I_{3} \end{matrix}]

The scale direction, $u$ , is unobservable (see (81)), if and only if $M'_{ℓ} u = 0$ , $\forall ℓ \geq k$ ; thus, we have

(99)

Π' (-^{R_{k}} \hat{p}'_{I_{ℓ}} + Γ'_{3} \hat{v}'_{I_{ℓ}} - Γ {'_{5}}^{ℓ} \hat{a}' +^{R_{k}} \hat{p}'_{L}) = 0

where

Π' (^{R_{k}} \hat{p}'_{L} -^{R_{k}} \hat{p}'_{I_{ℓ}}) = H {'_{p}}^{I_{ℓ}} \hat{p}'_{L} = 0

because ${}^{I_{ℓ}}{\hat{p'}}_{L}$ is in the right nullspace of $H'_{p}$ (see (61)–(62)). Then, what is left to show is

Π' (Γ'_{3} \hat{v}'_{I_{ℓ}} - Γ {'_{5}}^{ℓ} \hat{a}') = 0

where

Γ'_{3} \hat{v}'_{I_{ℓ}} - Γ {'_{5}}^{ℓ} \hat{a}' = - Δ t_{k, ℓ} \hat{v}'_{I_{ℓ}} - \int_{t_{k}}^{t_{ℓ}} \int_{t_{k}}^{τ} {}_{k}^{μ}{C_{\hat{\bar{q}}}^{⊤}} d μ d τ^{ℓ} \hat{a}'

To this end, we examine two special cases: (i) if no rotation (i.e., $ω = 0$ , $\forall τ \in [t_{k}, t_{ℓ}]$ ), then we have

(100)

\begin{matrix} Γ'_{3} \hat{v}'_{I_{ℓ}} - Γ {'_{5}}^{ℓ} \hat{a}' = - Δ t_{k, ℓ} \hat{v}'_{I_{ℓ}} - \int_{t_{k}}^{t_{ℓ}} \int_{t_{k}}^{τ} I_{3} d μ d τ^{ℓ} \hat{a}' \\ = - Δ t_{k, ℓ} \hat{v}'_{I_{ℓ}} - \frac{1}{2} Δ t_{k, ℓ} {}^{ℓ}{\hat{a}'} \end{matrix}

and (ii) if constant local acceleration (i.e., ${}^{τ}{a'} \equiv^{k} a'$ , $\forall τ \in [t_{k}, t_{ℓ}]$ ), then we have

(101)

\begin{matrix} Γ'_{3} \hat{v}'_{I_{ℓ}} - Γ {'_{5}}^{ℓ} \hat{a}' = - Δ t_{k, ℓ} \hat{v}'_{I_{ℓ}} - \int_{t_{k}}^{t_{ℓ}} \int_{t_{k}}^{τ} {}_{k}^{μ}{C_{\hat{\bar{q}}}^{⊤}} \hat{a}' d μ d τ \\ = - Δ t_{k, ℓ} \hat{v}'_{I_{ℓ}} - \int_{t_{k}}^{t_{ℓ}} \int_{t_{k}}^{τ} {}^{R_{k}}{\hat{a}'} (μ) d μ d τ \\ = - Δ t_{k, ℓ} \hat{v}'_{I_{ℓ}} - \int_{t_{k}}^{t_{ℓ}} (^{R_{k}} \hat{v}'_{I_{τ}} -^{R_{k}} \hat{v}'_{I_{k}}) d τ \\ = - Δ t_{k, ℓ} \hat{v}'_{I_{ℓ}} -^{R_{k}} \hat{p}'_{I_{ℓ}} + Δ t_{k, ℓ} \hat{v}'_{I_{k}} \end{matrix}

To ensure that (99) holds, both (100) and (101) should be equal to $0$ , and the conclusion of Lemma 2 is immediate.

Acknowledgements

The authors would like to thank Professor Stefan Leutenegger from Imperial College London and Simon Lynen from ASL/ETH Zurich (at that time) for sharing the Vicon Loops dataset and OKVIS data presented in , and give special thanks to our colleague Patrick Geneva from RPNG/UD for his invaluable help in the urban driving test.

Funding

This research was partially supported by the University of Delaware (UD) College of Engineering, UD Cybersecurity Initiative, the Delaware NASA/EPSCoR Seed Grant, the NSF (grant number IIS-1566129), the DTRA (grant number HDTRA1-16-1-0039), and Google Daydream.

ORCID iD

Zheng Huai

Notes

References

Agarwal

Mierle

and others (2010) Ceres Solver. Available at: http://ceres-solver.org.

Baker

Matthews

(2004) Lucas–Kanade 20 years on: A unifying framework. International Journal of Computer Vision 56(3): 221–255.

Bar-Shalom

Kirubarajan

(2001) Estimation with Applications to Tracking and Navigation. New York: John Wiley & Sons.

Bloesch

Burri

Omari

Hutter

Siegwart

(2017) Iterated extended Kalman filter based visual-inertial odometry using direct photometric feedback. The International Journal of Robotics Research 36(10): 1053–1072.

Bloesch

Omari

Hutter

Siegwart

(2015) Robust visual inertial odometry using a direct EKF-based approach. In: IEEE/RSJ International Conference on Intelligent Robots and Systems. Hamburg, Germany: IEEE, pp. 298–304.

Breckenridge

(1979) Quaternions proposed standard conventions. Technical report, NASA Jet Propulsion Laboratory.

Burri

Nikolic

Gohl

et al. (2016) The EuRoC micro aerial vehicle datasets. The International Journal of Robotics Research 35(10): 1157–1163.

Castellanos

Martinez-Cantin

Tardós

Neira

(2007) Robocentric map joining: Improving the consistency of EKF-SLAM. Robotics and Autonomous Systems 55(1): 21–29.

Chen

Jiang

Hung

(1990) Local observability matrix and its application to observability analyses. In: The 16th Annual Conference of IEEE Industrial Electronic Society, Pacific Grove, CA, pp. 100–103.

10.

Civera

Davison

Montiel

(2008) Inverse depth parametrization for monocular SLAM. IEEE Transactions on Robotics 24(5): 932–945.

11.

Civera

Grasa

Davison

Montiel

(2009) 1-point RANSAC for EKF-based structure from motion. In: IEEE/RSJ International Conference on Intelligent Robots and Systems, St. Louis, MO, pp. 3498–3504.

12.

Eckenhoff

Geneva

Huang

(2016) High-accuracy preintegration for visual–inertial navigation. In: International Workshop on the Algorithmic Foundations of Robotics, San Francisco, CA.

13.

Forster

Carlone

Dellaert

Scaramuzza

(2015) IMU preintegration on manifold for efficient visual–inertial maximum-a-posteriori estimation. In: Robotics: Science and Systems, Rome, Italy.

14.

Golub

Van Loan

(2012) Matrix Computations, Vol. 3. Baltimore, MD: JHU Press.

15.

Guo

Roumeliotis

(2013) IMU-RGBD camera 3D pose estimation and extrinsic calibration: Observability analysis and consistency improvement. In: IEEE International Conference on Robotics and Automation, Karlsruhe, Germany, pp. 2935–2942.

16.

Hesch

Kottas

Bowman

Roumeliotis

(2014a) Camera–IMU-based localization: Observability analysis and consistency improvement. The International Journal of Robotics Research 33(1): 182–201.

17.

Hesch

Kottas

Bowman

Roumeliotis

(2014b) Consistency analysis and improvement of vision-aided inertial navigation. IEEE Transactions on Robotics 30(1): 158–176.

18.

Huai

Huang

(2018) Robocentric visual-inertial odometry. Technical report, RPNG, University of Delaware. Available at: http://udel.edu/∼ghuang/papers/tr_rvio_ijrr.pdf.

19.

Huang

Kaess

Leonard

(2014) Towards consistent visual–inertial navigation. In: IEEE International Conference on Robotics and Automation, Hong Kong, China, pp. 4926–4933.

20.

Huang

Mourikis

Roumeliotis

(2009) A first-estimates Jacobian EKF for improving SLAM consistency. In: Experimental Robotics. New York: Springer, pp. 373–382.

21.

Huang

Mourikis

Roumeliotis

(2010) Observability-based rules for designing consistent EKF SLAM estimators. The International Journal of Robotics Research 29(5): 502–528.

22.

Indelman

Williams

Kaess

Dellaert

(2013) Information fusion in navigation systems via factor graph based incremental smoothing. Robotics and Autonomous Systems 61(8): 721–738.

23.

Jones

Soatto

(2011) Visual–inertial navigation, mapping and localization: A scalable real-time causal approach. The International Journal of Robotics Research 30(4): 407–430.

24.

Kelly

Sukhatme

(2011) Visual–inertial sensor fusion: Localization, mapping and sensor-to-sensor self-calibration. The International Journal of Robotics Research 30(1): 56–79.

25.

Kneip

Chli

Siegwart

(2011) Robust real-time visual odometry with a single camera and an IMU. In: British Machine Vision Conference.

26.

Leutenegger

Lynen

Bosse

Siegwart

Furgale

(2015) Keyframe-based visual–inertial odometry using nonlinear optimization. The International Journal of Robotics Research 34(3): 314–334.

27.

Mourikis

(2013a) High-precision, consistent EKF-based visual–inertial odometry. The International Journal of Robotics Research 32(6): 690–711.

28.

Mourikis

(2013b) Optimization-based estimator design for vision-aided inertial navigation. In: Robotics: Science and Systems, Berlin, Germany, pp. 241–248.

29.

Lupton

Sukkarieh

(2012) Visual–inertial-aided navigation for high-dynamic motion in built environments without initial conditions. IEEE Transactions on Robotics 28(1): 61–76.

30.

Maybeck

(1979) Stochastic Models, Estimation, and Control, Vol. 1. London: Academic Press.

31.

Mourikis

Roumeliotis

(2007) A multi-state constraint Kalman filter for vision-aided inertial navigation. In: IEEE International Conference on Robotics and Automation, Rome, Italy, pp. 3565–3572.

32.

Mourikis

Trawny

Roumeliotis

Johnson

Ansar

Matthies

(2009) Vision-aided inertial navigation for spacecraft entry, descent, and landing. IEEE Transactions on Robotics 25(2): 264–280.

33.

Mur-Artal

Tardós

(2017) Visual–inertial monocular SLAM with map reuse. IEEE Robotics and Automation Letters 2(2): 796–803.

34.

Qin

Shen

(2018) Vins-mono: A robust and versatile monocular visual–inertial state estimator. IEEE Transactions on Robotics 34(4): 1004–1020.

35.

Roumeliotis

Burdick

(2002) Stochastic cloning: A generalized framework for processing relative state measurements. In: IEEE International Conference on Robotics and Automation, Washington, DC, pp. 1788–1795.

36.

Shi

Tomasi

(1994) Good features to track. In: IEEE Computer Society Conference on Computer Vision and Pattern Recognition, Seattle, WA, pp. 593–600.

37.

Tardós

Neira

Newman

Leonard

(2002) Robust mapping and localization in indoor environments using sonar data. The International Journal of Robotics Research 21(4): 311–330.

38.

Trawny

Roumeliotis

(2005) Indirect Kalman filter for 3D attitude estimation. Technical report, Department of Computer Science and Engineering, University of Minnesota.

39.

Triggs

McLauchlan

Hartley

Fitzgibbon

(1999) Bundle adjustment: A modern synthesis. In: International Workshop on Vision Algorithms, Corfu, Greece, pp. 298–372.

40.

Troiani

Martinelli

Laugier

Scaramuzza

(2014) 2-point-based outlier rejection for camera–IMU systems with applications to micro aerial vehicles. In: IEEE International Conference on Robotics and Automation, Hong Kong, China, pp. 5530–5536.

41.

Usenko

Engel

Stückler

Cremers

(2016) Direct visual–inertial odometry with stereo cameras. In: IEEE International Conference on Robotics and Automation, Stockholm, Sweden, pp. 1885–1892.

42.

Weiss

Siegwart

(2011) Real-time metric state estimation for modular vision–inertial systems. In: IEEE International Conference on Robotics and Automation, Shanghai, China, pp. 4531–4537.

43.

Guo

Georgiou

Roumeliotis

(2017) VINS on wheels. In: IEEE International Conference on Robotics and Automation, Singapore, pp. 5155–5162.

44.

Zhang

Song

Huang

Dissanayake

(2017) Convergence and consistency analysis for a 3-D invariant-EKF SLAM. IEEE Robotics and Automation Letters 2(2): 733–740.

45.

Zhang

Scaramuzza

(2018) A tutorial on quantitative trajectory evaluation for visual(–inertial) odometry. In: IEEE/RSJ International Conference on Intelligent Robots and Systems, Madrid, Spain, pp. 7244–7251.