## Abstract

Adopting a joint approach toward state estimation and integrity monitoring results in unbiased integrity monitoring unlike traditional approaches. So far, a joint approach was used in particle RAIM (Gupta & Gao, 2019) for GNSS measurements only. In our work, we extend Particle RAIM to a GNSS-camera fused system for joint state estimation and integrity monitoring. To account for vision faults, we derived a probability distribution over position from camera images using map-matching. We formulated a Kullback-Leibler divergence (Kullback & Leibler, 1951) metric to assess the consistency of GNSS and camera measurements and mitigate faults during sensor fusion. Experimental validation on a real-world data set shows that our algorithm produces less than 11 m position error and the integrity risk over bounds the probability of HMI with 0.11 failure rate for an 8 m alert limit in an urban scenario.

## 1 INTRODUCTION

In urban environments, GNSS signals suffer from a lack of continuous satellite signal availability, non-line-of-sight (NLOS) errors, and multipath effects. Thus, it is important to quantify the integrity or measure of trust in the correctness of the positioning solution provided by the navigation system. Traditional integrity monitoring approaches estimate the state and detect faults sequentially (Zhu et al., 2018); they assume that the state estimation algorithm is correct and then assess the integrity of the point positioning solution.

While this sequential approach captures the uncertainty in the state in nominal scenarios, it biases the estimation error when multiple measurements are faulty or contain high bias values. Since traditional approaches do not account for multiple fault hypotheses, they can exclude the wrong fault in the above scenarios. Subsequently, they fail to quantify the uncertainty in the state which leads to erroneous positioning estimates.

Recently, an approach toward joint state estimation and integrity monitoring for GNSS measurements was proposed in *Particle RAIM for integrity monitoring* (Gupta & Gao, 2019). Instead of producing point-positioning estimates, particle RAIM uses a particle filter to form a multimodal probability distribution over position, represented as particles. Traditional RAIM (Lee, 1986) is used to assess the correctness of different ranging measurements and the particle weights are updated to form the distribution over the position. From the resulting probability distribution, the integrity risk is derived using an approximate upper bound to the probability of HMI or the reference risk. By incorporating the correctness of different measurement subsets directly into the state estimation, particle RAIM is able to exclude multiple faults in GNSS ranging measurements. However, due to large errors from GNSS measurements, particle RAIM requires the employment of conservative measures such as large alert limits to adequately bound the reference risk.

For urban applications, improved positioning accuracy from particle RAIM is necessary to provide adequate integrity for smaller alert limits. Since measurements from GNSS are not sufficient to provide the desired accuracy, it is helpful to augment GNSS with additional sensors that increase redundancy in measurements. Sensors such as cameras are effective complimentary sensors to GNSS. In urban regions, cameras have access to rich environmental features (Bhamidipati & Gao, 2019; Rife, 2012; Wang et al., 2020) and provide more superior sensing than GNSS which suffers from multi-path and NLOS errors (Chengyan et al., 2014; Steven Miller et al., 2015; Zhu et al., 2018).

Thus, with added vision, we need a framework to provide integrity for the fused GNSS-camera navigation system to account for two categories of faults. The first category includes data association errors across images, where repetitive features are found in multiple images creating ambiguity during feature and image association. This ambiguity is further amplified due to variations in lighting and environmental conditions. The second category is comprised of errors that arise during the sensor fusion of GNSS and camera measurements. Ensuring that faults in either measurement do not dominate the sensor fusion process is paramount for maximizing the complimentary characteristics of GNSS and camera.

Many works provide integrity for GNSS-camera fused systems utilizing a Kalman filter (Kalman, 1960) framework or an information filter (Wang et al., 2009). Vision-aided RAIM (Fu et al., 2015) introduced landmarks such as pseudo-satellites and integrated them into a linear measurement model alongside GPS observations. In Tanil et al. (2018), the authors implemented a sequential integrity monitoring approach to isolate single satellite faults. The integrity monitor used the innovation sequence output from a single Kalman filter to derive a recursive expression of the worst-case failure mode slopes and compute protection levels (PLs) in real time.

An information filter (IF) is used in Al Hage and El Najjar (2020) for data fusion wherein faults are detected based on the Kullback-Leibler divergence (KL divergence; Kullback & Leibler, 1951) between the predicted and updated distributions. After all detected faulty measurements were removed, the errors were modeled by a student’s T-distribution to compute a PL. A student’s T-distribution is also used in Al Hage et al. (2018) alongside informational sensor fusion for fault detection and exclusion.

The degree of the distribution was adapted in real time based on the computed residual from the information filter. A distributed information filter is proposed in Tmazirte et al. (2012) to detect faults in GPS measurement by checking the consistency through the log-likelihood ratio of information innovation of each satellite. These approaches model measurement fault distributions with a Gaussian distribution although for camera measurements, the true distribution might be non-linear, multi-modal, and arbitrary in nature. Using a simplified linear measurement probability distribution renders these frameworks infeasible and unreliable for safety-critical vision-augmented GNSS applications.

Another line of work builds on simultaneous localization and mapping (SLAM) based factor graph optimization techniques. Bhamidipati and Gao (2019) derived PL by modeling GPS satellites as global landmarks and introducing image pixels from a fish-eye camera as additional landmarks. The raw image was categorized into sky and nonsky pixels to further distinguish between LOS and NLOS satellites. The overall state is estimated using graph optimization along with an M-estimator.

Although this framework is able to exclude multiple faults in GPS measurements, it is not extendable to measurements from forward or rear-facing cameras that do not capture sky regions. Along similar lines, measurements from a stereo camera along with GNSS pseudoranges were jointly optimized in a graph optimization framework in Gong et al. (2018). GNSS satellites are considered as feature vision points and pose-graph SLAM is applied to achieve a positioning solution. However, graph optimization approaches also share the same limitation as Kalmanfilter-based approaches: They produce point-positioning estimates and do not account for the uncertainty in state estimation that biases integrity monitoring.

Overall, existing integrity monitoring algorithms for GNSS-camera fusion have the following limitations:

They address state estimation and integrity monitoring separately, similar to traditional RAIM approaches.

They accommodate camera measurements within a linear or linearizable framework such as KF, EKF, or IF, and become infeasible when camera measurements are not linearizable without loss of generality.

There is no standard way in literature to quantify the uncertainty in camera measurements directly from raw images.

They use outlier rejection techniques to perform fault detection and exclusion after obtaining the positioning solution. There is no framework that accounts for faults both independently in GNSS and camera as well as the faults that arise during sensor fusion.

In our work, we propose the following contributions. This paper is based on our recent ION GNSS+ 2020 conference paper (Mohanty et al., 2020):

We jointly address state estimation and integrity monitoring for GNSS-camera fusion with a particle-filtering framework. We treat both tasks under a single optimization framework with our proposed KL divergence metric in Section 5. First, we leverage a RAIM-based voting scheme to form the GNSS measurement likelihood in a fault-tolerant manner. The likelihood is represented with a Gaussian mixture model (GMM) where the weight of each measurement corresponds to the measurement quality. With this GMM and our proposed KL divergence metric, we then form the camera measurement likelihood in a fault-tolerant manner. The divergence metric optimizes the weights of the camera measurements by minimizing the divergence of the camera likelihood with respect to the GNSS likelihood. Note that both GNSS and camera likelihoods represent valid probability distributions of the state and have been assessed independently. The distributions are fused to form the joint distribution that reliably approximates the distribution of the state even in the presence of multiple faulty measurements. Our approach directly monitors the integrity in the 3D position while forming the probability distribution of the state.

We derived a probability distribution over position directly from images leveraging image registration.

We developed a metric based on KL divergence (Kullback & Leibler, 1951) to compute the joint measurement likelihood from both GNSS and camera measurements in a fault-tolerant manner. By minimizing the KL divergence of the distribution from each camera measurement with respect to the GNSS measurement distribution, we ensure that erroneous camera measurements do not affect the overall probability distribution. Stated otherwise, the divergence metric augments the shared belief over the position from both sensor measurements by minimizing cross-contamination during sensor fusion.

We experimentally validated our framework on an urban environment data set (Reisdorf et al., 2016) with faults in GNSS and camera measurements.

The rest of the paper is organized as follows: In Section 2, we describe the overall particle-filter framework for probabilistic sensor fusion; in Sections 3 and 4, we infer a distribution over position from GNSS and camera measurements, respectively; Section 5 elaborates on the probabilistic sensor fusion of GNSS and camera measurements along with the proposed KL divergence metric; in Section 6, we describe the integrity risk bounding; Sections 7 and 8 show the experimental setup and the results from experimental validation on the urban environment data set, respectively; and in Section 9, we conclude our work.

## 2 PARTICLE FILTER FRAMEWORK FOR PROBABILISTIC SENSOR FUSION

The distribution over the position inferred from GNSS and camera measurements is multi-modal due to faults in a subset of measurements. To model such distributions, we choose a particle-filtering approach that further allows us to keep track of multiple position hypotheses rather than a single position estimate. Although a particle-filtering approach was used in Gupta and Gao (2019), the authors only considered GNSS ranging measurements. In our work, we extend the framework to include measurements from a camera sensor.

Figure 1 represents our overall framework. We add the camera and probabilistic sensor fusion modules to the proposed framework in Gupta and Gao (2019).

**Problem Setup**: We consider a vehicle navigating in an urban environment. It receives GNSS measurements, camera measurements, and odometry measurements. The state vector consists of the vehicle’s 3D position in the Earth-centered, Earth-fixed (ECEF) coordinate frame. At any time instant 𝑡, the state vector is denoted by 𝑋^{𝑡} and is approximated by a probability distribution 𝜋^{𝑡}. Let us denote the set of GNSS pseudorange measurements with 𝑀^{𝑡} and the set of camera measurements with 𝑁^{𝑡}. We assume that the camera measurements have been synchronized leveraging motion data 𝑢^{𝑡}. Using a conventional particle filter, the probability distribution of the state is given by:

1

where 𝑃(𝑋^{𝑡}|𝑋^{𝑡‒1},𝑀^{𝑡},𝑁^{𝑡},𝑢^{𝑡}) is the conditional probability distribution that is obtained after applying the Bayes theorem and Markov assumption on the state 𝑋^{𝑡}. This assumption entails that the state 𝑋^{𝑡} depends only on the state 𝑋^{𝑡‒1} instead of the entire history of states and measurements (𝑋^{1}, 𝑋^{2},.. 𝑀^{1}, 𝑀^{2},., and 𝑁^{1}, 𝑁^{2},…). The above distribution can be further factored as follows after applying independence assumptions on 𝑀^{𝑡}, 𝑁^{𝑡}, and 𝑢^{𝑡}:

2

where 𝑃(𝑀^{𝑡},𝑁^{𝑡}|𝑋^{𝑡}) represents the joint likelihood from GNSS and camera measurements, and 𝑃(𝑋^{𝑡}|𝑋^{𝑡‒1},𝑢^{𝑡}) is the propagation term. We approximate this term by using the probability distribution at the previous time epoch and the odometry measurement.

In addition to the joint likelihood term 𝑃(𝑀^{𝑡},𝑁^{𝑡}|𝑋^{𝑡}), we introduce and utilize two other likelihoods in the paper– GNSS log likelihood and camera likelihood. The GNSS log likelihood is derived in Section 3. The camera likelihood and the joint likelihoods are derived in Section 5. Note that the joint likelihood is used to compute the particle weights and form the posterior distribution of the state.

Our algorithm is summarized in Algorithm 1. Below, we elaborate on different modules of our framework:

**Perturbation**: As a prior for the particle distribution, we use a normal distribution where we set the mean of the distribution to be the initial position. In the perturbation module, we generate a set of motion samples using odometry measurements. The motion samples are created using a uniform distribution around the mean motion sample which generates the final perturbation to the particle distribution 𝜋^{𝑡‒1}.**Propagation**: We use the extended state-space formulation proposed in Gupta and Gao (2019) where the state space is augmented to 𝑋^{𝑡‒1},𝜒. Here, 𝜒 represents the association of a particle with a current epoch measurement and takes values up to 𝑅, the total number of GNSS pseudorange measurements in the current time epoch.The extended state-space particles is propagated by the Markov Chain for odometry 𝑢

^{𝑡}as follows:3

where represents each motion sample from the perturbation step, and is computed by propagating the state using odometry and known system dynamics. Specifically, we use a linear Gaussian state space and first-order finite difference model as shown below:

4

where 𝑋

^{𝑡}represents the state at the current time epoch and 𝑋^{𝑡‒1}represents the state at the previous time epoch. 𝐴 represents the transition matrix which is considered to be the identity matrix. 𝐵 is considered as a matrix of all ones. 𝜖 denotes the stochastic term in propagation of each extended state-space particle that is drawn from a Gaussian noise distribution. We keep the standard deviation of this propagation noise as a controlled parameter.**GNSS module**: This module from Gupta and Gao (2019) takes GNSS ranging measurements from multiple satellites, some of which may be faulty, and outputs a probability distribution over position using a fault-tolerant weighting scheme as described in Section 3. The module consists of two steps—GMM weighting and RAIM voting.**Camera module and synchronization with motion data**: The camera module takes a camera image and matches it to the images in a map database using image registration to generate similarity scores. The underlying state of the best-matched image is extracted and propagated forward to the current GNSS time stamp by interpolating with IMU odometry. This step ensures that the probability distributions from camera and GNSS measurements are generated at the same time stamps. Finally, we use a categorical distribution function to transform the similarity scores into a probability distribution over position hypotheses as described in Section 4.**Probabilistic sensor fusion**: This module outputs a joint likelihood over positions from GNSS and camera measurements after fusing them with the proposed KL divergence metric in Section 5.1. Particles are resampled from the current distribution with sequential importance resampling (Gustafsson et al., 2002).**Risk bounding**: We execute this module at a lower frequency than the rest of the modules. For estimating the 3D position, we use a single posterior distribution from the particle filter. However, for deriving the integrity risk, we create multiple posterior distributions leveraging motion data from IMU. Each uniform perturbation of the IMU results in a different posterior distribution. The average pHMI from multiple such distributions results in the integrity risk bound which is computed using the formulation in Gupta and Gao (2019). According to generalization bounds from statistical learning theory (Bousquet et al., 2004), the derived risk bound forms a probabilistic upper bound to the reference risk. We elaborate on this module in Section 6.

## 3 GNSS MODULE: PARTICLE RAIM

A likelihood model for GNSS measurements is derived using the mixture-weighting method proposed in Gupta and Gao (2019). Instead of assuming the correctness of all GNSS measurements, the likelihood is modeled as a mixture of Gaussians to account for faults in some measurements. Individual measurement likelihoods are modeled as Gaussians with the expected pseudoranges as means and a fixed variance value of 5 m. The GMM (Šimandl & Duník, 2006; Sorenson & Alspach, 1971) is expressed as:

5

where 𝐿_{𝑡}(𝑚^{𝑡}) denotes the likelihood of measurement 𝑚 at time 𝑡. 𝛾 denotes the measurement responsibility or the weights of the individual measurement components and 𝑅 refers to the total number of GNSS ranging measurements. 𝜇 and 𝜎 represent the mean and the standard deviation of each Gaussian component. 𝑋 refers to the collection of position hypotheses denoted by particles and 𝑘 is the index of the number of Gaussians in the mixture. The weights are inferred with a single step of the Expectation-Maximization (EM) scheme (Vila & Schniter, 2013) as shown in Figure 2.

The equations for the EM step are directly adopted from Equations 5 and 6 in Gupta and Gao (2019). In the E-step, each particle votes for a local confidence in the measurement corresponding to its extended state-space variable. The voting is identical to residual-based RAIM based on the normalized residual for each particle. In the M-step, we normalize the votes for each particle. Repeating this step across all the particles gives us the weights in the GMM model.

With the GMM likelihood model, numerical errors due to finite precision can create particle degeneracy. To mitigate this, Gupta and Gao (2019) proposed to extend the input state space to contain additional copies of the state-space variable 𝜒 (introduced earlier), one for each GNSS measurement (Bishop, 2006). Now, the GMM likelihood is represented with a log likelihood. We modify the likelihood term to depict the augmented state space:

6

where 𝜒 is the index that denotes the associated GNSS measurement with the particle replica and 𝑘 keeps track of the index for the particle replicas. By exploiting the indicator function, we then compute the log likelihood from the above equation as given below.

7

where 𝕀 is the indicator function with respect to the variable 𝜒.

## 4 CAMERA MODULE

We propose an algorithm to directly quantify the uncertainty from camera images and generate a camera measurement likelihood (similar to the GNSS measurement likelihood). For quantifying the uncertainty from camera images, we use a map-matching algorithm that matches a camera image directly to an image present in a map database. Our method is implemented in OpenCV (Bradski, 2000) and is comprised of three sub-modules as shown in Figure 3.

We elaborate on each sub-module below.

**Preprocessing**: We assume prior knowledge of the geographical region where we are navigating. Based on the coordinates (from GPS), we use Google Street View Imagery to select images from the known area. These images, along with their associated coordinates, form the database. Features are extracted from these images and stored in a key point-descriptor format.**Image registration**: After receiving a camera test image, we extract features and descriptors with the ORB (Rublee et al., 2011) algorithm. Although we experimented with other feature extraction methods such as SIFT (Lowe, 2004), SURF (Bay et al., 2006), and AKAZE (Alcantarilla & Solutions, 2011), ORB was found most effective for extracting descriptors from highly blurred images. The descriptor vectors are clustered with a k-means algorithm (Lloyd, 1982) to form a vocabulary tree (Nister & Stewenius, 2006). Each node in the tree corresponds to an inverted file (i.e., a file containing the ID numbers of images in which a particular node is found and the relevance of each feature to that image). The database is then scored hierarchically based on Term Frequency Inverse Document Frequency (TF-IDF) scoring (Nister & Stewenius, 2006), which quantifies the relevance of the images in the database to the camera image. We refer to these scores as the*similarity scores*. The image with the highest score is chosen as the best match and the underlying state is extracted.**Mapping scores to a camera measurement likelihood**: After extracting the state from the best camera image in the database, we propagate the state to the same time stamp as the GNSS measurement. The raw vehicle odometry is first synchronized with GNSS measurements using the algorithm in Reisdorf et al. (2016). Using the time difference between the previous and current GNSS measurements, we linearly interpolate the extracted state with IMU motion data as shown below:8

where 𝑥

^{𝑡}refers to the 3D position at epoch 𝑡, 𝑑𝑡 refers to the time difference between successive camera measurements, and 𝑣 and 𝑎 are the interpolated IMU velocity and accelerations at epoch 𝑡. Next, we compute the Euclidean distance between the interpolated state and the current particle distribution to obtain new similarity scores. Note that the particle distribution represents the position hypotheses. A SoftMax function takes the scores and outputs an individual camera measurement likelihood. Normalization of the scores enforces a unit integral for the distribution:9

where 𝑄 is the probability distribution associated with camera measurement 𝑛 at time 𝑡 over the position domain 𝑋, represents computed distance score, and 𝑐 is the index for individual particles.

For successive test images, we generated individual measurement likelihoods using the above algorithm. These likelihoods are unimodal since each test image corresponds to a single peak over the particle distribution. As illustrated in Figure 4, we pass the individual likelihoods to the KL divergence module (described in Section 5) and then compute the overall camera measurement likelihood. Unlike the individual likelihoods, the overall likelihood is multi-modal, characterized by peaks of particle subsets in the current particle distribution.

## 5 PROBABILISTIC SENSOR FUSION

After obtaining the probability distributions from the GNSS and camera, we needed to form a joint distribution over the position. However, we also needed to ensure that faults in camera measurements did not degrade the distribution from GNSS measurements, one that is coarse but correct since the distribution accounts for faults in the ranging measurements through the RAIM voting scheme. Thus, we needed a metric to identify and exclude faulty camera measurements leveraging knowledge of the distribution from GNSS.

Additionally, the metric needed to assess the consistency of the probability distribution from each camera measurement with respect to the GNSS distribution and mitigate inconsistent distributions that resulted from vision faults. The KL divergence (van Erven & Harremos, 2014) represents one way to assess the consistency of two probability distributions. By minimizing the divergence between the distributions inferred from camera and GNSS, we ensure that both distributions are consistent.

Our KL divergence metric treats the GNSS and camera as complementary sensors. We assume that faults occur in both sensors independently of each other and correlated or coupled faults occur with a low probability. The metric also assumes that we always have access to GNSS pseudorange measurements albeit multiple of them could be faulty. When there are no GNSS measurements, we do not update the particle weights based on camera measurements only. Instead, we wait for the next GNSS measurement so that we can utilize all the camera measurements between successive GNSS time epochs.

### 5.1 KL divergence: Metric formulation

The KL divergence (van Erven & Harremos, 2014) between two discrete probability distributions, 𝑝 and 𝑞, in the same domain is defined as:

10

where 𝜁 represents the domain of both distributions and 𝑧 is each element of the domain. In our work, we propagate the same set of particles from the GNSS module to the camera module. As a result, the distributions from GNSS and camera share the same position domain. Additionally, to satisfy the requirements of our metric, we also propagate the log likelihood from the GNSS module to the current module.

Two important properties of the KL divergence are:

The KL divergence between two distributions is always non-negative and not symmetrical (van Erven & Harremos, 2014):

11

where 𝐷

_{𝐾𝐿}(𝑞||𝑝) is the reverse KL divergence between the distributions 𝑝 and 𝑞.𝐷

_{𝐾𝐿}(𝑝||𝑞) is convex in the pair (𝑝||𝑞) if both distributions represent probability mass functions (PMF; van Erven & Harremos, 2014).

Leveraging the discussed properties, we formulate our metric below.

**Mixture of Experts (MoE)**: We formed a mixture distribution to represent probability distributions from successive camera measurements, where a non-Gaussian probability distribution is derived from a single camera image. Each measurement is assigned a weight to represent its contribution in the mixture. Instead of setting arbitrary weights, we leverage the GNSS distribution to infer weights that directly correspond to whether a camera measurement is correct or faulty. Thus, highly faulty camera measurements are automatically assigned low weights in the MoE. The mixture distribution is given as:12

where 𝑄

^{∗}(𝑛^{𝑡}|𝑋^{𝑡}) represents the mixture distribution formed using 𝐾 camera images between two successive GNSS time epochs. is the likelihood of a single camera image recorded at time 𝑡 with as the normalized weight. 𝑋^{𝑡}are the particles representing position hypothesis and 𝑗 is the index for the camera images. The weights are normalized below to ensure that the MoE forms a valid probability distribution:13

where is the normalized weight, 𝛼

_{𝑗}is the weight prior to normalization, 𝑟 is the index for the number of camera images between two successive GNSS time epochs, and 𝐾 is the total number of camera measurements.**Set up KL divergence**: We set up a divergence minimization metric between the distributions from each camera measurement and all GNSS measurements:14

where || denotes the divergence between both probability distributions, 𝑆 represents the total number of particles or position hypotheses across both distributions, and 𝑖 is the index for the particles. is the probability distribution at epoch 𝑡 from GNSS measurements as defined in Equation (2), 𝛼

_{𝑗}is the unnormalized weight, and 𝑗 is the index for the camera measurement.**Minimize divergence**: Using the convexity of the KL divergence (Property 2), we minimize each divergence metric with respect to the unknown weight assigned to the likelihood of each camera measurement. We abbreviate as 𝑃(𝑥_{𝑖}) and as 𝑄(𝑥_{𝑖}) for brevity and expand Equation (9). Since 𝛼_{𝑗}is independent of the summation index, we keep it outside the summation and simplify our expansion below.15

Taking the first derivative with respect to 𝛼

_{𝑗}we obtain:16

Equating the expression on the right to 0 and solving for 𝛼

_{𝑗}gives us:17

We also perform a second derivative test to ensure that the 𝛼

_{𝑗}value inferred is a minimum value of the divergence measure. Since the exponential function with the natural base is always positive, 𝛼_{𝑗}is always positive as well. Thus, evaluating the second derivative gives us a positive value.18

**Joint probability distribution over position**: After obtaining the weights, we normalize them using Equation (8). We obtain the joint distribution assuming that the mixture distribution from camera measurements and the GMM from GNSS measurements are mutually independent. The joint distribution is given as:19

where is the probability distribution from GNSS measurements in Equation (2).

**Computing new particle weights**: With the above formulation, we compute the new weights for each particle as shown below. First, we compute the joint log likelihood:20

where is the joint log likelihood at one time epoch for each particle. The new particle weights are given as:

21

## 6 INTEGRITY RISK BOUNDING

We compute the integrity risk from the particle filter at a lower frequency compared to the state estimation. Here, the integrity risk is computed for the probability distribution of the state (in 3D-X,Y,Z) rather than a point solution, as illustrated in Figure 5. We upper bound the probability of HMI using the risk bounding framework introduced in Gupta and Gao (2019). For a single epoch, the probability of HMI for a given alert limit 𝑟 is defined as:

22

where 𝑅_{𝑥∗}(𝜋) is the probability of HMI with reference position 𝑥^{∗} and posterior distribution 𝜋, 𝑥 represents a sample from the posterior, and 𝑡 refers to the time epoch. Since the reference position is unknown, we need to evoke a surrogate loss function to compute the previous equation directly. First, let us rewrite the above equation by introducing a random variable:

23

where 𝑥′ refers to a random variable with mean of 𝑥^{∗} and 𝜋′ refers to a candidate posterior distribution. This distribution is approximated with a mean distribution created by all possible posterior likelihoods by perturbing the initial input. Note that these likelihoods are created by perturbing the initial particle distribution with noisy odometry samples (prior to the propagation step). With some rearrangement of the previous equation, the surrogate loss function is given as:

24

where 𝕀 denotes the indicator function with respect to the random variable 𝑥 and 𝑥′. When the distribution 𝜋′ approximates 𝑥^{∗}, the surrogate term equals the true loss. Thus, with increasing quality of measurements, the candidate posterior distribution should converge to the true position. Calculating the expectation over these posterior distributions gives us the expected risk:

25

where 𝐑_{𝑢}(𝜋) is the expected risk over the set of posterior distributions 𝜋, 𝑙(𝑥, 𝜋_{𝑢}) is the surrogate loss function, and 𝔼_{𝑢} is the expectation over odometry samples 𝑢 that perturb the initial particle distribution. To circum-vent the need to compute over infinite possible odometry samples, we first draw a finite number of samples and then define the empirical risk in regards to that sample size. As the number of samples approach infinity, the empirical risk should approximate the expected risk correctly:

26

where 𝐑_{𝑀}(𝜋^{𝑡}) represents the empirical risk, 𝑀 is the size of the perturbed odometry samples, and 𝜋_{𝑢𝑖} is the randomly drawn odometry sample. In our setup, we utilize a kernel density estimate (KDE) to compute the integral over a set of discrete particles. To evoke generalization error bounds for our setup, we characterize the surrogate loss function as a classification loss. The task is to classify whether the posterior distribution is classified as hazardous (with respect to the alert limit) given a motion sample 𝑢_{𝑖}.

From the PAC-Bayesian theory of generalization error bounds (Seeger, 2003), an appropriate bound for such a classification problem (Gupta & Gao, 2019) is given as:

27

where 𝐑(𝜋^{𝑡}) is the expected risk that is upper bound by the empirical risk 𝐑_{𝑀}(𝜋^{𝑡}) and a term 𝐑_{𝑀}((𝜋^{𝑡}), 𝜖) that denotes the divergence risk or the sample uncertainty due to unobserved samples.

As noted in Seeger (2003), the above bound is only a probabilistic bound and thus cannot be ensured at all times. The bound holds for any data distribution where the probability is over random i.i.d. samples (as is true in our case since the odometry perturbations are generated independently) drawn from the data distribution.

To compute the divergence risk, we first calculate the gap term 𝜖 using KL divergence (Kullback & Leibler, 1951) of the current distribution from the prior and a confidence requirement in the bound 𝛿:

28

where 𝛿 refers to the bound gap or the gap between the expected generalization error and the expected empirical error from the finite set of drawn samples. According to Equations 4 and 5 of Seeger (2003), any nonnegative value of the bound gap satisfies the inequality in Equation 26.

The means the prior and current distributions are taken as 𝜋^{𝑡‒1} and 𝜋^{𝑡}. The prior and current distributions are approximated as multivariate Gaussian distributions.

The Inverse Bernoulli Divergence (Gupta & Gao, 2019) is then defined as:

29

where 𝑞||𝑞+𝑡 is the KL divergence (Kullback & Leibler, 1951) between 𝑞 and 𝑞+𝑡 and 𝑞 is given by the empirical risk term. Adopting a Taylor series expansion (up to the second order), the Inverse Bernoulli Divergence (Gupta & Gao, 2019) is computed as follows:

30

We note that evoking the generalization error bounds enables us to generate an upper bound to the reference risk with a low failure rate. Since the number of odometry samples always remains finite, we cannot guarantee the upper bound at all time instances.

## 7 EXPERIMENTS

### 7.1 Data sets

We test our framework on a 2.3-km-long urban driving data set from Frankfurt (Reisdorf et al., 2016). We use GNSS pseudorange measurements, images from a forward-facing camera, ground truth from a NovAtel receiver, and odometry from the IMU. The data set contains NLOS errors in GNSS measurements and vision faults due to variations in illumination. In addition to the real-world data set, we create emulated data sets by inducing faults in GNSS and vision measurements with various controlled parameters.

### 7.2 Experimental setup and parameters

**Real-world data set**: We use GNSS ranging measurements with NLOS errors. For simplicity, we estimate the shared clock bias by subtracting the average residuals with respect to ground truth from all GNSS pseudoranges at one time epoch.**Emulated data set**: First, we vary the number of satellites with NLOS errors by adding back the pseudorange residuals to randomly selected satellites. This induces clock errors in some measurements which are perceived as faults. Although the residuals approximate the true distribution of errors in the pseudorange measurements, they do not encapsulate all types of faults that our framework can handle. Hence, to test our algorithm under various controlled parameters, we remove the NLOS errors and add Gaussian bias noise to pseudorange measurements from random satellites at random time instances. The number of faults are varied between 2–9 out of 12 available measurements at any given time step. We induce faults in camera measurements by adding blurring with a 21x21 Gaussian kernel and occlusions of 25% to 50% height and width to random images.

During the experimental simulation, a particle filter tracks the 3D position (x, y, z) of the car and uses faulty GNSS and camera measurements along with noisy odometry. Probability distributions are generated independently from GNSS and camera and fused with the KL divergence metric to form the joint distribution over positions. At each time epoch, the particle distribution with the highest total log likelihood is chosen as the estimated distribution for that epoch. The integrity risk is computed from 10 posterior distributions of the initial particle distribution and the reference risk is computed with ground truth. Our experimental parameters are listed in Table 1.

### 7.3 Baselines and metrics

We use particle RAIM as the baseline to evaluate our algorithm’s performance for state estimation. The metric for state estimation is the root mean square error (RMSE) of the estimated position with respect to ground truth for the entire trajectory. The risk bounding performance is evaluated with metrics derived from a failure event (i.e., when the derived risk bound fails to upper bound the reference risk). The metrics are the following: *failure ratio* (the fraction of cases where the derived risk bound fails to upper bound the reference risk), *failure error* (the mean error during all failure events), *bound gap* (average gap between the derived integrity risk), and *reference risk*.

For evaluating the integrity risk, we specify a performance requirement that the position should lie within the alert limit with at least 90% probability. A fault occurs if the positioning error exceeds the alert limit. The metrics for integrity risk are reported based on when the system has insufficient integrity or sufficient integrity (Pesonen, 2011), which respectively refer to the states when a fault is declared or not. The *false alarm rate* equals the fraction of the number of times the system declares insufficient integrity in the absence of a fault. The *missed identification rate* is defined as the fraction of the number of times the system declares sufficient integrity even though a fault is present.

## 8 RESULTS

### 8.1 State estimation

First, we test our algorithm with NLOS errors in GNSS ranging measurements and added camera faults. Quantitative results in Table 2 demonstrate that our algorithm produces 3D positioning estimates with overall RMSE of less than 11 m. Additionally, our algorithm reports lower errors compared to particle RAIM for all test cases. Our algorithm is able to compensate for the residual errors from particle RAIM by including camera measurements in the framework. This leads to improved accuracy in the positioning solution.

For qualitative comparison, we overlay the trajectories from our algorithm on ground truth and highlight regions with positioning error of greater than 10 m in Figures 6 and 7. Trajectories from particle RAIM show large deviations from ground truth in certain regions, either due to poor satellite signal availability or high NLOS errors in the faulty pseudorange measurements. However, similar deviations are absent from the trajectories from our algorithm which uses both GNSS and camera measurements. Our KL divergence metric is able to mitigate the errors from vision and the errors from cross-contamination during sensor fusion, allowing us to produce lower positioning error.

Secondly, we tested our algorithm with the emulated data sets. Quantitatively, we plotted the RMSE as a function of the added Gaussian bias value in Figure 8 and as a function of the number of faulty GNSS ranging measurements in Figure 9. For all validation cases, our algorithm produces an overall RMSE less than 10 m. Similar to the results from the real-world data set, our algorithm reports lower RMSE values than particle RAIM.

With a fixed number of faults, the errors generally increase with increasing bias. At a fixed bias value, the errors decrease with increasing number of faults up to six faulty GNSS measurements since a large number of faults are easily excluded by particle RAIM producing an improved distribution over the position.

The improved distribution from GNSS further enables the KL divergence metric to exclude faulty camera measurements and produce a tighter distribution over the position domain. However, with a higher number of faults, particle RAIM does not have enough redundant correct GNSS measurements to exclude the faulty measurements resulting in higher positioning error. Nevertheless, with added vision, our algorithm produces better positioning estimates for all test cases than particle RAIM.

### 8.2 Integrity monitoring

We evaluated the integrity risk bounding performance for two alert limits: 8 m and 16 m. For an alert limit of 8 m, Table 3 shows that the derived integrity risk satisfies the performance requirement with very low false alarm and missed identification rates. While the false alarm rates reported are zero for all test cases except two, the missed identification rates are always less than 0.11. Additionally, the integrity risk bound upper bounds the reference risk with a failure ratio of less than 0.11 and a bound gap of less than 0.4 for all cases.

Figures 10 and 11 further support the observation that the derived risk bound is able to over bound the reference risk with low failure rate for the same alert limit. The few instances when the derived risk bound fails to upper bound the reference risk occur due to large sudden jumps in the reference risk that go undetected considering the fixed size of our motion samples.

For the validation cases considered in this paper, the integrity risk satisfies the desired performance requirement and over bounds the reference risk with a low failure rate for an alert limit as small as 8 m. This choice of alert limit is allowed because of the low positioning errors that further enable non-conservative integrity risk bounds.

For an alert limit of 16 m, Table 4 shows that the integrity risk satisfies the integrity performance requirement with zero false alarm rates. Furthermore, the missed identification rates are always zero except for the test case with nine faults and 100 m added bias. Specifying a larger alert limit lowers the risk associated with the distribution over position since almost all particles from the perturbed distributions lie within the alert limit.

Thus, the integrity risk with a 16 m alert limit is reported to be much smaller compared to the risk obtained with a 8 m alert limit as shown in Figures 12 and 13. Additionally, the derived risk bound produces an even lower failure ratio of less than 0.07 and a tighter bound gap of less than 0.1. For three of the eight validation cases, the derived risk bound over bounds the reference risk as shown in Figures 10 and 11.

## 9 CONCLUSION

In this paper, we presented a framework for joint state estimation and integrity monitoring for a GNSS-camera fused system using a particle-filtering approach. To quantify the uncertainty in camera measurements, we derived a probability distribution directly from camera images leveraging a data-driven approach along with image registration.

Furthermore, we designed a metric based on KL divergence to probabilistically fuse measurements from the GNSS and camera in a fault-tolerant manner. The metric accounts for vision faults and mitigates the errors that arise due to cross-contamination of measurements during sensor fusion. We experimentally validated our framework on real-world data under NLOS errors, added Gaussian bias noise to GNSS measurements, and added vision faults.

Our algorithm reported lower positioning error compared to particle RAIM which uses only GNSS measurements. The integrity risk from our algorithm satisfied the integrity performance requirement for alert limits of 8 m and 16 m with low false alarm and missed identification rates. Additionally, the derived integrity risk successfully provided an upper bound to the reference risk with a low failure rate for both alert limits, making our algorithm suitable for practical applications in urban environments.

## HOW TO CITE THIS ARTICLE

Mohanty A, Gupta S, Gao GX. A particle-filtering framework for integrity risk of GNSS-camera sensor fusion. *NAVIGATION*. 2021;*68:*709–726. https://doi.org/10.1002/navi.455

## AUTHOR BIOGRAPHIES

**Adyasha Mohanty** is a graduate student in the Department of Aeronautics and Astronautics at Stanford University. She graduated with a B.S. in Aerospace Engineering from Georgia Institute of Technology in 2019.

**Shubh Gupta** is a graduate student in the Department of Electrical Engineering at Stanford University. He received his B.Tech degree in Electrical Engineering with a minor in Computer Science from the Indian Institute of Technology Kanpur in 2018.

**Grace Gao** is an assistant professor in the Department of Aeronautics and Astronautics at Stanford University. Before joining Stanford University, she was faculty at University of Illinois at Urbana-Champaign. She obtained her Ph.D. degree at Stanford University. Her research is on robust and secure positioning, navigation and timing with applications to manned and unmanned aerial vehicles, robotics, and power systems.

## ACKNOWLEDGMENTS

We express our gratitude to Akshay Shetty, Tara Mina, and other members of the Navigation of Autonomous Vehicles Lab for their feedback on early drafts of the paper.

## Footnotes

**Funding information**National Science Foundation, Grant/Award Number: 2006162

- Received October 2, 2020.
- Revision received August 11, 2021.
- Accepted August 14, 2021.

- © 2021 Institute of Navigation

This is an open access article under the terms of the Creative Commons Attribution License, which permits use, distribution and reproduction in any medium, provided the original work is properly cited.