Distributed Twice-Extended State Kalman Filter for Multi-Photoelectric Tracking System over Sensor Networks

Abstract

Multi-photoelectric tracking systems (MPTSs) provide high-precision line-of-sight (LOS) angles for long-range passive tracking, with each photoelectric tracking system (PTS) delivering bearing-only measurements. In practice, image-based target extraction and optical–mechanical pointing may suffer from intermittent errors when highly maneuverable targets are observed, thereby degrading tracking accuracy. To address this issue, we consider a time-varying uncertain dynamics model with a lumped uncertainty and its temporal difference. A centralized twice-extended state Kalman filter (CTESKF) is proposed to augment the kinematic state with the lumped uncertainty and its temporal difference, yielding a Kalman-type recursion with a computable covariance upper bound. Building on this, a diffusion-based twice-extended state Kalman filter (DTESKF), which combines local updates with single-round covariance-intersection diffusion fusion, is proposed to achieve distributed filtering with limited communication cost. Simulation results show that CTESKF and DTESKF achieve competitive accuracy–efficiency trade-offs in a weakly nonlinear setting and a 3D bearing-only MPTS scenario.

1. Introduction

Photoelectric tracking systems (PTSs) are passive imaging sensors that provide high-precision line-of-sight (LOS) information and are widely used in target tracking, long-range surveillance, space object observation, and optical communication terminals [1,2,3,4,5]. After image processing and LOS extraction, a typical PTS outputs bearing-only measurements, usually azimuth and elevation angles. Because direct range information is unavailable or unreliable at long distances, especially for dim targets or under adverse atmospheric conditions, bearing-only tracking is inherently nonlinear and strongly dependent on the sensor–target geometry. As a result, a single PTS may suffer from poor observability, slow convergence, and biased state estimates under unfavorable motion configurations. In particular, it is well known that a single observer moving at constant velocity cannot determine the target range without additional maneuvers.

A natural way to alleviate this observability bottleneck is to deploy multiple spatially separated PTS nodes to observe the same target. By introducing geometric diversity, a multi-photoelectric tracking system (MPTS) can recover information that is inaccessible to an individual sensor and thereby improve localization accuracy and tracking robustness [6,7,8]. However, converting this geometric advantage into reliable network-level estimation requires an effective fusion architecture. Although centralized fusion can, in principle, process all measurements directly, it usually imposes a heavy communication burden and creates a single point of failure. For this reason, distributed filtering is particularly attractive for MPTSs operating over sensor networks.

Designing a distributed filter for bearing-only MPTSs is nevertheless challenging for three main reasons. First, the measurement model is strongly nonlinear: the mapping from Cartesian coordinates to azimuth and elevation involves arctangent and square-root operations, and it may become ill-conditioned when the target is close to the sensor boresight or when the relative geometry is degenerate. Second, practical target motion often contains time-varying nonlinear uncertainty, such as target maneuvers, unmodeled dynamics, and environmental disturbances. If such uncertainty is not adequately represented, the filter may become overconfident or even diverge; this problem is especially severe in bearing-only tracking because the measurements provide only weak constraints on range and velocity. Third, sensor networks usually operate under communication constraints. Although consensus-based schemes can approach centralized performance through multiple rounds of information exchange within each sampling period [9,10,11], such multi-round protocols are often impractical for MPTS with limited bandwidth, non-negligible latency, or strict duty-cycle constraints.

Against this background, distributed Kalman filtering has been extensively studied for sensor networks, with consensus and diffusion representing two major paradigms [9,10,11,12,13]. Compared with consensus strategies, diffusion methods perform a local update followed by a single fusion step and are therefore better suited to real-time operation under limited communication [12]. A central difficulty, however, is that local estimates are generally correlated, whereas the corresponding cross-correlation terms are rarely available. Covariance intersection (CI) provides a consistent way to fuse correlated Gaussian estimates without requiring those cross-correlations explicitly, albeit at the price of some conservativeness [13]. Building on this idea, representative distributed nonlinear filters such as the distributed extended Kalman filter (DEKF), distributed unscented Kalman filter (DUKF), and distributed cubature Kalman filter (DCKF) combine local nonlinear updates with diffusion or consensus fusion. Nevertheless, these methods still rely mainly on a nominal motion model to capture the dominant target dynamics. When the target is subject to rapidly varying or poorly modeled uncertainty, their estimation accuracy can degrade substantially.

A complementary line of research is extended state estimation. The extended state observer (ESO) regards unknown dynamics as an augmented state and estimates the state online [14]. In stochastic settings, extended state Kalman filters (ESKFs) augment the state vector with a lumped uncertainty term and have shown improved robustness for uncertain nonlinear systems [15,16,17]. Distributed ESKF methods have also been investigated for networked estimation problems. He et al. proposed a distributed ESKF for stochastic uncertain systems under quantized data flows over switching sensor networks [18], and Peng et al. developed a distributed ESKF for complex networks with nonlinear uncertainty [19]. Liang et al. further analyzed the consistency and stability of distributed ESKF under uncertain dynamics [20]. More recently, Lin et al. studied distributed state estimation for nonlinear uncertain systems under estimate-based round-robin communication protocols and cyber attacks [21]. These distributed ESKF methods usually augment the original system with a single lumped uncertainty state. Such a structure is effective when the unknown dynamics vary slowly or can be locally approximated as constant. However, for rapidly time-varying uncertainty, a single augmented uncertainty state may not adequately capture the temporal evolution of the unknown dynamics, which can lead to model mismatch and overconfident covariance estimates. This limitation becomes particularly restrictive in bearing-only MPTSs, where weak range observability makes the estimators highly sensitive to dynamic uncertainty.

Motivated by the above observations, this paper studies distributed filtering for bearing-only MPTSs with time-varying dynamic uncertainty. We propose a centralized twice-extended state Kalman filter (CTESKF) and its distributed counterpart, the diffusion-based twice-extended state Kalman filter (DTESKF). The main idea is to augment the kinematic state not only with a lumped uncertainty term but also with its first-order temporal difference, while treating the second-order temporal difference as an injected uncertainty. In this way, the proposed framework can track time-varying nonlinear uncertainty more explicitly than existing extended state formulations while retaining a Kalman-type recursive structure with consistency guarantees.

The main contributions are:

• We develop a twice-extended state model that estimates an unknown time-varying nonlinear uncertainty together with its first-order temporal difference online, thereby improving the representation of rapidly varying dynamics.
• For centralized estimation, we derive a computable covariance upper-bound recursion under bounded second-order temporal differences of the uncertainty. The resulting CTESKF preserves a Kalman-type predictor–corrector structure while maintaining consistency.
• For distributed estimation, we construct the DTESKF by combining local twice-extended state filtering with a single-round CI-based diffusion fusion rule. The fusion is implemented in information form and does not require knowledge of inter-node cross-correlations, which makes it suitable for bandwidth-limited MPTSs.
• Numerical simulations, including a nonlinear uncertain benchmark, a parameter-sensitivity study, and a 3D bearing-only MPTS scenario, demonstrate that the proposed filters improve estimation accuracy and robustness under time-varying uncertainties while retaining competitive computational efficiency.

The remainder of this paper is organized as follows. Section 2 formulates the uncertain dynamics and the MPTS bearing-only measurement model. Section 3 presents the CTESKF and its covariance upper bound. Section 4 develops the DTESKF with CI-based diffusion fusion. Section 5 analyzes consistency properties of the DTESKF. Section 6 provides numerical simulations for multi-photoelectric tracking. Section 7 concludes the paper.

• Notation

${\mathbb{R}}^n$ denotes the n-dimensional real vector space. I denotes an identity matrix with appropriate dimension. For a vector x, $\parallel x\parallel$ is the Euclidean norm; for a matrix X, ${\parallel X\parallel }_2$ is the induced 2-norm. For symmetric matrices $X,Y$, $X⪰Y$ means $X-Y$ is positive semidefinite. A connected undirected graph $\mathcal{G}=(\mathcal{V},\mathcal{E})$ has node set $\mathcal{V}=\{1,\dots ,M\}$; the local fusion set of node i is denoted by ${\mathcal{N}}_i$ when introduced. For an angle $\varphi$, $\mathcal{W}\left(\varphi \right)$ denotes wrapping to $(-\pi ,\pi ]$; for an angle vector, the wrapping is applied element-wise.

2. Problem Formulation

2.1. Nonlinear Uncertain Discrete-Time Dynamics

We consider a discrete-time nonlinear uncertain system over $k\in {\mathbb{Z}}_{\ge 0}$,

$$x_{k+1}=f_k\left(x_k\right)+w_k,$$

(1)

where $x_k\in {\mathbb{R}}^n$ is the state and $w_k$ is zero-mean Gaussian process noise with covariance $Q_k$. We assume that $f_k$ admits the affine-in-uncertainty structure

$$f_k\left(x_k\right)=A_k{x}_k+B_{k}F(x_k,k),$$

(2)

where $F(·)\in {\mathbb{R}}^{\mathrm{\ell }}$ is a lumped nonlinear uncertainty capturing unmodeled dynamics, parametric drift, and residual nonlinearities. This structure is natural in kinematic tracking: $A_k$ denotes a nominal linear model, while $B_{k}F(·)$ represents unknown accelerations or maneuvering inputs.

2.2. MPTS Bearing-Only Measurement Model

The MPTS consists of M PTS devices and is shown in Figure 1. Each PTS measures the direction of the target relative to its own location. Let PTS i be located at a known position $s_i={[s_{x,i},s_{y,i},s_{z,i}]}^{\mathrm{\top }}\in {\mathbb{R}}^3$. For 3D tracking, we adopt a 6D kinematic state

$$x_k={[p_{x,k},p_{y,k},p_{z,k},v_{x,k},v_{y,k},v_{z,k}]}^{\mathrm{\top }},$$

(3)

where $p_k\in {\mathbb{R}}^3$ and $v_k\in {\mathbb{R}}^3$ denote position and velocity.

Each PTS i first extracts the target from its camera image and then outputs azimuth and elevation angles using the optical–mechanical pointing/encoder information, as illustrated in Figure 2.

$$y_{i,k}=h_i\left(x_k\right)+v_{i,k},i=1,\dots ,M,$$

(4)

where $y_{i,k}={[{\theta }_{i,k},{\varphi }_{i,k}]}^{\mathrm{\top }}\in {\mathbb{R}}^2$, and $v_{i,k}\sim \mathcal{N}(0,R_{i,k})$ is the measurement noise. Define the relative vector $r_{i,k}=p_k-s_i={[d_x,d_y,d_z]}^{\mathrm{\top }}$ and $\rho =\sqrt{d_x^2+d_y^2}$. The PTS bearing observation function is defined as

$$h_i\left(x_k\right)=\left[\begin{array}{c}\mathrm{atan2}(d_y,d_x)\\ \mathrm{atan2}(d_z,\rho )\end{array}\right].$$

(5)

Because angles are periodic, the innovation should be computed with wrapping:

$${\nu }_{i,k}:=\mathcal{W}\left(y_{i,k}-h_i\left({\widehat{x}}_{i,k}\right)\right),$$

(6)

where $\mathcal{W}(·)$ wraps each component to $(-\pi ,\pi ]$.

2.3. Local Linearization for Bearing-Only Measurements

To keep the core filter structure and analysis tractable, we let

$$H_{i,k}:={\left.{\displaystyle \frac{\partial h_i}{\partial x}}\right|}_{x={\widehat{x}}_{i,k}}$$

denote the Jacobian. For the bearing-only model in Equation (5), $H_{i,k}$ has nonzero entries only in the position components and admits a closed form as

$$H_{i,k}=\left[\begin{array}{cccccc}-{\displaystyle \frac{d_y}{{\rho }^2}}& {\displaystyle \frac{d_x}{{\rho }^2}}& 0& 0& 0& 0\\ -{\displaystyle \frac{d_z{d}_x}{({\rho }^2+d_z^2)\rho }}& -{\displaystyle \frac{d_z{d}_y}{({\rho }^2+d_z^2)\rho }}& {\displaystyle \frac{\rho }{{\rho }^2+d_z^2}}& 0& 0& 0\end{array}\right].$$

(7)

Using a first-order Taylor approximation,

$$y_{i,k}\approx h_i\left({\widehat{x}}_{i,k}\right)+H_{i,k}(x_k-{\widehat{x}}_{i,k})+v_{i,k}.$$

(8)

Based on the above primary model, we will develop the CTESKF and DTESKF in the next sections.

3. Centralized Twice-Extended State Kalman Filter Formulation

3.1. Twice-Extended State Augmentation

Define the augmented state

$${\xi }_k:=\left[\begin{array}{c}{x}_k\\ F_k\\ \Delta F_k\end{array}\right]\in {\mathbb{R}}^{n+2\mathrm{\ell }},$$

(9)

where $F_k:=F(x_k,k)$ and $\Delta F_k:=F_{k+1}-F_k$. Using $F_{k+1}=F_k+\Delta F_k$ and $\Delta F_{k+1}=\Delta F_k+{\Delta }^2{F}_k$, the augmented dynamics become

$${\xi }_{k+1}=\underset{:={\mathcal{A}}_k}{\underbrace{\left[\begin{array}{ccc}{A}_k& B_k& 0\\ 0& I_{\mathrm{\ell }}& I_{\mathrm{\ell }}\\ 0& 0& I_{\mathrm{\ell }}\end{array}\right]}}{\xi }_k+\underset{:=\mathcal{B}}{\underbrace{\left[\begin{array}{c}0\\ 0\\ I_{\mathrm{\ell }}\end{array}\right]}}{\Delta }^2{F}_k+\underset{:=\mathcal{D}}{\underbrace{\left[\begin{array}{c}{I}_n\\ 0\\ 0\end{array}\right]}}{w}_k,$$

(10)

where $w_k\sim \mathcal{N}(0,Q_k)$. Based on the local linearization in Equation (8), the augmented measurement equation is approximated as

$$\begin{array}{cc}\hfill y_{i,k}& \approx h_i\left({\widehat{x}}_{i,k}\right)+H_{i,k}(x_k-{\widehat{x}}_{i,k})+v_{i,k}\hfill \end{array}$$

(11)

$$\begin{array}{cc}\hfill & \approx \underset{:={\mathcal{C}}_{i,k}}{\underbrace{\left[\begin{array}{ccc}{H}_{i,k}& 0& 0\end{array}\right]}}{\xi }_k+(h_i\left({\widehat{x}}_{i,k}\right)-H_{i,k}{\widehat{x}}_{i,k})+v_{i,k}.\hfill \end{array}$$

(12)

In implementation, the update uses the Jacobian ${\mathcal{C}}_{i,k}$ together with the wrapped innovation ${\nu }_{i,k}$ in Equation (6). For the covariance analysis, we consider the corresponding linearized measurement model and write it compactly as $y_{i,k}={\mathcal{C}}_{i,k}{\xi }_k+v_{i,k}$. Therefore, the augmented system is

$${\xi }_{k+1}={\mathcal{A}}_k{\xi }_k+\mathcal{B}{\Delta }^2{F}_k+\mathcal{D}{w}_k,$$

(13)

$$y_{i,k}={\mathcal{C}}_{i,k}{\xi }_k+v_{i,k},i=1,\dots ,M.$$

(14)

Let $y_k:=\mathrm{col}(y_{1,k},\dots ,y_{M,k})$, ${\mathcal{C}}_k:=\mathrm{col}({\mathcal{C}}_{1,k},\dots ,{\mathcal{C}}_{M,k})$, and $R_k:=\mathrm{blkdiag}(R_{1,k},\dots ,R_{M,k})$. The measurements can then be stacked as

$$y_k={\mathcal{C}}_k{\xi }_k+v_k,v_k\sim \mathcal{N}(0,R_k).$$

(15)

Define the stacked innovation ${\nu }_k:=\mathrm{col}({\nu }_{1,k},\dots ,{\nu }_{M,k})$. For bearing-only measurements, each ${\nu }_{i,k}$ is computed by angle wrapping in Equation (6); for a linear measurement model, ${\nu }_k=y_k-{\mathcal{C}}_k{\widehat{\xi }}_k$.

Define the lifted process noise covariance for the augmented system as

$$Q_{k,1}:=\mathcal{D}{Q}_k{\mathcal{D}}^{\mathrm{\top }}⪰0.$$

(16)

We make the following assumptions concerning the initial state and the uncertainty variation.

Assumption 1.

$$\begin{array}{c}\hfill \mathbb{E}\left[({\xi }_0-{\widehat{\xi }}_0){({\xi }_0-{\widehat{\xi }}_0)}^{\mathrm{\top }}\right]⪯P_{0|0},\end{array}$$

(17)

where ${\widehat{\xi }}_0={\left[\begin{array}{ccc}{\widehat{x}}_0& {\widehat{F}}_0& {\widehat{\Delta F}}_0\end{array}\right]}^{\mathrm{\top }}$, ${\widehat{F}}_0={\widehat{\Delta F}}_0=0$, and $P_{0|0}\succ 0$ is known.

Assumption 2.

$$\begin{array}{c}\hfill \mathbb{E}\left[|{\Delta }^2{F}_{k,r}{|}^2\right]\le {\overline{q}}_{k,r},\forall k\ge 0,r=1,\dots ,\mathrm{\ell },\end{array}$$

(18)

where ${\overline{q}}_{k,r}$ is known and uniformly bounded. Moreover, for the covariance analysis, ${\widehat{{\Delta }^{2}F}}_k$ is measurable with respect to the information available up to time k, and ${\Delta }^2{F}_k$ is treated as an exogenous uncertainty sequence that is uncorrelated with $w_k$ and $v_k$.

Assumption 3.

The augmented pair $({\mathcal{A}}_k,{\mathcal{C}}_k)$ is uniformly observable.

Remark 1.

The theoretical development of the CTESKF and DTESKF relies on mild boundedness and observability assumptions, which are stated in Assumptions 1–3 after the augmented model is fully specified. Here we briefly interpret them for MPTS tracking. Assumption 2 only bounds the second-order temporal difference ${\Delta }^2{F}_k$, rather than requiring the lumped uncertainty $F_k$ itself to be small. This matches the twice-extended design: $F_k$ and $\Delta F_k$ are estimated as augmented states, while ${\Delta }^2{F}_k$ is treated as the remaining bounded injection. In MPTS tracking, $F_k$ may represent target maneuver acceleration, residual atmospheric disturbance, or image-extraction modeling error. Qualitatively, the bound ${\overline{q}}_{k,r}$ characterizes how rapidly the variation trend of this lumped uncertainty can change between adjacent sampling instants. More aggressive target maneuvers, stronger disturbance fluctuations, or intermittent image-extraction errors generally require a larger bound. For the same continuous-time uncertainty, a larger sampling interval also leads to a larger discrete second-order difference because more variation is accumulated between samples, whereas smoother motion or faster sampling allows a smaller bound. Assumption 3 requires the augmented pair $({\mathcal{A}}_k,{\mathcal{C}}_k)$ to be uniformly observable. In bearing-only tracking, observability is a collective property: a single station provides only LOS angles, but multiple stations with non-collinear baselines, together with informative target motion, can render the overall system observable. In practice, geometry degeneracy (e.g., nearly collinear stations or near-stationary LOS) should be avoided through sensor placement and scheduling.

3.2. Centralized Twice-Extended State Kalman Filter

We implement the CTESKF in a Kalman predictor–corrector form, which aligns with the distributed algorithm in Section 4 and avoids sign ambiguities in observer-form gains.

• Saturation-based uncertainty injection:

Define the element-wise saturation function $\mathrm{sat}(·,b)$ for $b>0$ as

$$\mathrm{sat}(z,b):=\mathrm{sign}\left(z\right)\min \left\{\right|z|,b\},$$

(19)

applied componentwise for vectors. The bounded second-difference injection is

$${\widehat{{\Delta }^{2}F}}_{k,r}=\mathrm{sat}\left({\widehat{{\Delta }^{2}F}}_{k,r}^{\mathrm{nom}},\sqrt{{\overline{q}}_{k,r}}\right),$$

(20)

where ${\widehat{{\Delta }^{2}F}}_{k,r}^{\mathrm{nom}}$ is a nominal second-difference estimate of $F(·)$, computed componentwise for $r=1,\dots ,\mathrm{\ell }$.

• Measurement update:

Introduce an effective measurement covariance

$${\overline{R}}_k:={\displaystyle \frac{1}{1+\theta }}{R}_k,\theta >0,$$

(21)

which will be used in the upper-bound recursion. Given the prior pair $({\widehat{\xi }}_k,P_k)$, compute

$$G_k:=P_k{\mathcal{C}}_k^{\mathrm{\top }}{({\mathcal{C}}_k{P}_k{\mathcal{C}}_k^{\mathrm{\top }}+{\overline{R}}_k)}^{-1},$$

(22)

$${\widehat{\xi }}_k^{+}:={\widehat{\xi }}_k+G_k{\nu }_k,$$

(23)

$$P_k^{+}:=(I-G_k{\mathcal{C}}_k)P_k{(I-G_k{\mathcal{C}}_k)}^{\mathrm{\top }}+G_k{\overline{R}}_k{G}_k^{\mathrm{\top }}.$$

(24)

• Time update:

Define the uncertainty-induced covariance inflation terms

$$Q_{k,2}:=4\mathrm{\ell }·\mathrm{diag}\left([{\overline{q}}_{k,1}{\overline{q}}_{k,2}\cdots {\overline{q}}_{k,\mathrm{\ell }}]\right)⪰0,$$

(25)

and

$${\tilde{Q}}_k:=Q_{k,1}+\left(1+{\displaystyle \frac{1}{\theta }}\right)\mathcal{B}{Q}_{k,2}{\mathcal{B}}^{\mathrm{\top }}.$$

(26)

Then the predicted estimate and covariance upper bound are

$${\widehat{\xi }}_{k+1}:={\mathcal{A}}_k{\widehat{\xi }}_k^{+}+\mathcal{B}{\widehat{{\Delta }^{2}F}}_k,$$

(27)

$$P_{k+1}:=(1+\theta ){\mathcal{A}}_k{P}_k^{+}{\mathcal{A}}_k^{\mathrm{\top }}+{\tilde{Q}}_k.$$

(28)

Theorem 1 (Computable covariance upper bound).

Under Assumptions 1 and 2, the recursion in Equations (24)–(28) yields a consistent covariance upper bound in the sense that

$$\mathbb{E}\left[({\xi }_{k+1}-{\widehat{\xi }}_{k+1}){({\xi }_{k+1}-{\widehat{\xi }}_{k+1})}^{\mathrm{\top }}\right]⪯P_{k+1},\forall k\ge 0.$$

(29)

Proof. 

Let $e_k:={\xi }_k-{\widehat{\xi }}_k$ denote the prior error and ${\tilde{d}}_k:={\Delta }^2{F}_k-{\widehat{{\Delta }^{2}F}}_k$ denote the injection mismatch. For the linearized measurement model used in the analysis, ${\nu }_k=y_k-{\mathcal{C}}_k{\widehat{\xi }}_k={\mathcal{C}}_k{e}_k+v_k$. Hence, from Equation (23), the posterior error is

$${\xi }_k-{\widehat{\xi }}_k^{+}=(I-G_k{\mathcal{C}}_k)e_k-G_k{v}_k.$$

(30)

Using Equations (13) and (27), the one-step prediction error becomes

$${\xi }_{k+1}-{\widehat{\xi }}_{k+1}={\mathcal{A}}_k({\xi }_k-{\widehat{\xi }}_k^{+})+\mathcal{D}{w}_k+\mathcal{B}{\tilde{d}}_k.$$

(31)

Taking second moments and using the independence of $(w_k,v_k)$ from $e_k$ yields

$$\begin{array}{cc}\hfill & \mathbb{E}\left[({\xi }_{k+1}-{\widehat{\xi }}_{k+1}){({\xi }_{k+1}-{\widehat{\xi }}_{k+1})}^{\mathrm{\top }}\right]\hfill \\ \hfill & ={\mathcal{A}}_k(I-G_k{\mathcal{C}}_k)\mathbb{E}\left[e_k{e}_k^{\mathrm{\top }}\right]{(I-G_k{\mathcal{C}}_k)}^{\mathrm{\top }}{\mathcal{A}}_k^{\mathrm{\top }}+{\mathcal{A}}_k{G}_k{R}_k{G}_k^{\mathrm{\top }}{\mathcal{A}}_k^{\mathrm{\top }}\hfill \\ \hfill & +Q_{k,1}+\mathcal{B}\mathbb{E}[{\tilde{d}}_k{\tilde{d}}_k^{\mathrm{\top }}]{\mathcal{B}}^{\mathrm{\top }}+{\mathcal{A}}_k(I-G_k{\mathcal{C}}_k)\mathbb{E}[e_k{\tilde{d}}_k^{\mathrm{\top }}]{\mathcal{B}}^{\mathrm{\top }}\hfill \\ \hfill & +\mathcal{B}\mathbb{E}[{\tilde{d}}_k{e}_k^{\mathrm{\top }}]{(I-G_k{\mathcal{C}}_k)}^{\mathrm{\top }}{\mathcal{A}}_k^{\mathrm{\top }}.\hfill \end{array}$$

(32)

First, by Assumption 2 and the saturation bound in Equation (20),

$$\mathbb{E}[{\tilde{d}}_k{\tilde{d}}_k^{\mathrm{\top }}]⪯2\mathbb{E}[{\Delta }^2{F}_k{({\Delta }^2{F}_k)}^{\mathrm{\top }}]+2\mathbb{E}[{\widehat{{\Delta }^{2}F}}_k{({\widehat{{\Delta }^{2}F}}_k)}^{\mathrm{\top }}].$$

(33)

For any $z\in {\mathbb{R}}^{\mathrm{\ell }}$, $z{z}^{\mathrm{\top }}⪯\mathrm{\ell }\mathrm{diag}(z_1^2,\dots ,z_{\mathrm{\ell }}^2)$. Hence,

$$\mathbb{E}\left[{\Delta }^2{F}_k{\left({\Delta }^2{F}_k\right)}^{\mathrm{\top }}\right]⪯\mathrm{\ell }\mathrm{diag}\left(\mathbb{E}\left[\right|{\Delta }^2{F}_{k,1}{|}^2],\dots ,\mathbb{E}[|{\Delta }^2{F}_{k,\mathrm{\ell }}{|}^2]\right)⪯\mathrm{\ell }\mathrm{diag}\left([{\overline{q}}_{k,1}\cdots {\overline{q}}_{k,\mathrm{\ell }}]\right).$$

(34)

Moreover, the componentwise saturation in Equation (20) gives

$$\mathbb{E}[{\widehat{{\Delta }^{2}F}}_k{({\widehat{{\Delta }^{2}F}}_k)}^{\mathrm{\top }}]⪯\mathrm{\ell }\mathrm{diag}([{\overline{q}}_{k,1}\cdots {\overline{q}}_{k,\mathrm{\ell }}]).$$

(35)

Therefore,

$$\mathbb{E}[{\tilde{d}}_k{\tilde{d}}_k^{\mathrm{\top }}]⪯4\mathrm{\ell }\mathrm{diag}\left([{\overline{q}}_{k,1}\cdots {\overline{q}}_{k,\mathrm{\ell }}]\right)=Q_{k,2}.$$

(36)

Second, for the cross terms, apply the matrix Young inequality: for any $\theta >0$,

$$U{V}^{\mathrm{\top }}+V{U}^{\mathrm{\top }}⪯\theta U{U}^{\mathrm{\top }}+{\displaystyle \frac{1}{\theta }}V{V}^{\mathrm{\top }}.$$

Let $U:={\mathcal{A}}_k(I-G_k{\mathcal{C}}_k)e_k$ and $V:=\mathcal{B}{\tilde{d}}_k$. Taking expectations gives

$$\begin{array}{cc}\hfill & {\mathcal{A}}_k(I-G_k{\mathcal{C}}_k)\mathbb{E}[e_k{\tilde{d}}_k^{\mathrm{\top }}]{\mathcal{B}}^{\mathrm{\top }}+\mathcal{B}\mathbb{E}\left[{\tilde{d}}_k{e}_k^{\mathrm{\top }}\right]{(I-G_k{\mathcal{C}}_k)}^{\mathrm{\top }}{\mathcal{A}}_k^{\mathrm{\top }}\hfill \\ \hfill & ⪯\theta {\mathcal{A}}_k(I-G_k{\mathcal{C}}_k)\mathbb{E}\left[e_k{e}_k^{\mathrm{\top }}\right]{(I-G_k{\mathcal{C}}_k)}^{\mathrm{\top }}{\mathcal{A}}_k^{\mathrm{\top }}+{\displaystyle \frac{1}{\theta }}\mathcal{B}\mathbb{E}[{\tilde{d}}_k{\tilde{d}}_k^{\mathrm{\top }}]{\mathcal{B}}^{\mathrm{\top }}.\hfill \end{array}$$

(37)

Combining Equations (32)–(37), using the inductive hypothesis $\mathbb{E}\left[e_k{e}_k^{\mathrm{\top }}\right]⪯P_k$ (true at $k=0$ by Assumption 1), and noting $R_k=(1+\theta ){\overline{R}}_k$, we obtain

$$\begin{array}{cc}\hfill & \mathbb{E}\left[({\xi }_{k+1}-{\widehat{\xi }}_{k+1}){({\xi }_{k+1}-{\widehat{\xi }}_{k+1})}^{\mathrm{\top }}\right]\hfill \\ \hfill & ⪯(1+\theta ){\mathcal{A}}_k{P}_k^{+}{\mathcal{A}}_k^{\mathrm{\top }}+Q_{k,1}+\left(1+{\displaystyle \frac{1}{\theta }}\right)\mathcal{B}{Q}_{k,2}{\mathcal{B}}^{\mathrm{\top }}\hfill \\ \hfill & =(1+\theta ){\mathcal{A}}_k{P}_k^{+}{\mathcal{A}}_k^{\mathrm{\top }}+{\tilde{Q}}_k=P_{k+1},\hfill \end{array}$$

(38)

which proves Equation (29).    □

Remark 2.

Unlike the standard Riccati recursion, Equation (28) includes an inflation factor $(1+\theta )$ and an additional term for the bounded injection mismatch. This is necessary because ${\Delta }^2{F}_k$ is not modeled as Gaussian noise but only bounded in mean square. The parameter θ is therefore a free positive tuning parameter introduced by the Young-inequality split. Although the covariance upper-bound inequality holds for any $\theta >0$, different choices redistribute conservativeness between the state-propagation inflation and the uncertainty-injection term. In the simulations, we choose a nominal initial value by a trace-balance heuristic. At $k=0$, this heuristic balances the two θ-dependent contributions as

$${\theta }_0\mathrm{tr}\left(P_0\right)={\displaystyle \frac{1}{{\theta }_0}}\mathrm{tr}\left(Q_{\Delta ,0}\right),Q_{\Delta ,0}:=\mathcal{B}{Q}_{0,2}{\mathcal{B}}^{\mathrm{\top }},$$

(39)

which gives

$${\theta }_0=\sqrt{{\displaystyle \frac{\mathrm{tr}\left(Q_{\Delta ,0}\right)}{\mathrm{tr}\left(P_0\right)}}}.$$

(40)

Hence, we summarize the CTESKF as follows in Algorithm 1.

Algorithm 1 CTESKF algorithm (predictor–corrector form)

1: Inputs: ${\mathcal{A}}_k,\mathcal{B},{\mathcal{C}}_k,Q_{k,1},Q_{k,2},R_k,\theta$ 2: Initialization: ${\widehat{\xi }}_0,P_0$ 3: for  $k=0,1,2,\dots$ do 4:     Compute bounded injection ${\widehat{{\Delta }^{2}F}}_k$ via Equation (20) 5:     Set ${\overline{R}}_k={\displaystyle \frac{1}{1+\theta }}{R}_k$ 6:     Compute innovation ${\nu }_k$ using Equation (6) for bearing-only measurements; for linear measurements, ${\nu }_k=y_k-{\mathcal{C}}_k{\widehat{\xi }}_k$. 7:     Measurement update:    $G_k=P_k{{\mathcal{C}}_k}^{\mathrm{\top }}{({\mathcal{C}}_k{P}_k{{\mathcal{C}}_k}^{\mathrm{\top }}+{\overline{R}}_k)}^{-1}$    ${\widehat{\xi }}_k^{+}={\widehat{\xi }}_k+G_k{\nu }_k$    $P_k^{+}=(I-G_k{\mathcal{C}}_k)P_k{(I-G_k{\mathcal{C}}_k)}^{\mathrm{\top }}+G_k{\overline{R}}_k{G}_k^{\mathrm{\top }}$ 8:     Time update:    ${\widehat{\xi }}_{k+1}={\mathcal{A}}_k{\widehat{\xi }}_k^{+}+\mathcal{B}{\widehat{{\Delta }^{2}F}}_k$    $P_{k+1}=(1+\theta ){\mathcal{A}}_k{P}_k^{+}{\mathcal{A}}_k^{\mathrm{\top }}+Q_{k,1}+(1+{\displaystyle \frac{1}{\theta }})\mathcal{B}{Q}_{k,2}{\mathcal{B}}^{\mathrm{\top }}$ 9: end for

The CTESKF requires aggregating all measurements at a fusion center, incurring heavy communication cost and creating a single point of failure. Prior distributed Kalman designs embed consensus on observation or information pairs, but often need many exchanges within one sampling period. Since communication is typically more energy-intensive than computation, excessive rounds are impractical. Hence, it is necessary to design a distributed filter that can operate with limited communication resources.

4. Diffusion-Based Twice-Extended State Kalman Filter

We consider distributed measurements $(y_{i,k},R_{i,k})$ over a connected graph with a row-stochastic mixing matrix $W=\left[w_{ij}\right]$. Each node runs a local filter step and then performs a single-round fusion with neighbors via a CI-type diffusion rule.

4.1. Network Model and Diffusion Weights

The communication graph is $\mathcal{G}=(\mathcal{V},\mathcal{E})$ with $\mathcal{V}=\{1,\dots ,M\}$. Let $w_{ij}\ge 0$ denote the weight node i assigns to neighbor j. Define the neighbor set ${\mathcal{N}}_i:=\{j:(i,j)\in \mathcal{E}\}\cup \left\{i\right\}$. We use standard diffusion weight conditions, which imply primitivity when combined with connectivity for undirected graphs.

Assumption 4.

The undirected graph $\mathcal{G}$ is connected. The weight matrix $W=\left[w_{ij}\right]$ satisfies

$$w_{ij}\ge 0,w_{ij}=0\mathrm{if}j\notin {\mathcal{N}}_i,\sum _{j\in {\mathcal{N}}_i}{w}_{ij}=1,w_{ii}>0.$$

(41)

Moreover, W is primitive, i.e., there exists $L_w\in {\mathbb{Z}}_{+}$ such that $W^{L_w}$ has all entries strictly positive.

Remark 3.

For a connected undirected graph, common choices such as Metropolis weights yield a primitive row-stochastic matrix W. Primitivity is essential to ensure that information from any node diffuses to the entire network in finite steps.

Remark 4.

We intentionally use fixed row-stochastic diffusion weights rather than optimizing CI weights at every sampling instant [22]. Fixed weights keep the fusion rule simple, avoid solving an online constrained optimization problem, and preserve a deterministic single-round communication schedule and computation load, which facilitates deployment on embedded PTS nodes. Optimized CI weighting can reduce conservativeness by minimizing objectives such as $\mathrm{tr}\left(P_{i,k+1}\right)$ or $\log \det P_{i,k+1}$, but it introduces additional local optimization at each sampling instant. For the nearly symmetric ring topology considered here, where neighboring PTS nodes have comparable sensing quality, fixed weights are expected to offer a favorable accuracy–complexity trade-off. We therefore adopt fixed weights in the main DTESKF design and regard adaptive CI weighting as an important extension.

4.2. Local Filter Structure

Node i observes $y_{i,k}={\mathcal{C}}_{i,k}{\xi }_k+v_{i,k}$, where ${\mathcal{C}}_{i,k}=\left[H_{i,k}00\right]$ comes from the local linearization in Equation (8). To streamline analysis, we rewrite the local TESKF in a predictor–corrector form with an effective measurement covariance ${\overline{R}}_{i,k}:={\displaystyle \frac{1}{1+\theta }}{R}_{i,k}$, consistent with Section 3.

Let $({\widehat{\xi }}_{i,k},P_{i,k})$ denote node i’s prior estimate and its associated covariance upper bound at time k after diffusion fusion at time k. Define the local innovation

$${\nu }_{i,k}:=\mathcal{W}\left(y_{i,k}-h_i\left({\widehat{x}}_{i,k}\right)\right),$$

(42)

which reduces to ${\nu }_{i,k}=y_{i,k}-{\mathcal{C}}_{i,k}{\widehat{\xi }}_{i,k}$ for a linear measurement model.

• Measurement update (local correction):

$$G_{i,k}:=P_{i,k}{{\mathcal{C}}_{i,k}}^{\mathrm{\top }}{({\mathcal{C}}_{i,k}{P}_{i,k}{{\mathcal{C}}_{i,k}}^{\mathrm{\top }}+{\overline{R}}_{i,k})}^{-1},$$

(43)

$${\widehat{\xi }}_{i,k}^{+}:={\widehat{\xi }}_{i,k}+G_{i,k}{\nu }_{i,k},$$

(44)

$$P_{i,k}^{+}:=(I-G_{i,k}{\mathcal{C}}_{i,k})P_{i,k}{(I-G_{i,k}{\mathcal{C}}_{i,k})}^{\mathrm{\top }}+G_{i,k}{\overline{R}}_{i,k}{G}_{i,k}^{\mathrm{\top }}.$$

(45)

• Time update (local prediction with uncertainty injection):

Define the bounded injection ${\widehat{{\Delta }^{2}F}}_{i,k}$ using Equation (20). Let

$${\tilde{Q}}_k:=Q_{k,1}+\left(1+{\displaystyle \frac{1}{\theta }}\right)\mathcal{B}{Q}_{k,2}{\mathcal{B}}^{\mathrm{\top }}.$$

(46)

Then the local predicted estimate is

$${\psi }_{i,k+1}:={\mathcal{A}}_k{\widehat{\xi }}_{i,k}^{+}+\mathcal{B}{\widehat{{\Delta }^{2}F}}_{i,k},$$

(47)

and the corresponding covariance upper bound is propagated as

$${\overline{P}}_{i,k+1}:=(1+\theta ){\mathcal{A}}_k{P}_{i,k}^{+}{\mathcal{A}}_k^{\mathrm{\top }}+{\tilde{Q}}_k.$$

(48)

Remark 5.

The pair $({\psi }_{i,k+1},{\overline{P}}_{i,k+1})$ is computed locally using only $y_{i,k}$ and then shared once with neighbors for diffusion fusion. No inner-loop consensus iterations are required.

4.3. Diffusion Fusion via Covariance Intersection

After the local prediction, each node i broadcasts its information pair $\{{\overline{P}}_{i,k+1}^{-1},{\overline{P}}_{i,k+1}^{-1}{\psi }_{i,k+1}\}$ to neighbors $j\in {\mathcal{N}}_i\setminus \left\{i\right\}$ and then fuses the received pairs together with its own pair in the CI form:

$$P_{i,k+1}^{-1}:=\sum _{j\in {\mathcal{N}}_i}{w}_{ij}{\overline{P}}_{j,k+1}^{-1},$$

(49)

$${\widehat{\xi }}_{i,k+1}:=P_{i,k+1}\sum _{j\in {\mathcal{N}}_i}{w}_{ij}{\overline{P}}_{j,k+1}^{-1}{\psi }_{j,k+1}.$$

(50)

The fused pair $({\widehat{\xi }}_{i,k+1},P_{i,k+1})$ becomes the prior for the next sampling time and the whole DTESKF algorithm is summarized in Algorithm 2.

Algorithm 2 DTESKF algorithm

1: Inputs: ${\{{\mathcal{A}}_k,\mathcal{B},{\mathcal{C}}_{i,k},Q_{k,1},Q_{k,2},R_{i,k},\theta \}}_{i=1}^M$, mixing weights $W=\left[w_{ij}\right]$   2: Initialization (each node i): ${\widehat{\xi }}_{i,0}$, $P_{i,0}\succ 0$   3: for  $k=0,1,2,\dots$do   4:     Compute local bounded injection ${\widehat{{\Delta }^{2}F}}_{i,k}$ componentwise using Equation (20)   5:     Set ${\overline{R}}_{i,k}={\displaystyle \frac{1}{1+\theta }}{R}_{i,k}$   6:     Compute local innovation ${\nu }_{i,k}$ using Equation (6) for bearing-only measurements; for linear measurements, ${\nu }_{i,k}=y_{i,k}-{\mathcal{C}}_{i,k}{\widehat{\xi }}_{i,k}$.   7:     Local measurement update:    $G_{i,k}=P_{i,k}{{\mathcal{C}}_{i,k}}^{\mathrm{\top }}{({\mathcal{C}}_{i,k}{P}_{i,k}{{\mathcal{C}}_{i,k}}^{\mathrm{\top }}+{\overline{R}}_{i,k})}^{-1}$    ${\widehat{\xi }}_{i,k}^{+}={\widehat{\xi }}_{i,k}+G_{i,k}{\nu }_{i,k}$    $P_{i,k}^{+}=(I-G_{i,k}{\mathcal{C}}_{i,k})P_{i,k}{(I-G_{i,k}{\mathcal{C}}_{i,k})}^{\mathrm{\top }}+G_{i,k}{\overline{R}}_{i,k}{G}_{i,k}^{\mathrm{\top }}$   8:     Local time update:    ${\psi }_{i,k+1}={\mathcal{A}}_k{\widehat{\xi }}_{i,k}^{+}+\mathcal{B}{\widehat{{\Delta }^{2}F}}_{i,k}$    ${\overline{P}}_{i,k+1}=(1+\theta ){\mathcal{A}}_k{P}_{i,k}^{+}{\mathcal{A}}_k^{\mathrm{\top }}+Q_{k,1}+(1+{\displaystyle \frac{1}{\theta }})\mathcal{B}{Q}_{k,2}{\mathcal{B}}^{\mathrm{\top }}$   9:     Broadcast: Send $\{{\overline{P}}_{i,k+1}^{-1},{\overline{P}}_{i,k+1}^{-1}{\psi }_{i,k+1}\}$ to neighbors $j\in {\mathcal{N}}_i\setminus \left\{i\right\}$ 10:     CI diffusion fusion:    $P_{i,k+1}={\left({\sum }_{j\in {\mathcal{N}}_i}{w}_{ij}{\overline{P}}_{j,k+1}^{-1}\right)}^{-1}$    ${\widehat{\xi }}_{i,k+1}=P_{i,k+1}{\sum }_{j\in {\mathcal{N}}_i}{w}_{ij}{\overline{P}}_{j,k+1}^{-1}{\psi }_{j,k+1}$ 11: end for

Remark 6.

After local corrections, the estimation errors across nodes become correlated due to common process noise and repeated information exchange. CI fusion guarantees a conservative but consistent covariance bound without requiring any knowledge of cross-correlations. This makes it suitable for diffusion with a single communication per step.

4.4. Communication Cost of One-Round Diffusion

Table 1. Per-step communication cost of different fusion strategies in Experiment 2.

Strategy	Rounds/Steps	Scalars/Node/Step
DTESKF one-round CI diffusion	1	$2\left({\displaystyle \frac{12\times 13}{2}}+12\right)=180$
L-round consensus on same information pair	L	$180L$
Centralized raw-measurement uplink †	1	2

^† The centralized row counts only the measurement uplink from each PTS to the fusion center; it does not include the fusion-center bottleneck, storage burden, or the loss of peer-to-peer robustness.

summarizes the per-step communication cost of different fusion strategies. Let $n_{\xi }=n+2\mathrm{\ell }$ denote the augmented-state dimension and let $d_i:=\left|{\mathcal{N}}_i\right|-1$ be the number of neighbors of node i. If symmetry of the information matrix is exploited, then node i transmits

$$c_i^{\mathrm{DTESKF}}=d_i\left({\displaystyle \frac{n_{\xi }(n_{\xi }+1)}{2}}+n_{\xi }\right)$$

(51)

real scalars per sampling instant, because it broadcasts one symmetric information matrix and one information vector to each neighbor. Thus the communication cost scales linearly with node degree and remains independent of any inner-loop iteration count. By contrast, an L-round consensus strategy operating on the same information pair would require $L{c}_i^{\mathrm{DTESKF}}$ scalars per step. For the 3D MPTS experiment, $n_{\xi }=12$ and each node has two neighbors in the ring graph, so DTESKF sends 180 scalars per node per step, whereas a five-round consensus protocol would require 900 scalars. Therefore, we adopt the one-round CI diffusion as the main fusion strategy in DTESKF to achieve a favorable accuracy–communication trade-off.

5. Consistency Analysis of DTESKF

We analyze DTESKF in terms of consistency. The propagated matrices $P_{i,k}$ are designed as upper bounds for the true error covariance, accounting for uncertainty injection and unknown inter-sensor correlations.

5.1. Consistency Definition

Definition 1 (Consistency).

Let $\widehat{\xi }$ be an estimate of ξ and $\widehat{P}⪰0$ be an associated covariance matrix. The pair $(\widehat{\xi },\widehat{P})$ is consistent if

$$\mathbb{E}\left[(\xi -\widehat{\xi }){(\xi -\widehat{\xi })}^{\mathrm{\top }}\right]⪯\widehat{P}.$$

(52)

5.2. Local Filter Consistency

Theorem 2.

Under Assumptions 1–2, for each node i and each time k,

$$\mathbb{E}\left[({\xi }_{k+1}-{\psi }_{i,k+1}){({\xi }_{k+1}-{\psi }_{i,k+1})}^{\mathrm{\top }}\right]⪯{\overline{P}}_{i,k+1}.$$

(53)

Proof. 

The local update steps (Lines 6 and 7 in Algorithm 2) for each node i are mathematically identical to the CTESKF recursion (Algorithm 1) acting on the local measurement pair $(y_{i,k},{\mathcal{C}}_{i,k})$ with noise covariance $R_{i,k}$. Since Assumptions 1 and 2 hold, the derivation in Theorem 1 applies directly to the local filter at each node. Specifically, the local predicted covariance ${\overline{P}}_{i,k+1}$ is constructed using the same scaling factors and injection bounds as in Equation (28). Therefore, by Theorem 1, the local prediction satisfies the consistency condition in Equation (53).    □

5.3. CI Diffusion Fusion Preserves Consistency

We now prove that the CI diffusion rule in Equations (49) and (50) preserves consistency even when cross-correlations between local predictors are unknown.

Lemma 1 (Two-estimate CI consistency).

Let ${\psi }_1,{\psi }_2$ be two estimates of ξ with errors $e_1:=\xi -{\psi }_1$ and $e_2:=\xi -{\psi }_2$. Assume $\mathbb{E}\left[e_1{e}_1^{\mathrm{\top }}\right]⪯{\overline{P}}_1$ and $\mathbb{E}\left[e_2{e}_2^{\mathrm{\top }}\right]⪯{\overline{P}}_2$ for some ${\overline{P}}_1,{\overline{P}}_2\succ 0$, while the cross-correlation $\mathbb{E}\left[e_1{e}_2^{\mathrm{\top }}\right]$ is unknown. For any $\omega \in (0,1)$, define the CI fusion

$$P^{-1}:=\omega {\overline{P}}_1^{-1}+(1-\omega ){\overline{P}}_2^{-1},$$

(54)

$$\widehat{\xi }:=P\left(\omega {\overline{P}}_1^{-1}{\psi }_1+(1-\omega ){\overline{P}}_2^{-1}{\psi }_2\right).$$

(55)

Then $(\widehat{\xi },P)$ is consistent: $\mathbb{E}[(\xi -\widehat{\xi }){(\xi -\widehat{\xi })}^{\mathrm{\top }}]⪯P$.

Proof. 

Let $S_1:={\overline{P}}_1^{-1}$, $S_2:={\overline{P}}_2^{-1}$, and $S:=P^{-1}=\omega S_1+(1-\omega )S_2$. From Equation (55), the fused error is

$$\xi -\widehat{\xi }=P\left(\omega S_1{e}_1+(1-\omega )S_2{e}_2\right).$$

(56)

Hence,

$$\begin{array}{cc}\hfill & \mathbb{E}\left[(\xi -\widehat{\xi }){(\xi -\widehat{\xi })}^{\mathrm{\top }}\right]\hfill \\ \hfill & =P\mathbb{E}\left[\left(\omega S_1{e}_1+(1-\omega )S_2{e}_2\right){\left(\omega S_1{e}_1+(1-\omega )S_2{e}_2\right)}^{\mathrm{\top }}\right]P\hfill \\ \hfill & =P({\omega }^2{S}_1\mathbb{E}\left[e_1{e}_1^{\mathrm{\top }}\right]S_1+{(1-\omega )}^2{S}_2\mathbb{E}\left[e_2{e}_2^{\mathrm{\top }}\right]S_2\hfill \end{array}$$

(57)

$$\begin{array}{cc}\hfill & +\omega (1-\omega )\left(S_1\mathbb{E}\left[e_1{e}_2^{\mathrm{\top }}\right]S_2+S_2\mathbb{E}\left[e_2{e}_1^{\mathrm{\top }}\right]S_1\right))P.\hfill \end{array}$$

(58)

Because the joint covariance of ${\left[e_1^{\mathrm{\top }}{e}_2^{\mathrm{\top }}\right]}^{\mathrm{\top }}$ is positive semidefinite, for any scalar $\alpha >0$ we have

$${[\alpha S_1{e}_1-{\alpha }^{-1}{S}_2{e}_2]}^{\mathrm{\top }}[\alpha S_1{e}_1-{\alpha }^{-1}{S}_2{e}_2]\ge 0,$$

which implies the matrix inequality

$$\begin{array}{cc}\hfill & S_1\mathbb{E}\left[e_1{e}_2^{\mathrm{\top }}\right]S_2+S_2\mathbb{E}\left[e_2{e}_1^{\mathrm{\top }}\right]S_1\hfill \\ \hfill & ⪯{\alpha }^2{S}_1\mathbb{E}\left[e_1{e}_1^{\mathrm{\top }}\right]S_1+{\alpha }^{-2}{S}_2\mathbb{E}\left[e_2{e}_2^{\mathrm{\top }}\right]S_2.\hfill \end{array}$$

(59)

Choose ${\alpha }^2={\displaystyle \frac{1-\omega }{\omega }}$. Substituting Equation (59) into Equation (58) yields

$$\begin{array}{cc}\hfill & \mathbb{E}\left[(\xi -\widehat{\xi }){(\xi -\widehat{\xi })}^{\mathrm{\top }}\right]\hfill \\ \hfill & ⪯P\left(\omega S_1\mathbb{E}\left[e_1{e}_1^{\mathrm{\top }}\right]S_1+(1-\omega )S_2\mathbb{E}\left[e_2{e}_2^{\mathrm{\top }}\right]S_2\right)P\hfill \\ \hfill & ⪯P\left(\omega S_1{\overline{P}}_1{S}_1+(1-\omega )S_2{\overline{P}}_2{S}_2\right)P\hfill \\ \hfill & =P\left(\omega S_1+(1-\omega )S_2\right)P=PSP=P,\hfill \end{array}$$

(60)

which completes the proof.    □

Theorem 3 (Multi-estimate CI consistency).

Fix time $k+1$. Suppose that for each neighbor $j\in {\mathcal{N}}_i$, the local predicted pair $({\psi }_{j,k+1},{\overline{P}}_{j,k+1})$ is consistent: $\mathbb{E}\left[({\xi }_{k+1}-{\psi }_{j,k+1}){({\xi }_{k+1}-{\psi }_{j,k+1})}^{\mathrm{\top }}\right]⪯{\overline{P}}_{j,k+1}.$ Then the CI fused pair $({\widehat{\xi }}_{i,k+1},P_{i,k+1})$ defined by Equations (49) and (50) is also consistent:

$$\mathbb{E}\left[({\xi }_{k+1}-{\widehat{\xi }}_{i,k+1}){({\xi }_{k+1}-{\widehat{\xi }}_{i,k+1})}^{\mathrm{\top }}\right]⪯P_{i,k+1}.$$

(61)

Proof. 

Suppress the time index and fix node i. Enumerate the positive-weight set ${\mathcal{N}}_i^{+}:=\{j\in {\mathcal{N}}_i:w_{ij}>0\}=\{1,\dots ,N\}$ and denote weights $w_j:=w_{ij}$, so that $w_j>0$ and ${\sum }_{j=1}^N{w}_j=1$. Define $S_j:={\overline{P}}_j^{-1}$ and $S:={\sum }_{j=1}^N{w}_j{S}_j$, so that $P=S^{-1}$ and $\widehat{\xi }=P{\sum }_{j=1}^N{w}_j{S}_j{\psi }_j$.

We prove consistency by induction using Lemma 1. Let $({\widehat{\xi }}^{\left(1\right)},P^{\left(1\right)}):=({\psi }_1,{\overline{P}}_1)$, which is consistent by assumption. For $t\ge 2$, let $W_t:={\sum }_{j=1}^t{w}_j$ and define the normalized weights ${\omega }_t:={\displaystyle \frac{W_{t-1}}{W_t}}\in (0,1)$. Fuse the consistent pair $({\widehat{\xi }}^{(t-1)},P^{(t-1)})$ with $({\psi }_t,{\overline{P}}_t)$ via Lemma 1 using weight ${\omega }_t$, i.e.,

$$\begin{array}{cc}\hfill & {\left(P^{\left(t\right)}\right)}^{-1}:={\omega }_t{\left(P^{(t-1)}\right)}^{-1}+(1-{\omega }_t){\overline{P}}_t^{-1},\hfill \\ \hfill & {\widehat{\xi }}^{\left(t\right)}:=P^{\left(t\right)}\left({\omega }_t{\left(P^{(t-1)}\right)}^{-1}{\widehat{\xi }}^{(t-1)}+(1-{\omega }_t){\overline{P}}_t^{-1}{\psi }_t\right).\hfill \end{array}$$

Lemma 1 guarantees that $({\widehat{\xi }}^{\left(t\right)},P^{\left(t\right)})$ is consistent.

It remains to show that $({\widehat{\xi }}^{\left(N\right)},P^{\left(N\right)})$ equals the one-shot CI fusion in Equations (49) and (50). By construction, ${\left(P^{\left(t\right)}\right)}^{-1}={\displaystyle \frac{1}{W_t}}{\sum }_{j=1}^t{w}_j{\overline{P}}_j^{-1}$; hence

$${\left(P^{\left(N\right)}\right)}^{-1}={\displaystyle \sum _{j=1}^N}{w}_j{\overline{P}}_j^{-1}=P^{-1}.$$

Similarly, the recursion for ${\widehat{\xi }}^{\left(t\right)}$ yields ${\widehat{\xi }}^{\left(N\right)}=P{\sum }_{j=1}^N{w}_j{\overline{P}}_j^{-1}{\psi }_j$. Therefore, $({\widehat{\xi }}^{\left(N\right)},P^{\left(N\right)})=(\widehat{\xi },P)$, which is consistent.    □

Remark 7.

Theorems 2 and 3 together imply that all DTESKF pairs $({\widehat{\xi }}_{i,k},P_{i,k})$ remain consistent for all values of k, provided Assumptions 1 and 2 and the graph condition in Assumption 4 hold.

6. Numerical Simulation

This section validates the proposed CTESKF and DTESKF framework through two experiments. Experiment 1 uses a low-dimensional nonlinear uncertain system with linear measurements to isolate the effect of the proposed algorithms. Experiment 2 considers the target application: a 3D MPTS, where each PTS device provides bearing-only measurements and the target experiences velocity-dependent maneuver accelerations. Unless otherwise stated, all algorithms were implemented on a laptop equipped with an AMD Ryzen 9 8945HX CPU (2.50 GHz).

6.1. Experiment 1: Nonlinear Uncertain System with Linear Measurements

We consider the nonlinear discrete-time system

$$x_{k+1}=\left[\begin{array}{cc}1& T_s\\ -T_s& 1-T_s\end{array}\right]x_k+\left[\begin{array}{c}0\\ T_s\end{array}\right]\left(\sin (x_{1,k}^2+x_{2,k}^2)+d_k\right)+w_k,$$

(62)

where $x_k={[x_{1,k},x_{2,k}]}^{\mathrm{\top }}$, $T_s=0.1$ s, and $w_k\sim \mathcal{N}(0,Q)$ with $Q=0.1I_2$. The nominal nonlinear model used by the filters is $F_{\mathrm{nom}}\left(x_k\right)=\sin (x_{1,k}^2+x_{2,k}^2)$. To test robustness against model mismatch, the true plant further includes a smooth unmodeled input $d_k=0.3\sin \left(0.1k\right)$ in the nonlinear channel. This term is not provided to the filters and is treated as part of the lumped model uncertainty. The initial state is $x_0={[0,1]}^{\mathrm{\top }}$ and the running time is set to $k=2500$.

The sensor network has $M=10$ nodes arranged in the ring topology of Figure 3. Each node i observes the first state component,

$$\begin{array}{c}\hfill y_{i,k}=\left[\begin{array}{cc}1& 0\end{array}\right]x_k+v_{i,k},\end{array}$$

(63)

with $v_{i,k}\sim \mathcal{N}(0,R_i)$ and $R_i=25$ for all i. The communication graph is undirected and connected, with diffusion weights $w_{ii}=0.5$ and $w_{ij}=0.25$ for the two neighbors of node i. The CTESKF and DTESKF are implemented with the same uncertainty bound ${\overline{q}}_{k,r}=1\times {10}^{-6}$.

Let ${\widehat{x}}_{i,k}$ denote the estimate produced by node i at time k. For centralized algorithms, we treat the estimator as a single node ($M=1$). The reported running root mean square error (RMSE) is the node-averaged metric

$$\mathrm{RMSE}\left(k\right)=\sqrt{{\displaystyle \frac{1}{Mk}}\sum _{i=1}^M\sum _{j=1}^k{∥{\widehat{x}}_{i,j}-x_j∥}^2}.$$

(64)

Figure 4 compares the CTESKF and DTESKF using the metric in Equation (64). The DTESKF attains an RMSE close to that of the CTESKF, with a modest performance loss that is expected from the diffusion strategy. Figure 5 and Figure 6 further compare the proposed methods with centralized and distributed nonlinear Kalman filters. The running RMSE and time cost are shown in

Table 2. RMSE and time cost comparison of different algorithms in Experiment 1.

Algorithm	${\mathbf{RMSE}}_{{\mathit{x}}_1}$	${\mathbf{RMSE}}_{{\mathit{x}}_2}$	${\mathbf{RMSE}}_{\mathbf{tot}}$	Time (s)
CTESKF (proposed)	0.8059	0.9285	1.2295	0.0419
Centralized EKF	1.3093	1.0922	1.7051	0.0322
Centralized ESKF	0.9288	1.2700	1.5734	0.0536
Centralized UKF	1.2764	1.0891	1.6779	0.0426
Centralized CKF	1.2915	1.0904	1.6902	0.0443
DTESKF (proposed)	1.0601	0.9948	1.4538	0.2256
DEKF	1.3762	1.1402	1.7872	0.2342
DESKF	1.1174	1.1218	1.5834	0.2329
DUKF	1.3681	1.1410	1.7814	0.4323
DCKF	1.3661	1.1396	1.7790	0.4087

. Overall, the CTESKF achieves accuracy close to CKF while being computationally lighter, and the DTESKF provides a competitive accuracy–runtime trade-off relative to the DUKF and DCKF under the same diffusion strategy. Overall, the proposed methods achieve a better balance between accuracy and efficiency, which makes them more practical for real-time applications.

We further evaluate the influence of the uncertainty-injection bound and the covariance-inflation parameter on estimation accuracy and covariance consistency. A 40-run Monte Carlo sensitivity study is conducted for the CTESKF on the same benchmark by varying non-negligible common scalar values q with ${\overline{q}}_{k,r}=q$ and different values of $\theta$. Figure 7 reports the kinematic RMSE together with an empirical consistency ratio

$$\eta :={\displaystyle \frac{1}{T}}{\displaystyle \sum _{k=1}^T}{\displaystyle \frac{\mathrm{tr}\left({\widehat{\Sigma }}_k\right)}{\mathrm{tr}\left(P_k\right)}},{\widehat{\Sigma }}_k:={\displaystyle \frac{1}{N_{\mathrm{MC}}}}{\displaystyle \sum _{s=1}^{N_{\mathrm{MC}}}}{e}_k^{\left(s\right)}{e}_k^{\left(s\right)\mathrm{\top }},$$

(65)

where $P_k$ is the propagated covariance upper bound and $\eta >1$ indicates that the empirical error covariance exceeds the assumed bound. The results show that smaller values of q may reduce the RMSE in this weakly nonlinear case, but they can also lead to an optimistic covariance bound ($\eta >1$). Increasing q improves consistency, and $q\approx {10}^{-6}$ makes $\eta$ close to one with only moderate loss of accuracy. Thus, choosing q too small can be interpreted as underestimating the uncertainty-injection bound. In practical deployment, q should be initialized conservatively according to the expected maneuver intensity and sampling interval, and then increased when consistency indicators, such as $\eta$ on validation data, reveal optimistic covariance. For fixed $q={10}^{-6}$, choosing $\theta$ in the range ${10}^{-3}$–${10}^{-2}$ provides the best balance between accuracy and conservativeness, which is consistent with Remark 2. These observations are consistent with Assumption 2: stronger or faster-varying uncertainty requires a larger bound ${\overline{q}}_{k,r}$, represented by a larger scalar q in this sensitivity study. The role of $\theta$ is instead governed by the covariance upper-bound recursion in Remark 2; in this benchmark, a moderate $\theta$ balances accuracy and conservativeness.

6.2. Experiment 2: 3D Photoelectric Bearing-Only Tracking with Nonlinear Velocity-Dependent Uncertainty

We consider a 6D kinematic state $x_k={[p_k^{\mathrm{\top }},v_k^{\mathrm{\top }}]}^{\mathrm{\top }}$. The nominal motion model is constant velocity, while the true dynamics include a nonlinear uncertainty term that depends on velocity:

$$\begin{array}{cc}\hfill p_{k+1}& =p_k+T_s{v}_k+w_{p,k},\hfill \\ \hfill v_{k+1}& =v_k+T_s{a}_{\mathrm{nl}}\left(v_k\right)+w_{v,k},\hfill \end{array}$$

(66)

where $w_k={\left[w_{p,k}^{\mathrm{\top }}{w}_{v,k}^{\mathrm{\top }}\right]}^{\mathrm{\top }}\sim \mathcal{N}(0,Q)$ and $T_s=0.1$ s. The nonlinear uncertainty term is applied element-wise as $a_{\mathrm{nl}}\left(v\right)=\alpha \sin \left(\beta v^2\right)$ with $\alpha =1.2$ and $\beta =2.5$. The process noise covariance is set to $Q={\sigma }_w^2{I}_6$ with ${\sigma }_w=1$. The initial true state is $x_0={[0,0,0,2.0,1.0,1.5]}^{\mathrm{\top }}$. The running time is set to $k=1000$.

We consider $M=10$ PTS devices and they are placed on a horizontal ring with small altitude variations as

$$s_i={[400\cos {\theta }_i,400\sin {\theta }_i,50\sin \left(2{\theta }_i\right)]}^{\mathrm{\top }},{\theta }_i={\displaystyle \frac{2\pi (i-1)}{M}}.$$

(67)

Each PTS device measures azimuth and elevation angle according to Equation (5) with additive Gaussian noise. The angular noise standard deviation is 1 deg for both angles—i.e., $R_i={\sigma }_{\theta }^2{I}_2$ with ${\sigma }_{\theta }=\pi /180$. Angle wrapping is applied to compute innovations on $(-\pi ,\pi ]$. PTS devices communicate over a ring graph with the same communication topology as in Experiment 1.

The CTESKF and DTESKF are implemented with the same uncertainty bound ${\overline{q}}_{k,r}=1\times {10}^{-6}$. The initial covariance matrices are set to $P_0=I_{12}$ for the CTESKF and DTESKF.

Let ${\widehat{p}}_{i,k}$ and ${\widehat{v}}_{i,k}$ denote the position and velocity estimates at node i. Define the instantaneous errors $e_{p,i,k}=\parallel p_k-{\widehat{p}}_{i,k}\parallel$ and $e_{v,i,k}=\parallel v_k-{\widehat{v}}_{i,k}\parallel$. The reported running RMSEs are node-averaged over the network:

$$\begin{array}{cc}\hfill {\mathrm{RMSE}}_p\left(k\right)& =\sqrt{{\displaystyle \frac{1}{Mk}}\sum _{i=1}^M\sum _{j=1}^k{e}_{p,i,j}^2},\hfill \end{array}$$

(68)

$$\begin{array}{cc}\hfill {\mathrm{RMSE}}_v\left(k\right)& =\sqrt{{\displaystyle \frac{1}{Mk}}\sum _{i=1}^M\sum _{j=1}^k{e}_{v,i,j}^2}.\hfill \end{array}$$

(69)

We also define ${\mathrm{RMSE}}_{\mathrm{tot}}=\sqrt{{\mathrm{RMSE}}_p{\left(k\right)}^2+{\mathrm{RMSE}}_v{\left(k\right)}^2}$ for comparison.

Figure 8 shows the true 3D target trajectory and the deployment of the PTS devices. Even though each device provides only bearing information, the network can reconstruct the 3D trajectory through geometric diversity and diffusion fusion. The DTESKF tracks the trajectory smoothly and remains stable throughout the horizon. Figure 9 and Figure 10 show the RMSE curves and boxplots of the DTESKF, DESKF, DUKF, and DCKF in this highly nonlinear setting. The DEKF exhibits severe error growth, whereas the other four methods maintain substantially lower RMSE levels; therefore, we report the DEKF only in

Table 3. Tracking performance of DTESKF, DUKF, DCKF, DESKF, and DEKF in the MPTS.

Algorithm	${\mathbf{RMSE}}_{\mathit{p}}$	${\mathbf{RMSE}}_{\mathit{v}}$	${\mathbf{RMSE}}_{\mathbf{tot}}$	Time (s)
DTESKF	6.7649	8.5797	10.9259	0.3103
DUKF	7.0591	7.0758	9.9949	0.5848
DCKF	7.5545	7.2560	10.4747	0.5671
DESKF	7.3625	9.4077	11.9461	0.3733
DEKF	69.3945	22.0093	72.8011	0.3196

to avoid compressing the RMSE plots. The poor performance of the DEKF is mainly attributed to linearization-induced inconsistency in this strongly nonlinear bearing-only scenario. Although the nonlinear acceleration is included in the DEKF prediction model, the term $a_{\mathrm{nl}}\left(v\right)=\alpha \sin \left(\beta v^2\right)$ has pronounced curvature with respect to velocity, and each PTS provides only azimuth and elevation measurements. As a result, when the predicted state deviates from the true trajectory, the process and measurement Jacobians evaluated at the local estimate may no longer represent the nonlinear transformations over the actual uncertainty region. This can lead to underestimated covariance, inappropriate gains, and accumulated tracking errors. DESKF alleviates this problem by augmenting the lumped dynamic component, while DTESKF further augments its temporal difference and propagates a covariance upper bound with CI diffusion. Therefore, the remaining second-order uncertainty is handled conservatively and the distributed fusion remains consistent. The comparison in Table 3 shows that DTESKF achieves the lowest position RMSE and requires less runtime than the sigma-point diffusion filters, although DUKF and DCKF obtain slightly lower total RMSE. Since position accuracy is often the primary concern in practical MPTS applications, these results indicate that DTESKF offers a favorable robustness–accuracy–runtime trade-off rather than uniform dominance across all metrics.

7. Conclusions

This work develops a centralized twice-extended state Kalman filter (CTESKF) and a diffusion-based twice-extended state Kalman filter (DTESKF) for nonlinear uncertain systems over MPTSs. The nonlinear uncertainty and its temporal difference are incorporated as augmented states, enabling online estimation of both the system state and the dominant uncertainty trend. We derive a computable upper bound for the estimation error covariance under bounded second-order uncertainty injection. Building on the CTESKF, the DTESKF is obtained via a one-round covariance-intersection (CI) diffusion strategy, and its covariance consistency is established. Simulation studies confirm that the proposed algorithms improve robustness against unmodeled maneuvering dynamics and provide a favorable balance between computational efficiency and tracking precision compared with representative distributed filters. A limitation of the current study is that the validation remains numerical. Future work will extend the evaluation to a semi-physical or real MPTS platform, where practical factors such as image-extraction errors, encoder noise, communication delay, packet loss, and hardware timing constraints can be explicitly examined.