Integrity monitoring of Graph-SLAM using GPS and fish-eye camera

2.1 Pre-processing the measurements

We consider the following measurements as inputs to our SLAM-based IM algorithm: GPS pseudoranges and C/N₀ values from the GPS receiver, pixel intensities from the fish-eye image, motion input obtained from the vehicle motion model, and satellite ephemeris decoded from the navigation message.

2.1.1 Vision module

We pre-process the raw image obtained from the fish-eye camera using our hybrid sky detection algorithm to distinguish the sky-pixels from the non-sky pixels. The pipeline of our hybrid sky detection is seen in Figure 3(a). Our hybrid sky detection takes into account not only the pixel intensities but also prior knowledge regarding the spatial location of the sky pixels.

• Download figure
• Open in new tab
• Download powerpoint

FIGURE 3

An example showing the pipeline of pre-processing the fish-eye image in the vision module

We convert the raw image to gray scale and then perform the median blur operation. The median blur (Wang, Li, Yang, & Gong, 2010) is a nonlinear filter that reduces the noise in the image while keeping the edges relatively sharp. Next, we compute the gradient by combining the magnitude obtained via two Sobel operations (Gao, Zhang, Yang, & Liu, 2010), one executed in horizontal and the other in vertical direction. An example of the image obtained from the fish-eye camera operating in urban areas and pointing upwards is seen in Figure 3(b).

We observe that the probability of sky is highest close to the center and exponentially decreases outwards (Haque, Murshed, & Paul, 2008). Therefore, the corresponding location-based sky probability, denoted by p_loc is given by

1

where u is the two-dimensional (2D) image coordinates, such that u ∈ Π, Π represents the pre-defined domain of the image coordinates. |⋅| denotes the cardinality of the image domain, and ‖⋅‖ denotes the 2-norm residual. c_loc ⊂ u ∈ Π denotes the pre-defined center coordinates in the 2D image frame.

Combining the location probability with Otsu’s method of intensity thresholding (Moghaddam, Cheriet, & Adotsu, 2012), we compute the hybrid optimal border, seen in Figure 3(c), that separates the sky region, represented by subscript sky, from that of the infrastructure, denoted by subscript inf. We minimize the variance of sky and infrastructure to estimate the Otsu’s intensity threshold I_otsu as

2

where

• – I denotes the intensity vector that stacks all the pixel intensities in the fish-eye image, such that I = {I(u)| u ∈ Π}, where I(u) ∶ Π → ℝ denotes the intensity of any 2D pixel coordinates u;

• – ω_sky(k) and ω_inf(k) denote the weights associated with the sky and building infrastructure, respectively, such that and ;

• – and denote the variance of the pixel intensities associated with the sky and building infrastructure.

Utilizing Equations (1) and (2), we compute the hybrid sky probability, denoted by p_sky at any 2D image coordinate u, u ∈ Π as

3

where I_max and I_min are the maximum and minimum intensity values in the fish-eye image, such that I_max = max_u∈Π I(u) and I_min = min_u∈Π I(u), respectively. Considering η as the pre-defined sky threshold, if p_sky(u) > η, it is categorized as sky pixel and non-sky pixel otherwise. The sky-enclosed area in the fish-eye image is seen in Figure 3(d). The value of η is set during initialization and explained later in Section 3.2.

Using the non-sky detected pixels, we describe the vision measurement model of an omni-directional fish-eye camera in Eq (4), which is formulated via direct image alignment (Caruso, Engel, & Cremers, 2015) and semi-dense inverse depth map estimation (Engel, Schöps, & Cremers, 2014). Instead of utilizing standard algorithms (Klein & Murray, 2007; Mur-Artal, Montiel, & Tardos, 2015) that rely on feature detection and extraction, we perform direct image alignment that directly processes the pixel intensities of keyframe and current image frame to estimate the relative change in position and orientation of the vehicle. The selected pixels of the key frame, termed as key pixels, refer to the ones that exhibit a high intensity gradient. Details regarding the keyframe selection are explained in the existing literature (Engel et al., 2014).

4

with η_vis(u) is pixel noise and from Caruso et al. (2015),

where the subscript kf refers to keyframe,

• – I_kf(u) ∶ Π_kf → ℝ denotes the intensity of any 2D pixel coordinates u in the reference keyframe, and Π_kf ⊂ ℝ² denotes the image domain of keyframe;

• – I_t(u) ∶ Π_ns → ℝ denotes the intensity of any 2D pixel coordinates u in the current frame, and Π_ns ⊆ Π denotes the image domain consisting of non-sky pixels;

• – π ∶ ℝ³ → Π_ns denotes the mapping function of any 3D world position coordinates p = [p_x,p_y,p_z] to a 2D pixel in the image frame;

• – w(Δμ, u) denotes the 3D warp function that unprojects the pixel coordinates u and transforms it by a relative state vector Δμ. The relative state vector Δμ indicates the difference between the current vehicle pose, denoted by μ_t = [x, ψ]_t with respect to that of the keyframe, denoted by μ_kf, such that Δμ = μ_t ‒μ_kf; here, x = [x,y,z] denotes the 3D vehicle position and ψ denotes the 3D orientation with respect to the world frame; R ∈ SO(3) and t ∈ ℝ³ denotes the rotation matrix and translation vector of Δμ, respectively;

• – π^‒1 ∶ Π_ns ×ℝ⁺ → ℝ³ denotes the inverse mapping of 2D pixel coordinates to 3D world coordinates via an inverse distance represented by d. Here, and denotes the transformed 2D pixel coordinates. The intrinsic camera parameters are estimated during initialization and are denoted by f_x, f_y, c_x, c_y and ξ;

• – d_kf(u) denotes the inverse distance of the pixel coordinates in the keyframe.

Direct image alignment accounts for the similarity in urban building infrastructure by preserving the spatial context of the image. We compute a semi-dense map that estimates the inverse-depth of key pixels in the keyframe by executing a short-temporal baseline matching across images. This vision measurement model is utilized later in our extended graph optimization to formulate the corresponding vision odometry-based component of the cost function.

2.2 Extended graph optimization

In our extended graph optimization module, our cost function consists of four error terms, namely GPS pseudoranges, non-sky pixel intensities, receiver motion model, and satellite orbital model, as follows:

7

where

• – θ_t denotes the overall state vector comprising of the state vector of the vehicle, GPS satellites, and key pixels in the world frame, given by and is unknown to be estimated via graph optimization;

• – Λ denotes the M-estimator function of weighted residuals, where M-estimator (Shepard & Humphreys, 2014) represents a robust regression method that accounts for data containing outliers, extreme observations, or that does not follow a normal distribution. Here, Λ(⋅) is a symmetric, positive-definite function with a unique minimum at zero. For instance, the conventional least-squares is a special case of M-estimator, where Λ(Δz) = (Δz)². The choice of M-estimator for this work is given in Section 3.2;

• – denotes the estimated fault status of the received GPS pseudorange from k^th satellite at the past time instant; similarly, denotes the estimated vision fault status of any 2D pixel u ∈ Π_kf at the previous time instant; the GPS and vision fault status are estimated later in Section 2.3.1;

• – h denotes the GPS measurement model obtained from Equation (5), such that ; g denotes the motion model of the receiver and f denotes the satellite orbital model; and denote the estimated state vector of the vehicle and k^th satellite, respectively, at the previous time instant; u_x,t and denote the motion control inputs of the vehicle and k^th satellite, respectively; later in Section 3.2, we describe the choice of vehicle motion model and satellite orbital model used to validate our algorithm;

• – and denote the estimated covariance matrices of the vehicle state vector and k^th satellite state vector. For simplicity, we estimate the state covariances via a statistical process known as propagation of uncertainty (Engel et al., 2014);

• – denotes the integrity vector comprising of protection levels associated with each term in the vehicle state vector x_t. Details regarding estimating the covariances and protection levels are explained later in Section 2.3;

• – denotes the measurement covariance of the k^th satellite and is estimated from Equation (6); similarly, ω_t(u) denotes the covariance associated with the intensity of non-sky pixel u and is estimated based on existing literature (Eckenhoff, Geneva, & Huang, 2019).

The first three terms in the cost function e_t given in Equation (7) correspond to the residuals associated with the GPS pseudoranges, satellite ephemeris, and vehicle state vector. These terms are equivalent to the terms considered in our prior work on GPS-only SLAM-based FDI (Bhamidipati & Gao, 2018). The last term represents the summation of intensity residuals across non-sky pixels based on the vision measurement model explained in Equation (4). In particular, we perform sub-graph optimization at each instant, as seen in Equation (8), where the cost function is formulated using the past history of measurements.

8

where T denotes the number of time instants utilized in the sub-graph optimization thread and denotes our SLAM-based IM estimate of the overall state vector computed during the sub-graph optimization. We estimate the key pixels in the world frame, represented by , via inverse-mapping defined in Equation (4).

In addition to sub-graph optimization, which is executed at each instant, we also periodically perform full-graph optimization, which optimizes the entire graph and binds the overall drift errors in our 3D world map. In this work, when a new reference keyframe is chosen, we perform full-graph optimization to compute its corresponding semi-dense inverse depth map. This occurs at a slower update rate as compared to sub-graph optimization so as to reduce the overall computational complexity. We referred to Engel et al. (2014) for executing the full-graph optimization.

2.3 IM for multi-sensor Graph-SLAM framework

By expressing our extended graph optimization, explained earlier in Section 2.2, as a nonlinear weighted LS problem, we compute the protection levels of the estimated vehicle position using worst-case failure mode slope analysis (Joerger et al., 2014). For this, we refer to the existing rich literature (Joerger et al., 2014; Salós et al., 2013) on worst-case failure mode slope analysis for a linear LS estimator. The formulation of our extended graph optimization as a nonlinear WLS problem is shown later in Section 2.3.2. However, there are challenges involved in applying worst-case failure slope analysis for the protection-level computation of Graph-SLAM framework. The challenges and our design solutions are listed as follows:

1. The worst-case failure mode slope is derived for a linear measurement model, but the cost function associated with the Graph-SLAM is nonlinear. So, we linearize the formulation of the cost function at the estimated overall state vector.

2. The Graph-SLAM is a sequential method, whereas the worst-case failure mode slope falls under the category of snapshot IM technique. Therefore, we formulate a batch realization of the linearized Graph-SLAM that incorporates not only the measurements received at the current time instant, but also the past time history. This incorporates the temporal aspect of Graph-SLAM in computing the protection level.

3. Our Graph-SLAM framework consists of a large number of states and measurements. However, evaluating all possible measurement fault modes is computationally cumbersome. Therefore, using multiple FDI modules, we directly estimate one fault mode, which is a binary vector, such that an element of the fault mode is one if the corresponding measurement is flagged as faulty and zero if otherwise.

2.3.2 Protection-level computation

In accordance with the design solutions listed earlier, we linearize the sub-graph optimization framework, seen in Equations (7) and (8), using the Taylor-series expansion (Shepard & Humphreys, 2014). Using measurement equations described in Equations (4) and (5), we derive the batch realization of linearized Graph-SLAM in Equation (11), which is based on the formulation of batch realization derived for a linear LS estimator in Joerger and Pervan (2013). Batch realization refers to stacking the measurement equations obtained across the desired time history.

11

where

• – Δz_T denotes the overall measurement vector of subgraph optimization, such that Δz_T = [Δz_t‒T,…,Δz_t]^⊤ where Δz_t denotes the measurement vector at t^th time and concatenates the GPS pseudoranges, vehicle motion input, satellite ephemeris, and keyframe pixel intensities; similarly, η_T denotes the overall measurement noise;

• – C_T denotes the linearized overall measurement model that represents the sub-graph optimization, such that C_T = diag{C_t‒T,…,C_t}. Here, C_t denotes the measurement model at t^th time that vertically stacks the Jacobian associated with the GPS pseudoranges, denoted by H, vehicle motion model and satellite orbital model, denoted by A and non-sky pixel intensities, denoted by B, such that ;

• – f_T denotes the overall fault vector associated with the overall measurement vector and, thereby, stacks measurement faults obtained from individual sensor sources and across time instants from t‒T to t.

Re-writing Equations (7) and (8), we express our M-estimator-based Graph-SLAM formulation that is solved via Levenberg-Marquardt (Ranganathan, 2004) algorithm, as a weighted nonlinear LS problem in Equation (12).

12

where

• – K_T denotes the overall estimation matrix, such that K_T = diag{K_t‒T,…,K_t}. Here, K_t denotes the estimation matrix at t^th time, such that and V denotes the pseudo-inverse matrix, such that ;

• – β denotes the damping factor of the LM algorithm that is set during initialization, which is described later in Section 3.2.

• – S_t, R_t, P_t denotes the M-estimator-based weight functions for the GPS pseudoranges, motion models of vehicle and satellites, and non-sky pixel intensities, respectively, and evaluated at . Details regarding the weight function of an M-estimator are explained later in our prior work (Bhamidipati & Gao, 2018);

Next, we define the overall test statistic, denoted by ζ, as the summation of the weighted squared residuals across all the measurements. We consider an assumption that the overall test statistic is the chi-square distributed, denoted by under non-faulty conditions, and non-central chi-squared, denoted by , under the presence of GPS faults or vision faults or both.

13

with

14

where k denotes the number of redundant measurements, i.e., the difference between the number of overall measurements, denoted by n and overall states, denoted by l, such that k = n‒l. λ indicates the non-centrality parameter associated with the overall test statistic during faulty conditions.

According to the worst-case failure mode slope analysis (Salós et al., 2013), as seen in Figure 5, the protection level is calculated as the projection in the position domain of the measurement faults that would generate a non-centrality parameter λ = λ_th in the overall test statistic ζ with the maximum slope. In particular, the non-centrality parameter λ_th is estimated from the false-alarm, denoted by p_FA and mis-detection rates, denoted by p_MD, which are set according to the pre-defined integrity requirements.

We formulate the overall measurement fault mode, denoted by b_T, where b_T = [b_t‒T,…,b_t]^⊤. Here, b_t is the measurement fault mode, which is estimated in Equation (15) using GPS and vision fault status obtained via Equations (9) and (10). For this, we consider a pre-defined fault threshold, denoted by κ, such that if the fault status is above κ, the measurement is flagged as faulty in the computation of protection levels. Given that we consider measurement faults in only GPS and vision, the fault status of receiver and satellite motion models is set to zero. However, the corresponding fault vector, which comprises of the exact measurement fault magnitudes, is still unknown. According to Salós et al. (2013), for a given fault mode, the worst-case fault direction that maximizes the integrity risk is the one that maximizes the failure mode slope, which is seen in Figure 5 and is denoted by g_b.

• Download figure
• Open in new tab
• Download powerpoint

FIGURE 4

Pipeline for superpixel-based piecewise RANSAC that is used to estimate the fault status of vision-based superpixels

• Download figure
• Open in new tab
• Download powerpoint

FIGURE 5

Protection levels computed as the intersection of worst-case failure mode slope and non-centrality parameter (Salós et al., 2013)

15

In Equation (16), we define the square of failure mode slope, denoted by , as the ratio of squared state estimation error in the vehicle position, which is denoted by ε, over the squared overall test statistic, which is denoted by ζ. Using the linearized equations seen in Equations (11)-(13), we derive the failure slope for the sub-graph optimization framework in terms of the unknown fault vector. For this, we consider C_TK_TC_T is negligible, which is a valid approximation after the iterative convergence of the graph optimization at any time instant since β << 1. Considering n_b to be the number of non-zero entries in the fault mode b_T estimated via multiple FDI module, we define the fault matrix, denoted by B_b, as B_b = [I_nb×nb,O_(n‒nb)×nb]^T, where I_nb×nb is an identity matrix of size n_b ×n_b and O_(n‒nb)×nb is a zero matrix of size (n ‒ n_b)×n_b. Note that I is different from the intensity vector I defined earlier in Equation (2). We re-arrange the rows of the m_ε and M_ζ matrices, such that the faulty measurements are stacked first. Thereafter, we define a transformed fault vector, denoted by f_ζ, such that f_T = B_bM_ζf_ζ. Based on the above-mentioned steps, we describe the failure slope formulation of Graph-SLAM framework in Equation (16).

16

where α extracts a state of interest from the overall state vector θ, such that α^T = [O_{1×(l‒1‒la}), 1, O_1×la] where l_a indicates the number of states before the state of interest. M_ζ denotes the residual matrix, such that and m_ε represents the state gain matrix, such that .

Referring to Joerger et al. (2014), for a given fault mode but unknown fault vector, the worst-case failure slope equals the maximum eigenvalue of the corresponding failure slope formulation. Therefore, we express the worst-case failure slope of the Graph-SLAM framework as

17

Next, we compute protection level for the state of interest considered in α^T, seen in Equation (18), as the y-coordinate that corresponds to the integrity metric λ_th along the line passing through the origin and with slope given by .

18

3.1 Experimental Setup

We utilize a ground vehicle to perform two real-world experiments: one in the alleyway of Stanford, California and the other in the semi-urban area of Champaign, Illinois. Our experimental setup comprises of a UBLOX NEO-M8U GPS receiver that is connected to a roof-mounted patch antenna. We also have an upward-facing ELP USB camera with 8 mega pixel resolution and fitted with a 180° FOV fish-eye lens. In addition, we utilize the navigation solution estimated by a high-fidelity RTK-GNSS Trimble BD940-INS receiver equipped with a Zephyr antenna as the ground truth reference. We utilize the Robot Operating System (ROS) to publish the measurements obtained from the GPS receiver and fish-eye camera. We record the data in rosbag files, which are later post-processed to validate the proposed algorithm. We chose ROS as our data collection platform because it provides an easy-to-implement interface to obtain measurements from varied sensors and synchronize them to a common UNIX epoch time. The frame transformations between the body frame of GPS receiver, fish-eye camera, and ground truth is measured before the start of the experiment and is given as a user-defined input during the initialization of our SLAM-based IM algorithm.

3.2 Implementation details

Before we post-process the experimental data, we refer to the existing work (Scaramuzza, Martinelli, & Siegwart, 2006) on omni-directional cameras for executing the calibration procedure. In particular, we capture a multiple images of the checkerboard taken from different viewpoints, to estimate the intrinsic parameters of the fish-eye camera, namely f_x,f_y,c_x,c_y, ξ, which are defined earlier in Equation (4). These intrinsic parameters map the image plane to a 3D camera frame, where the origin of the camera frame is at its optical center, x and y axes are aligned with the image plane. This process also undoes the distortion caused in the images because of its wide FOV.

As explained earlier in Section 2, we perform initialization in two stages: one in open-sky and the other in an urban setting. In the first stage of open-sky initialization, we create a 3D world map using the PVT of receiver and GPS satellites, which are estimated via weighted LS (Salós et al., 2013). Here, world frame represents the Earth-Centered Earth-Fixed (ECEF) frame in which the GPS operates. We also empirically analyze the pseudorange residuals to independently compute the non-faulty Gaussian distribution of all visible GPS satellite signals. We estimate the vehicle motion inputs by added simulated measurement noise to the ground truth velocity measurements. This is justified because, as explained earlier, in this work, we consider no faults/anomalies in the vehicle motion model and satellite ephemeris. However, for readers interested in accounting for faults in the vehicle motion model, the motion inputs can be extracted from an IMU sensor and the fault mode vector b_t, described in Equation (15), can be modified accordingly. The second stage of initialization is executed after the vehicle enters the semi-urban/urban surroundings. We process multiple instances of short-temporal baseline images to select a reference keyframe. Based on the keyframe, we compute the extrinsic parameters, i.e., frame transformation from the camera to world frame.

We utilize a combination of Huber M-estimator (Huber, 2004) and Tukey bisquare M-estimator (Hoaglin, 2003) to formulate the cost function. The justification regarding our choice of M-estimator is explained in our prior work (Bhamidipati & Gao, 2018). During initialization and when the total number of landmarks are ≤ 15, we utilize Huber M-estimator to ensure convergence and in all the other conditions, we opt for a bisquare M-estimator to achieve better accuracy. Detailed explanation regarding the M-estimator equations and the corresponding weight functions are discussed in our prior work (Bhamidipati & Gao, 2018). Based on heuristic analysis, we set the parameters of different modules in the proposed multi-sensor SLAM-based IM as follows: vision coefficients as b_LOS = 2, b_NLOS = 3, a_LOS = 5 and a_NLOS = 50, number of superpixels as Γ = 50, damping factor of LM algorithm as β = 0.02. To minimize mis-detection rates, we set a conservative value of η = 0.7 for the pre-defined sky threshold described in Equation (3). Also, the fault status threshold of our multi-sensor SLAM-based IM is set as κ = 0.65.

3.3 Experiment-1: Alleyway in Stanford, California

The first set of experiments are conducted in an alleyway in Stanford, California where the ground vehicle drove along the street for a duration of 45 s. As seen in Figure 6, the entire duration of the experiment can be divided into three urban contexts: first is the magenta highlighted region during t < 8 s that represents relatively open-sky conditions, second is the red highlighted region between t = 8‒25 s that represents the region where the ground vehicle passes from under an overhead bridge, and third is the blue highlighted region for t > 25 s that indicates a semi-urban setting with feature-rich visual surroundings. The aim of these experiments is to analyze the convergence trend of the proposed multi-sensor SLAM-based IM algorithm and its performance in the presence of different urban challenges.

• Download figure
• Open in new tab
• Download powerpoint

FIGURE 6

Route taken by a ground vehicle during the first experiment conducted for 45 s. For t < 8 s, which is highlighted in magenta, the vehicle operates in open-sky conditions. Between t = 8‒25 s, which is indicated by the red highlighted region, the vehicle passes from under an overhead bridge and suffers from severely degraded satellite visibility. For t > 25 s, which is highlighted in blue, the vehicle operates in the semi-urban region with tall buildings that block a section of the sky

In Figure 7, we demonstrate an improved performance of the proposed algorithm that utilizes the GPS and fish-eye camera. Our first and second stage of initialization, explained earlier in Section 3.2, is carried out till t = 2 s. We observe that the weighted LS approach using GPS-only shows large localization errors both when the ground vehicle passes from under the overhead bridge and also when surrounded by tall buildings. Unlike the LS approach that shows a mean of 19.51 m and standard deviation of 9.12 m, the proposed multi-sensor SLAM-based IM algorithm demonstrates a low mean of 6.14 m and standard deviation of 0.97 m. We also demonstrate robust protection levels that accurately bind the localization errors for the entire duration. The convergence of our protection levels is achieved at around t ≈ 7 s. The size of the protection level shows a low mean of 6.9 m and standard deviation of 2.44 m. Here, the size of the protection level is indicated in Figure 7 by the gap between the black line, which denotes the protection level and blue line, which denotes the localization error of SLAM-based IM.

• Download figure
• Open in new tab
• Download powerpoint

FIGURE 7

Comparison of the localization errors obtained via multi-sensor SLAM-based IM that utilizes GPS and fish-eye camera, which is plotted in blue, with that of the traditional least squares, which is plotted in red. Tight protection levels are also achieved via multi-sensor SLAM-based IM that accurately binds the localization errors for the entire duration

3.4 Experiment-2: Semi-urban area of Champaign, Illinois

The second set of experiments are conducted in the semi-urban area of Champaign, Illinois for a duration of 100 s. During t = 0‒70 s, the ground vehicle operates in open-sky conditions, thereby experiencing no GPS faults but less visual features. In Figure 8, the blue highlighted region suffers from vision challenges, namely illumination variations due to sunlight and shadows, that cause data association errors across images. Similarly, the red highlighted region is enclosed with tall buildings that lead to multipath effects in the GPS measurements. For instance, at t = 78 s, we showcase the true overlap of the GPS satellite positions over the fish-eye image, where three out of the seven visible satellites are likely to be affected by multipath.

• Download figure
• Open in new tab
• Download powerpoint

FIGURE 8

Route taken by a ground vehicle during the experiment conducted for 100 s. Between t = 70 ‒ 100 s, the vehicle experiences GPS faults due to multipath and vision faults due to illumination variations. At t = 78 s, the overlap of the skyplot of GPS satellites with the fish-eye image shows the multipath-affected GPS measurements

Figure 9 shows the average fault status of GPS pseudoranges and vision superpixels, as indicated in red and blue, respectively. Given that the ground vehicle navigates in open-sky conditions for t < 70 s, the average GPS fault status estimated via our multiple FDI module is low, whereas the average vision fault status is high due to the feature-less surroundings. As the vehicle passes through the red highlighted region shown in Figure 8 that represents the semi-urban area, the average fault status of vision is low but that of the GPS increases due to multipath. Next, we further analyze the performance of our multiple FDI modules in the challenging semi-urban area, i.e., for t > 70 s during which the ground vehicle experiences GPS and vision faults. Figure 10(a) plots that the individual GPS fault status of three out of the seven visible satellites with PRNs 6, 12, and 2. In accordance with the skyplot shown in Figure 8, our proposed SLAM-based IM algorithm successfully flags the satellites with PRN 6 and 12 as faulty while accurately estimating the high-elevation satellite with PRN 2 as non-faulty. A key point to note is that we consider fault status to simply indicate the confidence in the reliability of the landmark-measurement pair. Based on our earlier discussion related to Figure 1, the fault status doesn’t flag the measurement based on whether its a reflected signal or not, but on the relevance of information provided by the measurement. During the same duration, we also analyze the vision fault status associated with the superpixels. In Figure 10(b), at each time instant, we plot the top four fault statuses of the superpixels, such that each marker represents a superpixel. We observe that in the urban region, the value of the associated vision fault status decreases due to feature-rich tall buildings in urban areas. However, when the vehicle enters the blue highlighted region seen in Figure 10(b), the illumination variations induced by the bright sunlight causes the fault status associated with certain superpixels to show an increasing trend.

• Download figure
• Open in new tab
• Download powerpoint

FIGURE 9

Performance of our multiple FDI module via average fault status of GPS pseudoranges, indicated in red, and vision superpixels, indicated in blue. When the ground vehicle navigates through the semi-urban region, i.e., for t > 70 s, the average fault status associated with GPS is high due to multipath, whereas vision is low due to rich features

• Download figure
• Open in new tab
• Download powerpoint

FIGURE 10

Estimated fault status of (a) GPS measurements and (b) vision superpixels during t = 70 ‒ 100 s, i.e., when the ground vehicle navigates through the semi-urban area. In (a), our multiple FDI module successfully detects satellites with PRN 6, 12 as faulty while accurately estimating the PRN 2 as non-faulty. In (b), where each marker indicates a superpixel, the trend of fault status associated with superpixels is low given the rich features but, later, increases due to illumination changes

We demonstrate the improved performance of the SLAM-based IM algorithm that utilizes GPS and fish-eye camera seen in Figure 11(a), as compared to the SLAM-based IM algorithm that utilizes GPS-only seen in Figure 11(b). By utilizing GPS and fish-eye camera, we demonstrate higher localization accuracy, with a mean of 8.8 m and standard deviation of 1.73 m, as compared to employing GPS-only that shows a mean of 16.2 m and standard deviation of 2.86 m. We also validate that a lower mean size of protection levels are estimated using GPS and fish-eye camera, i.e., 6.5 m than using GPS-only, i.e., 10.5 m thereby, achieving tighter protection levels. Here, the size of the protection level denotes the gap between the black line, which indicates the protection level, and blue line, which indicates the localization error associated with SLAM-based IM.

• Download figure
• Open in new tab
• Download powerpoint

FIGURE 11

Comparison of SLAM-based IM in Champaign, Illinois: (a) using GPS and fish-eye camera; (b) using GPS-only. Lower localization errors and tighter protection levels are achieved via GPS and fish-eye camera as compared to GPS-only

In Figure 11(a), we also compare the localization accuracy of the proposed multi-sensor SLAM-based IM, indicated by the blue line, with that of multi-sensor SLAM-based IM that excludes GPS satellites as landmarks, indicated by the green line. To perform this analysis, instead of considering the GPS satellite state vector y^k as unknown in the overall state vector θ, we consider this to be known information that is computed via satellite ephemeris. We observe that by excluding GPS satellites as landmarks, we achieve localization errors that are better than GPS-only Graph-SLAM but equal or worse than multi-sensor Graph-SLAM. This is justified because, as explained earlier in Section 1.1, not considering GPS satellites as landmarks potentially lowers the available measurement redundancy in the presence of multipath effects.