Cooperative Localization under Ionospheric Scintillation Events

Abstract

Ionospheric scintillation causes major impairments to Global Navigation Satellite System (GNSS) in low-latitude regions. In severe scenarios, this event can lead to complete loss of lock, thus making GNSS measurements unusable for navigation. In this paper, we derive a cooperative localization algorithm where a set of partially connected aircraft exchange messages with neighboring nodes on the network to improve their own position estimates. We consider the scintillation events as abrupt changes in the measurement variance, which are modeled by a discrete-valued Markov process at the nodes which have access to GNSS measurements. Simulation results show that Markovian modeling and cooperation via factor graph message passing reduce the average 3D root mean square localization error and yield an average vertical position error that meets civil aviation standards for approach and landing.

1 Introduction

Global Navigation Satellite System (GNSS) is currently used in a wide range of applications, including transportation, surveillance, agriculture and meteorology [1,2,3,4,5,6,7,8,9]. The system is composed of multiple satellite constellations which provide accurate and reliable positioning, navigation and timing information worldwide [10]. Despite its benefits, in low-latitude regions the GNSS solution is frequently degraded by ionospheric scintillation [11,12,13,14]. This phenomenon causes rapid fluctuations in the amplitude and phase of GNSS signals, which in turn can make the measurements unusable for positioning in critical applications such as civil aviation [15].

In the last decades, substantial effort has been made to deal with GNSS degradation caused by scintillation events. The use of multiple-frequency receivers is currently the most common approach to improve the positioning accuracy of faded signals [16,17,18]. However, these sensors are nowadays undergoing certification in order to meet the strict civil aviation operational standards [19]. Machine learning (ML) algorithms constitute another prevalent solution to predict scintillation events and improve the positioning performance [20,21,22,23]. However, this approach often relies on huge datasets composed of past data for training the ML models. Advanced integration schemes are also complimentary solutions for improving positioning accuracy based on sensor fusion techniques [24,25,26]. However, these schemes require the use of additional hardware devices, such as pseudolites or high-resolution cameras, further increasing the costs.

In this paper, we aim at mitigating the scintillation effects and improving positioning and navigation of a set of partially connected aircraft within a wireless communication network by means of cooperative localization [27,28,29,30,31,32,33, 56]. To that end, as in [34, 35], we model this distributed estimation problem in a Bayesian fashion following the factor graph framework to apply the sum-product algorithm (SPA) (introduced in [36, 37]) and solve for the marginal posterior distributions of the unknown state variables at each network aircraft, also referred in this paper as a network node. In addition, we introduce some $a \ priori$ information and make use of discrete-time Markov processes to model the scintillation occurrences at each node. Following this approach, it is possible to predict when a scintillation event is likely to occur and adjust the cooperative localization algorithm accordingly.

We follow the lead in [31] and present an efficient implementation of the message passing scheme based on Gaussian approximations that reduce the internode communication overhead. We also present a sequential Monte Carlo (SMC) filtering procedure [38, 39] that runs the SPA in an efficient way at each network node and computes the approximate marginal posteriors of interest. By following this approach, the information is diffused over the network by building Monte Carlo representations of the marginal posteriors at each node conditioned on the available measurements over the entire network up to the current time step.

This paper is organized as follows. Section 1 is this Introduction. In Sect. 2 we review the main concepts from Factor graph theory. Section 3 presents the state model of the nodes and the measurement models. Section 4 presents the derived message passing scheme for the proposed factor graph and sum-product algorithm messages. In Sect. 5, we introduce the Gaussian–SMC implementation of the messages. Section 6 shows the numerical simulation setup and results. Finally, Sect. 7 summarizes the present work with conclusions.

Notation    In this paper, we do not distinguish the notations for random variables/ vectors and real variables/ vectors, including samples of random variables or vectors. The proper interpretation is implied in context or otherwise explicitly stated, when required. We make use, however, of boldfaced fonts to denote vectors and matrices, and italic fonts to denote scalar objects. The notation $p({\textbf{x}})$ denotes both probability density functions (p.d.f.) and mixed probability density functions, which are defined as probability density functions in the continuous variables and probability mass functions in the discrete variables, see, for example, [40]. The notation $P({\textbf{x}})$ is reserved for probability mass functions (p.m.f.).

2 Factor graphs and sum-product algorithm

Let $p({\textbf{x}}_1,{\textbf{x}}_2,\dots ,{\textbf{x}}_n)$ represent a joint p.d.f. of n continuous random vectors, which can be factorized as

$$\begin{aligned} p({\textbf{x}}_1,{\textbf{x}}_2,\dots ,{\textbf{x}}_n) = \prod _{i=1}^{N_{f}} \psi _i ({{\varvec{X}}}_i) \end{aligned}$$

(1)

where $\{\psi _1,\psi _2,\dots ,\psi _{N_f}\}$ is the set of factors, ${{\varvec{X}}}_i \subset {\mathcal {X}}$ is a subset of variables that are arguments of the factor $\psi _i$, and ${\mathcal {X}} \triangleq \lbrace {\textbf{x}}_{1}, {\textbf{x}}_{2}, \dots , {\textbf{x}}_{n} \rbrace$.

A factor graph [36, 37] provides a visual representation of the factorization in (1), streamlining the computation of the marginal p.d.f.’s $p({\textbf{x}}_{j})$, where $j \in \lbrace 1,2,\dots , n\rbrace$. This computational efficiency is achieved through message passing operations over the graph, extracting insights from the joint p.d.f. $p({\textbf{x}}_{1}, {\textbf{x}}_{2}, \dots , {\textbf{x}}_{n})$. A factor graph is composed of variable nodes representing arguments of the function on the left-hand side of (1) and factor nodes corresponding to the real-valued functions $\psi _{i}$ on the right-hand side. The interconnection between variable node ${\textbf{x}}_{j}$ and factor node $\psi _{i}$ occurs if and only if ${\textbf{x}}_{j} \in {{\varvec{X}}}_{i}$. As an example, Fig. 1 presents a simple factor graph with the factorization of the joint p.d.f.

$$\begin{aligned} p({\textbf{x}}_1,{\textbf{x}}_2,{\textbf{x}}_3) = \psi _1({\textbf{x}}_1)\psi _2({\textbf{x}}_1,{\textbf{x}}_2,{\textbf{x}}_3)\psi _3({\textbf{x}}_2), \end{aligned}$$

(2)

where factor and variable nodes are represented by squares and circles, respectively. The solid arrows are related to the message passing strategy presented next.

Fig. 1

Simple factor graph. Factor and variable nodes are represented by squares and circles, respectively. Bold lines represent the dependency of factors and variables

Sum-Product Algorithm The sum-product algorithm (SPA) is a message passing scheme that, in its broad configuration, calculates two messages for every edge in the graph, with one message propagated in each direction [37].

Let $\eta _{\psi _i \rightarrow {\textbf{x}}_j}$ and $\eta _{{\textbf{x}}_j \rightarrow \psi _k}$ denote a message from a factor node $\psi _i$ to a variable node ${\textbf{x}}_j$ and a message from a variable node ${\textbf{x}}_j$ to a factor node $\psi _k$, respectively. Let also ${\mathcal {X}}_{i}$ denote the set of all variable nodes that are connected to the factor node $\psi _{i}$ and, similarly, let $\mathbf {\Psi }_{j}$ denote the set of all factor nodes that are connected to variable node ${\textbf{x}}_{j}$. The SPA computes the messages $\eta _{\psi _i \rightarrow {\textbf{x}}_j}$ and $\eta _{{\textbf{x}}_j \rightarrow \psi _k}$ over the respective edges of the graph as follows:

$$\begin{aligned} \eta _{\psi _i \rightarrow {\textbf{x}}_j}({\textbf{x}}_j)\propto & {} \int \cdots \int \psi _i ({{\varvec{X}}}_i) \prod _{\ell \in {\mathcal {X}}_i \backslash \{j\}} \eta _{{\textbf{x}}_\ell \rightarrow \psi _i}({\textbf{x}}_\ell )d{\textbf{x}}_\ell , \end{aligned}$$

(3)

$$\begin{aligned} \eta _{{\textbf{x}}_j \rightarrow \psi _k} ({\textbf{x}}_{j})\propto & {} \prod _{\ell \in \mathbf {\Psi }_j \backslash \{k\}} \eta _{\psi _\ell \rightarrow {\textbf{x}}_j} ({\textbf{x}}_j), \end{aligned}$$

(4)

where ${\mathcal {A}} \backslash {\mathcal {B}} \triangleq \{a | a \in {\mathcal {A}} \wedge a \notin {\mathcal {B}}\}$ denotes the set difference operation between sets ${\mathcal {A}}$ and ${\mathcal {B}}$.

In particular, the message from a factor with no incoming connections, i.e., ${\mathcal {X}}_i = \emptyset$, is the corresponding factor function $\psi _i(\cdot )$, and a message from a variable node ${\textbf{x}}_j$ that has no incoming messages, i.e., $\mathbf {\Psi }_j = \emptyset$, is just a constant. Finally, the outgoing message of a variable node of degree 2 is just the copy of the incoming message.

Specifically, the message from a factor with no incoming connections (i.e., ${\mathcal {X}}_i = \emptyset$) consists of the associated factor function $\psi _i(\cdot )$. Similarly, a message from a variable node ${\textbf{x}}_j$ with no incoming messages (i.e., $\mathbf {\Psi }_j = \emptyset$) is just a constant Lastly, for a variable node with a degree of 2, the outgoing message is simply a duplication of the incoming message.

Following this framework, the exact expression for the marginal density function of ${\textbf{x}}_j$ in a tree-like graph is given by

$$\begin{aligned} p({\textbf{x}}_j) \propto \prod _{m \in \mathbf {\Psi }_j} \eta _{\psi _m \rightarrow {\textbf{x}}_j} ({\textbf{x}}_j). \end{aligned}$$

(5)

For a cyclic graph, the SPA is executed iteratively [37]. The expression in (5) serves as an approximation for the marginal p.d.f. of interest. It is important to note that the SPA messages in (3), (4), and (5) must be normalized, such that $\int \eta _{\psi _i \rightarrow {\textbf{x}}_j} ({\textbf{x}}_j) d{\textbf{x}}_j = 1$, $\int \eta _{{\textbf{x}}_j \rightarrow \psi _k} ({\textbf{x}}_j) d{\textbf{x}}_j = 1$, and $\int p({\textbf{x}}_j) d{\textbf{x}}_j = 1$.

3 Problem formulation

Consider a collection of $N_a$ aircraft, each equipped with onboard inertial navigation system (INS), altimeter and GNSS receivers, which collaborate through a message passing algorithm over a partially connected network to improve their local position estimates. We model the network topology as an undirected graph, ${\mathcal {G}} = ({\mathcal {V}},{\mathcal {E}})$, with vertices ${\mathcal {V}} \triangleq \{1,2,\dots ,N_a\}$ representing the aircraft and unconstrained edges ${\mathcal {E}} \triangleq \{(r,s) \in {\mathcal {V}} \times {\mathcal {V}} \wedge s \in {\mathcal {R}}(r)\}$ denoting the communication links between nodes r and s, where ${\mathcal {R}}(r)$ represents the set of nodes directly connected to node r. We also model the ionospheric scintillation events at each GNSS receiver following a multimodal formulation, where we can have different observation models according to the absence or presence of scintillation. Following this approach, we extend the state vector with a discrete switching vector, which represents scintillation occurrences at the receiver. For this, denote as ${\textbf{X}}_{k,r} = [{\textbf{x}}_{k,r}^T \ \mathbf {\Lambda }_{k,r}^T]^T$ the state vector containing the unknown states detailed in the sequel.

3.1 State model

3.1.1 Position states/GNSS biases

Let ${\textbf{x}}_{k,r} \triangleq [{\textbf{p}}_{k,r}^T \ b_{k,r} \ \dot{b}_{k,r}]^T$ represent the unobserved continuous state vector composed of the tridimensional (3D) position state vector of aircraft r at the discrete-time instant k, the GNSS receiver clock bias and its derivative, respectively. The unobserved state vector is given at instant $k+1$ by

$$\begin{aligned} {\textbf{x}}_{k+1,r} = {\textbf{F}}_{k,r}{\textbf{x}}_{k,r} + {\textbf{u}}_{k,r} + {\textbf{w}}_{k,r} \end{aligned}$$

(6)

where

$$\begin{aligned} {\textbf{F}}_{k,r} = \begin{bmatrix}{\textbf{I}}_{3} &{} {\textbf{0}}_{2} \\ {\textbf{0}}_{2} &{} {\textbf{C}}_{k,r} \end{bmatrix}, \quad {\textbf{C}}_{k,r} = \begin{bmatrix}1 &{} T_s \\ 0 &{} 1\end{bmatrix}, \end{aligned}$$

(7)

${\textbf{I}}_n$ and ${\textbf{0}}_n$ denote, respectively, an identity matrix and a zero matrix of dimension $n \times n$, ${\textbf{u}}_{k,r} \triangleq [\Delta x_{k,r} \ \Delta y_{k,r} \ \Delta z_{k,r} \ 0 \ 0]^T$ represents an augmented input vector containing the displacement—estimated by an INS—within the 3D space between instants k and $k+1$, $T_{s}$ is the sampling period, and $\{{\textbf{w}}_{k,r}\}$, $k \ge 0$, is an independent and identically distributed (i.i.d.) sequence of Gaussian random vectors with zero mean and covariance matrix ${\textbf{Q}}_r = \text {blkdiag} \{\mathbf {Q_1},\mathbf {Q_2}\}$, and

$$\begin{aligned} \mathbf {Q_1}&= \sigma _f^2 {\textbf{I}}_3 \\ \mathbf {Q_2}&= \begin{bmatrix} \sigma _b^2T_s + \sigma _d^2\frac{T_s^3}{3} &{} \sigma _d^2\frac{T_s^2}{2} \\ \sigma _d^2\frac{T_s^2}{2} &{} \sigma _d^2T_s \end{bmatrix}, \end{aligned}$$

with $\sigma _f^2$, $\sigma _b^2$, $\sigma _d^2$ representing, respectively, the 3D position, bias and bias drift noise variances.

3.1.2 Scintillation switching variable

 Let $\mathbf {\Lambda }_{k,r}$ denote the unobserved scintillation switching vector of node r at instant k. The switching vector is modeled in terms of a discrete Markov chain with finite set of states ${\mathcal {M}}=\{0,1\}^{N_s}$, where $N_{s}$ denotes the number of in-view satellites at node r. The finite set ${\mathcal {M}}$ consists then of $2^{N_s} = |{\mathcal {M}}|$ elements denoted by $\{{\mathcal {M}}^i\}_{i=1,\dots ,|{\mathcal {M}}|}$. The state transition matrix is composed of the probabilities of transition between different modes in ${\mathcal {M}}$, e.g., $p_{ij} = {\mathbb {P}}(\{\mathbf {\Lambda }_{k,r} = {\mathcal {M}}^i|\mathbf {\Lambda }_{k-1,r} = {\mathcal {M}}^j\}), \forall i,j \in \{1,\dots ,|{\mathcal {M}}|\}$, which vary according to the ionosphere dynamics. In this paper, however, we assume, without loss of generality, that the transition probabilities $p_{ij}$ are set $a \ priori$ and fixed over the simulation window.

3.2 Observation models

3.2.1 Pseudorange Model

Assuming that at instant k, the receiver r is connected to $N_s = |{\mathcal {S}}(r)|$ satellites, where ${\mathcal {S}}(r)$ is the set of in-view satellites for receiver r, we write the pseudorange between r and $s \in {\mathcal {S}}(r)$¹ as

$$\begin{aligned} y_{k,r \leftarrow s} = ||{\textbf{p}}_{k,r} - {\textbf{s}}_{k,s}|| + b_{k,r} + m_{I_{k,s}} + \sqrt{\sigma ^2_{k,s}(\lambda _{k,r \leftarrow s})} \, \, \varepsilon _{k,r \leftarrow s} \end{aligned}$$

(8)

where ${\textbf{s}}_{k,s}$ is the satellite s ephemeris for instant k received in the navigation message, $|| \cdot ||$ denotes the Euclidean norm in ${\mathbb {R}}^{3}$, $m_{I_{k,s}}$ is the ionospheric delay error, $\sigma ^2_{k,s}$ stands for the pseudorange variance, which is a function of the discrete-valued mode switching variable $\lambda _{k, r \leftarrow s}$, and $\lbrace \varepsilon _{k,r \leftarrow s}\rbrace$, $k \ge 1$, is an independent, identically distributed (i.i.d.) sequence of zero-mean Gaussian variables with unit variance, i.e., $\varepsilon _{k,r \leftarrow s} \sim {\mathcal {N}}(0,1)$.

We assume for simplicity that the ionospheric delay $m_{I_{k,s}}$ is known and can be subtracted from the receiver measurements since corrections to that effect are often received in the navigation messages or estimated off-line using current existing models, such the Autoregressive-Moving-Average (ARMA) model [41, 42] and Global Ionospheric Maps (GIM/CODE) [43], and broadcast by ground stations to the aircraft. However, ionospheric scintillations due to randomly occurring Equatorial Plasma Bubbles (EPBs), particularly in low-latitude regions, lead to rapid fluctuations in the amplitude and phase of GPS signals [44], causing additional random positioning errors. Any positioning system must account then for both ionosphere delay estimation and detection of scintillation.

In this paper, we follow the approach in [45] and model the effect of ionosphere scintillation as random changes in the variance of the pseudorange measurement error. Specifically, the discrete-valued switching random variable $\lambda _{k, r \leftarrow s}$ is defined such that $\lambda _{k, r \leftarrow s} = 0$ means that no scintillation occurs at instant k in the channel between aircraft r and satellite s. Conversely, $\lambda _{k, r \leftarrow s} = 1$ denotes presence of scintillation at instant k in the same channel.

In the nominal condition of absence of scintillation, $\sigma ^2_{k,s}(\lambda _{k,r \leftarrow s} = 0)$ becomes the standard GNSS positioning error variance, whereas, in scintillation mode, $\sigma ^2_{k,s}(\lambda _{k,r \leftarrow s} = 1)$ becomes the scintillation error variance. The values of $\sigma ^2_{k,s}(\lambda _{k,r \leftarrow s})$ were chosen according to an $\alpha$-$\mu$ distribution, which is a simple and flexible fading model, introduced in [46], that describes the distribution of signal amplitudes, during scintillation events, in terms of fading parameters $\alpha$ and $\mu$. The p.d.f. of the normalized amplitude envelope of the received signal, assuming that the resulting average signal power is equal to 1, is given by

$$\begin{aligned} f(r) = \dfrac{\alpha r^{\alpha \mu -1}}{\xi ^{\alpha \mu /2} \Gamma (\mu )}, \,\, \xi = \dfrac{\Gamma (\mu )}{\Gamma (\mu +2/\alpha )}, \end{aligned}$$

(9)

where $\Gamma (\cdot )$ is the gamma function. The use of this particular fading model presents better results when compared to other distributions, see [46,47,48,49] for more details. Following [48], we define $\sigma ^2_{k,s}(\lambda _{k,r \leftarrow s})$ as

$$\begin{aligned} \sigma _{k,s}^2 (\lambda _{k,r \leftarrow s} = 0)&= \dfrac{B_nd}{2c/n_0}\Bigg (1 + \dfrac{1}{\eta c/n_0}\Bigg ) \end{aligned}$$

(10)

$$\begin{aligned} \sigma _{k,s}^2 (\lambda _{k,r \leftarrow s} = 1)&= \dfrac{B_nd}{2c/n_0 \xi \Gamma (\mu )}\Bigg (\Gamma (\mu - 2 / \alpha ) + \dfrac{\Gamma (\mu - 4/\alpha )}{\eta c/n_0\xi } \Bigg ) \end{aligned}$$

(11)

where $B_n$ is the phase-locked loop (PLL) single-sided noise equivalent bandwidth, $c/n_0$ is the nominal carrier to noise density ratio for the coarse/acquisition (C/A) L1 carrier and $\eta$ is the pre-detection integration period.

3.2.2 Peer-distance Model

We model the measured distance between nodes r and $s \in {\mathcal {R}}(r)$ at instant k as

$$\begin{aligned} z_{k,r\leftarrow s} = ||{\textbf{p}}_{k,r} - {\textbf{p}}_{k,s}|| + \zeta _{k,r\leftarrow s} \end{aligned}$$

(12)

where $\{\zeta _{k,r\leftarrow s}\}$, $k \ge 1$, is an i.i.d. sequence of zero-mean Gaussian random variables with variance $\sigma _{z}^2$.

3.2.3 Problem Statement

We formulate the problem as a recursive estimation of the augmented state vector ${\textbf{X}}_{k,r}$, given all available measurements ${\textbf{Y}}_k$ and ${\textbf{Z}}_k$, where ${\textbf{Y}}_k$ is the collection of all available GNSS pseudorange measurements over the network in the set $\{y_{1:k, r \leftarrow s} | r \in {\mathcal {V}} \wedge s \in {\mathcal {S}}(r)\}$ and ${\textbf{Z}}_k$ is the collection of all peer-distance measurements over the network in the set $\{z_{1:k, r \leftarrow s} | (r,s)\in {\mathcal {V}} \times {\mathcal {V}} \wedge s \in {\mathcal {R}}(r)\}$. This recursive estimation can be achieved with the marginalization of the mixed posterior density function, $p({\textbf{X}}_{k} | {\textbf{Y}}_k, {\textbf{Z}}_k)$. However, as direct marginalization is computationally intensive, we make use of factor graphs and the sum-product algorithm (SPA) to obtain an approximation of $p({\textbf{X}}_{k,r} | {\textbf{Y}}_k, {\textbf{Z}}_k)$.

4 Message passing scheme

In this Section, we present the message passing scheme that approximates the marginal posteriors $p({\textbf{X}}_{k,r} | {\textbf{Y}}_k, {\textbf{Z}}_k)$ of each node r in the network. To accomplish that, we restrict the propagation of messages in time to be forward-only. Such assumption is in accordance with real-world applications, where messages flow only in one direction. This also reduces internode communication cost and CPU load [34].

From the conditional independence assumptions in the signal model, it follows that $p({\textbf{X}}_{k} | {\textbf{Y}}_k, {\textbf{Z}}_k)$ is factored as

$$\begin{aligned} p({\textbf{X}}_{k} | {\textbf{Y}}_k, {\textbf{Z}}_k)= & {} \prod _{n=1}^{k} \prod _{r \in {\mathcal {V}}} p({\textbf{X}}_{n,r}| {\textbf{X}}_{n-1,r}) \left[ \prod _{s \in {\mathcal {S}}(r)} p(y_{n, r \leftarrow s}| {\textbf{X}}_{n,r}) \right. \nonumber \\{} & {} \times \left. \prod _{s \in {\mathcal {R}}(r)} p(z_{n, r \leftarrow s}| {\textbf{X}}_{n,r}, {\textbf{X}}_{n,s})\right] . \end{aligned}$$

(13)

Figure 2 shows the factor graph representation of the factorization in (13) for a fixed time instant k, and the corresponding message passing scheme over the graph. In the illustration, we drop the variable nodes and message arguments for simplicity, thus any function $f_{k,r}({\textbf{X}}_{k,r})$ becomes $f_{k,r}$ and any message $\eta _{f_{k,r} \rightarrow {\textbf{X}}_{k,r}}({\textbf{X}}_{k,r})$ becomes $\eta _{f_{k,r} \rightarrow {\textbf{X}}_{k,r}}$. In addition, in the following derivation, we drop for simplicity of notation the conditional dependence on the observations, such that the posterior $p({\textbf{X}}_{k,r} | {\textbf{Y}}_{k}, {\textbf{Z}}_k)$ is denoted simply as $p_{k|k,r}({\textbf{X}}_{k,r})$.

Fig. 2

Factor graph and message passing scheme for the proposed cooperative localization scheme, where f represents the dynamic model, g the GNSS likelihood model and h communication likelihood. Also, red arrows refer to local messages, whereas blue arrows represent messages sent over the network

Consider node r and time step k, using the dynamic model in (6), the factor $f_{k,r}$ is given by

$$\begin{aligned} f_{k,r}&\triangleq f_{k,r}({\textbf{X}}_{k,r},{\textbf{X}}_{k-1,r}) \nonumber \\&= p({\textbf{X}}_{k,r}|{\textbf{X}}_{k-1,r}) \nonumber \\&= p({\textbf{x}}_{k,r},\mathbf {\Lambda }_{k,r}|{\textbf{x}}_{k-1,r},\mathbf {\Lambda }_{k-1,r}) \nonumber \\&= p({\textbf{x}}_{k,r}|{\textbf{x}}_{k-1,r}) P(\mathbf {\Lambda }_{k,r}|\mathbf {\Lambda }_{k-1,r}) \nonumber \\&={\mathcal {N}}({\textbf{x}}_{k,r}|{\textbf{F}}_{k-1,r}{\textbf{x}}_{k-1,r} + {\textbf{u}}_{k-1,r},{\textbf{Q}}_r) \nonumber \\&\quad \times P(\mathbf {\Lambda }_{k,r}|\mathbf {\Lambda }_{k-1,r}), \end{aligned}$$

(14)

where

$$\begin{aligned} P(\mathbf {\Lambda }_{k,r}^{\prime }|\mathbf {\Lambda }_{k-1,r}^{*}) = \text{ Prob }( \lbrace \mathbf {\Lambda }_{k,r} = \mathbf {\Lambda }_{k,r}^{\prime } \rbrace | \lbrace \mathbf {\Lambda }_{k-1,r} = \mathbf {\Lambda }_{k-1,r}^{*}\rbrace ) \end{aligned}$$

with $\mathbf {\Lambda }_{k,r}^{\prime }$ representing the value assumed by the scintillation switching vector at instant k and node r, and $\mathbf {\Lambda }_{k-1,r}^{*}$ is the value assumed by the switching vector at instant $k-1$ and node r. Also,

$$\begin{aligned} {\mathcal {N}}({\textbf{x}}|{\textbf{m}},{\textbf{S}}) = \dfrac{1}{\sqrt{(2\pi )^{N_x} \ \text {det}({\textbf{S}})}} \text {exp} \left( - \frac{1}{2} ({\textbf{x}}-{\textbf{m}})^T {\textbf{S}}^{-1} ({\textbf{x}}-{\textbf{m}}) \right) , \end{aligned}$$

where ${\textbf{m}} \in {\mathbb {R}}^{N_x},$ represents the mean vector and ${\textbf{S}}$ represents the covariance matrix.

Let ${\textbf{Y}}_{k,r} = \lbrace y_{k,r \leftarrow s} \mid s \in {\mathcal {S}}(r) \rbrace$ be the collection of all pseudorange measurements available at node r at instant k. Factor $g_{k,r}$ is associated with the GNSS observation model in (8) and is given by

$$\begin{aligned} g_{k,r}&\triangleq g_{k,r}({\textbf{X}}_{k,r}) \nonumber \\&= g_{k,r}({\textbf{x}}_{k,r},\mathbf {\Lambda }_{k,r}) \nonumber \\&= p({\textbf{Y}}_{k,r} | {\textbf{x}}_{k,r},\mathbf {\Lambda }_{k,r}) \nonumber \\&= \prod _{s \in {\mathcal {S}}(r)} p(y_{k,r\leftarrow s} | {\textbf{x}}_{k,r},\lambda _{k,r \leftarrow s}) \nonumber \\&= \prod _{s \in {\mathcal {S}}(r)} {\mathcal {N}}(y_{k,s} | \ ||{\textbf{p}}_{k,r} - {\textbf{s}}_{k,s}|| + m_{I_{k,s}}+ b_{k,r}, \sigma _{k,s}^2(\lambda _{k,r \leftarrow s})). \end{aligned}$$

(15)

Note that the satellite ephemeris ${\textbf{s}}_{k,s}$ does not appear as a variable node on the factor graph since it is received in the GNSS data and is, therefore, known at each node r. Likewise, as discussed in Sec. 3.2, we assume that the ionospheric delay $m_{I_{k,s}}$ is estimated off-line and broadcast to the GNSS receiver, so this term is also treated in our model as a deterministic and known parameter rather than a random state variable.

Let now $\tilde{{\textbf{X}}}_{k,r}$ be a vector that collects both ${\textbf{X}}_{k,r}$ and $\lbrace {\textbf{X}}_{k,s} \mid s \in {\mathcal {R}}(r) \rbrace$, i.e., all state vectors in the closed neighborhood of node r at instant k. In turn, let ${\textbf{Z}}_{k,r} = \lbrace z_{k,r \leftarrow s} \mid s \in {\mathcal {R}}(r) \rbrace$ be the set of all (unconstrained) internode distance measurements available at node r at instant k. Based on (12), the cooperative factor $h_{k,r}$ is given by

$$\begin{aligned} h_{k,r}&\triangleq h_{k,r}(\tilde{{\textbf{X}}}_{k,r}) \nonumber \\&= p({\textbf{Z}}_{k,r} | \tilde{{\textbf{X}}}_{k,r}) \nonumber \\&= \prod _{s \in {\mathcal {R}}(r)} p(z_{k,r\leftarrow s} | {\textbf{X}}_{k,r},{\textbf{X}}_{k,s}) \nonumber \\&= \prod _{s \in {\mathcal {R}}(r)} {\mathcal {N}}(z_{k,r\leftarrow s} | \ ||{\textbf{p}}_{k,r} - {\textbf{p}}_{k,s}||,\sigma _{z}^2). \end{aligned}$$

(16)

4.1 Messages

We now introduce the messages outlined in Fig. 2. In the proposed factor graph, there are five different messages: temporal messages, GNSS messages, messages to neighbors, messages from neighbors and beliefs. The messages can be divided into local messages, where no network exchange is required, and cooperative messages. In this context, temporal and GNSS messages can be seen as local messages, whereas messages to/from neighbors are cooperative messages, which require internode communication. Beliefs are hybrid messages since, depending upon the cooperation status, they could be both local or cooperative messages.

4.1.1 Local Messages

The message $\eta _{f_{k,r} \rightarrow {\textbf{X}}_{k,r}}$ is computed based on factor $f_{k,r}$ in (14), and the previous belief $p_{k-1|k-1,r}({\textbf{X}}_{k-1,r})$. Assuming that the previous belief has the form

$$\begin{aligned} p_{k-1|k-1,r}({\textbf{X}}_{k-1,r})&= p_{k-1|k-1,r}({\textbf{x}}_{k-1,r},\mathbf {\Lambda }_{k-1,r})\nonumber \\&= p_{k-1|k-1,r}({\textbf{x}}_{k-1,r}|\mathbf {\Lambda }_{k-1,r}) P_{k-1|k-1,r}(\mathbf {\Lambda }_{k-1,r}) \end{aligned}$$

(17)

we write the message $\eta _{f_{k,r} \rightarrow {\textbf{X}}_{k,r}}$ as

$$\begin{aligned} \eta _{f_{k,r} \rightarrow {\textbf{X}}_{k,r}}&\triangleq \eta _{f_{k,r} \rightarrow {\textbf{X}}_{k,r}}({\textbf{X}}_{k,r}) \nonumber \\&\propto \sum _{\mathbf {\Lambda }_{k-1,r}} \Bigl [ \int f_{k,r} ({\textbf{X}}_{k,r},{\textbf{X}}_{k-1,r}) p_{k-1|k-1,r}({\textbf{X}}_{k-1,r})d{\textbf{x}}_{k-1,r} \Bigr ] \nonumber \\&= \sum _{\mathbf {\Lambda }_{k-1,r}} \Bigl [ \int p({\textbf{x}}_{k,r}|{\textbf{x}}_{k-1,r}) P(\mathbf {\Lambda }_{k,r}|\mathbf {\Lambda }_{k-1,r}) p_{k-1|k-1,r}({\textbf{X}}_{k-1,r})d{\textbf{x}}_{k-1,r} \Bigr ] \nonumber \\&= \sum _{\mathbf {\Lambda }_{k-1,r}} \Bigl [ \int p({\textbf{x}}_{k,r}|{\textbf{x}}_{k-1,r}) P(\mathbf {\Lambda }_{k,r}|\mathbf {\Lambda }_{k-1,r}) \nonumber \\&\quad \times p_{k-1|k-1,r}({\textbf{x}}_{k-1,r}|\mathbf {\Lambda }_{k-1,r}) P_{k-1|k-1,r}(\mathbf {\Lambda }_{k-1,r})d{\textbf{x}}_{k-1,r} \Bigr ] \nonumber \\&= \sum _{\mathbf {\Lambda }_{k-1,r}} P(\mathbf {\Lambda }_{k,r}|\mathbf {\Lambda }_{k-1,r}) \Bigl [ \int p({\textbf{x}}_{k,r}|{\textbf{x}}_{k-1,r}) \nonumber \\&\quad \times p_{k-1|k-1,r}({\textbf{x}}_{k-1,r}|\mathbf {\Lambda }_{k-1,r})d{\textbf{x}}_{k-1,r} \Bigr ] P_{k-1|k-1,r}(\mathbf {\Lambda }_{k-1,r})\nonumber \\&= \sum _{\mathbf {\Lambda }_{k-1,r}} P(\mathbf {\Lambda }_{k,r}|\mathbf {\Lambda }_{k-1,r}) \nonumber \\&\quad \times \Bigl [ \int {\mathcal {N}}({\textbf{x}}_{k,r} | {\textbf{F}}_{k-1,r}{\textbf{x}}_{k-1,r} + {\textbf{u}}_{k-1,r},{\textbf{Q}}_r) \nonumber \\&\quad \times p_{k-1|k-1,r}({\textbf{x}}_{k-1,r}|\mathbf {\Lambda }_{k-1,r}) d{\textbf{x}}_{k-1,r} \Bigr ] P_{k-1|k-1,r}(\mathbf {\Lambda }_{k-1,r}). \end{aligned}$$

(18)

GNSS messages

The GNSS messages $\eta _{g_{k,r} \rightarrow {\textbf{X}}_{k,r}}$ simply propagate the GNSS factor $g_{k,r}$ in (15), since this factor has no incoming connections, i.e.,

$$\begin{aligned} \eta _{g_{k,r} \rightarrow {\textbf{X}}_{k,r}}&\triangleq \eta _{g_{k,r} \rightarrow {\textbf{X}}_{k,r}}({\textbf{X}}_{k,r})\nonumber \\&= g_{k,r}({\textbf{X}}_{k,r}) \nonumber \\&= p({\textbf{Y}}_{k,r} | {\textbf{X}}_{k,r}) \nonumber \\&= p({\textbf{Y}}_{k,r} | {\textbf{x}}_{k,r},\mathbf {\Lambda }_{k,r}) \nonumber \\&= \prod _{s \in {\mathcal {S}}(r)} {\mathcal {N}}(y_{k,s} | \ ||{\textbf{p}}_{k,r} - {\textbf{s}}_{k,s}|| + m_{I_{k,s}} + b_{k,r},\sigma _{k,s}^2(\lambda _{k,r\leftarrow s})). \end{aligned}$$

(19)

4.1.2 Cooperative messages

As the factor graph in Fig. 2 presents a cyclic nature, cooperation can only be achieved in a loop with $N_{L}$ iterations. Therefore, in the following derivations, we include the superscript l in the messages to/from neighbors, denoting the iteration inside the cooperative loop.

Messages to neighbors

At each iteration step of the cooperative loop, each node broadcasts the message $\eta ^{(l)}_{{\textbf{X}}_{k,r} \rightarrow h_{k,s}}$, representing the last calculated belief of ${\textbf{X}}_{k,r}$. Thus, each node s in the neighborhood ${\mathcal {R}}(r)$ receives

$$\begin{aligned} \eta ^{(l)}_{{\textbf{X}}_{k,r} \rightarrow h_{k,s}}&\triangleq \eta ^{(l)}_{{\textbf{X}}_{k,r} \rightarrow h_{k,s}} ({\textbf{X}}_{k,r}) \nonumber \\&= p^{(l-1)}_{k|k,r}({\textbf{X}}_{k,r}). \end{aligned}$$

(20)

Messages from neighbors

Messages coming from neighboring nodes at instant k use the likelihood of all peer-distance measurements $z_{k,r\leftarrow s},\forall s \in {\mathcal {R}}(r)$, received by node r, given the states ${\textbf{X}}_{k,r}$ and ${\textbf{X}}_{k,s}$. This message is computed as follows:

$$\begin{aligned} \eta _{h_{k,r} \rightarrow {\textbf{X}}_{k,r}}^{(l)}&\triangleq \eta _{h_{k,r} \rightarrow {\textbf{X}}_{k,r}}^{(l)}({\textbf{X}}_{k,r}) \nonumber \\&\propto \prod _{s \in {\mathcal {R}}(r)} \int \sum _{\Lambda _{k,s}} p(z_{k, r \leftarrow s}| {\textbf{X}}_{k,r}, {\textbf{X}}_{k,s}) p^{(l-1)}_{k | k,s}({\textbf{X}}_{k,s})d{\textbf{x}}_{k,s} \nonumber \\&\propto \prod _{s \in {\mathcal {R}}(r)} \int p(z_{k, r \leftarrow s}| {\textbf{x}}_{k,r}, {\textbf{x}}_{k,s}) \left[ \sum _{\Lambda _{k,s}} p^{(l-1)}_{k | k,s}({\textbf{X}}_{k,s}) \right] d{\textbf{x}}_{k,s} \nonumber \\&= \prod _{s \in {\mathcal {R}}(r)} \int {\mathcal {N}}(z_{k,r\leftarrow s} | \ ||{\textbf{p}}_{k,r} - {\textbf{p}}_{k,s}||,\sigma _{z}^2) \, \, p^{(l-1)}_{k|k,s}({\textbf{x}}_{k,s})d{\textbf{x}}_{k,s}. \end{aligned}$$

(21)

Beliefs

The belief at node r at instant k is an approximation of the marginal posterior $p_{k|k,r}({\textbf{X}}_{k,r})$, based on all information available at node r. Following the SPA scheme, beliefs are computed as

$$\begin{aligned} p^{(l)}_{k|k,r}({\textbf{X}}_{k,r})&\propto \eta _{f_{k,r} \rightarrow {\textbf{X}}_{k,r}}({\textbf{X}}_{k,r}) \nonumber \\&\quad \times \eta _{g_{k,r} \rightarrow {\textbf{X}}_{k,r}}({\textbf{X}}_{k,r}) \nonumber \\&\quad \times \eta _{h_{k,r} \rightarrow {\textbf{X}}_{k,r}}^{(l)}({\textbf{X}}_{k,r}) \,\,.&\end{aligned}$$

(22)

Note that, when no cooperation is performed we have $N_L = 0$, thus the last term of (22) is omitted, that is, node r is flying using GNSS-INSS fusion only. After $N_L$ iterations the belief $p^{(N_L)}_{k|k}({\textbf{X}}_{k,r})$ is propagated to the next time step and the whole process is repeated for step $k+1$. Finally, it is noteworthy that when node r is flying in a GNSS-denied scenario, the GNSS messages are not calculated.

5 Gaussian–SMC implementation

In order to reduce computational and communication load, we compute the aforementioned messages following a parametric approximation. Furthermore, to assimilate the incoming nonlinear measurements at each step k, we employ a Monte Carlo representation of the beliefs and the message $\eta _{h_{k,r} \rightarrow {\textbf{X}}_{k,r}}^{(l)}({\textbf{X}}_{k,r})$ at each iteration loop l.

5.1 Temporal Messages

We assume that the belief $p_{k-1|k-1,r}({\textbf{X}}_{k-1,r})$ at instant $k-1$ and at node r is represented by a weighted sample set $\{(w_{k-1,r}^{(i)},{\textbf{X}}_{k-1,r}^{(i)})\}$, $i \in \{1,\dots ,N_p\}$. Using a linear minimum mean square error (LMMSE) approach, we approximate $p_{k-1|k-1,r}({\textbf{x}}_{k-1,r} | \mathbf {\Lambda }_{k-1,r}^{(i)})$ by a multivariate Gaussian function

$$\begin{aligned} p_{k-1|k-1,r}({\textbf{x}}_{k-1,r} | \mathbf {\Lambda }_{k-1,r}^{(i)}) \approx {\mathcal {N}}({\textbf{x}}_{k-1,r} | {\textbf{m}}(\mathbf {\Lambda }_{k-1,r}^{(i)}), {\textbf{P}}_{k-1,r}), \end{aligned}$$

(23)

with parameters defined as

$$\begin{aligned} {\textbf{m}}(\mathbf {\Lambda }_{k-1,r}^{(i)})&= \bar{{\textbf{x}}}_{k-1,r} + \Sigma _{{\textbf{x}}\mathbf {\Lambda }} (\Sigma _{\mathbf {\Lambda }\mathbf {\Lambda }})^{-1} (\mathbf {\Lambda }_{k-1,r}^{(i)} - \bar{\mathbf {\Lambda }}_{k-1,r})\\ {\textbf{P}}_{k-1,r}&= \Sigma _{{\textbf{x}}{\textbf{x}}} - \Sigma _{{\textbf{x}}\mathbf {\Lambda }} ( \Sigma _{\mathbf {\Lambda }\mathbf {\Lambda }})^{-1} \Sigma _{{\textbf{x}}\mathbf {\Lambda }}^T, \end{aligned}$$

where the posterior means $\bar{{\textbf{x}}}_{k-1,r}$ and $\bar{\mathbf {\Lambda }}_{k-1,r}$, and the posterior covariance matrix

$$\begin{aligned} \Sigma&\triangleq \begin{bmatrix} \Sigma _{{\textbf{x}}{\textbf{x}}} &{} \Sigma _{{\textbf{x}}\mathbf {\Lambda }} \\ \Sigma _{{\textbf{x}}\mathbf {\Lambda }}^T &{} \Sigma _{\mathbf {\Lambda }\mathbf {\Lambda }} \end{bmatrix} \end{aligned}$$

are estimated using the sample means and sample covariance or cross-covariance matrices associated with the weighted sample set $\{(w_{k-1,r}^{(i)},{\textbf{x}}_{k-1,r}^{(i)}, \mathbf {\Lambda }_{k-1,r}^{(i)})\}$, that is,

$$\begin{aligned} \bar{{\textbf{x}}}_{k-1,r}= & {} \sum _{i=1}^{N_{p}} w_{k-1,r}^{(i)} {\textbf{x}}_{k-1,r}^{(i)}\\ \bar{\mathbf {\Lambda }}_{k-1,r}= & {} \sum _{i=1}^{N_{p}} w_{k-1,r}^{(i)} \mathbf {\Lambda }_{k-1,r}^{(i)}\\ \Sigma _{{\textbf{x}}{\textbf{x}}}= & {} \sum _{i=1}^{N_{p}} w_{k-1,r}^{(i)} ({\textbf{x}}_{k-1,r}^{(i)}- \bar{{\textbf{x}}}_{k-1,r})({\textbf{x}}_{k-1,r}^{(i)}- \bar{{\textbf{x}}}_{k-1,r})^{T} \\ \Sigma _{{\textbf{x}}\mathbf {\Lambda }}= & {} \sum _{i=1}^{N_{p}} w_{k-1,r}^{(i)} ({\textbf{x}}_{k-1,r}^{(i)}- \bar{{\textbf{x}}}_{k-1,r})(\mathbf {\Lambda }_{k-1,r}^{(i)}- \bar{\mathbf {\Lambda }}_{k-1,r})^{T} \\ \Sigma _{\mathbf {\Lambda }\mathbf {\Lambda }}= & {} \sum _{i=1}^{N_{p}} w_{k-1,r}^{(i)} (\mathbf {\Lambda }_{k-1,r}^{(i)}- \bar{\mathbf {\Lambda }}_{k-1,r})(\mathbf {\Lambda }_{k-1,r}^{(i)}- \bar{\mathbf {\Lambda }}_{k-1,r})^{T} \,. \end{aligned}$$

Following this approach, we can approximate the temporal message $\eta _{f_{k,r} \rightarrow {\textbf{X}}_{k,r}}$ in (18) as

$$\begin{aligned} \tilde{\eta }_{f_{k,r} \rightarrow {\textbf{X}}_{k,r}} ({\textbf{X}}_{k,r})&= \frac{1}{N_{p}} \sum _{i^\prime =1}^{N_p} P(\mathbf {\Lambda }_{k,r} | \mathbf {\Lambda }_{k-1,r}^{(i^\prime )}) \nonumber \\&\quad \times {\mathcal {N}}({\textbf{x}}_{k,r} | {\textbf{m}}_{k|k-1,r}^{(i^\prime )},{\textbf{P}}_{k|k-1,r}), \end{aligned}$$

(24)

where $\mathbf {\Lambda }_{k-1,r}^{(i^\prime )}\sim P_{k-1|k-1,r}(\mathbf {\Lambda }_{k-1,r})$ with equal weights $1/N_{p}$, and ${\textbf{m}}_{k|k-1,r}^{(i^\prime )} = {\textbf{F}}_{k-1,r}{\textbf{m}}(\mathbf {\Lambda }_{k-1,r}^{(i^\prime )}) + {\textbf{u}}_{k-1,r}$ and ${\textbf{P}}_{k|k-1,r} = {\textbf{F}}_{k-1,r}{\textbf{P}}_{k-1,r}{\textbf{F}}_{k-1,r}^T + {\textbf{Q}}_{r}$ are obtained from the integral

$$\begin{aligned} \int&p({\textbf{x}}_{k,r}|{\textbf{x}}_{k-1,r}) p_{k-1|k-1,r}({\textbf{x}}_{k-1,r} | \mathbf {\Lambda }_{k-1,r}^{(i^\prime )}) d{\textbf{x}}_{k-1,r} =\\&\int {\mathcal {N}}({\textbf{x}}_{k,r} | {\textbf{F}}_{k-1,r}{\textbf{x}}_{k-1,r} + {\textbf{u}}_{k-1,r},{\textbf{Q}}_r) {\mathcal {N}}({\textbf{x}}_{k-1,r} | {\textbf{m}}(\mathbf {\Lambda }_{k-1,r}^{(i^{\prime })}), {\textbf{P}}_{k-1,r}) d{\textbf{x}}_{k-1,r}. \end{aligned}$$

We then sample new particles ${\textbf{X}}_{k,r}^{(i,0)} \sim \tilde{\eta }_{f_{k,r} \rightarrow {\textbf{X}}_{k,r}} ({\textbf{X}}_{k,r})$, $i \in \lbrace 1, \dots , N_{p} \rbrace$ and update the particle weights as $w_{k,r}^{(i,0)} \propto \eta _{g_{k,r} \rightarrow {\textbf{X}}_{k,r}}({\textbf{X}}_{k,r}^{(i,0)})$, followed by a normalization, such that $\sum _{i=1}^{N_p}w_{k,r}^{(i,0)}= 1$. The weight update in this case corresponds to the assimilation of local GNSS pseudorange measurements.

5.2 Neighboring Messages

Cooperation is achieved by sharing the belief $p^{(l-1)}_{k|k,r}({\textbf{X}}_{k,r})$ with neighboring nodes $s \in {\mathcal {R}}(r)$. However, since the belief follows a Monte Carlo representation this process results in communication burden. To overcome this, we build a Gaussian approximation of the last marginal belief $p_{k|k,r}^{ (l-1)}({\textbf{x}}_{k,r})$ of the continuous state vector ${\textbf{x}}_{k,r}$ at each cooperative iteration, using the Monte Carlo representation $\{(w_{k,r}^{(i,l-1)},{\textbf{x}}_{k,r}^{(i,l-1)})\}$. The parameters of this approximation are then sent to neighboring nodes of each node r. Upon receiving the Gaussian parameters from node $s \in {\mathcal {R}}(r)$, node r resamples $N_p$ particles from that received parametric approximation. Therefore, at instant k and cooperative iteration l, each node r has an approximate Monte Carlo representation $\{(w_{k,s}^{(j,l-1)},{\textbf{x}}_{k,s}^{(j,l-1)})\}$, $j \in \{1,\dots ,N_p\}$, of the broadcast belief of ${\textbf{x}}_{k,s}$ of each neighbor $s \in {\mathcal {R}}(r)$, such that $w_{k,s}^{(j,l-1)} = 1/N_p$. Equation (21) can then be approximated as

$$\begin{aligned} \tilde{\eta }_{h_{k,r} \rightarrow {\textbf{X}}_{k,r}}^{(l)}({\textbf{X}}_{k,r})&\approx \prod _{s \in {\mathcal {R}}(r)} \sum _{j=1}^{N_p} w_{k,s}^{(j,l-1)} {\mathcal {N}} (z_{k,r\leftarrow s}| \ ||{\textbf{p}}_{k,r}-{\textbf{p}}_{k,s}^{(j,l-1)}||,\sigma _{z}^2) \end{aligned}$$

(25)

Each node r then builds a new updated Monte Carlo representation $\{(w_{k,r}^{(i,l)},{\textbf{X}}_{k,r}^{(i,l)})\}$ of the belief $p^{(l)}({\textbf{X}}_{k,r})$, at iteration l, by propagating the particles and updating the weights as

$$\begin{aligned} {\textbf{X}}_{k,r}^{(i,l)}&= {\textbf{X}}_{k,r}^{(i,l-1)} \end{aligned}$$

(26)

$$\begin{aligned} w_{k,r}^{(i,l)}&\propto \eta _{g_{k,r} \rightarrow {\textbf{X}}_{k,r}} ({\textbf{X}}_{k,r}^{(i,0)}) \cdot \tilde{\eta }_{h_{k,r} \rightarrow {\textbf{X}}_{k,r}}^{(l)}({\textbf{X}}_{k,r}^{(i,l)}) \nonumber \\&= w_{k,r}^{(i,0)} \cdot \tilde{\eta }_{h_{k,r} \rightarrow {\textbf{X}}_{k,r}}^{(l)}({\textbf{X}}_{k,r}^{(i,l)}) \end{aligned}$$

(27)

After $N_L$ iterations of the cooperative loop, the approximated marginal beliefs ${p}_{k|k,r}({\textbf{x}}_{k,r})$ and ${P}_{k|k,r}(\mathbf {\Lambda }_{k,r})$ at each node r are computed based on the particle set $\{( w_{k,r}^{(i,N_{L})}, {\textbf{X}}_{k,r}^{(i,N_{L})})\}$ as follows:

$$\begin{aligned} {p}_{k|k,r}({\textbf{x}}_{k,r})&\approx \sum _{i=1}^{N_p} {w}_{k,r}^{(i,N_L)} \delta ({\textbf{x}}_{k,r} -{\textbf{x}}_{k,r}^{(i, N_L)}) \end{aligned}$$

(28)

$$\begin{aligned} {P}_{k|k,r}(\mathbf {\Lambda }_{k,r})&\approx \sum _{i=1}^{N_p} w_k^{(i,N_L)} {\mathcal {I}} \lbrace \mathbf {\Lambda }_{k,r}^{(i,N_L)} = \mathbf {\Lambda }_{k,r} \rbrace . \end{aligned}$$

(29)

where $\delta$ in (28) is the Dirac delta and ${\mathcal {I}}$ in (29) is the indicator function. The weighted particle set is then propagated to the next time step.

Algorithm 1 briefly summarizes the implementation of the algorithm for instant k.

Algorithm 1

Algorithm for step k

5.3 Complexity analysis

The complexity of the algorithm is dominated by the approximated message from neighbors in (25), which scales to ${\mathcal {O}}(CN_LN_p^2)$, where $C = |{\mathcal {R}}(r)|$ is the number of neighboring nodes of node r. Also, for node r at instant k, the complexity of Algorithm 1 scales as ${\mathcal {O}}(N_p (1 + N_s) + CN_LN_p^2)$. Note that, in case of no cooperation, the complexity reduces to ${\mathcal {O}}(N_p(1 + N_s))$.

6 Numerical simulations

In this Section, we first introduce the simulation setup and the performance metrics, and then, we present and discuss the numerical simulation results.

6.1 Simulation setup

We simulated $N_a=9$ aircraft flying on the surrounding area of a Brazilian airport for a period of 500 time steps, using flight trajectories obtained in [50]. In the simulations, we considered a scenario where all aircraft have access to GNSS observations of $N_s = 6$ satellites in view and cooperate in order to improve their own local estimates. The static network topology is depicted in Fig. 3 for time instants (150, 175, 200, 225) seconds.

Fig. 3

Static network topology used in our simulation with aircraft (red circles) and communication links (blue lines)

We then simulated our cooperative algorithm considering the Markov transition matrices depicted in Fig. 4. Each matrix, depicted in the panels of Fig. 4, was chosen in a manner that favors at least one channel in scintillation mode. At each simulation, we select for simplicity a subset of satellites in the full set of in-view satellites for the nodes, and assume that only the selected ones are subject to scintillation events. In addition, since EPBs are usually spread for hundreds of kilometers [51, 52], the chosen subset of satellites is the same for the $N_a = 9$ aircraft. In the transition matrix in Panel (a), we assume that only one GNSS channel can be faded by ionospheric scintillation events, whereas, in Panels (b) and (c), we assume, respectively, two and three affected channels. For the channels that are assumed to be unaffected by possible scintillation, we make the switching variable equal to zero at all time instants.

Fig. 4

Probabilities of transition for different scenarios. Panels (a), (b) and (c) represent the probabilities for the case where 1, 2 and 3 channels can be affected by scintillation events, respectively

The ground truth for the position/GNSS biases of each aircraft was generated using the process model in (6) with $\sigma _f = 3$ m and GNSS clock noise parameters as in [53]. The displacement of the aircraft from instant k to instant $k+1$ was generated using the flight trajectories, in such way that the process noise ${\textbf{w}}_{k,r}$ also accounts for INS uncertainties as in [54]. In addition, we make $\sigma _z = 5$ m and set, for simplicity, the pseudorange standard deviation $\sigma _{k,s}(\lambda _{k,r \leftarrow s})$ to 4 m and 20 m for the cases in the absence and presence of scintillation, respectively. Also, the ground truth for the scintillation switching vector of each node was generated according to the steady-state distribution of the Markov chain, computed according to the transition probability matrix.

In order to properly initialize the filter, the prior belief of ${\textbf{x}}_{0,r}$ was assumed to be a Gaussian p.d.f. centered at $\hat{\textbf{x}}_{0,r}$ with covariance matrix $\hat{\textbf{P}}_{0,r}=\text {diag}\{25^2,25^2,25^2,1^2,0.1^2\}$. The initial state estimate $\hat{\textbf{x}}_{0,r}$ was then sampled from a Gaussian distribution centered at the true state with the same aforementioned covariance matrix. In addition, the prior belief of $\mathbf {\Lambda }_{0,r}$ was initialized with equal probability for the different modes.

For the experiments, we fixed the maximum number of iterations in the cooperative loop to $N_L = 1$, since we did not empirically verify a sizeable performance gain when the number of iterations was increased. We performed $M=300$ independent Monte Carlo runs and used $N_p=500$ particles.

6.2 Performance metrics

In order to measure the performance of the proposed algorithm, we define the following metrics. Let ${\textbf{p}}_{k,r}^{(m)}$ and $\hat{{\textbf{p}}}_{k,r}^{(m)}$ denote the ground truth and the estimated 3D position of aircraft r at instant k in the m-th Monte Carlo run, respectively. The root mean square error (RMSE) of the position estimate at node r at instant k is defined as

$$\begin{aligned} e_{k,r} = \sqrt{\dfrac{1}{M} \sum _{m=1}^{M} ||{\textbf{p}}_{k,r}^{(m)} - \hat{{\textbf{p}}}_{k,r}^{(m)}||^2}, \end{aligned}$$

(30)

where M is the total number of Monte Carlo runs.

We also compute the network RMSE considering the position estimates at all network nodes $r \in \{1, \dots , N_a\}$ at instant k as

$$\begin{aligned} e_{k} = \sqrt{\dfrac{1}{M} \sum _{m=1}^{M} \dfrac{1}{N_a}\sum _{r=1}^{N_a}||{\textbf{p}}_{k,r}^{(m)} - \hat{{\textbf{p}}}_{k,r}^{(m)}||^2}. \end{aligned}$$

(31)

Finally, we define the overall RMSE considering the network position estimates at all time instants $k \ge 0$ as

$$\begin{aligned} e = \sqrt{\dfrac{1}{M} \sum _{m=1}^{M} \dfrac{1}{N_a}\sum _{r=1}^{N_a} \dfrac{1}{N}\sum _{k=1}^{N}||{\textbf{p}}_{k,r}^{(m)} - \hat{{\textbf{p}}}_{k,r}^{(m)}||^2}, \end{aligned}$$

(32)

where N is the total number of steps in the simulation.

6.3 Simulation results

We started our analysis by setting the number of channels subject to fading events to 2. We then simulated both non-cooperative and cooperative versions of the proposed localization algorithm, labeled ‘GMarkov-SMC non-coop’ and ‘GMarkov-SMC coop,’ respectively, and compared them to two standard extended Kalman filters (EKF)—an optimistic version, referred as ‘EKF-opt,’ which assumes complete absence of scintillation and a pessimistic version, ‘EKF-pes,’ which assumes the presence of scintillation at every instant k and receiver r. For both EKF simulations, each aircraft performs localization with no cooperation. Next, we computed the network position RMSE using the metrics defined in Sec. 6.2. The 3D positioning errors for each tested algorithm are shown in Fig. 5. As noted in the Figure, the aircraft can navigate safely, even without cooperation, as the network position RMSEs do not diverge. However, both EKF algorithms (‘EKF-opt’ and ‘EKF-pes’) present higher position estimation error compared to our proposed algorithms (‘GMarkov-SMC non-coop’ and ‘GMarkov-SMC coop’).

Fig. 5

Network 3D position estimation error for the EKF and non-cooperative and cooperative version of the proposed Gaussian–Markov filter

The overall position RMSE for the different algorithms is shown in Fig. 6. In this Figure, we see that the optimistic EKF yielded an overall position error of 10.33 meters, while the pessimistic version overall error was 13.28 m. The non-cooperative version of the proposed GMarkov-SMC method ended up with an average RMS error of 6.13 meters, thus resulting in more than 4 meters of difference when compared to the optimistic EKF. Considering the scenario where the aircraft cooperate with neighboring devices, we end up with an overall error of 4.89 m.

Fig. 6

Overall 3D position estimation error of the entire network at all time instants and all Monte Carlo simulation—assuming that only one channel can be affected by ionospheric scintillation events

We also computed the cumulative probability distribution functions (c.d.f.s) of the global vertical position errors over all nodes in the network, shown in Fig. 7. One can readily note that for the cooperative version of the proposed filter (‘GMarkov-SMC coop’), the vertical error remains below 4 m (approximately 3.3 m) for $95\%$ of time, which meets some standards of navigation for aircraft in the approach and landing phases [55].

Fig. 7

CDF of vertical position error for cooperative, non-cooperative and EKF simulations

Next, we compared the proposed algorithm in this paper with the Gaussian–SMC algorithm as in [34, 56], denoted here ‘GSMC,’ whose equations do not handle scintillation effects. In the comparison, we simulated both non-cooperative and cooperative versions of the GMarkov-SMC and GSMC algorithms, and simulated different configurations regarding the number of channels that can be affected by scintillation. Figure 8 illustrates the overall position RMSE for each simulated algorithm. It can be noted in Fig. 8 that our proposed GMarkov-SMC algorithm delivers the smallest positioning errors, both in the non-cooperative and cooperative versions, in all three scenarios where 1, 2, 3 and 4 channels can be affected by scintillation. Also, in the scenario where 3 channels are affected, GMarkov-SMC coop (hard blue bars) achieves an overall position error reduction of more than 1.5 meters when compared to GMarkov-SMC non-coop, and more than 5 meters when compared to GSMC non-coop. For the scenario where 4 channels are affected, the ‘GSMC non-coop’ simulation delivered an overall position RMSE above 13 meters, while the proposed algorithm delivers 9.21 m. Also, when enabling cooperation for this scenario, the proposed approach reduces the overall position RMSE by 3 meters. Although the occurrence of scintillation events in several channels is not a likely event [17], the filter was able to deliver small stable RMSE values for these scenarios.

Fig. 8

Overall position estimation error of the non-cooperative and cooperative versions of the proposed filter and a Gaussian–SMC algorithm, for different configurations where 1, 2, 3 and 4 channels can be affected by scintillation

To test filter robustness, we then simulated a mismatched version of the algorithm. In this simulation setup, while the ground truth for the pseudorange measurements was generated using $\sigma _{k,s}(\lambda _{k,r \leftarrow s}) = 20$ m, the filter equations considered $\sigma _{k,s}(\lambda _{k,r \leftarrow s}) = 12$ m. Figure 9 presents the simulation results for the aforementioned setup. In the illustration, it can be readily seen in Fig. 9 that the filter can handle mismatched values of pseudorange standard deviation.

Fig. 9

Overall position estimation error of the non-cooperative and cooperative versions of the proposed filter and a Gaussian–SMC algorithm, for different configurations where several channels might be affected by scintillation, considering a mismatch of pseudorange standard deviation

Figure 10 represents a simulation following the same setup described in Sect. 6.1, but with no mismatch between the filter and the ground models, $\sigma _{k,s}(\lambda _{k,r \leftarrow s} = 0) = 4$ m and $\sigma _{k,s}(\lambda _{k,r \leftarrow s} = 1) = 12$ m. In Fig. 10, we noticed the same previous observed pattern in the filter responses. However, as the gap between the pseudorange standard deviations used in the simulations was diminished, the ‘GSMC coop’ algorithm delivered an overall position RMSE which is lower than that of the ‘GMarkov-SMC non-coop’ scheme. Once again, the ‘GMarkov-SMC coop’ algorithm delivered the lowest overall RMSE, despite the decreased gap between pseudorange standard deviations, respectively, in the absence and presence of scintillation.

Fig. 10

Overall position estimation error of the non-cooperative and cooperative versions of the proposed filter and a Gaussian–SMC algorithm, for different configurations where several channels might be affected by scintillation, considering $\sigma _{k,s}(\lambda _{k,r \leftarrow s} = 0) = 4$ m and $\sigma _{k,s}(\lambda _{k,r \leftarrow s} = 1) = 12$ m

Communication Cost    At each iteration loop, each node r needs to broadcast its parametric representation of $p^{(l-1)}_{k|k,r}({\textbf{x}}_{k,r})$, where ${\textbf{x}}_{k,r}$ is a 5 $\times$ 1 vector that collects the 3D position ${\textbf{p}}_{k,r}$ of the aircraft r, the receiver bias $b_{k,r}$, and the bias drift $\dot{b}_{k,r}$. As we use a Gaussian parametric approximation for internode communication, the broadcast messages over each internode communication link consist of 20 real values, i.e., 5 for the mean vector and 15 for the symmetric covariance matrix. For the network topology, depicted in Fig. 3, there are 9 nodes and 60 internode links, thus resulting in an average of 133.34 real numbers exchanged per node at each iteration loop l. Nodes with maximum cardinality (in this case, equal to 8) transmit 160 real numbers per loop, whereas nodes with minimum cardinality (equal to 4 in the assumed topology) send 80 real numbers per loop. As remarked before, only one loop iteration was sufficient in our simulations to achieve accurate positioning.

7 Conclusions

We presented in this paper a cooperative localization method following the factor graph framework and the SPA algorithm. We simulated a set of aircraft flying on the surrounding area of a Brazilian airport, exchanging messages with neighboring nodes in order to improve the self-localization problem and decrease the state estimation error.

We developed the factor graph formulation for the problem as a distributed message passing algorithm, using a Gaussian parametric approximation that reduces the internode communication load. Since each aircraft may be subject to scintillation events, commonly present in low-latitude regions, we also modeled the occurrence of these events with a discrete-time Markov process at each node. We then evaluated both non-cooperative and cooperative versions of the algorithm according to different simulation setups. For each simulation, we evaluated the position estimation performance for both versions based on pre-defined RMSE metrics.

Simulation results showed improvements in the 3D position estimation error when compared to standard EKFs, where each node performs self-localization without communication, and Gaussian–SMC approaches that ignore scintillation events. In addition, cooperation significantly reduced the overall position RMSE even in scenarios where multiple channels can be affected by scintillation events.

In summary, our results suggest that the proposed cooperative localization algorithm is potentially a suitable candidate for air navigation in scenarios where the aircraft may be subject to ionospheric scintillation events.