A consensus-based square-root cubature Kalman filter for manoeuvring target tracking in sensor networks

Abstract

In this paper, a novel distributed tracking method is proposed for the problem of manoeuvring target tracking in sensor networks. Firstly, an adaptive adjustment tracking model is established by extended state observer (ESO) theory. Then, the consensus-based square-root cubature Kalman filter (SCKF) algorithm is proposed in order to improve the global accuracy and stability. In addition, the integrated model could reduce the influence of measurement noise. Finally, simulation is performed to verify the effectiveness of the scheme, whereby comparison results show that the estimation accuracy of the method proposed is higher than that of the traditional ESO and SCKF.

Keywords

Distributed state estimation manoeuvring target tracking;square-root cubature Kalman filter extended state observer consensus algorithm

Introduction

Locating and tracking the manoeuvring target is of great importance since it has wide applications in military surveillance, mobile robotics, traffic control, etc. (Zhang et al., 2019a). A sensor network is significant for applications in target tracking (Keshavarz-Mohammadiyan and Khaloozadeh, 2018; Yu, 2017; Zhang et al., 2019b). In the distributed sensor networks scheme, each sensor only exchanges its local information with its neighbours and carries out the local computation. Distributed state estimation (DSE) is one of the crucial problems in large-scale sensor networks, namely developing distributed algorithms to estimate the concerned state is more scalable and effective (Chen et al., 2018). Many techniques have been developed to deal with the DSE problems over various sensor networks in the past two decades (Wang et al., 2016; Zhang et al., 2015).

The consensus algorithm is an effective method for DSE in sensor networks. Among consensus-based distributed approaches, distributed Kalman filters (KFs) have been widely explored for linear and Gaussian systems (Olfati-Saber, 2007). A weighted average consensus approach has been developed within the framework of the unscented Kalman filter (UKF) for nonlinear systems (Li et al., 2016). Consensus-based distributed particle filters (PFs) are the natural choice for the DSE of nonlinear/non-Gaussian systems (Hlinka et al., 2013). An unscented information consensus filter based on the interacting multiple model (IMM-ICF) was proposed by Ding et al. (2018) to improve the performance of manoeuvring target tracking with multiple sensors. The disadvantages of these nonlinear consensus algorithms are the high communication requirements and the complex calculations. The cubature Kalman filter (CKF) and square-root cubature Kalman filter (SCKF) proposed by Arasaratnam and Haykin (2009) can provide more precise and stable state estimations compared with traditional nonlinear filters (Li and Jia, 2012; Pakki et al., 2011), which are considered to be one of the most popular techniques for state estimation problems in high-dimensional nonlinear systems. Although the decentralized SCKF has been designed in a networked environment (Liu et al., 2014), the consensus-based SCKF is not applied to improve the accuracy and reduce the computational burden.

The modelling of a moving target through the implementation of a filter to estimate, track and predict its motion state is one of the important processes in the manoeuvring target tracking problem (Zhang et al., 2019c). Since the manoeuvring ability of the target is improved with quick development, it is a critical problem because it is difficult to come up with a single motion model (e.g., the Constant Velocity (CV) model, Constant Acceleration (CA) model, Singer model, Jerk model and Current Statistic (CS) model; Li and Jilkov, 2003) that can account for all possible target manoeuvres. Thus, the current solution to track a manoeuvring target involves multiple-model (MM) methods, where a bank of motion models is used to approximate the target motion (e.g., the Multiple Model Adaptive Estimator (MMAE), Interacting Multiple Model (IMM) estimator and Variable Structure Multiple Model (VSMM) estimator; George, 2019). The MM methods require motion models to describe the manoeuvring target, which costs a great deal of running time. It is necessary to study a general model with better tracking performance.

In the past years, many results with an extended state observer (ESO) method have been found in various applications (Cui et al., 2019; Guo and Zhao, 2011; Li et al., 2012). The ESO was first proposed in the context of active disturbance rejection control (ADRC) (Han, 2009). The ESO can estimate both the internal dynamics and external disturbances of the plant in real-time. However, the ESO is always used in the linear observer model without noise. It is significant to develop the elegant estimation ability of the ESO against disturbance, and solve the nonlinear observer with noise meanwhile.

The motivation of this paper is to derive a general tracking model and a filter algorithm that could be applied to solve the manoeuvring target tracking problem in sensor networks. Firstly, taking into consideration the dynamic changing of the manoeuvring target, a general target tracking single model by ESO theory, named ESOM, is newly established. Secondly, the SCKF is an effective technique for state estimation in the nonlinear system. Based on the consensus algorithm, a distributed SCKF for sensor networks is developed. The estimated state of each node can reach a consensus in sensor networks.

The remainder of this paper is organized as follows: the second section gives a review of the mathematics tool that is necessary for the research. The ESOM for manoeuvring target tracking is presented in the third section. A consensus-based SCKF is designed in the fourth section. Simulation results are provided in the fifth section. Concluding statements are given in the sixth section.

Backgrounds and preliminaries

Problem statement

In the combat environment, estimations of the target states are important for the missile when it intercepts a manoeuvring target. However, because of the limitation of the traditional KF, there is no general tracking model. In addition, estimation error is unavoidable in the sensor networks. Therefore, on the basis of the general tracking model, an effective and reasonable data fusion of multiple sensors is needed for the sensor networks.

The scheme of manoeuvring target tracking for sensor networks is described in Figure 1. The networks consist of N sensors, where each sensor gets the measurement with noise from the manoeuvring target, then a local SCKF is constructed by the novel ESOM that can get the local estimation. Through the sensor communication topology, each sensor exchanges information with its neighbour. Finally, the consensus algorithm is adopted to fuse the estimated states of the target.

Graph theory

Let $G (N, E)$ be a graph that describes the communication topology of the sensor networks. $N = {1, 2, \dots, N}$ describes the set of sensor nodes, and the set of links between nodes is denoted by $E$ . Moreover, an edge $(i, j) \in E$ means that node j can receive data from node i. Let $N_{i} = {j | (i, j) \in E}$ denote the set of neighbour of node $i$ .

To discuss the conditions for weighted average consensus in sensor networks, we first recall an important lemma:

Lemma 1 (Ren et al., 2007): If there exists a positive column vector $λ \in R^{n}$ and a nonnegative matrix $A \in R^{n \times n}$ such that $1^{T} λ = 1$ and $A$ is a primitive row-stochastic matrix, then $lim_{k \to \infty} A^{k} = 1 λ^{T}$ .

Target tracking model design based on extended state observer theory

Brief review of ESO

A brief review of this method is presented in this section for the sake of completeness and ready reference. The following ESO was proposed by Han (1995)

{\begin{matrix} {\overset{\cdot}{z}}_{1} (t) = z_{2} (t) - β_{1} g_{1} (z (t) - y (t)) \\ {\overset{\cdot}{z}}_{2} (t) = z_{3} (t) - β_{2} g_{2} (z (t) - y (t)) \\ ⋮ \\ {\overset{\cdot}{z}}_{n} (t) = z_{n + 1} (t) - β_{n} g_{n} (z_{1} (t) - y (t)) + u (t) \\ {\overset{\cdot}{z}}_{n + 1} (t) = - β_{n + 1} g_{n + 1} (z_{1} (t) - y (t)) \end{matrix}

(1)

For an n-dimensional single-input and single-output (SISO) nonlinear system

{\begin{matrix} {\overset{\cdot}{x}}_{1} (t) = x_{2} (t), \\ {\overset{\cdot}{x}}_{2} (t) = x_{3} (t), \\ ⋮ \\ {\overset{\cdot}{x}}_{n} (t) = d + u (t), \\ y (t) = x_{1} (t) \end{matrix}

(2)

where $u \in C (R, R)$ is the input and y is the output. It should be noted that $d$ is called total disturbance and it is assumed that $M = \overset{\cdot}{d}$ is bounded. $β_{i}, i = 1, 2, \dots, n + 1$ are adjustable gains. The states of the observer $z_{i}, i = 1, 2, \dots, n$ and $z_{n + 1}$ are considered as the approximations of the corresponding states $x_{i}$ for $i = 1, 2, \dots, n$ and the total disturbance $d$ . Defining the error $e_{i} = z_{i} - x_{i} (i = 1, . . ., n + 1)$ , it yields

\overset{\cdot}{e} = Ae + B M + Γ Δ_{e}

(3)

where $B = {[\begin{matrix} 0 & \dots & 0 & - 1 \end{matrix}]}_{1 \times (n + 1)}^{T}$ , $A = [\begin{matrix} - β_{1} & 1 & 0 & \dots & 0 \\ - β_{2} & 0 & 1 & \dots & 0 \\ ⋮ & ⋮ & ⋮ & ⋮ & ⋮ \\ - β_{n} & 0 & 0 & \dots & 1 \\ - β_{n + 1} & 0 & 0 & \dots & 0 \end{matrix}]$ , $M = \overset{\cdot}{d}$ . $Γ = diag ([\begin{matrix} β_{1} & \dots & β_{n + 1} \end{matrix}])$ , $Δ_{e} = {[\begin{matrix} e_{1} - g_{1} (e_{1}) & e_{1} - g_{2} (e_{1}) & \dots & e_{1} - g_{n + 1} (e_{1}) \end{matrix}]}^{T}$ . The parameters and function $g_{i} ()$ should make the error dynamic (3) stable. The linear extended state observer (LESO) was proposed by Gao (2003) as special case of (1), $g_{i} (x) = x$ , and the roots of the characteristic polynomial $λ (s) = s^{n} + β_{1} s^{n - 1} + \dots + β_{n}$ are all in the left-hand half plane.

The LESO is bounded-input bounded-output (BIBO) stable because the roots of the characteristic polynomial of $A$ $λ (s) = s^{n} + β_{1} s^{n - 1} + \dots + β_{n}$ are all in the left-hand half plane and $M$ is bounded. When the dynamic is stable, the stable error of the ESO estimate is $e_{i} = - \frac{β_{i - 1}}{β_{n + 1}} M$ $(β_{0} = 1, i = 1, 2, . . ., n + 1)$ .

Target tracking model design based on ESO theory

The manoeuvring target tracking problem is to estimate the position, velocity and even acceleration state of the target. It is essential for non-cooperative targets, such as submarine vehicles, flight vehicles, etc. It is worth noting that the target acceleration is always changing with the actual environment. The tracking task is to estimate the position, velocity and acceleration information as accurately as possible.

Let $ζ$ , $ξ$ , $ς$ be the target position, velocity and acceleration along a generic direction, respectively. Specifically

\overset{\cdot\cdot}{ζ} = ς

(4)

The acceleration is viewed as disturbance and denoted as $ς$ in (4), which is supposed to be differentiable and its differential is bounded.

The single models, CV, CA, CS, etc., differ in how the function $ς$ is defined. For example, the CV model can be derived when $ς$ equals 0, the CA model can be derived when $ς$ equals a constant and the Singer model assumes that the target acceleration $ς$ is a zero-mean first-order stationary Markov process. It is impossible to find the function $ς$ to satisfy all kinds of models. Therefore, the single model is limited in manoeuvring target tracking because the acceleration always changes both in value and direction.

Equation (4) can be written as

{\begin{matrix} \overset{\cdot}{ζ} = ξ \\ \overset{\cdot}{ξ} = ς \end{matrix}

(5)

where $ζ \in R^{r_{p}}, ξ \in R^{r_{p}}, ς \in R^{r_{p}}$ ; $r_{p}$ is the dimension to describe the target motion. According to ESO theory (1), (2), $ς$ could be viewed as disturbance, and the ESO for (5) is constructed as

{\begin{matrix} {\overset{\cdot}{z}}_{1} = z_{2} - β_{1} g_{1} (z_{1} - ζ) \\ {\overset{\cdot}{z}}_{2} = z_{3} - β_{2} g_{2} (z_{1} - ζ) \\ {\overset{\cdot}{z}}_{3} = - β_{3} g_{3} (z_{1} - ζ) \end{matrix}

(6)

where $z_{1} \in R^{r_{p}}$ , $z_{2} \in R^{r_{p}}$ , $z_{3} \in R^{r_{p}}$ is the estimation of position $ζ$ , velocity $ξ$ , acceleration $ς$ in real-time, respectively. $g_{i} (x) = {[\begin{matrix} g_{i} (x_{1}) & g_{i} (x_{2}) & \dots & g_{i} (x_{r_{p}}) \end{matrix}]}^{T} (i = 1, 2, . . ., r_{p})$ , $g_{i} \in C (R^{r_{p}}, R^{r_{p}}), g_{i} () \in C (R, R)$ . The acceleration is observed by (6) instead of making some assumption before, so it is convenient to track the manoeuvring target through the ESO.

Remark 1: As is shown in (6), if the position $ζ$ is measured without noise, the ESO can estimate the target states. In practice, each sensor node measures the target information of range and bearing, which are measurements with noise. Therefore, only the ESO cannot solve the target tracking problem. Combining the ESO with the filtering algorithm, a method to resolve the measurements with noise is proposed in this paper.

The target acceleration can be estimated by the ESO in (6) in real-time. Because most of the time the acceleration is smooth and the ESO can convergence to its true value finally, the estimated acceleration from (6) can be used in (5) to predicate the target state. The sensor measurement reflects the estimated error and is used in the new estimated progress. Then the ESOM is proposed as

\overset{\cdot}{x} = φ (x) = {[\begin{matrix} ξ \\ z_{3} \\ z_{2} - β_{1} g_{1} (z_{1} - ζ) \\ z_{3} - β_{2} g_{2} (z_{1} - ζ) \\ - β_{3} g_{3} (z_{1} - ζ) \end{matrix}]}_{5 \times r_{p}}

(7)

where $x = {[\begin{matrix} ζ^{T} & ξ^{T} & z_{1}^{T} & z_{2}^{T} & z_{3}^{T} \end{matrix}]}^{T}, x \in R^{5 \times r_{p}} .$ If different $g_{i} (x)$ is selected, the convergence rates of the ESO are also different. For the convenience of the research, let $g_{i} (x) = x$ . The parameters were chosen as $β_{1} = 3 ω$ , $β_{2} = 3 ω^{2}$ , $β_{2} = ω^{3}$ , $ω > 0$ to stabilize the error dynamic (3). Then the discrete form tracking model can be obtained

x_{k} = f (x_{k - 1}) = F x_{k - 1}

(8)

where $F = (Φ \otimes I_{r_{p} \times r_{p}})$ , $Φ = ℓ^{- 1} [{(s I - Λ)}^{- 1}]$ ; an approximated model can be calculated as $Φ \approx I_{5 \times 5} + Λ T + Λ^{2} T^{2} / 2 + Λ^{3} T^{3} / 6$ ,

$Λ = [\begin{matrix} 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 \\ β_{1} & 0 & - β_{1} & 1 & 0 \\ β_{2} & 0 & - β_{2} & 0 & 1 \\ β_{3} & 0 & - β_{3} & 0 & 0 \end{matrix}]$ . $Φ = [\begin{matrix} ϕ_{11} & ϕ_{12} & 1 - ϕ_{11} & T - ϕ_{12} & 0.5 T^{2} \\ ϕ_{21} & ϕ_{22} & - ϕ_{21} & 1 - ϕ_{22} & T \\ ϕ_{31} & ϕ_{32} & 1 - ϕ_{31} & T - ϕ_{32} & 0.5 T^{2} \\ ϕ_{41} & ϕ_{42} & - ϕ_{41} & 1 - ϕ_{42} & T \\ ϕ_{51} & ϕ_{52} & - ϕ_{51} & - ϕ_{52} & 1 \end{matrix}]$ , where $T$ is the sampling time.

ϕ_{11} = \frac{ω T}{3} + \frac{e^{- 3 ω T / 2} (\cos \frac{\sqrt{3} ω T}{2} - \sqrt{3} \sin \frac{\sqrt{3} ω T}{2})}{3} + \frac{2}{3},

ϕ_{21} = \frac{ω}{3} - \frac{ω e^{- 3 ω T / 2} (\cos \frac{\sqrt{3} ω T}{2} + \sqrt{3} \sin \frac{\sqrt{3} ω T}{2})}{3},

ϕ_{31} = \frac{ω T}{3} - \frac{2 e^{- 3 ω T / 2} (\cos \frac{\sqrt{3} ω T}{2} - \frac{5 \sqrt{3}}{3} \sin \frac{\sqrt{3} ω T}{2})}{3} + \frac{2}{3},

ϕ_{41} = \frac{ω}{3} - \frac{ω e^{- 3 ω T / 2} (\cos \frac{\sqrt{3} ω T}{2} + \frac{5 \sqrt{3}}{3} \sin \frac{\sqrt{3} ω T}{2})}{3},

\begin{matrix} ϕ_{51} = \frac{2 \sqrt{3} ω^{2} \sin (\frac{\sqrt{3} ω T}{2}) e^{- 3 ω T / 2}}{3}, \\ ϕ_{12} = \frac{2 T}{3} + \frac{ω T^{2}}{6} + \frac{2}{9 ω} - \frac{2 \cos (\sqrt{3} \frac{ω T}{2}) e^{- 3 ω T / 2}}{9 ω}, \end{matrix}

ϕ_{22} = \frac{ω T}{3} + \frac{e^{- 3 ω T / 2} (\cos (\frac{\sqrt{3} ω T}{2}) + \frac{\sqrt{3}}{3} \sin (\frac{\sqrt{3} ω T}{2}))}{3},

\begin{matrix} ϕ_{32} = \frac{2 T}{3} + \frac{ω T^{2}}{6} + \frac{2}{9 ω} \\ - \frac{2 e^{- 3 ω T / 2} (\cos (\sqrt{3} \frac{ω T}{2}) + \frac{\sqrt{3}}{3} \sin (\frac{\sqrt{3} ω T}{2}))}{9 ω}, \end{matrix}

ϕ_{42} = \frac{ω T}{3} + \frac{2 e^{- 3 ω T / 2} (\cos (\frac{\sqrt{3} ω T}{2}) + \frac{4 \sqrt{3}}{3} \sin (\frac{\sqrt{3} ω T}{2}))}{3},

ϕ_{52} = \frac{ω}{3} - \frac{ω e^{- 3 ω T / 2} (\cos \frac{\sqrt{3} ω T}{2} - \sqrt{3} \sin \frac{\sqrt{3} ω T}{2})}{3} .

Remark 2: The acceleration is viewed as a disturbance, which allows for changing the target velocity and position. Thanks to the ESO, the acceleration is observed in real-time and there is no need to make any assumption for acceleration. Therefore, the ESOM can be used to estimate the target states, no matter what the true motion model is or how the dynamic changes, which is a general model with robustness.

Remark 3: Considering the problems of the ESOM are high-dimensional, as is shown in (7), and the measurements have noise, the filter algorithm chosen should have the ability to estimate state in high-dimensional nonlinear systems.

Consensus-based square-root cubature Kalman filter design

The CKF, essentially a nonlinear filter, is based on the spherical–radial cubature rule to compute the multi-dimensional gauss integrals. The SCKF introduces the square-root of the state error covariance matrix on the basis of the CKF, ensuring the positive definiteness and further improving the filtering stability. It is an effective tool to solve the problem of the high-dimensionality of the ESOM. In this section, the SCKF algorithm is given first for each local node in the distributed sensor networks, then the consensus-based SCKF algorithm is presented.

System model

Consider the following discrete-time nonlinear stochastic systems

\begin{matrix} x_{k} = f (x_{k - 1}) + ω_{k - 1} \\ z_{k}^{i} = h^{i} (x_{k}) + ν_{k}^{i}, i = 1, 2, \dots, N \end{matrix}

(9)

where $x_{k} \in R^{5 \times r_{p}}$ represents the state vector of the dynamic system at discrete-time $k$ and $z_{k}^{i} \in R^{m}$ represents the measurement of sensor $i$ ; $f ()$ is the ESOM, $h ()$ is known nonlinear function. Assume the process noise $ω_{k - 1} \in R^{5 \times r_{p}}$ and the measurement noise of the ith sensor $ν_{k}^{i} \in R^{m}$ are uncorrelated Gaussian white sequences with zero-mean and covariance matrices with $Q_{k - 1}$ and $R_{k}^{i}$ , respectively.

Local SCKF for each node in the sensor networks

Each sensor node can estimate the target states through the SCKF and then exchange information with its neighbours. For each sensor node $i (i \in N)$ , initialize the estimation of state ${\hat{x}}_{0}^{i}$ and error variance matrix $P_{0}^{i}$ according to

\begin{matrix} {\hat{x}}_{0}^{i} = E (x_{0}), \\ P_{0}^{i} = E [(x_{0} - {\hat{x}}_{0}^{i}) {(x_{0} - {\hat{x}}_{0}^{i})}^{T}] \end{matrix}

(10)

where $i = 1, 2, \dots, N .$ $N$ is the number of nodes in the sensor network.

Using the Cholesky decomposition approach yields

P_{k}^{i} = {(S_{k}^{i})}^{T} S_{k}^{i}

(11)

Then $2 n$ cubature points can be obtained with

X_{j, k}^{i} = S_{k}^{i} ζ_{j}^{i} + {\hat{x}}_{k}^{i}

(12)

where j = 1,2,…,2n, $ζ_{j}$ is the jth column of the equation $\sqrt{1 / 2 n} [\begin{matrix} I_{n} & - I_{n} \end{matrix}]$ and $I_{n}$ is an n order unit matrix. Then the propagated cubature points can be obtained by

X_{j, k + 1}^{i *} = f (X_{j, k}^{i})

(13)

Next, the one-step prediction can be completed according to

{\bar{x}}_{k + 1}^{i} = \frac{1}{2 n} \sum_{j = 1}^{2 n} X_{j, k + 1}^{i *}, {\bar{S}}_{k + 1}^{i} = Tria ([\begin{matrix} χ_{k + 1}^{i *} & S_{Q_{i}} \end{matrix}])

(14)

where $Tria ()$ , $S = Tria (A)$ mean QR for matrix $A^{T}$ . $χ_{k + 1}^{i *}$ and $S_{Q_{i}}$ are defined by

\begin{matrix} χ_{k + 1}^{i *} = 1 / (\sqrt{2 n}) \\ [X_{1, k + 1}^{i *} - {\bar{x}}_{k + 1}^{i}, X_{2, k + 1}^{i *} - {\bar{x}}_{k + 1}^{i}, \dots, X_{2 n, k + 1}^{i *} - {\bar{x}}_{k + 1}^{i}] \\ S_{Q_{i}} = chol (Q_{i}) \end{matrix}

(15)

where $chol ()$ is the Cholesky decomposition approach.

The obtained cubature points can be transformed into the forms as below based on the measurement function

\begin{matrix} X_{j, k + 1}^{i} = {\bar{S}}_{k + 1}^{i} ζ_{j} + {\bar{x}}_{k + 1}^{i} \\ Z_{j, k + 1}^{i} = h (X_{j, k + 1}^{i}) \\ {\bar{z}}_{k + 1}^{i} = \frac{1}{2 n} \sum_{j = 1}^{2 n} Z_{j, k + 1}^{i} \end{matrix}

(16)

S_{zz, k + 1}^{i} = Tria ([\begin{matrix} γ_{k + 1}^{i} & S_{R_{i}} \end{matrix}])

(17)

where $γ_{k + 1}$ and $S_{R_{i}}$ are defined by

\begin{matrix} γ_{k + 1}^{i} = 1 / (\sqrt{2 n}) \\ [Z_{1, k + 1}^{i} - {\bar{z}}_{k + 1}^{i}, Z_{2, k + 1}^{i} - {\bar{z}}_{k + 1}^{i}, \dots, Z_{2 n, k + 1}^{i} - {\bar{z}}_{k + 1}^{i}] \\ S_{R_{i}} = chol (R_{i}) \end{matrix}

(18)

Then the prediction of measurement and error covariance can be obtained with

P_{xz, k + 1}^{i} = χ_{k + 1}^{i} {(γ_{k + 1}^{i})}^{T}

(19)

where $χ_{k + 1}^{i}$ is defined as $χ_{k + 1}^{i} = 1 / (\sqrt{2 n}) [X_{1, k + 1}^{i} - {\bar{x}}_{k + 1}^{i}, X_{2, k + 1}^{i} - {\bar{x}}_{k + 1}^{i}, \dots, X_{2 n, k + 1}^{i} - {\bar{x}}_{k + 1}^{i}]$ .

The filter gain can be calculated by

K_{k + 1}^{i} = (P_{xz, k + 1}^{i} / {(S_{zz, k + 1}^{i})}^{T}) / S_{zz, k + 1}^{i}

(20)

In the final step, the state estimation and error variance of node i at time k can be updated with

\begin{matrix} {\hat{x}}_{k + 1}^{i} = {\bar{x}}_{k + 1}^{i} + K_{k + 1}^{i} (z_{k + 1}^{i} - {\bar{z}}_{k + 1}^{i}) \\ S_{k + 1}^{i} = Tria ([\begin{matrix} χ_{k + 1}^{i} - K_{k + 1}^{i} γ_{k + 1}^{i} & K_{k + 1}^{i} S_{R_{i}} \end{matrix}]) \end{matrix}

(21)

Consensus-based SCKF algorithm

The consensus method can be summarized as

({\hat{x}}_{k}^{*}, S_{k}^{*}) = lim_{l \to \infty} ({\hat{x}}_{k, l}^{i}, S_{k, l}^{i})

(22)

where $l$ represents the iteration times of consensus. ${\hat{x}}_{k}^{i}$ and $S_{k}^{i}$ are the state estimation and covariance matrix, respectively, of node $i$ at time $k$ . ${\hat{x}}_{k, l}^{i}$ and $S_{k}^{i}$ are variables of the $l th$ step in the consensus process, which satisfies

\begin{matrix} {\hat{x}}_{k, l + 1}^{i} = π_{i, i} {\hat{x}}_{k, l}^{i} + \sum_{j \in N_{i}} π_{i, j} {\hat{x}}_{k, l}^{j} \\ S_{k, l + 1}^{i} = π_{i, i} S_{k, l}^{i} + \sum_{j \in N_{i}} π_{i, j} S_{k, l}^{j} \end{matrix}

(23)

where $N_{i}$ is the neighbour of node $i$ . $π_{i, j} (j \in N_{i})$ is the consensus weight, which satisfies

π_{i, i} + \sum_{j \in N_{i}} π_{i, j} = 1, π_{i, i} \geq 0

(24)

Theorem 1 Take a sensor network with the topology $G (N, E)$ under consideration. Suppose that the consensus weight matrix $Π = {(π_{i, j})}_{n \times n}$ is primitive, then each information pair ${({\hat{x}}_{k, l}^{i}, S_{k, l}^{i})}_{i \in N}$ can come to a weighted average consensus.

Subsequently, Chen et al. (2016) is used to give the proof of Theorem 1.

Proof: Defining ${\hat{x}}_{k | k} \overset{Δ}{=} col ({\hat{x}}_{k | k}^{s}, s \in N)$ , Equation (23) can be written as

{\hat{x}}_{k, l + 1} = (Π^{ℓ + 1} \otimes I) {\hat{x}}_{k, 0}

(25)

Recalling that $π_{i, i} + \sum_{j \in N_{i}} π_{i, j} = 1, π_{i, i} \geq 0$ , thus it is obvious that matrix $Π$ is row-stochastic. In view of Lemma 1

lim_{ℓ \to \infty} (Π^{ℓ + 1}) = 1 λ^{T}

(26)

where $λ$ is an $n$ -dimensional column vector. As $ℓ$ goes to $\infty$

{\hat{x}}_{k, l + 1} = (1 λ^{T} \otimes I) {\hat{x}}_{k, 0}

(27)

that is

{\hat{x}}_{k, l + 1}^{i} = λ_{1} {\hat{x}}_{k, 0}^{1} + λ_{2} {\hat{x}}_{k, 0}^{2} + \dots + λ_{n} {\hat{x}}_{k, 0}^{n} = {\hat{x}}_{k | k}^{*}

(28)

In a similar way

S_{k + 1}^{i} = λ_{1} S_{k, 0}^{1} + λ_{2} S_{k, 0}^{2} + \dots + λ_{n} S_{k, 0}^{n} = S_{k | k}^{*}

(29)

This theorem has been proved.

Remark 4: In the consensus iteration process, each sensor node interacts information with its neighbour. In the final of the consensus process, all the nodes reach consensus.

Combing the consensus algorithm with the SCKF, the consensus-based SCKF algorithm proposed in this paper is summarized in Algorithm 1. One cycle of the consensus-based SCKF algorithm flow is shown in Figure 2. Algorithm 1 is constructed by a local SCKF state estimation algorithm and a state consensus fusion algorithm. The local SCKF state estimation algorithm includes three steps: time update, measurement update and state update. After the state estimation is complete, information pairs can be utilized by the state consensus fusion algorithm. Every sensor node exchange information pairs with its neighbours according to the communication topology. Finally, the consensus estimated value of target states will be given by Algorithm 1.

Algorithm 1 Consensus-based SCKF algorithm
Initial ${\hat{x}}_{k}^{i}, P_{k}^{i}, Q_{i}, R_{i}$ for the ith sensor.
$P_{k}^{i} = S_{k}^{i} {(S_{k}^{i})}^{T}$
for $k = 1, 2, . . ., k_{stop}$
Time update:
Step 1: Evaluate the cubature points $(j = 1, 2, . . ., 2 n)$
$X_{j, k}^{i} = S_{k}^{i} ζ_{j}^{i} + {\hat{x}}_{k}^{i}$
Step 2: Evaluate the propagated cubature points $(j = 1, 2, . . ., 2 n)$
$X_{j, k + 1}^{i *} = f (X_{j, k}^{i})$
Step 3: Estimate the predicted state
${\bar{x}}_{k + 1}^{i} = \frac{1}{2 n} \sum_{j = 1}^{2 n} X_{j, k + 1}^{i *}$
Step 4: Estimate the square-root factor of the predicted error covariance
${\bar{S}}_{k + 1}^{i} = Tria ([\begin{matrix} χ_{k + 1}^{i *} & S_{Q_{i}} \end{matrix}])$
Measurement update
Step 1: Evaluate the cubature points $(j = 1, 2, . . ., 2 n)$
$X_{j, k + 1}^{i} = {\bar{S}}_{k + 1}^{i} ζ_{j} + {\bar{x}}_{k + 1}^{i}$
Step 2: Evaluate the propagated cubature points $(j = 1, 2, . . ., 2 n)$
$Z_{j, k + 1}^{i} = h (X_{j, k + 1}^{i})$
Step 3: Estimate the predicted measurement
${\bar{z}}_{k + 1}^{i} = \frac{1}{2 n} \sum_{j = 1}^{2 n} Z_{j, k + 1}^{i}$
Step 4: Estimate the square-root of the innovation covariance matrix
$S_{zz, k + 1}^{i} = Tria ([\begin{matrix} γ_{k + 1}^{i} & S_{R_{i}} \end{matrix}])$
Step 5: Estimate the cross-covariance matrix
$P_{xz, k + 1}^{i} = χ_{k + 1}^{i} {(γ_{k + 1}^{i})}^{T}$
State update
Step 1: Estimate the Kalman gain
$K_{k + 1}^{i} = (P_{xz, k + 1}^{i} / {(S_{zz, k + 1}^{i})}^{T}) / S_{zz, k + 1}^{i}$
Step 2: Estimate the updated state
${\hat{x}}_{k + 1}^{i} = {\bar{x}}_{k + 1}^{i} + K_{k + 1}^{i} (z_{k + 1}^{i} - {\bar{z}}_{k + 1}^{i})$
Step 3: Estimate the square-root factor of the corresponding error covariance
$S_{k + 1}^{i} = Tria ([\begin{matrix} χ_{k + 1}^{i} - K_{k + 1}^{i} γ_{k + 1}^{i} & K_{k + 1}^{i} S_{R_{i}} \end{matrix}])$
Apply consensus algorithm for $l = 0, 2, . . ., L - 1$
Each node exchanges the $({\hat{x}}_{k, l}^{i}, S_{k, l}^{i})$ information with its neighbours. Combine the state estimation and error covariance according to the consensus algorithm:
${\hat{x}}_{k, l + 1}^{i} = π_{i, i} {\hat{x}}_{k, l}^{i} + \sum_{j \in N_{i}} π_{i, j} {\hat{x}}_{k, l}^{j}$
$S_{k, l + 1}^{i} = π_{i, i} S_{k, l}^{i} + \sum_{j \in N_{i}} π_{i, j} S_{k, l}^{j}$
Then the state estimation can be obtained by
${\hat{x}}_{k, l + 1}^{i} = {\hat{x}}_{k, L}^{i}$
$S_{k, l + 1}^{i} = S_{k, L}^{i}$
end

where $k_{stop}$ is the total steps.

Simulation results

Tracking the manoeuvring target in three-dimensional space

Choose $r_{p} = 3,$ $g_{i} (x) = x,$ $ω = 80,$ $β_{1} = 3 ω,$ $β_{2} = 3 ω^{2},$ $β_{3} = ω^{3},$ $ζ_{0} = {[\begin{matrix} - 1000 & 17321 & 5000 \end{matrix}]}^{T} m,$ $ξ_{0} = {[\begin{matrix} 0 & 300 & 0 \end{matrix}]}^{T} m / s,$ $ς$ is changing, as shown in Table 1. The initial positions of the nodes are $η_{0}^{1} = (0, 0, 0) m,$ $η_{0}^{2} = (5000, 0, 0) m,$ $η_{0}^{3} = (0, 0, 5000) m,$ $η_{0}^{4} = (3000, 0, 8000) m .$

Table 1.

Movement of the target.

Manoeuvring time/s	Acceleration in x-axis/(m/s²)	Acceleration in y-axis/(m/s²)	Acceleration in z-axis/(m/s²)
0	0	0	0
10	4 × 9.8	−4 × 9.8	0
60	−4 × 9.8	4 × 9.8	0
110	−4 × 9.8 × (cos( $\frac{π}{30}$ (t– 110)))	−4 × 9.8 × (cos( $\frac{π}{30}$ (t– 110)))	2 × 9.8 × (cos( $\frac{π}{30}$ (t– 110)))
160	0.1 × ( t– 150 ) × 9.8	0.1 × (t– 150) × 9.8	−4 × 9.8
210	0	0	0

The measurement vector of the ith sensor is $z_{k}^{i} = {[\begin{matrix} r_{k}^{i} & {\overset{\cdot}{r}}_{k}^{i} & q_{yk}^{i} & q_{zk}^{i} \end{matrix}]}^{T} + v_{k}^{i}$ , where $r_{k}^{i} = \sqrt{{(ζ_{xk} - η_{xk}^{i})}^{2} + {(ζ_{yk} - η_{yk}^{i})}^{2} + {(ζ_{zk} - η_{zk}^{i})}^{2}}$ , ${\overset{\cdot}{r}}_{k}^{i} = \frac{ζ_{xk} ξ_{xk} + ζ_{yk} ξ_{yk} + ζ_{zk} ξ_{zk}}{\sqrt{{(ζ_{xk} - η_{xk}^{i})}^{2} + {(ζ_{yk} - η_{yk}^{i})}^{2} + {(ζ_{zk} - η_{zk}^{i})}^{2}}}$ , $q_{yk}^{i} = \arctan \frac{(ζ_{yk} - η_{yk}^{i})}{\sqrt{{(ζ_{xk} - η_{xk}^{i})}^{2} + {(ζ_{zk} - η_{zk}^{i})}^{2}}}$ , $q_{zk}^{i} = \arctan \frac{- (ζ_{zk} - η_{zk}^{i})}{(ζ_{xk} - η_{xk}^{i})}$ , $v_{k}^{i} ~ (0, R_{k}^{i})$ , $R_{k}^{1} = diag ([\begin{matrix} 30^{2} & 10^{2} & {0.001}^{2} & {0.001}^{2} \end{matrix}])$ , $R_{k}^{2} = diag ([\begin{matrix} 35^{2} & 12^{2} & {0.012}^{2} & {0.012}^{2} \end{matrix}])$ , $R_{k}^{3} = diag ([\begin{matrix} 40^{2} & 14^{2} {0.009}^{2} & {0.009}^{2} \end{matrix}])$ and $R_{k}^{1} = diag ([\begin{matrix} 45^{2} & 16^{2} & {0.01}^{2} & {0.01}^{2} \end{matrix}])$ . The sensor network topology can be described as shown in Figure 3 and the consensus weight matrix can be selected as

Π = [\begin{matrix} \frac{5}{12} & \frac{1}{3} & \frac{1}{4} & 0 \\ \frac{1}{3} & \frac{5}{12} & \frac{1}{4} & 0 \\ \frac{1}{4} & \frac{1}{4} & \frac{1}{4} & \frac{1}{4} \\ 0 & 0 & \frac{1}{4} & \frac{3}{4} \end{matrix}] .

Case 1: In order to demonstrate the effectiveness and superiority of the proposed ESOM, three methods are introduced for comparison simulation with the same condition.

The ESO with a low-pass filter (LPF). Considering the noise influence, a LPF is used to filter the node measurements. The position is calculated by $x = {\hat{r}}^{1} \cos ({\hat{q}}_{y}^{1}) \cos ({\hat{q}}_{z}^{1})$ , $y = {\hat{r}}^{1} \sin ({\hat{q}}_{y}^{1})$ , $z = - {\hat{r}}^{1} \cos ({\hat{q}}_{y}^{1}) \sin ({\hat{q}}_{z}^{1})$ . $\frac{{\hat{r}}^{1}}{r^{1}} = \frac{1}{Ts + 1}$ , $\frac{{\hat{q}}_{y}^{1}}{q_{y}^{1}} = \frac{1}{Ts + 1}$ , $\frac{{\hat{q}}_{z}^{1}}{q_{z}^{1}} = \frac{1}{Ts + 1}$ , where $T = 1$ . The parameters of the ESO are selected as $β_{1} = 3 ω,$ $β_{2} = 3 ω^{2},$ $β_{3} = ω^{3},$ $ω = 2$ .

The SCKF using the CA model.

The SCKF using the ESOM.

The average of the root-mean square error (RMSE) is applied to illustrate the overall estimation error. The RMSE for each node at time instant $k$ is defined as

RMSE (k) = \sqrt{\frac{1}{M} \sum_{m = 1}^{M} ‖ {\hat{ε}}_{k}^{m} - ε_{k}^{m} ‖}

where $ε_{k}^{m}$ and ${\hat{ε}}_{k}^{m}$ are the true and estimated states in the $m th$ Monte Carlo algorithm simulation cycle, respectively. $M$ is the number of Monte Carlo algorithm simulation cycles. A 100 times Monte Carlo simulation is conducted to show the RMSE of three methods in the same condition. The difference-value of the RMSEs between different methods are summarized in Table 2 and Figure 4. $Q_{1}$ , $Q_{2}$ and $Q_{3}$ denote the RMSE in 25%, 50% and 75% of the whole simulation time, respectively.

Table 2.

The distributions of the root-mean square errors (RMSEs).

RMSEs (m/s²)	$Q_{1}$ (25%)	$Q_{2}$ (50%)	$Q_{3}$ (75%)
ESO	11.3841	17.0939	25.2362
SCKF (CA)	1.0872	7.8560	25.9082
SCKF (ESOM)	1.7178	4.9778	18.8125

ESO: extended state observer; SCKF: square-root cubature Kalman filter; CA: Constant Acceleration; ESOM: general target tracking single model by ESO theory.

The RMSE represents the estimation performance of the designed method: the smaller RMSE, the higher accuracy. It can be seen from Table 2 that in 75% of the simulation time, the RMSE of the SCKF using ESOM is smaller than that of the ESO with the LPF. It shows that the performance of the ESO is influenced by measurement noise easily. Figure 5 also demonstrates this conclusion. In 50% of the simulation time, the SCKF using ESOM has a smaller RMSE than the SCKF using the CA model because some models of the target movement mismatch with the CA model.

Figure 5 shows the estimation results of the three methods. All of the methods could estimate the target acceleration. However, the estimated results of the ESO with the LPF are influenced by noise easily. When the target motion mismatches with the designed CA model, the estimation performance of the SCKF became worse. Comparing with the ESO with LPF, the SCKF using ESOM has a better performance under the influence of noise. Comparing with the SCKF using the CA model, the SCKF using the ESOM could estimate the states of different target motion. Lag exists in the estimate progress, which is clearer when the target moves with sine wave acceleration.

Case 2: Consensus simulation comparative analysis

This case considers running the consensus-based SCKF algorithm in sensor networks with four nodes to track a manoeuvring target in three dimensions. This paper only illustrates the acceleration result as an example. Figure 6 and 7 show the acceleration curves using the consensus-based SCKF and local SCKF, respectively. The tracking model is the same with the ESOM. Firstly, it can be seen clearly that both algorithms perform well to estimate the acceleration, which indicates the outstanding tracking performance of the ESOM. Although the target motion includes constant velocity motion, constant acceleration motion, sine wave motion and bangbang motion in this scenario, they all use the same tracking model, the ESOM. It can be concluded the ESOM is a general method that is suitable for target motion with robustness. It provides a new method to solve the dynamic changing problem in manoeuvring tracking. Then, on the drive of the consensus algorithm, each sensor node in the network estimate state reaches consensus regarding the different measurement noise. The single sensor is influenced by measurement noise easily, as can be seen from node 4 in Figure 7. In Figure 6, the consensus algorithm weakens the influences of measurement noise and enhances the accuracy, stability and robustness of the global networks.

Conclusion

The problem of manoeuvring target tracking in sensor networks has been investigated in this paper via the proposed consensus-based SCKF using the ESOM. The ESOM is a general model with robustness that is convenient to use with other algorithms. The consensus-based SCKF weakens the influences of measurement noise and enhances the accuracy, stability and robustness of the global networks. The results verify the effectiveness of the scheme. In future work, packet loss and time delay in the channel of the remote estimation will be further considered.

Footnotes

Declaration of conflicting interests

The authors declared no potential conflicts of interest with respect to the research, authorship and/or publication of this article.

Funding

The authors disclosed receipt of the following financial support for the research, authorship and/or publication of this article: This work was supported by the National Natural Science Foundation of China (grant no. 61903099) and the Fundamental Research Funds for the Central Universities (3072020CF0406).

ORCID iD

Enjiao Zhao

References

Arasaratnam

Haykin

(2009) Cubature Kalman filters. IEEE Transactions on Automatic Control 54(6): 1254–1269.

Chen

Wang

Yin

(2016) Distributed state estimations based on cubature Kalman filtering. In: proceedings of the 28th Chinese control and decision conference, Yinchuan, China, 28–30 May 2016, pp.3384–3389. IEEE.

Chen

Yin

Zhou

, et al. (2018) Hybrid consensus-based cubature Kalman filtering for distributed state estimation in sensor networks. IEEE Sensors Journal 18(11): 4561–4569.

Cui

Chen

Yang

, et al. (2019) Extended state observer-based integral sliding mode control for an underwater robot with unknown disturbances and uncertain nonlinearities. IEEE Transactions on Industrial Electronics 66(10): 8279-8280.

Ding

Liu

, et al. (2018) Unscented information consensus filter for maneuvering target tracking based on interacting multiple model. In: 2018 21st international conference on information fusion, Cambridge, England, 10–13 July 2018, pp.1535–1540. IEEE.

Gao

(2003) Scaling and bandwidth-parameterization based controller tuning. In: proceedings of the 2003 American control conference, Denver, CO, USA, 4–6 June 2003, pp.4989–4996. IEEE.

George

(2019) Distributed tracking of maneuvering target: A finite-time algorithm. In: 2019 IEEE international conference on acoustics, speech and signal processing, Brighton, United Kingdom,12–17 May 2019. pp.4420–4424. IEEE.

Guo

B-Z

Zhao

Z-L

(2011) On the convergence of an extended state observer for nonlinear systems with uncertainty. Systems & Control Letters 60(6): 420–430.

Han

(1995) A class of extended state observers for uncertain systems. Control and Decision 10(1): 85-88.

10.

Han

(2009) From PID to active disturbance rejection control. IEEE Transactions on Industrial Electronics 56(3): 900-906.

11.

Hlinka

Hlawatsch

Djuric

(2013) Distributed particle filtering in agent networks. IEEE Signal Processing Magazine 30(1): 61-81.

12.

Keshavarz-Mohammadiyan

Khaloozadeh

(2018) Consensus-based distributed unscented target tracking in wireless sensor networks with state-dependent noise. Signal Processing 144: 283-295.

13.

Yang

Chen

W-H

, et al. (2012) Generalized extended state observer based control for systems with mismatched uncertainties. IEEE Transactions on Industrial Electronics 59(12): 4792–4802.

14.

Jia

(2012) Location of mobile station with maneuvers using an IMM-based cubature Kalman filter. IEEE Transactions on Industrial Electronics 59(11): 4338–4348.

15.

Wei

Han

, et al. (2016) Weighted average consensus-based unscented Kalman filtering. IEEE Transactions on Cybernetics 46(2): 558-567.

16.

Jilkov

(2003) Survey of maneuvering target tracking. Part I: Dynamic models. IEEE Transactions on Aerospace and Electronic Systems 39(4): 1333–1364.

17.

Liu

Wang

(2014) Squared-root cubature information consensus filter for non-linear decentralised state estimation in sensor networks. IET Radar Sonar and Navigation 8(8): 931-938.

18.

Olfati-Saber

(2007) Distributed Kalman filtering for sensor networks. In: proceedings of the 46th IEEE conference on decision and control, New Orleans, LA, USA,12–14 December 2007, pp.1814–1820. IEEE.

19.

Pakki

Chandra

D-W

, et al. (2011) Cubature information filter and its applications. In: 2011 American control conference, San Francisco, CA, USA, 29 June–1 July 2011, pp.3609-3614. IEEE.

20.

Ren

Beard

Atkins

(2007) Information consensus in multivehicle cooperative control. IEEE Control Systems Magazine 27(2): 71-82.

21.

Wang

, et al. (2016) Distributed filtering for switched nonlinear positive systems with missing measurements over sensor networks. IEEE Sensors Journal 16(12): 4940–4948.

22.

(2017) Consensus-based distributed linear filter for target tracking with uncertain noise statistics. IEEEs Sensors Journal 17(15): 4875–4885.

23.

Zhang

W-A

(2015) Energy efficient distributed filtering for a class of nonlinear systems in sensor networks. IEEE Sensors Journal 15(5): 3026-3036.

24.

Zhang

Zhou

Wang

, et al. (2019a) Maneuvering target tracking with event-based mixture Kalman filter in mobile sensor networks. IEEE Transactions on Cybernetics1–12. DOI: 10.1109/TCYB.2019.2901515.

25.

Zhang

Zhou

Wang

, et al. (2019b) Adaptive consensus-based distributed target tracking with dynamic cluster in sensor networks. IEEE Transactions on Cybernetics 49(5): 1580–1591.

26.

Zhang

Huang

Ding

, et al. (2019c) Hummingbirds optimization algorithm-based particle filter for maneuvering target tracking. Nonlinear Dynamics 97(2): 1227–1243.