GNSS-Denied Joint Cooperative Terrain Navigation and Target Tracking Using Factor Graph Geometric Average Fusion

We propose a fully distributed methodology based on factor graphs for joint cooperative localization and distributed noncooperative target tracking in a 3-D scenario where multiple surveillance aircraft fly in formation without access to global navigation satellite system (GNSS) measurements or communication with anchor nodes. Our approach is based on the adapt-then-combine (ATC) diffusion scheme, which is integrated into the factor graph by the introduction of special combine factors to perform geometric average fusion of the target beliefs over a partially connected network. The updated target belief held by each aircraft following the combine step is also fed back to improve the aircraft's own self-localization, assimilating the target measurements. Simulation results show that the proposed distributed algorithm performed close to the posterior Cramér-Rao lower bound of the optimal centralized solution and that the agents approached a consensus about the target state estimate.


I. INTRODUCTION
In a previous work [1], we introduced an algorithm for anchorless, GNSS-free, 2-D cooperative self-localization of multiple aircraft using terrain-aided navigation, inertial sensor measurements, and peer-to-peer relative distance measurements.In this article, we extend that previous algorithm to a more challenging scenario where the surveillance aircraft not only estimate their own position, but also jointly track a noncollaborative target in a distributed fashion.To account for more realistic evasive maneuvers that may be taken by the tracked target, including possible changes in altitude, we perform joint cooperative terrain-aided navigation and distributed target tracking (J-CTAN-DTT) in a 3-D surveillance space, as opposed to the previously considered 2-D scenario.As in [1], we use a Bayesian approach that employs factor graphs [2], [3], [4].The main original contribution of the article is, however, to handle the distributed target tracking task using an adapt-then-combine (ATC) diffusion [5], [6], [7], which is completely integrated into the self-localization factor graph framework, and implemented in a fully distributed fashion by the introduction in the graph of artificial factors to perform the Combine step.
We maintain, as in [1], a scenario where none of the surveillance aircraft have available global navigation satellite system (GNSS) data, relying only on inertial unit measurements and terrain-aided navigation (TAN) techniques [8], [9], [10], [11], [12] in addition to peer-to-peer relative distance measurements.To account for the new 3-D scenario and the additional integrated joint distributed target tracking task, we employ an additional set of sensor measurements compared to [1] at each aircraft node, namely, barometric altitude, ground clearance (measured by a radar altimeter and used in conjunction with a 2-D terrain elevation map), and target measurements (slant range, azimuth angle, and elevation angle) collected at each node by an airborne 3-D radar.

A. Related Work and Article Contributions
Advances in communication technologies have enabled the use of wireless sensor networks in many different engineering applications [13], [14], [15].Multiagent networks are of special interest for tactical coordination over military data links.In addition to self-localizing, agents-typically military units such as aircraft that are partially connected by a communication network with no data fusion centermust also cooperate [16] to simultaneously track a noncooperative target and agree with each other regarding the tracked target state.The joint cooperative localization and distributed target tracking problem is particularly challenging because tracking performance depends greatly on the ability of each agent that is observing the target to maintain an accurate estimate of its own location, which is unknown a priori in the considered scenario.On top of that, GNSS jamming and spoofing techniques are becoming ubiquitous in modern battlefield, so GNSS-free localization techniques are crucial in contemporary military applications.
Previous references in the literature, which use factor graphs combined with belief propagation, usually address cooperative location and distributed target tracking separately as fully decoupled problems.While [16], [17], [18], [19], and [20] only address cooperative localization based on factor graphs, [21], [22], and [23], for example, only address target tracking based on factor graphs.
The previous works [24], [25], and [26] handled the joint localization and distributed target tracking problem within a factor graph framework using a consensus approach that has a high communication cost, and might not be adequate for real-time systems.In contrast to the aforementioned references, our proposed diffusion scheme performs joint cooperative self-localization and distributed noncooperative target tracking with a much lower internode communication cost than the consensus algorithms in [24], [25], and [26], while maintaining at the same time a performance that is close to the posterior Cramér-Rao lower bound (PCRLB) for both the localization and tracking errors of the optimal nonlinear filter that uses a central data fusion hub to process all measurements in the network, in batch, at each time instant.Furthermore, we empirically show that the different agents approach, over time, a consensus about the target state without the need, as in previous literature, of multiple iterative data exchange between nodes, between consecutive sensor observations.Specifically, our algorithm seeks to approximate the adapt step of a classic distributed ATC tracker by replacing the likelihood of the local target observations at each agent node given the target and the agent positions by its expected value with respect to the latest belief (i.e., posterior probability distribution) that the agent has about its own state.Next, the aforementioned special factors combine the updated target state beliefs following the adapt step at each node using a geometric average (GA) fusion criterion [27], which can be shown to minimize an average Kullback-Leibler (KL) divergence metric [28], [29].For Gaussian beliefs, the employed combine step rule reduces to the well-known method of covariance intersection in sensor fusion [30].Furthermore, after the distributed combine process, each aircraft uses its updated target belief to improve its own self-localization.As in [1] and [33], we implement the sum-product algorithm (SPA) operations using a scheme that combines Gaussian mixture models (GMMs) and sequential Monte Carlo methods, which significantly reduces the amount of data exchanged over the network.
The rest of this article is organized as follows.We present, in section II, the problem statement and detail the state and observation models.In section III, we derive the joint localization and tracking algorithm using a distributed message passing approach over factor graphs.A communication-efficient implementation of the said algorithm using GMMs and sequential Monte Carlo methods is then presented in section IV.Performance results in a simulated 3-D scenario with real terrain elevation maps are presented and discussed in section V, assuming different cooperative and noncooperative operation modes and no available GNSS data.Finally, section VI concludes this article.

II. PROBLEM STATEMENT
In the cooperative terrain-aided navigation and distributed target tracking scenario considered in this article, we have multiple cooperative aircraft and a single noncooperative target flying in a 3-D environment where GNSS data are unavailable, either due to hostile actions such as spoofing and jamming, or natural causes such as low-latitude ionospheric interference that degrades GNSS signals.The aircraft must, therefore, cooperate within local neighborhoods over a wireless communication network to estimate both their own position and the position of the noncooperative target.As in [1], the topology of the communication network is modeled as a graph G consisting of a set of vertices V {1, . . ., A}, representing the location of each cooperative agent and a set of undirected edges E = {(a, b) ∈ V × V} that represent the communication links between agent nodes.
Each aircraft measures the distance to the terrain beneath them, also known as ground clearance, using an onboard radar altimeter.A barometric sensor is employed in turn to measure the aircraft's altitude with respect to sea level.In addition, each cooperative aircraft has an onboard 3-D radar to measure the slant range as well as the azimuth and elevation angles to the noncooperative target's 3-D location based on the received echoed radar pulses from the target.
REMARK Although we consider a single target scenario, it should be stressed that the solution presented in this article could be extended in a straightforward way to a scenario with multiple targets provided that, as considered in [24], perfect a priori target data association is available and there is no clutter, missed detections, or false alarms.In a scenario where the data association problem also has to be solved jointly with the cooperative localization and target tracking tasks, a different algorithm would have to be derived, but that is not the focus of this present article.A comprehensive review of state-of-the-art approaches for multitarget tracking using belief propagation methods may be found in [22], [31], and [32].

A. Distributed Signal Model
Aircraft dynamics: Let1 x n,a [x n,a y n,a z n,a ] T be the unknown 3-D position of the aircraft a at the time stamp t n = nT s , n ∈ Z + , where T s is the sampling period.The deterministic and known input vector u n,a [ x n,a y n,a z n,a ] T represents, on the other hand, the displacement in the 3-D space from instant n to n + 1, which is measured by an inertial navigation system (INS) processor.As in [1], [9], and [10], the state vector evolves over time according to the model where {w n,a } are Gaussian random vectors that are mutually independent and have identical distribution w n,a ∼ N (0, Q n,a ), for all a ∈ V and all n ∈ Z + .Furthermore, as argued in [9], the state noise vector w n,a in (1) also includes the INS measurement error.
Target dynamics: Since the target is noncooperative, each aircraft also needs to estimate the target position and velocity.We then specify the target kinematics using the constant velocity model [34].More precisely, let Barometric altitude observation: For the sake of simplicity, the barometric altitude y n,a←ba2 measured at time n at node a is assumed to be an unbiased observation of the aircraft altitude z n,a such that where ba(x n,a ) = z n,a and v n,a←ba is a Gaussian noise with zero mean and variance σ 2 ba .Ground clearance observation: An onboard radar altimeter measures the ground clearance, i.e., the distance from the aircraft to the terrain directly below it.So, the ground clearance is modeled as the real aircraft altitude z n,a minus the elevation of the terrain below the aircraft y n,a←gc = gc(x n,a ; M) + v n,a←gc (4) where gc(x n,a ; M) = z n,a − ge(x n,a ; M) and ge(x; M) is a function that outputs the terrain elevation, given the position and a known 2-D digital elevation map M, and the noise {v n,a←gc }, n ≥ 0, is an i.i.d.sequence with Gaussian distribution v n,a←gc ∼ N (0, σ 2 gc ).The discrete terrain elevation map is interpolated at each continuous position location as described in [1,Appendix A].
Aircraft distance observations: The 3-D peer-to-peer distance, a.k.a.slant range, between agents a and b measured from node a at time n is where {v n,a←b }, n ≥ 0, is a sequence of mutually independent Gaussian random variables with identical distribu- and x 2 x T x ∀x ∈ R 3 .To avoid clock synchronization issues [18], [35], [37], the measurements in (5) are obtained using round-trip time of arrival (RTOA) technique.
Let N(n, a) be the open neighborhood of node a at instant n defined as the set of indexes of all nodes in the communication network that are connected to node a at that same instant.For each node a, we collect the state vectors {x n,b } for all nodes b ∈ N(n, a), in an extended vector denoted X n,a .In a similar fashion, the internode distance measurements {y n,a←b , ∀b ∈ N(n, a)}, are stacked into the extended vector Y n,a at node a.It follows from (5) that the observations in Y n,a are independent conditioned on any pair of states of the type (x n,a , x n,b ), b ∈ N(n, a), at instant n.Thus, we can write Target observation: Each aircraft has a radar sensor capable of making 3-D observations of the target location.The target observation, measured by the cooperative aircraft a, to the noncooperative target o at instant n is modeled then as where y n,a←o = y (rg) n,a←o y (az)  n,a←o y (el )   n,a←o the elevation angle.We further assume that the covariance matrix R to has a time-invariant diagonal structure such that R to = diag σ 2 ro σ 2 az σ 2 el .Lastly, note that all cooperative aircraft a ∈ V make observations of the same single target state x n,o independently.

III. JOINT CTAN-DTT FACTOR GRAPH
In this section, we propose a fully distributed algorithm that enables joint cooperative estimation of the aircraft states, referred to in this article as cooperative terrain-aided navigation, or CTAN, and distributed target tracking, abbreviated DTT.
The DTT routine is based on an adapt-then-combine (ATC) diffusion technique with a modified adapt step where the likelihood of the target measurements for a known aircraft state is approximated by its expected value with respect to (w.r.t.) the latest available belief of the aircraft state after aircraft motion prediction and assimilation of the local barometric altitude and ground clearance sensor measurements.The combine step is then implemented via geometric average (GA) fusion in local network neighborhoods.We show how the proposed ATC scheme can be represented exactly by a factor graph model in which we introduce special factor nodes that enable a distributed implementation of the GA fusion step and the computation of the aforementioned expected values of the likelihood of the target observations at each aircraft node.
The CTAN routine, without feedback from the DTT subgraph-the connection between CTAN and DTT routines/subgraphs will be discussed later-computes in turn the marginal posterior p.d.f. of the aircraft state at each node a via message passing over a factor subgraph that represents the factorization of the join p.d.f. of the aircraft states and barometric altitude, ground clearance, and internode distance measurements.The computed marginal is further refined by adding an extra message in the graph that represents the expected value of the likelihood of the target observations w.r.t. the latest available belief about the target state outputted by the DTT subgraph.By introducing a special factor node that enables the computation of the aforementioned expected value, we are able to connect the CTAN and DTT subgraphs and arrive at a full joint CTAN-DTT (J-CTAN-DTT) factor graph in which, by following a noniterative message passing schedule according to the usual SPA rules, we can implement the J-CTAN-DTT algorithm described in this section exactly.
To enable a fully distributed implementation, each node a in the network tracks a local copy x (a)  n,o of the target state vector x n,o .As an artifact of our distributed implementation, we also denote the target state x (a)  n,o before the Combine step as x(a)  n,o , and the aircraft state x n,a before the assimilation of the internode distance measurements as xn,a .We emphasize, however, that both notations, with and without the diacritic mark, represent the same physical state vector.

A. Factor Notation
To obtain a factor graph representation of the operations in this section, we consider the variable nodes x n−1,a ; xn,a ; x n,a ; x (a)  n−1,o ; x(a) n,o and x (a)  n,o at each network node a ∈ {1, 2, . .., A}, and define the following local factor nodes at the same node.1) Aircraft motion factor 2) Ground clearance factor gc n,a gc n,a (x n,a ) p(y n,a←gc |x n,a ).( 9) 3) Barometric altitude factor

B. Decoupled Cooperative Localization
Let γ n,a (x n,a ) denote the aircraft state belief at each node a at instant n.To update the belief γ n,a sequentially in time, the aircraft network may perform cooperative localization independently of the distributed tracking task using the factor graph-based CTAN algorithm in [1].
Specifically, in a decoupled cooperative localization scenario, let X n denote the collection of all aircraft states {x k,a }, Z n denote the collection of all local ground clearance and barometric altitude measurements {y k,a←gc ; y k,a←ba }, and let Y n represent the set of all internode measurements {y k,a←b } for a ∈ {1, . .., A}, b ∈ {1, . .., A}, a = b and k ∈ {0, . .., n}.Given the conditional independence assumptions in the model, the joint p.d.f.can be written as As explained before, for convenience and to enable the integration between the CTAN and DTT subgraphs, we introduce an auxiliary variable xn,a which is actually identical to x n,a .We then rewrite the factorization in ( 14) as (15) where f n,a , gc n,a , ba n,a , and ad n,a←b are the factors previously introduced in (8), ( 9), (10), and (11), respectively.
The factorization in ( 15) is represented by the factor graph in Fig. 1, shown here for two neighboring network nodes a and b.The messages computed at nodes a and b are highlighted in blue and red, respectively, with messages that are passed between nodes shown in dashed lines and intranode messages in solid lines.The factor represented by "=" symbol corresponds to the Dirac delta operator in (15).
By applying the standard SPA rules, the noniterative message passing schedule at node a corresponding to the factor graph in Fig. 1 runs as described in the sequel.First, we compute the aircraft motion prediction message α n,a such that (16) Next, we define the barometric altitude and ground clearance messages, respectively, κ n,a and β n,a , such that The assimilation of the intranode distance measurements is done by computing the messages where λ n,a←b (x n,b ) is identical to the message ι n,b (x n,b ) and the functions n,a and ι n,a are also identical.
The final update belief of the state x n,a is the message ξ n,a←b (x n,a ).(22) Note that messages ( 16)-( 18) correspond to the selflocalization (self-loc) subgraph highlighted in Fig. 1, whereas messages ( 19)- (22) belong to the cooperative localization (coop-loc) subgraph.

C. Adapt-Then-Combine Distributed Target Tracking
ATC diffusion is a well-known method for distributed target tracking in the literature [5], [6], [7].Unlike consensus methods, diffusion techniques do not force all network nodes to have the same target state estimate at each time instant.Instead, each node computes its own posterior p.d.f. of the target state, which is, in principle, different from the posterior computed by the other nodes.Ideally, over time however, as n → ∞, the local posteriors should converge to a consensus posterior [6] so that the variance of the local state estimates is reduced.The practical advantage of diffusion methods over consensus techniques is that they do not require multiple iterative communication between network nodes in the time interval between consecutive sensor measurements and, therefore, are more suitable for use in real-time scenarios due to their reduced internode communication cost.
In the sequel, we propose an approach to integrate ATC diffusion with factor graph cooperative localization.We detail the proposed adapt and combine steps separately and then show how to represent them exactly by a factor graph model.
1) Adapt Step: For known aircraft positions, the algorithm would start with a local adapt step at each node a consisting of the computation of the predicted target state p.d.f.using the target motion transition p.d.f.followed by the correction of that predicted p.d.f, using the likelihood function of the target observations.
In our signal model, however, since the aircraft states are unknown, we propose to replace the ideal likelihood of the target observations given the true (deterministic) aircraft state by the expected value of the same likelihood given the unknown (random) aircraft state w.r.t. to the latest available local posterior p.d.f.pn,a of the aircraft a at node a at instant n, i.e., We obtain pn,a (x n,a ) from the previous posterior p n−1,a (x n−1,a ) using the dynamic model for the agent a and assimilating the ground clearance and barometric altitude observations, respectively, y n,a←gc and y n,a←ba , which are available at node a at instant n.Specifically, we make pn,a (x n,a ) ∝ p(y n,a←gc |x n,a )p(y n,a←ba |x n,a ) The complete adapt step expression is written then as with pn,a (x n,a ) computed as in (24).
2) Distributed Combine Step: Let N(n, a) denote the closed neighborhood of the node a, defined as in [1] where r a,b is a set of network combination coefficients satisfying b∈N(n,a) r a,b = 1, and r a,b > 0 ∀a and ∀b ∈ N(n, a).
It follows then that the desire merged p.d.f. is computed at node a using geometric average fusion [27], [28], [29] as: After some algebraic manipulations, it is possible to show [28], [29] that the fusion rule in (27)  where denotes the expectation of its argument, which is computed using the p.d.f.f .
In particular, if p(b) such that n,o (29) or, equivalently In that case 3) DTT Factor Graph Representation: To obtain a factor graph representation of the ATC distributed target tracking algorithm, we start with the incoming messages at instant n − 1, where γ n−1,a is the same message previously used in the CTAN routine in (16).
Next, using the messages α n,a , κ n,a , and β n,a also previously defined in the CTAN subgraph, respectively, in ( 16), (17), and (18), it follows from (24) and the definition of the factor f n,a in (8) that the message ζ n,a (x n,a ) pn,a (x n,a ) is computed as: On the other hand, defining the message ν (a)   n,o x(a) x(a) n,o , we can rewrite (25) using ( 12), (13), and (33) as where In the sequel, we introduce the local and remote combine factors, respectively, as cd (a←b)   n,o cd (a←b)   n,o Finally, using (38) and (39) and introducing the message we rewrite (27) as where N(n, a) denotes again the open neighborhood of the node a at instant n (i.e., excluding node a itself) and θ (a←b)   n,o x (a)   n,o cd (a←b)   n,o Equations ( 32)-( 44) can be represented by a DTT factor subgraph as illustrated in Fig. 2 with the output messages computed using the usual SPA rules.Specifically, messages (35)-( 37  the SPA over the distributed target tracking (dist-tt) subgraph.

D. Cooperative Localization With Distributed Target
Tracking Feedback Let O n represent the set of all target measurements {y k,a←o } for a ∈ {1, . .., A}, and k ∈ {0, . .., n}.The belief of the unknown aircraft state at node a and instant n may be improved if we also assimilate the target observations {y n,a←o } to infer the state x n,a .In fact, if the target state were perfectly known, say xn,o , we could marginalize instead the joint p.d.f.
where, for the purpose of this discussion, xk,o in (45) is interpreted as being deterministic and known, as the position of an anchor node, for example.The factorization in (45) introduces an additional factor in Fig. 1 and modifies the expression for γ n,a (x n,a ) in (22) to However, as we do not know the actual xk,o , we propose again an approximation where we replace the last term on the right-hand side of (46) with its expected value with respect to the most recent belief that node a has about the state of x n,o at instant n.Specifically, we propose to compute that expectation using the belief γ (a)  n,o x (a)  n,o in (41) obtained at the end of the ATC distributed target tracking procedure in section III-C.
For convenience, we introduce a message ρ (a) n,o x (a)   n,o which is identical to γ (a) n,o x (a) n,o .We then define the so-called feedback factor The modified updated belief of x n,a at node a at instant n is now where the message By adding the factor f b (a)  n,o and the previous factor to (a)   n,o in (12), and defining the messages τ n,a in (49) and φ (a) n,o in (37), it is possible to further connect the dist-tt and coop-loc subgraphs into a single joint CTAN-DTT (J-CTAN-DTT) factor graph shown in Fig. 3, with the message passing equations obtained from the application of the SPA to this graph being identical to the message equations shown in sections III-B, III-C, and III-D.

IV. MESSAGE PASSING IMPLEMENTATION
In this section, we propose a GMM/SMC implementation for the message passing schedule in the factor graph in Fig. 3 that represents the J-CTAN-DTT algorithm.

A. Prediction and Local Observations
In our proposed implementation, we assume that the incoming messages γ n−1,a and γ (a)  n−1,o , which represent, respectively, the final beliefs of the aircraft a and the target state representation on aircraft a at instant n − 1, are modeled by parametric GMMs as where L denotes the number of modes in the GMM model.On the other hand, from the aircraft kinematic model in (1), it follows that: Therefore, by substituting (52) and (50) into ( 16), we approximate the message α n,a (x n,a ) as where η n|n−1,a = (l ) n−1,a + Q a ∀l ∈ {1, . . ., L}.Similarly, from the target dynamic model in (2), we write Thus, by substituting (54) and ( 51) into (36), the target prediction message α (a) n,o (x (a) n,o ) is approximated as In the sequel, at node a, we sample  also depends on xn,a and we already have an MC representation {( w(i) n,a , x(i) n,a )}, i ∈ {1, . . ., N a }, of the posterior ζ n,a (x n,a ), it results that, for each particle x(a,k) n,o , we have In the sequel, we create the GMM approximations λn,a xn,a and λ(a)

B. Cooperative Localization
The cooperative localization is analogous to the procedure described in [1].Each aircraft a sends the parametric representation λn,a (x n,a ) to the nodes in their neighborhood, instead of sending N a particles, to decrease the communication costs.ad ).(58) Then, the message ξ n,a←b x (i)  n,a , defined in ( 21) is computed, for each particle n,a , as Next, we make w (i) n,a ∝ w(i) n,a b∈ N(n,a) ξ n,a←b x (i) n,a with proportionality constant such that N a i=1 w (i)  n,a = 1.The new weighted particle set w (i)  n,a , x (i) n,a , i ∈ {1, . . ., N a }, is now the updated MC representation of the belief of the position of the aircraft a following the assimilation of the distance measurements between the neighboring aircraft.

C. Distributed Target Tracking
Each aircraft a also broadcasts the parametric representations λ(a) n,o , it follows from ( 28) and ( 29 where Using ( 43) and ( 44), the local and remote combine messages are then parameterized as The combined estimate of the target state x (a)  n,o is used to improve the self-localization estimate of the state x n,a of the aircraft a via the factor f b (a)  n,o in (47).The input message ρ (a)  n,o x (a) n,o , which is a copy of γ (a) n,o x (a)  n,o in (41) can now be calculated as the product of the messages ψ (a)  n,o x (a) n,o and θ (a←b)   n,o (x (a)  n,o ) where The output message τ n,a (x n,a ) of the factor f b (a) n,o is computed using (47) and ( 49) such that However, since the target observation to x n,a , x (a)  n,o defined in ( 7) is a nonlinear function, we employ the previously introduced approach in section IV-B to solve for the righthand side of (67).Thus, to process the factor f b (a)  n,o , we create an MC representation w (a,k)  n,o , x (a,k)   n,o , k ∈ {1, . . ., N o }, of the belief of the target state x (a)  n,o , by sampling x (a,k)  n,o ∼ ρ (a)  n,o x (a)  n,o and making w (a,k) n,o = 1/N o .Since we already have an MC representation w (i)  n,a , x (i) n,a , i ∈ {1, . . ., N a }, of the belief of the aircraft state estimate x n,a , just after the cooperative localization message assimilation described in section IV-B, we can approximate the output message τ n,a x n,a , by computing τ n,a x (i)  n,a for each particle

E. Final Messages
The final message γ (a) n,o x (a) n,o , which represents the belief of the target state at node a after the combine step, is exactly the same as the message ρ (a)  n,o x (a) n,o defined in (64), i.e., γ (a)  n,o To compute the final message γ n,a (x n,a ), which represents the final belief of the state of the aircraft a at instant n, we first update the weights w (i)  n,a of the MC representation w (i)  n,a , x (i) n,a , assimilating the feedback message τ n,a x (i)  n,a by making w (i) n,a ∝ w (i) n,a τ n,a x (i) n,a and normalizing the weights such that they add up to one.Before the time step is incremented, we fit then a GMM representation γn,a x n,a to the message γ n,a x n,a using the updated MC representation { w (i)  n,a , x (i) n,a }.

V. SIMULATION RESULTS
To test the proposed algorithm, we used the simulation setup described in [1], consisting of 12 cooperative aircraft flying over a terrain in the state of São Paulo, Brazil, for which we have a real digital elevation map provided by INPE. 3 Unlike in [1], however, we now simulate 3-D trajectories for the surveillance aircraft, including variations in altitude, and also add to the scenario a noncooperative target.The terrain map, initial target and aircraft positions and the communication network topology are shown in Fig. 4. We used 200 independent MC runs in our performance studies.In Fig. 5, we show all 200 simulated trajectories of each aircraft and the target.The target starts flying from the left side of the scenario, as shown in gray, while the

TABLE I Factors and Messages Enabling for Each Factor Graph Configuration
aircraft are flying in formation, starting from the bottom left side of the scenario, monitoring the area.The target and aircraft speeds are set as 50 and 100 m/s, respectively.The total simulation window lasts 600 s and T s = 1 s.The surveillance aircraft maneuvers were simulated using a 3-D coordinated turn model.
The proposed algorithm was evaluated using three different factor graph configurations in the "6-TAN 6-INS" scenario, where none of the 12 aircraft have GNSS measurements, but six aircraft perform terrain navigation while the six remaining aircraft navigate with INS data only.Each of the proposed configurations or operation modes correspond to a subset of the full factor graph shown in Fig. 3, in which we add or remove the corresponding factors and messages according to Table I.
We employed N a = 3000 particles to represent the state vector of each surveillance aircraft and N o = 1000 particles for the local representation of the target state at each node.In the cooperative localization loop, the GMMs representing the aircraft beliefs that are received from neighboring nodes were resampled with N b = 3000 particles.We use GMMs with one single mode for both aircraft and target representations.
We also computed the PCRLB by performing 1000 independent MC runs for the centralized version of the joint cooperative terrain-aided navigation and distributed target tracking (J-CTAN-DTT) problem, see Appendix A. In the aircraft dynamic model (1), we assumed covariance matrix Q a = σ 2 acc,a * I 3 with σ acc,a = 1 m ∀a.For the target's constant velocity model in (2), we set σ acc,o = 0.1 m in the process noise w n,o .
For filtering, we used σ acc = 1.5 m/s 2 for the NC configuration and σ acc = 6 m/s 2 for both the CTAN and J-CTAN-DTT configurations.For the target, we used σ acc,o = 0.75 m/s 2 in all configurations.
We sampled the estimate of the initial state of each aircraft x0,a using a Gaussian distribution with mean x 0,a and covariance P 0,a σ 2 pos * I, while the estimate of the initial target representation of each aircraft x(a) 0,o was sampled from a Gaussian distribution with mean x (a)  0,o and covariance P 0,o diag([σ 2  pos σ 2 vel σ 2 pos σ 2 vel σ 2 pos σ 2 vel ]), with σ pos = 50 m and σ vel = 5 m/s.
We implemented a scheduler for the process noise covariances and number of particles to improve the filter robustness, specially in the initial time steps, when the uncertainty of the state variables/vectors are bigger.We apply a scale factor to the parameters that define the covariance matrices and the amount of particles according to a simple schedule such that, as time progresses, the scaling factor is reduced and then removed.Table II shows the schedule and the scale factors.
The performance of each scenario configuration was assessed using the root-mean-square error (RMSE) of the position estimates for both cooperative localization e n and target tracking e n,o , averaged as a function of time, average over all network nodes and all MC simulations, as explained in detail in [1].
Moreover, the performance metrics e n and e n,o are minorized by the centralized PCRLB as follows: where tr(R) represents the trace of the matrix R, P n,a , and P n,o , associated, respectively, with the aircraft a position and the target state (excluding velocities), are 3 × 3 diagonal blocks of [J n ] −1 , where J n is the Fisher information matrix (see Appendix A).

A. Numerical Results and Discussion
The cooperative localization experiments results are presented in Fig. 6 and compared to the PCRLB, in gray dashed line -as in (70).
In the noncooperative (NC) configuration, since only half of the aircraft perform TAN, the results diverge.The results of CTAN and J-CTAN-DTT are in turn very similar to each other, with J-CTAN-DTT showing only a small improvement, mainly in the beginning of the simulation.
The results of the target tracking experiments are shown in Fig. 7 and compared to the associated PCRLB -gray dashed line -as in (71).
In Fig. 7, the results of the CTAN configuration are better than those of NC, even without the distributed target tracking messages, because the target position estimate is based on the aircraft position estimate.Without cooperative localization, the six aircraft that do not perform TAN (i.e., are flying only with INS) cannot estimate their own position accurately, and therefore do not perform good target  tracking.In addition, for the other six aircraft that perform TAN, cooperative localization improves the aircraft position estimate, which in turn, allows for a better target observation assimilation.In the J-CTAN-DTT configuration, where the cooperative localization, distributed target tracking, and feedback messages are enabled, the results are close to the PCRLB, shown in gray.
Besides the accuracy improvement in target tracking, the proposed algorithm also decreased the dispersion of the target state estimate for each aircraft, suggesting that the aircraft approach a weak consensus on the target state estimate.To highlight this precision improvement, we show in Fig. 8, the target tracking results for all configurations, comparing the average network tracking RMSE (in black) with target tracking RMSE e (a)  n,o for each aircraft a.In Table III, we show the average execution time, per iteration, per node, for the proposed algorithm, in a C++ multithread implementation, running in a Intel i9-12900H computer.

1) Comm Cost J-CTAN-DTT:
The proposed algorithm J-CTAN-DTT described in section III has some characteristics that are desirable in real-time distributed algorithms.For example: 1) It does not need random number generation synchronization between the different network nodes since each of them stores a different set of particles to represent its own state and the state of the target.2) It does not require iterative message passing between nodes at the same time stamp n (i.e., between two consecutive sets of sensor measurements).
3) The number of network nodes can be unknown and vary over time.
In terms of internode communication overhead, at each instant n, each aircraft a needs to broadcast to the neighborhood only the parameters that define the local state variables xn,a and Considering the scenario described in this article where D a = 3, D o = 6, and |O| = 1, the total number of real values the aircraft a broadcasts over each internode communication link is 36.
2) Comm Cost Average Consensus: As a comparison, using the average consensus approach described in [24], the total number of real values the agent a broadcasts at instant n is where using the same notation as in [24] 1) P is number of message passing iterations; 2) C is number of consensus iterations; 3) J is the number of particles used to represent either target or agent states; 4) I is diameter of the communication graph; 5) L is the dimension of the agent states (only position); 6) |O| is the number of objects; 7) N c P(C + I )J|O|; and 8) N NBP PJL.
Considering the same scenario described in this article, with the aircraft being the agents and the object being the target, we have P = 1, I = 4, L = D a = 3, |O| = 1.In addition, for the purposes of this discussion, we assume the same number of consensus iterations C = 6 and the same number of particles J = 1000 used in the dynamic scenario described in [24].Thus, using the formula in (72), the total number of real values that the agent a would have to broadcast over each network link in the same consensus approach as in [24] is 13 000.Note that even though in our scenario we use 3000 particles for the aircraft representation, we are assuming J = 1000 for both aircraft and target states, which decreases the communication cost.
Even if we considered only 1 consensus iteration (C = 1) and also considered a fixed network configuration with known network diameter size, in which case we could set I to zero in (72), see [24], the total number of real values that the agent a would have to broadcast over each link using the average consensus approach would be 4000 in the same scenario as in our article.
The high communication cost presented in [24] is driven by the need to have J average consensus protocols-one for each particle-between consecutive sets of sensor observations, where each average consensus protocol requires, in turn, C iterations.Moreover, the J particles have to be identical in all nodes, thus requiring random number generators that are synchronized between the different nodes.The internode communication cost in [24] can be significantly reduced if an alternative scheme called likelihood consensus proposed in [38] is used.Specifically, likelihood consensus eliminates the linear growth of the internode communication cost with the number of particles J and also eliminates the need for random number generator synchronization across nodes.Furthermore, likelihood consensus also eliminates the need to do I additional max consensus iterations to determine the number of nodes in the network.However, even in a likelihood consensus implementation, the internode communication cost still grows linearly with the number of consensus iterations C and, if that parameter is high, for example, C = 15 as in the second scenario considered in [24], the communication requirements between nodes are still about an order of magnitude higher than in the diffusion approach proposed in this article.

VI. CONCLUSION
We proposed in this work an approach based on ATC diffusion [6], [7], which is seamlessly integrated into the factor graph framework and implemented in a fully distributed fashion using the SPA by introducing novel special factors to perform the distributed combine step.The proposed solution approximates the adapt step of a classic distributed ATC tracker by replacing the likelihood of the local target observations at each agent node given the target and the agent positions by its expected value with respect to the latest belief (i.e., posterior probability distribution) that the agent has about its own state.Next, the aforementioned special factors combine the updated target state beliefs following the adapt step at each node using an optimal minimum average KL divergence criterion, which, for Gaussian beliefs, reduces to the well-known method of covariance intersection in sensor fusion.With the proposed factor-graph-based combine step, we improved the overall target tracking accuracy and precision, in such a way that all aircraft in the network have a better and almost identical target state estimate, agreeing with each other on their target position estimates, using a low-cost communication scheme.In addition, after the combine step, each agent uses their updated target belief as if it were an anchor, to improve their self-localization, thus assimilating the target measurements.
We extended the scenario from our previous work [1] and tested three different configurations.In all cases, the error performance was compared to the PCRLB for the error of the optimal data fusion filter that processes all available measurements on the network at each time step on a central hub.The proposed distributed algorithm was shown to perform close to the centralized PCRLB, for both self-localization and target tracking errors, in a simulated GNSS-denied scenario and a partially connected network with a low-cost communication scheme.Even aircraft that cannot locate themselves alone, using the proposed algorithm, they can locate themselves, track the target, and agree with each other on their estimates of the target's position.

APPENDIX A
A. Performance Bounds 1) Centralized Signal Model: As in [1], for the centralized filtering approach, we stack together all cooperative aircraft and noncooperative target state vectors in an extended state vector and do the same for the observations, now including also target radar measurements and barometric altitude measurements.We then make the factor graph corresponding to the distributed signal model into a tree-like graph by removing all loops between variable nodes.So, the extended state vector x c n and the extended observation vector y c n are defined, respectively, as4 x c n = x T n,1 x T n,2 . . .  in which, as derived in [39], the Fisher matrix J n+1 at instant n + 1 is recursively computed from J n as to the barometric altitude, ground clearance, aircraft distance, and target observations, respectively.Note that the Jacobian matrix H c n,to includes block diagonal Jacobian submatrices for all available target observations-slant-range, azimuth, and elevation-at instant n.

10 ) 4 ) 11 ) 5 ) 12 ) 6 )
ba n,a ba n,a (x n,a ) p(y n,a←ba |x n,a ).(Internode distance factor between node a and node b ad n,a←b ad n,a←b (x n,a , xn,b ) = p(y n,a←b |x n,a , xn,b ).(Target observation factor to (a) n,o to (a) n,o xn,a , x(a) n,o p y n,a←o |x n,a , x(a) n,o .(Target motion factor
as N(n, a) ∪ {a}, at instant n.Given now the target state p.d.f.'s p(b) n,o (x (b) n,o ) at each node b ∈ N(n, a), we want to obtain an artificial transition p.d.f.c (a,b) ) implement the SPA over the local target tracking (local-tt) subgraph.Messages (41)-(44) in turn implement
n,o and setting the weights w(a,k) n,o ∝ φ (a) n,o x(a,k) n,o , where the message φ (a) n,o x(a) n,o , introduced in (37), assimilates the target observation, and we normalize the weights again such that N o i=1 w(a,k) n,o = 1.Since the target observation factor to (a) n,o xn,a , x(a) n,o = p y n,a←o |x n,a , x(a) n,o = N y n,a←o |to xn,a , x(a) n,o , R to (56) After receiving λn,b xn,b from each node b ∈ N(n, a), N b particles are resampled at node a from each of the received GMMs.That procedure results in a new set of weighted particles w( j) n,b , x( j) n,b for each b ∈ N(n, a), where x( j) n,b ∼ λn,b (x n,b ) and w( j) n,b = 1/N b ∀ j ∈ {1, . . ., N b }.From the internode distance model in (5), we write ad n,a←b (x n,a , xn,b ) = p(y n,a←b |x n,a , xn,b ) = N (y n,a←b |rg x n,a , xn,b , σ 2 n,o .After receiving { λ(b) n,o x(b) n,o } from all b ∈ N(n, a), we set the network coefficients r a,b 1/|N(n, a)|, where |N(n, a)| denotes the cardinality of the closed neighborhood of a, N(n, a), which we recall is given by N(n, a) N(n, a) ∪ {a}.In this case, |N(n, a)| is simply equal to the number of received messages plus one and, then, the factors cl (a) n,o and cd (a←b) n,o can be computed analytically.Assuming that the input message ν (a) n,o x(a) n,o is represented by a Gaussian p.d.f.N x(a) n,o | m(a) n,o , ˜ (a)n,o and, similarly, λ(a←b) ) setting r a,b = 1/|N(n, a)| ∀b ∈ N(n, a), that:

Fig. 5 .
Fig. 5.All 200 trajectories of the target (gray) and of each aircraft.

Fig. 8 .
Fig. 8. Target tracking dispersion results.The black curve shows the corresponding scenario configuration RMSE result, while the gray curves show the RMSE results of each aircraft.J-CTAN-DTT has a significantly lower dispersion than NC and CTAN.

h 1 n
c (x c n ) = ba(x c n ) T gc(x c n ; M) T ad(x c n ) T to(x c n ) T T .(81) 2) PCRLB: The posterior Cramér-Rao lower bound of the state estimation error for the centralized filtering problem has the form E ||x c n − xc n || 2 ≥ tr J −

J n+1 = D 22 n − D 21 n J n + D 11 n− 1
D12  n .(83)Thecoefficients D11  n , D12  n , D21  n , and D22  n in (83), induced by the dynamic and observation models, defined in model (75) and (76), respectively, are computed such that n+1 = ∇ x c n+1 h c x c n+1 .Then, we compute H c n asH c n = ∇ x c n h c x c n = [H c n,ba ] T [H c n,gc ] T [H c n,ad ] T [H c n,to ] T T (88)whereH c n,ba = ∇ x c n ba(x c n ), H c n,gc = ∇ x c n gc(x c n ; M), H c n,ad = ∇ x c n ad(x c n ),and H c n,to = ∇ x c n to(x c n ) are the Jacobians related x n,o [ x n,o ẋn,o y n,o ẏn,o z n,o żn,o ] T denote, at instant n, the unknown position and velocity of the noncooperative target o in 3-D Cartesian coordinates.The kinematic dynamic model for the target is x(a) n,o .Let 1) D a denote the dimension of the aircraft state vector; 2) D o denote the dimension of the target state vector; 3) D cov,a denote the number of real-valued free elements of the (symmetric) aircraft state covariance matrix, i.e., (D 2 a + D a )/2; 4) D cov,o denote the number of real-valued free elements of the (symmetric) target state covariance matrix, i.e., (D 2 o + D o )/2, and 5) |O| denote the number of objects, which, as noted before, could be greater than one if there are no data association issues.Using the aforementioned notation and, since the state p.d.f's are represented by Gaussian functions, it follows that the total number of real values that the aircraft a broadcasts at instant n over a given internode communication link is (D a + D cov,a ) + [(D o + D cov,o ) * |O|].