Adaptive Fifth-Degree Cubature Information Filter for Multi-Sensor Bearings-Only Tracking

Abstract

Standard Bayesian filtering algorithms only work well when the statistical properties of system noises are exactly known. However, this assumption is not always plausible in real target tracking applications. In this paper, we present a new estimation approach named adaptive fifth-degree cubature information filter (AFCIF) for multi-sensor bearings-only tracking (BOT) under the condition that the process noise follows zero-mean Gaussian distribution with unknown covariance. The novel algorithm is based on the fifth-degree cubature Kalman filter and it is constructed within the information filtering framework. With a sensor selection strategy developed using observability theory and a recursive process noise covariance estimation procedure derived using the covariance matching principle, the proposed filtering algorithm demonstrates better estimation accuracy and filtering stability. Simulation results validate the superiority of the AFCIF.

1. Introduction

Bearings-only tracking (BOT), which is also named target motion analysis (TMA), has been widely used in military applications, such as underwater tracking, aircraft surveillance and electronic warfare [1]. As an efficient passive means, BOT tracks the target of interest without emitting electromagnetic waves, which can prevent the tracking system from being detected and improve the fighting capabilities. The goal of BOT is to acquire kinematic parameters of the target using bearing measurements corrupted by noise [2].

For single-sensor BOT problem, the sensor must outmaneuver the target to guarantee the observability of the tracking system. Therefore, how the sensor moves becomes a crucial factor that influences the tracking accuracy and many researchers have devoted themselves to this research topic [3,4,5,6,7]. However, it is always difficult and sometimes unrealistic to design an optimal trajectory for the sensor. Under the premise that the sensor moves according to a given trajectory, there are also many focusing on the tracking algorithms. Over the past decades, the evolution of Bayesian filtering technology provides BOT with numerous choices, among which the most important and frequently used method is the extended Kalman filter (EKF) [8,9,10]. Although simple to implement, tracking algorithms that are based on the EKF linearize the nonlinear system model by utilizing Taylor-series expansion, which may lead to unstable and even divergent filtering results. In recent years, sigma-point based filters, including deterministic-point-based ones that are represented by cubature Kalman filter (CKF) [11,12,13] and random-point-based ones represented by particle filter (PF) [14], have attracted much attention. Instead of approximating the nonlinear functions, like EKF, this kind of filters adopt different numerical rules to approximate the probability density distributions of nonlinear systems. When filtering accuracy, numerical stability, and computational demands are all considered, the CKF seems to be a desirable choice for state estimation in BOT. This is because of the evident drawbacks existing in other sigma-point based filters, including the possible invalidation of the unscented Kalman filter (UKF) if the number of system states is more than three and the heavy computational burden of quadrature Kalman filter (QKF) and PF, etc. The conventional CKF adopts the third-degree cubature rule, which can guarantee at least second-order Taylor precision [15]. In order to further improve filtering precision and numerical stability, a new class of CKFs based on the high-degree cubature rules [16] is developed.

With the quick development of electronic technology, sensors become low-cost and tracking systems equipped with multiple sensors are becoming increasingly preferable for BOT. The bearing measurements received by sensors at different locations can be fused, thus the system observability and tracking performance can both be improved significantly. Nonetheless, unobservable cases may still occur in multi-sensor BOT. Therefore, the sensors that are used for tracking the target should be selected properly and saving sensor network resource should also be put into consideration at the same time. So we should try to achieve the optimal tracking efficacy with the fewest sensors. The information filter [17], which is algebraically equivalent to the classical Bayesian filter, is more suitable for multi-sensor data fusion. In the information filtering framework, the estimated parameters are the information-form states and matrix. Compared with conventional filtering algorithms, the information-form counterparts [18,19,20] have the advantages of easy initialization, easy decentralization, and low computational burden.

The standard Bayesian filtering algorithms are efficient, provided only that the statistic of process noise is exactly known. In real combat scenarios, however, this assumption is not usually reasonable. Thus, adaptive filtering algorithms are needed to solve this model mismatching problem. This type of filters can be mainly divided into innovation-based ones and the ones using multiple models [21]. Innovation-based filters [21,22,23,24] adopt covariance matching method or maximum likelihood estimation to modify the statistic of the process noise. Drawbacks, such as heavy computation burden, non-positive matrix, and offline estimation prevent these methods from being utilized in reality. Since being proposed, the interacting multiple model (IMM) method [25] has been extensively used for state estimation under unknown system parameters, but its performance depends on the selection of model sets.

In this paper, we propose an adaptive nonlinear estimation approach, called the adaptive fifth-degree cubature information filter (AFCIF), which shows better performance than the standard algorithms for multi-sensor BOT especially when the statistic of process noise is unknown. It is built on the basis of the newly introduced fifth-degree CKF [16] and adopts the structure of information filter. A prominent feature of this algorithm is the recursive process noise covariance estimation procedure, which is derived using covariance matching principle [22]. This can address the issue of state estimation with unknown process noise covariance in BOT. In addition, a sensor selection strategy is presented to enhance the system observability. Various kinds of criteria [26,27,28,29] have been proposed for sensor selection in multi-sensor target tracking, including Fisher information, estimation error covariance, and maximum entropy, etc. Our strategy is based on maximizing the inverse of the condition number [30] of the observability matrix, intending to achieve the best observable condition with the given sensor network. Combined with the sensor selection strategy and the recursive process noise covariance estimation procedure, the novel algorithm demonstrates better adaptivity, filtering accuracy, and stability.

The rest of the paper is organized as follows. Section 2 describes the problems that we will solve in multi-sensor BOT. The Bayesian filtering theory is reviewed in Section 3. Section 4 derives the sensor selection strategy and Section 5 introduces the AFCIF algorithm for multi-sensor BOT. The convergence of the proposed algorithm is analyzed in Section 6. Simulations for a certain multi-sensor BOT scenario and performance analysis are given in Section 7. Section 8 summarizes our main contributions.

2. Problem Formulation

The multi-sensor BOT problem in a two-dimensional (2-D) Cartesian coordinate plane is depicted in Figure 1. The position and velocity of the target at time k are denoted by $(x_k,y_k)$ and $({\dot{x}}_k,{\dot{y}}_k)$, and the state vector of the target at time k is defined as ${\mathit{x}}_k={\left[x_k{y}_k{\dot{x}}_k{\dot{y}}_k\right]}^{\mathrm{T}}$, then the system model can be written as

$${\mathit{x}}_k=\mathit{F}{\mathit{x}}_{k-1}+{\mathit{w}}_{k-1}$$

(1)

$${\mathit{z}}_k={\mathit{h}}_k({\mathit{x}}_k)+{\mathit{v}}_k$$

(2)

where ${\mathit{x}}_k\in {\mathbb{R}}^n$, $n(=4)$ is the dimension of the state vector and $\mathit{F}$ denotes the state transition matrix that decides the motion model of the target and is given by

$$\mathit{F}=\left[\begin{array}{cccc}1& 0& \mathsf{\Delta }T& 0\\ 0& 1& 0& \mathsf{\Delta }T\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$

(3)

where $\mathsf{\Delta }T$ represents the sample interval. ${\mathit{z}}_k\in {\mathbb{R}}^m$ is the measurement vector at time k and $m(=2)$ is its dimension. ${\mathit{h}}_k(\cdot )$ is the measurement function at time k, ${\mathit{w}}_{k-1}$ and ${\mathit{v}}_k$ represent process noise and measurement noise, respectively.

At time k, two sensors among the available sensor network are selected to measure the azimuths between the target and themselves. So ${\mathit{h}}_k({\mathit{x}}_k)$ can be written as follows:

$${\mathit{h}}_k({\mathit{x}}_k)=\left[\begin{array}{c}{h}_{k,1}({\mathit{x}}_k)\\ h_{k,2}({\mathit{x}}_k)\end{array}\right]=\left[\begin{array}{c}{\beta }_{k,1}\\ {\beta }_{k,2}\end{array}\right]=\left[\begin{array}{c}\arctan (\frac{x_k-{\mathit{s}}_1^x}{y_k-{\mathit{s}}_1^y})\\ \arctan (\frac{x_k-{\mathit{s}}_2^x}{y_k-{\mathit{s}}_2^y})\end{array}\right]$$

(4)

and

$${\mathit{v}}_k=\left[\begin{array}{c}{\mathit{v}}_{k,1}\\ {\mathit{v}}_{k,2}\end{array}\right]$$

(5)

where $({\mathit{s}}_i^x,{\mathit{s}}_i^{\mathit{y}})$ and ${\mathit{v}}_{k,i}$ denote the location and measurement noise, respectively, of the ith sensor. ${\mathit{w}}_{k-1}$ and ${\mathit{v}}_{k,i}$ are assumed to be uncorrelated noises with Gaussian distributions $\mathcal{N}({\mathit{w}}_{k-1};\mathbf{0},{\mathit{Q}}_{k-1})$ and $\mathcal{N}({\mathit{v}}_{k,i};0,{\mathit{R}}_i)$, and ${\mathit{v}}_k~\mathcal{N}(0,\mathit{R})$ with $\mathit{R}=\mathrm{diag}({\mathit{R}}_1,{\mathit{R}}_2)$.

Problem 1.

The process noise covariance is assumed to be time-invariant but unknown. In this case, standard filtering algorithms will collapse and some improvements should be made on them.

Problem 2.

At each time step k, given a set of sensors at different fixed locations,$\mathit{S}=\{{\mathit{s}}_1,{\mathit{s}}_2,\cdots ,{\mathit{s}}_{N_k}\}$,${\mathit{s}}_i=({\mathit{s}}_i^x,{\mathit{s}}_i^y),i=1,2,\cdots ,N_k$and the state estimates of the target${\mathit{x}}_k$, the sensor selection problem can be formulated as

$$\begin{array}{c}\underset{{\mathit{s}}_1,{\mathit{s}}_2}{\max }OD({\mathit{s}}_1,{\mathit{s}}_2)\\ \mathrm{subject}\mathrm{to}{\mathit{s}}_i\in \mathit{S},i=1,2.\end{array}$$

(6)

where$OD({\mathit{s}}_1,{\mathit{s}}_2)$denotes the observability degree of the BOT system when${\mathit{s}}_1,{\mathit{s}}_2$are selected to track the target.

Remark 1.

In this paper, we only consider the problem of tracking the target using the bearing measurements from two selected sensors, which is a special case of multi-sensor BOT. With the number of passive sensors increasing, the system observability and tracking performance will be enhanced gradually. Therefore, the two-sensor case seems to be the most difficult in multi-sensor BOT and worth studying.

3. Bayesian Filtering

The aim of BOT is to acquire estimates of the target state ${\mathit{x}}_k$ given a set of measurements ${\mathit{z}}_{1:k}$. Based on the posterior density $p({\mathit{x}}_{k-1}|{\mathit{z}}_{1:k-1})$ at time $k-1$, the predicted density of the state can be firstly evaluated through one-step prediction in Bayesian filtering techniques

$$p({\mathit{x}}_k|{\mathit{z}}_{1:k-1})={\displaystyle \int p({\mathit{x}}_k|{\mathit{x}}_{k-1})p({\mathit{x}}_{k-1}|{\mathit{z}}_{1:k-1})\mathrm{d}}{\mathit{x}}_{k-1}$$

(7)

and then the posterior density at time k can be computed recursively by

$$p({\mathit{x}}_k|{\mathit{z}}_{1:k})=\frac{p({\mathit{z}}_k|{\mathit{x}}_k)p({\mathit{x}}_k|{\mathit{z}}_{1:k-1})}{{\displaystyle \int p({\mathit{z}}_k|{\mathit{x}}_k)p({\mathit{x}}_k|{\mathit{z}}_{1:k-1})\mathrm{d}}{\mathit{x}}_k}$$

(8)

where $p({\mathit{x}}_k|{\mathit{x}}_{k-1})$ is the state transition density and $p({\mathit{z}}_k|{\mathit{x}}_k)$ is the measurement likelihood.

It is obvious that the multidimensional integrals in Equations (7) and (8) are intractable if nonlinear relationships exist in the system model. Since the measurement function in the BOT system is nonlinear, the traditional Kalman filter (KF) could not be directly applied and nonlinear solutions are needed. By linearizing the nonlinear function in the system model, the EKF has the similar filtering procedure to KF. However, since the nonlinearity of the measurement function (arc tangent) in BOT system is severe, the performance of the EKF will be degraded significantly.

Instead of approximating the nonlinear function in the system model, the kind of deterministic-point-based filters approximate the probability density distribution of the system state through a group of deterministic weighted points. Under the assumption that the state is following the Gaussian distribution, the main goal of deterministic-point-based filters is to approximately calculate the multidimensional integrals, which can be expressed as the product of a nonlinear function and a Gaussian probability density, as follows:

$$\begin{array}{cc}\hfill \mathit{I}(\mathit{g})& ={\displaystyle \int \mathit{g}(\mathit{x})\mathcal{N}(\mathit{x};\widehat{\mathit{x}},\mathit{P})\mathrm{d}\mathit{x}}\hfill \\ & ={\displaystyle \int \mathit{g}(\sqrt{\mathit{P}}\mathit{x}+\widehat{\mathit{x}})\mathcal{N}(\mathit{x};\mathbf{0},{\mathbf{I}}_n)\mathrm{d}\mathit{x}}\hfill \\ & ={\displaystyle \sum _{i=1}^{N_{\xi }}{\mathit{\omega }}_i\mathit{g}(\sqrt{\mathit{P}}{\mathit{\xi }}_i+\widehat{\mathit{x}})}\hfill \end{array}$$

(9)

where $\mathit{g}(\cdot )$ is an arbitrary nonlinear function and $\mathcal{N}(\mathit{x};\widehat{\mathit{x}},\mathit{P})$ denotes a Gaussian distribution with mean $\widehat{\mathit{x}}$ and covariance $\mathit{P}$. ${\xi }_i$ and ${\omega }_i$ are the chosen deterministic points and their corresponding weights, which can be generated by various rules, such as the unscented transform, the cubature rules, and the Gauss-Hermite quadrature rule. For more details of these rules, please refer to [15,16,18,19,20].

Taking both estimation precision and time complexity into account, we adopt the fifth-degree cubature rule [16] to approximate the multidimensional integrals. Since being proposed, this new rule for CKF has attracted a lot of attention in various research areas [31,32,33]. The set of $N_{\xi }=2n^2+1$ deterministically selected cubature points and their corresponding weights that are based on this rule are defined in Equations (10) and (11).

$${\mathit{\xi }}_i=\{\begin{array}{ll}{\left(0,0,\cdots ,0\right)}^{\mathrm{T}}\hfill & i=0\hfill \\ \sqrt{n+2}{\mathit{e}}_i& i=1,2,\cdots ,n\hfill \\ -\sqrt{n+2}{\mathit{e}}_{i-n}& i=n+1,\cdots ,2n\hfill \\ \sqrt{n+2}{\mathit{\eta }}_{i-2n}^{+}& i=2n+1,\cdots ,n(n+3)/2\hfill \\ -\sqrt{n+2}{\mathit{\eta }}_{i-n(n+3)/2}^{+}& i=n(n+3)/2+1,\cdots ,n(n+1)\hfill \\ \sqrt{n+2}{\mathit{\eta }}_{i-n(n+1)}^{-}& i=n(n+1)+1,\cdots ,n(3n+1)/2\hfill \\ -\sqrt{n+2}{\mathit{\eta }}_{i-n(3n+1)/2}^{-}& i=n(3n+1)/2+1,\cdots ,2n^2\hfill \end{array}$$

(10)

$${\omega }_i=\{\begin{array}{cc}\frac{2}{n+2}& i=0\hfill \\ \frac{4-n}{2{(n+2)}^2}& i=1,2,\cdots ,2n\hfill \\ \frac{1}{{(n+2)}^2}& i=2n+1,\cdots ,2n^2\hfill \end{array}$$

(11)

where n is the dimension of the state vector, ${\mathit{e}}_i\in {\mathbb{R}}^n$ is the unit vector with the ith component being 1, and

$$\left\{{\mathit{\eta }}_j^{+}\right\}=\left\{\sqrt{\frac{1}{2}}\left({\mathit{e}}_k+{\mathit{e}}_l\right):k<l,k,l=1,2,\cdots ,n\right\}$$

(12)

$$\left\{{\mathit{\eta }}_j^{-}\right\}=\left\{\sqrt{\frac{1}{2}}\left({\mathit{e}}_k-{\mathit{e}}_l\right):k<l,k,l=1,2,\cdots ,n\right\}$$

(13)

Another category of point-based filters, which choose points randomly, can be represented by PF. Although the performance is better, the heavy computational burden restricts their application in reality. For detailed rules of choosing points and the filtering process, refer to [34].

Being algebraically equivalent to the conventional filters, information filter has been widely used in multi-sensor information fusion. Instead of estimating the state $\widehat{\mathit{x}}$ and the covariance $\mathit{P}$, it computes the information states $\widehat{\mathit{y}}$ and information matrix $\mathit{Y}$ at each time step. The relationships between $\widehat{\mathit{x}},\mathit{P}$ and $\widehat{\mathit{y}},\mathit{Y}$ are given by

$$\widehat{\mathit{x}}={\mathit{Y}}^{-1}\widehat{\mathit{y}}$$

(14)

$$\mathit{P}={\mathit{Y}}^{-1}$$

(15)

4. Sensor Selection

In this section, a sensor selection strategy is developed to address the problem 2 introduced in Section 2. Based on the classical control theory and the condition number [30], an observability metric is derived as the criterion to select the best combination of sensors. The continuous kind of the multi-sensor BOT system model (1) and (2) is given by

$$\{\begin{array}{l}{\dot{\mathit{x}}}_p={\mathit{f}}_p({\mathit{x}}_p)={[\dot{x},\dot{y}]}^{\mathrm{T}}\\ \mathit{z}=\mathit{h}({\mathit{x}}_p)\end{array}$$

(16)

where ${\mathit{x}}_p={[x,y]}^{\mathrm{T}}$ denotes the position vector of the target and

$$\mathit{h}({\mathit{x}}_p)=\left[\begin{array}{c}{h}_1({\mathit{x}}_p)\\ h_2({\mathit{x}}_p)\end{array}\right]=\left[\begin{array}{c}{\beta }_1\\ {\beta }_2\end{array}\right]=\left[\begin{array}{c}\arctan (\frac{x-{\mathit{s}}_1^x}{y-{\mathit{s}}_1^y})\\ \arctan (\frac{x-{\mathit{s}}_2^x}{y-{\mathit{s}}_2^y})\end{array}\right]$$

The local nonlinear observability matrix of the multi-sensor BOT system can be computed as

$$\mathit{O}=\left[\begin{array}{c}\nabla {\mathcal{L}}_{f_p}^0(h_1)\\ \nabla {\mathcal{L}}_{f_p}^1(h_1)\\ ⋮\\ \nabla {\mathcal{L}}_{f_p}^0(h_2)\\ \nabla {\mathcal{L}}_{f_p}^1(h_2)\\ ⋮\end{array}\right]=\left[\begin{array}{cc}\frac{y-{\mathit{s}}_1^y}{d_1^2}& \frac{-(x-{\mathit{s}}_1^x)}{d_1^2}\\ {\mathit{O}}_{21}^{h_1}& {\mathit{O}}_{22}^{h_1}\\ ⋮& ⋮\\ \frac{y-{\mathit{s}}_2^y}{d_2^2}& \frac{-(x-{\mathit{s}}_2^x)}{d_2^2}\\ {\mathit{O}}_{21}^{h_2}& {\mathit{O}}_{22}^{h_2}\\ ⋮& ⋮\end{array}\right]$$

(17)

where $\nabla {\mathcal{L}}_{f_p}^n(h_i)$ represents the $n\text{-order}$ Lie derivative of $h_i$ along ${\mathit{f}}_p$ and

$$d_i=\sqrt{{(x-{\mathit{s}}_i^x)}^2+{(y-{\mathit{s}}_i^y)}^2},i=1,2$$

denotes the relative distance between the target and the ith sensor.

For simplicity, we neglect ${\mathit{O}}_{21}^{h_1}$, ${\mathit{O}}_{22}^{h_1}$, ${\mathit{O}}_{21}^{h_2}$, ${\mathit{O}}_{22}^{h_2}$, and other high-order terms, then the observability matrix can be simplified as

$$\mathit{O}=\left[\begin{array}{cc}\frac{y-{\mathit{s}}_1^y}{d_1^2}& \frac{-(x-{\mathit{s}}_1^x)}{d_1^2}\\ \frac{y-{\mathit{s}}_2^y}{d_2^2}& \frac{-(x-{\mathit{s}}_2^x)}{d_2^2}\end{array}\right]$$

(18)

Through some basic trigonometric transform, Equation (18) can be rewritten as

$$\mathit{O}=\left[\begin{array}{cc}\frac{\cos {\beta }_1}{d_1}& \frac{-\sin {\beta }_1}{d_1}\\ \frac{\cos {\beta }_2}{d_2}& \frac{-\sin {\beta }_2}{d_2}\end{array}\right]$$

(19)

The range information is weakly locally observable if the observability matrix $\mathit{O}$ has full column rank. However, the rank could not tell us how well the tracking system is observable. In order to solve Problem 2, we use the inverse of the condition number of $\mathit{O}$, defined as the ratio of the smallest singular value to the largest one, given by

$$OD=C^{-1}(\mathit{O})=\frac{{\sigma }_{\min }(\mathit{O})}{{\sigma }_{\max }(\mathit{O})}$$

(20)

to describe the observability degree of the BOT system when ${\mathit{s}}_1,{\mathit{s}}_2$ are used for tracking the target. A larger $OD$ means a better observability and $OD\in [0,1]$.

The singular values of $\mathit{O}$ are equal to the square-root of the eigenvalues of the symmetric matrix, given as

$$\mathbb{O}={\mathit{O}}^{\mathrm{T}}\mathit{O}$$

(21)

and

$$\{\begin{array}{c}{\sigma }_{\min }(\mathit{O})=\sqrt{{\lambda }_{\min }(\mathbb{O})}\\ {\sigma }_{\max }(\mathit{O})=\sqrt{{\lambda }_{\max }(\mathbb{O})}\end{array}$$

(22)

After some fundamental calculation, we can obtain the singular values of the observability matrix for the multi-sensor BOT system, as follows:

$$\begin{array}{c}{\mathit{\sigma }}_{\min }(\mathit{O})=\sqrt{\frac{(d_1^2+d_2^2)-\sqrt{{(d_1^2+d_2^2)}^2-4d_1^2{d}_2^2{\sin }^2({\beta }_1-{\beta }_2)}}{2d_1^2{d}_2^2}}\\ {\mathit{\sigma }}_{\max }(\mathit{O})=\sqrt{\frac{(d_1^2+d_2^2)+\sqrt{{(d_1^2+d_2^2)}^2-4d_1^2{d}_2^2{\sin }^2({\beta }_1-{\beta }_2)}}{2d_1^2{d}_2^2}}\end{array}$$

(23)

By substituting Equation (23) into Equation (20), the observability metric, which is used to select the best pair of sensors, can be computed, as follows

$$OD=\sqrt{\frac{(d_1^2+d_2^2)-\sqrt{{(d_1^2+d_2^2)}^2-4d_1^2{d}_2^2{\sin }^2({\beta }_1-{\beta }_2)}}{(d_1^2+d_2^2)+\sqrt{{(d_1^2+d_2^2)}^2-4d_1^2{d}_2^2{\sin }^2({\beta }_1-{\beta }_2)}}}$$

(24)

At time step k, the OD of each possible couple of sensors is calculated and the one with the largest value will be selected to track the target. Since the true target state is not usually available, we use the one-step prediction of the state, namely ${\mathit{x}}_{k|k-1}$, to compute OD.

Remark 2.

It is obvious that$OD=0$when${\beta }_1={\beta }_2$or${\beta }_1={\beta }_2+\pi$, which means that the selected pair of sensors should not at the same position or on a straight line with the target.

Remark 3.

This sensor selection strategy can help improve the observability of the tracking system as well as enhance the filtering precision. Besides, well-estimated state ensures that the process noise covariance estimation which is introduced in Section 5 could converge to the true value quickly.

5. Adaptive Fifth-Degree Cubature Information Filter

The fifth-degree cubature information filter (FCIF) is algebraically equivalent to the fifth-degree CKF. In this section, two improvements, including the sensor selection strategy that was developed in Section 4 and a recursive process noise covariance estimation strategy are introduced into FCIF, thus both Problems 1 and 2 can be solved.

Assume that the posterior state and covariance estimates at time $k-1$ are ${\widehat{\mathit{x}}}_{k-1|k-1}$ and ${\mathit{P}}_{k-1|k-1}$, the proposed AFCIF algorithm incorporates the following parts.

5.1. Time Update

The one-step prediction of the information state and covariance are given by

$${\widehat{\mathit{y}}}_{k|k-1}={\mathit{Y}}_{k|k-1}{\widehat{\mathit{x}}}_{k|k-1}={\mathit{Y}}_{k|k-1}\mathit{F}{\widehat{\mathit{x}}}_{k-1|k-1}$$

(25)

$${\mathit{Y}}_{k|k-1}={\mathit{P}}_{k|k-1}^{-1}={(\mathit{F}{\mathit{P}}_{k-1|k-1}{\mathit{F}}^{\mathrm{T}}+{\mathit{Q}}_{k-1})}^{-1}$$

(26)

5.2. Measurement Update with Sensor Selection

In order to improve the observability of the range information and the precision of the target state estimation, the sensor selection strategy that was constructed in last section will be employed. By using this strategy, a pair of sensors that has the largest value of $C^{-1}(\mathit{O})$ will be selected and the azimuth measurements that were received by them will be utilized to update the state and covariance.

Assume that the sensor network has $N_k$ individual sensors at time k and there will be $\frac{N_k(N_k-1)}{2}$ kinds of combinations, including $({\mathit{s}}_1,{\mathit{s}}_2),({\mathit{s}}_1,{\mathit{s}}_3),\cdots ,({\mathit{s}}_1,{\mathit{s}}_{{\mathit{N}}_k}),\cdots ,({\mathit{s}}_{{\mathit{N}}_k-1},{\mathit{s}}_{{\mathit{N}}_k})$, then we calculate $C^{-1}(\mathit{O})$ of all combinations and choose the one that achieves the largest value.

In the measurement update, the generated cubature points and propagated ones are given by

$${\mathit{\chi }}_{i,k|k-1}=\sqrt{{\mathit{P}}_{k|k-1}}{\mathit{\xi }}_i+{\widehat{\mathit{x}}}_{k|k-1}$$

(27)

$${\mathit{z}}_{i,k|k-1}={\mathit{h}}_k({\mathit{\chi }}_{i,k|k-1})$$

(28)

the predicted measurement is

$${\widehat{\mathit{z}}}_{k|k-1}={\displaystyle \sum _{i=1}^{2n^2+1}{\omega }_i{\mathit{z}}_{i,k|k-1}}$$

(29)

According to [19], once the measurement at time k is available, the filtered information state and the covariance can be computed as

$${\widehat{\mathit{y}}}_{k|k}={\widehat{\mathit{y}}}_{k|k-1}+{\mathit{Y}}_{k|k-1}{\mathit{P}}_{k|k-1}^{xz}{\mathit{R}}^{-1}\left[{\mathit{z}}_k-{\mathit{h}}_k({\widehat{\mathit{x}}}_{k|k-1})+{\mathit{P}}_{k|k-1}^{xz,\mathrm{T}}{\mathit{Y}}_{k|k-1}^{\mathrm{T}}{\widehat{\mathit{x}}}_{k|k-1}\right]$$

(30)

$${\mathit{Y}}_{k|k}={\mathit{Y}}_{k|k-1}+{\mathit{Y}}_{k|k-1}{\mathit{P}}_{k|k-1}^{xz}{\mathit{R}}^{-1}{\mathit{P}}_{k|k-1}^{xz,\mathrm{T}}{\mathit{Y}}_{k|k-1}^{\mathrm{T}}$$

(31)

where the cross covariance matrix is given by

$${\mathit{P}}_{k|k-1}^{xz}={\displaystyle \sum _{i=1}^{2n^2+1}{\omega }_i({\mathit{\chi }}_{i,k|k-1}-{\widehat{\mathit{x}}}_{k|k-1}){({\mathit{z}}_{i,k|k-1}-{\widehat{\mathit{z}}}_{k|k-1})}^{\mathrm{T}}}$$

(32)

At last the state and covariance can be recovered by

$${\widehat{\mathit{x}}}_{k|k}={\mathit{Y}}_{k|k}^{-1}{\widehat{\mathit{y}}}_{k|k}$$

(33)

$${\mathit{P}}_{k|k}={\mathit{Y}}_{k|k}^{-1}$$

(34)

5.3. Recursive Estimation of Process Noise Covariance

In reality, statistic of the process noise is not usually available as described in Problem 1, so we build a recursive estimation strategy through covariance matching principle.

Consider one-step prediction of state ${\widehat{\mathit{x}}}_{k|k-1}$ at time $k-1$ and the filtered state ${\widehat{\mathit{x}}}_{k|k}$ at time k, the residual between them can be represented by

$${\mathit{\zeta }}_k={\widehat{\mathit{x}}}_{k|k}-{\widehat{\mathit{x}}}_{k|k-1}$$

(35)

Given the residual data from time $k-N+1$ to time k, the mean and the covariance of ${\mathit{\zeta }}_k$ can be estimated by

$${\overline{\mathit{\zeta }}}_k=\frac{1}{N}{\displaystyle \sum _{i=k-N+1}^k{\mathit{\zeta }}_i}$$

(36)

$${\mathit{\Sigma }}_{{\mathit{\zeta }}_k}=\frac{1}{N-1}{\displaystyle \sum _{i=k-N+1}^k\left({\mathit{\zeta }}_i-{\overline{\mathit{\zeta }}}_k\right){\left({\mathit{\zeta }}_i-{\overline{\mathit{\zeta }}}_k\right)}^{\mathrm{T}}}$$

(37)

where N is an adjustable parameter. Inspired by the measurement noise covariance update rule derived in [24], we can obtain the following results.

Theorem 1.

Under the premise that the covariance of process noise stay constant, the recursive relation between${\mathit{Q}}_{k-1}$and${\mathit{Q}}_k$can be described as

$${\mathit{Q}}_k=\frac{N-1}{N}{\mathit{Q}}_{k-1}+\mathsf{\Delta }{\mathit{Q}}_k$$

(38)

where

$$\mathsf{\Delta }{\mathit{Q}}_k=\frac{1}{N-1}\left({\mathit{\zeta }}_k-{\overline{\mathit{\zeta }}}_k\right){\left({\mathit{\zeta }}_k-{\overline{\mathit{\zeta }}}_k\right)}^{\mathrm{T}}-\frac{1}{N}\left(\mathit{F}{\mathit{P}}_{k-1|k-1}{\mathit{F}}^{\mathrm{T}}-{\mathit{P}}_{k|k}\right)$$

(39)

Proof. 

Under the condition that the process noise covariance is constant, the expectation of Equation (37) is given by [22]

$$\mathbb{E}({\mathit{\Sigma }}_{\mathit{\zeta }})=\frac{1}{N}{\displaystyle \sum _{i=k-N+1}^k(\mathit{F}{\mathit{P}}_{i-1|i-1}{\mathit{F}}^{\mathrm{T}}-{\mathit{P}}_{i|i})}+{\mathit{Q}}_k$$

(40)

Combine Equations (37) and (40), the covariance of process noise can be approximately calculated by

$${\mathit{Q}}_k=\frac{1}{N-1}{\displaystyle \sum _{i=k-N+1}^k({\mathit{\zeta }}_i-{\overline{\mathit{\zeta }}}_k){({\mathit{\zeta }}_i-{\overline{\mathit{\zeta }}}_k)}^{\mathrm{T}}}-\frac{1}{N}{\displaystyle \sum _{i=k-N+1}^k(\mathit{F}{\mathit{P}}_{i-1|i-1}{\mathit{F}}^{\mathrm{T}}-{\mathit{P}}_{i|i})}$$

(41)

Similarly, ${\mathit{Q}}_{k-1}$ can be computed as

$${\mathit{Q}}_{k-1}=\frac{1}{N-2}{\displaystyle \sum _{i=k-N+1}^{k-1}({\mathit{\zeta }}_i-{\overline{\mathit{\zeta }}}_k){({\mathit{\zeta }}_i-{\overline{\mathit{\zeta }}}_k)}^{\mathrm{T}}}-\frac{1}{N-1}{\displaystyle \sum _{i=k-N+1}^{k-1}(\mathit{F}{\mathit{P}}_{i-1|i-1}{\mathit{F}}^{\mathrm{T}}-{\mathit{P}}_{i|i})}$$

(42)

Through some simple mathematical transformation, Equation (41) can be rewritten as the following form

$${\mathit{Q}}_k=\frac{N-1}{N}{\mathit{X}}_k+\mathsf{\Delta }{\mathit{Q}}_k$$

(43)

where

$${\mathit{X}}_k=\frac{N}{{(N-1)}^2}{\displaystyle \sum _{i=k-N+1}^{k-1}({\mathit{\zeta }}_i-{\overline{\mathit{\zeta }}}_k){({\mathit{\zeta }}_i-{\overline{\mathit{\zeta }}}_k)}^{\mathrm{T}}}-\frac{1}{N-1}{\displaystyle \sum _{i=k-N+1}^{k-1}(\mathit{F}{\mathit{P}}_{i-1|i-1}{\mathit{F}}^{\mathrm{T}}-{\mathit{P}}_{i|i})}$$

(44)

If N is large enough, the difference between $N/{(N-1)}^2$ and $1/(N-2)$ is negligible. So, ${\mathit{X}}_k$ can be approximated as

$${\mathit{X}}_k\approx \frac{1}{N-2}{\displaystyle \sum _{i=k-N+1}^{k-1}({\mathit{\zeta }}_i-{\overline{\mathit{\zeta }}}_k){({\mathit{\zeta }}_i-{\overline{\mathit{\zeta }}}_k)}^{\mathrm{T}}}-\frac{1}{N-1}{\displaystyle \sum _{i=k-N+1}^{k-1}(\mathit{F}{\mathit{P}}_{i-1|i-1}{\mathit{F}}^{\mathrm{T}}-{\mathit{P}}_{i|i})}$$

(45)

Obviously, the above equation has the equivalent form with ${\mathit{Q}}_{k-1}$, therefore we can obtain Equation (38). This completes the proof. Similarly, the recursive estimation equation of ${\overline{\mathit{\zeta }}}_k$ is given by

$${\overline{\mathit{\zeta }}}_k=\frac{N-1}{N}{\overline{\mathit{\zeta }}}_{k-1}+\frac{1}{N}{\mathit{\zeta }}_k$$

(46)

So, we can use Equations (35), (38), (39) and (46) to update the covariance of process noise. □

Remark 4.

The parameter N in Equations (38) and (46) needs to be adjusted when this noise covariance estimation strategy is used in reality. A large N means that relatively accurate statistical information of the process noise can be obtained and the updated${\mathit{Q}}_k$is very close to${\mathit{Q}}_{k-1}$. A small N, however, indicates that there is no knowledge of the process noise available and${\mathit{Q}}_k$is more dependent on the newly estimated state.

5.4. Adaptive Fifth-Degree Cubature Information Filter for Multi-Sensor Estimation

When compared with the conventional Bayesian filters, the information-form ones are preferred in multi-sensor tracking due to the simpler measurement update process. In the information filter, the measurements received by the sensors at different locations can be fused by adding each measurement contribution to the information state and matrix, and then the filtered information state and matrix can be calculated as

$${\widehat{\mathit{y}}}_{k|k}={\widehat{\mathit{y}}}_{k|k-1}+{\displaystyle \sum _{i=1}^2{\mathit{P}}_{k|k-1}^{-1}{\mathit{P}}_{i,k|k-1}^{xz}{\mathit{R}}_i^{-1}\left[{\mathit{z}}_{k,i}-h_{k,i}({\widehat{\mathit{x}}}_{k|k-1})+{\mathit{P}}_{i,k|k-1}^{xz,\mathrm{T}}{\mathit{P}}_{k|k-1}^{-\mathrm{T}}{\widehat{\mathit{x}}}_{k|k-1}\right]}$$

(47)

$${\mathit{Y}}_{k|k}={\mathit{Y}}_{k|k-1}+{\displaystyle \sum _{i=1}^2{\mathit{P}}_{k|k-1}^{-1}{\mathit{P}}_{i,k|k-1}^{xz}{\mathit{R}}_i^{-1}{\mathit{P}}_{i,k|k-1}^{xz,\mathrm{T}}{\mathit{P}}_{k|k-1}^{-\mathrm{T}}}$$

(48)

where ${\mathit{P}}_{i,k|k-1}^{xz}$ is the ith column of ${\mathit{P}}_{k|k-1}^{xz}$, ${\mathit{R}}_i$ is the measurement noise covariance of the ith sensor and ${\mathit{z}}_{k,i}$ is the measurement of the ith sensor at time k.

6. Convergence Analysis

Now, we analyze the convergence of the novel algorithm when used in multi-sensor BOT for state estimation. Firstly, we define the posterior state estimation error, the predicted state error and measurement error at time k, respectively, by

$${\tilde{\mathit{x}}}_k={\mathit{x}}_k-{\widehat{\mathit{x}}}_{k|k}$$

(49)

$${\tilde{\mathit{x}}}_{k|k-1}={\mathit{x}}_k-{\widehat{\mathit{x}}}_{k|k-1}$$

(50)

$${\tilde{\mathit{z}}}_k={\mathit{z}}_k-{\mathit{h}}_k({\widehat{\mathit{x}}}_{k|k-1})$$

(51)

and the relationships between them are described, as follows

$${\tilde{\mathit{x}}}_k={\tilde{\mathit{x}}}_{k|k-1}-{\mathit{K}}_k{\tilde{\mathit{z}}}_k$$

(52)

$${\tilde{\mathit{x}}}_{k|k-1}=\mathit{F}{\tilde{\mathit{x}}}_{k-1}$$

(53)

For simplicity, Equation (51) can be transformed into the following linearized form

$${\tilde{\mathit{z}}}_k={\mathit{\alpha }}_k{\mathit{H}}_k{\widehat{\mathit{x}}}_{k|k-1}$$

(54)

where ${\mathit{\alpha }}_k$ is a diagonal matrix. Since the CKF is a kind of derivative-free algorithm and no linearization error can be produced, we use this exact form just for the convenience of theory analysis. Here, instead of ${\mathit{H}}_k$, ${\mathit{\alpha }}_k{\mathit{H}}_k$ becomes the measurement matrix.

Theorem 2.

The proposed AFCIF algorithm is stable and can guarantee the estimation of the multi-sensor BOT system convergent if the following conditions hold

• The filtered covariance matrix is bounded.

  $$a{\mathbf{I}}_n\le {\mathit{P}}_{k-1|k-1}^{-1}\le b{\mathbf{I}}_n$$

  (55)
• The state transition matrix is invertible and bounded.

  $$\Vert \mathit{F}\Vert \le c$$

  (56)
• The updated process noise covariance satisfies

  $$(1-\lambda )\underset{\_}{\sigma }(\mathit{F}{\mathit{P}}_{k-1|k-1}{\mathit{F}}^{\mathrm{T}}+{\mathit{Q}}_{k-1})\ge \overline{\sigma }{(\mathit{F})}^2\overline{\sigma }({\mathit{P}}_{k-1|k-1})$$

  (57)

  where $a,b,c$ are positive real numbers, $0<\lambda <1$, $\underset{\_}{\sigma }$, and $\overline{\sigma }$ denote the minimum and maximum singular values.

Proof. 

The Lyapunov function at time k is defined as

$$\mathit{V}({\tilde{\mathit{x}}}_k)={\tilde{\mathit{x}}}_k^{\mathrm{T}}{\mathit{P}}_{k|k}^{-1}{\tilde{\mathit{x}}}_k$$

(58)

Similarly,

$$\mathit{V}({\tilde{\mathit{x}}}_{k-1})={\tilde{\mathit{x}}}_{k-1}^{\mathrm{T}}{\mathit{P}}_{k-1|k-1}^{-1}{\tilde{\mathit{x}}}_{k-1}$$

(59)

and it is obvious that $\mathit{V}(\mathbf{0})=\mathbf{0}$.

The Kalman gain and the inverse of the error covariance can be calculated as

$${\mathit{K}}_k={\mathit{P}}_{k|k-1}{\mathit{H}}_k^{\mathrm{T}}{\mathit{\alpha }}_k{({\mathit{\alpha }}_k{\mathit{H}}_k{\mathit{P}}_{k|k-1}{\mathit{H}}_k^{\mathrm{T}}{\mathit{\alpha }}_k+\mathit{R})}^{-1}={\mathit{P}}_{k|k}{\mathit{H}}_k^{\mathrm{T}}{\mathit{\alpha }}_k{\mathit{R}}^{-1}$$

(60)

$${\mathit{P}}_{k|k}^{-1}={\mathit{P}}_{k|k-1}^{-1}+{\mathit{H}}_k^{\mathrm{T}}{\mathit{\alpha }}_k{\mathit{R}}^{-1}{\mathit{\alpha }}_k{\mathit{H}}_k$$

(61)

$\mathit{V}({\tilde{\mathit{x}}}_k)$ becomes

$$\mathit{V}({\tilde{\mathit{x}}}_k)={\tilde{\mathit{x}}}_{k|k-1}^{\mathrm{T}}{\mathit{P}}_{k|k}^{-1}{\tilde{\mathit{x}}}_{k|k-1}-{\tilde{\mathit{x}}}_{k|k-1}^{\mathrm{T}}{\mathit{H}}_k^{\mathrm{T}}{\mathit{\alpha }}_k{\mathit{R}}^{-1}{\tilde{\mathit{z}}}_k-{\tilde{\mathit{z}}}_k^{\mathrm{T}}{\mathit{R}}^{-1}{\mathit{\alpha }}_k{\mathit{H}}_k{\tilde{\mathit{x}}}_{k|k-1}+{\tilde{\mathit{z}}}_k^{\mathrm{T}}{\mathit{R}}^{-1}{\mathit{\alpha }}_k{\mathit{H}}_k{\mathit{P}}_{k|k}{\mathit{H}}_k^{\mathrm{T}}{\mathit{\alpha }}_k{\mathit{R}}^{-1}{\tilde{\mathit{z}}}_k$$

(62)

by substituting (60) into (52) and (52) into (58). Then, substitute (61) into (62), we obtain

$$\begin{array}{ll}\hfill \mathit{V}({\tilde{\mathit{x}}}_k)=& {\tilde{\mathit{x}}}_{k|k-1}^{\mathrm{T}}{\mathit{P}}_{k|k-1}^{-1}{\tilde{\mathit{x}}}_{k|k-1}+{\tilde{\mathit{x}}}_{k|k-1}^{\mathrm{T}}{\mathit{H}}_k^{\mathrm{T}}{\mathit{\alpha }}_k{\mathit{R}}^{-1}{\mathit{\alpha }}_k{\mathit{H}}_k{\tilde{\mathit{x}}}_{k|k-1}\hfill \\ & -{\tilde{\mathit{x}}}_{k|k-1}^{\mathrm{T}}{\mathit{H}}_k^{\mathrm{T}}{\mathit{\alpha }}_k{\mathit{R}}^{-1}{\tilde{\mathit{z}}}_k-{\tilde{\mathit{z}}}_k^{\mathrm{T}}{\mathit{R}}^{-1}{\mathit{\alpha }}_k{\mathit{H}}_k{\tilde{\mathit{x}}}_{k|k-1}+{\tilde{\mathit{z}}}_k^{\mathrm{T}}{\mathit{R}}^{-1}{\mathit{\alpha }}_k{\mathit{H}}_k{\mathit{P}}_{k|k}{\mathit{H}}_k^{\mathrm{T}}{\mathit{\alpha }}_k{\mathit{R}}^{-1}{\tilde{\mathit{z}}}_k\hfill \end{array}$$

(63)

Combined with (26), (53), and (54), (63) becomes

$$\begin{array}{ll}\hfill \mathit{V}({\tilde{\mathit{x}}}_k)& ={\tilde{\mathit{x}}}_{k-1}^{\mathrm{T}}{\mathit{F}}^{\mathrm{T}}{(\mathit{F}{\mathit{P}}_{k-1|k-1}{\mathit{F}}^{\mathrm{T}}+{\mathit{Q}}_{k-1})}^{-1}\mathit{F}{\tilde{\mathit{x}}}_{k-1}-{\tilde{\mathit{z}}}_k^{\mathrm{T}}({\mathit{R}}^{-1}-{\mathit{R}}^{-1}{\mathit{\alpha }}_k{\mathit{H}}_k{\mathit{P}}_{k|k}{\mathit{H}}_k^{\mathrm{T}}{\mathit{\alpha }}_k{\mathit{R}}^{-1}){\tilde{\mathit{z}}}_k\hfill \\ & ={\tilde{\mathit{x}}}_{k-1}^{\mathrm{T}}{\mathit{F}}^{\mathrm{T}}{(\mathit{F}{\mathit{P}}_{k-1|k-1}{\mathit{F}}^{\mathrm{T}}+{\mathit{Q}}_{k-1})}^{-1}\mathit{F}{\tilde{\mathit{x}}}_{k-1}-{\tilde{\mathit{z}}}_k^{\mathrm{T}}{\mathit{P}}_{\mathit{z}\mathit{z}}^{-1}{\tilde{\mathit{z}}}_k\hfill \\ & \le {\tilde{\mathit{x}}}_{k-1}^{\mathrm{T}}{\mathit{F}}^{\mathrm{T}}{(\mathit{F}{\mathit{P}}_{k-1|k-1}{\mathit{F}}^{\mathrm{T}}+{\mathit{Q}}_{k-1})}^{-1}\mathit{F}{\tilde{\mathit{x}}}_{k-1}\hfill \end{array}$$

(64)

where ${\mathit{R}}^{-1}-{\mathit{R}}^{-1}{\mathit{\alpha }}_k{\mathit{H}}_k{\mathit{P}}_{k|k}{\mathit{H}}_k^{\mathrm{T}}{\mathit{\alpha }}_k{\mathit{R}}^{-1}={\mathit{P}}_{\mathit{z}\mathit{z}}^{-1}$, as shown in [12]. Then according to (57), (59), and (64),

$$\mathit{V}({\tilde{\mathit{x}}}_k)-(1-\lambda )\mathit{V}({\tilde{\mathit{x}}}_{k-1})\le 0$$

(65)

further

$$0\le a{\tilde{\mathit{x}}}_k^{\mathrm{T}}{\tilde{\mathit{x}}}_k\le \mathit{V}({\tilde{\mathit{x}}}_k)\le {(1-\lambda )}^k\mathit{V}({\tilde{\mathit{x}}}_0)$$

(66)

Since

$$0\le a\underset{k\to \infty }{\lim }({\tilde{\mathit{x}}}_k^{\mathrm{T}}{\tilde{\mathit{x}}}_k)\le \underset{k\to \infty }{\lim }\mathit{V}({\tilde{\mathit{x}}}_k)\le \mathit{V}({\tilde{\mathit{x}}}_0)\underset{k\to \infty }{\lim }{(1-\lambda )}^k=0$$

(67)

we can obtain

$$\underset{k\to \infty }{\lim }({\widehat{\mathit{x}}}_k-{\mathit{x}}_k)=0$$

(68)

Therefore, the state estimation error will converge to 0 over time. This completes the proof. □

Remark 5.

The sensor selection strategy helps make the tracking system locally observable, which ensures that Condition I is satisfied. Condition II is naturally satisfied according to Equation (3).

Remark 6.

An easy way to keep the filtering stability is to use a sufficiently large${\mathit{Q}}_{k-1}$. However, this may cause degenerated estimation performance. On the contrary, a small${\mathit{Q}}_{k-1}$could lead to unstable performance, even though it may produce better filtering precision at some time. The proposed AFCIF algorithm can provide accurate estimation of${\mathit{Q}}_k$as$k\to \infty$, thus both the precision and stability can be guaranteed.

7. Simulation Analysis

In this section, the proposed AFCIF algorithm is compared with FCIF through 500 Monte Carlo runs in a multi-sensor BOT scenario. The motion of the target follows the dynamic equation that is described by (1). The sample interval $\mathsf{\Delta }\mathrm{T}=1\mathrm{s}$ and the target’s motion lasts for 150 s. The initial target state is ${\mathit{x}}_0={[-10\mathrm{km},10\mathrm{km},50\mathrm{m}/\mathrm{s},-100\mathrm{m}/\mathrm{s}]}^{\mathrm{T}}$ and the initial covariance is set to be ${\mathit{P}}_0=\mathrm{diag}([{10}^4{\mathrm{m}}^2,{10}^4{\mathrm{m}}^2,{10}^2{\mathrm{m}}^2/{\mathrm{s}}^2,{10}^2{\mathrm{m}}^2/{\mathrm{s}}^2])$. The initial state estimate ${\widehat{\mathit{x}}}_0$ follows the Gaussian distribution $\mathcal{N}({\widehat{\mathit{x}}}_0;{\mathit{x}}_0,{\mathit{P}}_0)$. We assume that a group of four passive sensors are ready to track the target using bearing measurements and only two of them will be selected at each time. The fixed localizations of the four sensors are given by

$$\begin{array}{l}{\mathit{s}}_1^x=4\mathrm{km},{\mathit{s}}_1^y=-10\mathrm{km}{\mathit{s}}_2^x=5\mathrm{km},{\mathit{s}}_2^y=-7\mathrm{km}\\ {\mathit{s}}_3^x=0\mathrm{km},{\mathit{s}}_3^y=-10\mathrm{km}{\mathit{s}}_4^x=-8\mathrm{km},{\mathit{s}}_4^y=-7\mathrm{km}\end{array}$$

The measurement noises of all the sensors are assumed to be white Gaussian with covariance ${\mathit{R}}_i={({0.1}^{\circ })}^2,i=1,2,3,4$. The process noise is also assumed to be zero-mean Gaussian with unknown covariance ${\mathit{Q}}_{\mathrm{true}}=\mathrm{diag}([100{\mathrm{m}}^2,100{\mathrm{m}}^2,1{\mathrm{m}}^2\cdot {\mathrm{s}}^{-2},1{\mathrm{m}}^2\cdot {\mathrm{s}}^{-2}])$. The prior process noise covariance is set to be ${\mathit{Q}}_{\mathrm{big}}=100{\mathit{Q}}_{\mathrm{true}}$ and ${\mathit{Q}}_{\mathrm{small}}=0.01{\mathit{Q}}_{\mathrm{true}}$. Under these two initializations, we conduct simulations using the standard FCIF and the proposed AFCIF with different combinations of sensors.

We use the root-mean-square error (RMSE) as our performance metric to compare the accuracy and stability of these two algorithms. The RMSE of position at time k is defined as

$$\mathrm{RMS}{\mathrm{E}}_k^{\mathrm{pos}}=\sqrt{\frac{1}{M}{\displaystyle \sum _{i=1}^M{({\widehat{x}}_k^i-x_k^i)}^2+{({\widehat{y}}_k^i-y_k^i)}^2}}$$

(69)

where M is the overall number of Monte Carlo runs, $(x_k^i,y_k^i)$ and $({\widehat{x}}_k^i,{\widehat{y}}_k^i)$ are the true position and estimated position of the target, respectively, in the ith run at time k. Similarly, the RMSE of velocity at time k is defined as

$$\mathrm{RMS}{\mathrm{E}}_k^{\mathrm{vel}}=\sqrt{\frac{1}{M}{\displaystyle \sum _{i=1}^M{({\widehat{\dot{x}}}_k^i-{\dot{x}}_k^i)}^2+{({\widehat{\dot{y}}}_k^i-{\dot{y}}_k^i)}^2}}$$

(70)

where $({\dot{x}}_k^i,{\dot{y}}_k^i)$ and $({\widehat{\dot{x}}}_k^i,{\widehat{\dot{y}}}_k^i)$ are true velocity and estimated velocity of the target, respectively, in the ith run at time k.

The RMSE of process noise covariance is defined as

$$\mathrm{RMS}{\mathrm{E}}_k^{\mathrm{cov}}=\sqrt{\frac{1}{M}{\displaystyle \sum _{i=1}^M{\displaystyle \sum _{j=1}^4{({\widehat{\mathit{Q}}}_{k,i}^{jj}-{\mathit{Q}}_{true}^{jj})}^2}}}$$

(71)

where ${\mathit{Q}}_{\mathrm{true}}^{jj}$ and ${\widehat{\mathit{Q}}}_{k,i}^{jj}$ represent the jth component of the true process noise covariance and the estimated one in the ith run at time k. Here, without considering units, we define this $\mathrm{RMS}{\mathrm{E}}_k^{\mathrm{cov}}$ by integrating each diagonal element in the covariance matrix.

In the target tracking literature, the posterior Cramér-Rao lower bound (PCRLB) has been extensively used as an indication of the best possible performance that a filtering algorithm can get close to, but typically not achieve. The PCRLB for the RMSE of position and velocity at time k are computed, as follows:

$$\mathrm{PCRLB}(\mathrm{RMS}{\mathrm{E}}_k^{\mathrm{pos}})=\sqrt{{\mathit{J}}_k^{-1}(1,1)+{\mathit{J}}_k^{-1}(2,2)}$$

(72)

$$\mathrm{PCRLB}(\mathrm{RMS}{\mathrm{E}}_k^{\mathrm{vel}})=\sqrt{{\mathit{J}}_k^{-1}(3,3)+{\mathit{J}}_k^{-1}(4,4)}$$

(73)

where ${\mathit{J}}_k^{-1}(i,i)$ denotes the ith element of the principal diagonal of ${\mathit{J}}_k^{-1}$. In [35], Tichavský developed the recursive formula below for calculating the Fisher information matrix, namely ${\mathit{J}}_k$, at each time step:

$${\mathit{J}}_k={\mathit{D}}_k^{22}-{\mathit{D}}_k^{21}{[{\mathit{J}}_{k-1}+{\mathit{D}}_k^{11}]}^{-1}{\mathit{D}}_k^{12}$$

(74)

where

$$\begin{array}{l}{\mathit{D}}_k^{11}=\mathbb{E}\left\{{\mathit{F}}_k^{\mathrm{T}}{\mathit{Q}}_k^{-1}{\mathit{F}}_k\right\}={\mathit{F}}^{\mathrm{T}}{\mathit{Q}}_{\mathrm{true}}^{-1}\mathit{F}\\ {\mathit{D}}_k^{12}=-\mathbb{E}\left\{{\mathit{F}}_k^{\mathrm{T}}{\mathit{Q}}_k^{-1}\right\}=-{\mathit{F}}^{\mathrm{T}}{\mathit{Q}}_{\mathrm{true}}^{-1}\\ {\mathit{D}}_k^{21}=-\mathbb{E}\left\{{\mathit{Q}}_k^{-1}{\mathit{F}}_k\right\}=-{\mathit{Q}}_{\mathrm{true}}^{-1}\mathit{F}\\ {\mathit{D}}_k^{22}=\mathbb{E}\left\{{\mathit{H}}_k^{\mathrm{T}}{\mathit{R}}_k^{-1}{\mathit{H}}_k\right\}+{\mathit{Q}}_k^{-1}={\mathit{H}}_k^{\mathrm{T}}{\mathit{R}}^{-1}{\mathit{H}}_k+{\mathit{Q}}_{\mathrm{true}}^{-1}\end{array}$$

and ${\mathit{H}}_k$ is the Jacobian of the measurement function.

The simulation results are illustrated in Figure 2, Figure 3, Figure 4, Figure 5, Figure 6, Figure 7, Figure 8 and Figure 9. The PCRLB curves in Figure 2, Figure 4 and Figure 3, Figure 5 denote the posterior Cramér-Rao lower bounds for the RMSEs of position and velocity, respectively. The PCRLB curves shown in the figures are based on the situation when all the sensors are selected for tracking the target. Hence, the Jacobian of the measurement function used for computing the PCRLBs is given by

$${\mathit{H}}_k=\left[\begin{array}{cccc}\frac{y_k-{\mathit{s}}_1^y}{d_{k,1}^2}& \frac{-(x_k-{\mathit{s}}_1^x)}{d_{k,1}^2}& 0& 0\\ \frac{y_k-{\mathit{s}}_2^y}{d_{k,2}^2}& \frac{-(x_k-{\mathit{s}}_2^x)}{d_{k,2}^2}& 0& 0\\ \frac{y_k-{\mathit{s}}_3^y}{d_{k,3}^2}& \frac{-(x_k-{\mathit{s}}_3^x)}{d_{k,3}^2}& 0& 0\\ \frac{y_k-{\mathit{s}}_4^y}{d_{k,4}^2}& \frac{-(x_k-{\mathit{s}}_4^x)}{d_{k,4}^2}& 0& 0\end{array}\right]$$

(75)

where $d_{k,i}=\sqrt{{(x_k-{\mathit{s}}_i^x)}^2+{(y_k-{\mathit{s}}_i^y)}^2},i=1,2,3,4$ denotes the relative distance between the ith sensor and the target at time k.

As depicted in Figure 2, the estimation performance of AFCIF on target position is better than that of each standard FCIF when the initial estimate of process noise covariance is much larger. Although experiencing some obvious fluctuations, the RMSE curves of the FCIFs with different combinations of sensors stay descending until the last second, except the one with ${\mathit{s}}_1,{\mathit{s}}_4$ and the one with ${\mathit{s}}_2,{\mathit{s}}_4$ become gradually divergent at the last 20 s. However, the curve of the AFCIF keeps the downtrend with smaller fluctuations, which indicates better filtering stability. As for the estimation performance on target velocity, it can be seen in Figure 3 that the RMSE curves of AFCIF and FCIFs with ${\mathit{s}}_1,{\mathit{s}}_4$, ${\mathit{s}}_2,{\mathit{s}}_4$, and ${\mathit{s}}_3,{\mathit{s}}_4$ almost overlap in the last 50 s. Overall, when the initial process noise covariance is larger than the true value, the performance difference between FCIF and AFCIF is decreasing over time.

While if the initial value of process noise covariance is smaller than the true one, the simulation results are different. As shown in Figure 4 and Figure 5, the RMSE curves of AFCIF are easy to be distinguished from those of the FCIFs and the performance differences keep a growing tendency in most of the time. This can be explained by the convergence condition that is shown in (57). A smaller ${\mathit{Q}}_{k-1}$ will lead to unstable and divergent results. With no accurate statistic of the process noise, we can use a larger initial value to guarantee the stability and convergence of the filtering algorithm. No matter how the initial process noise covariance is assumed, the RMSE curves of AFCIF approach the PCRLB gradually over time (as shown in Figure 2, Figure 3, Figure 4 and Figure 5), which validates the superiority of AFCIF and the correctness of the theoretical analysis.

Under the condition of unknown process noise covariance, the performance gap between FCIF and AFCIF is due to the model mismatching, which means that the standard FCIF always uses an inaccurate process noise covariance in the filtering procedure. However, the proposed AFCIF estimates and modifies the covariance online, which can guarantee the estimation results stable and convergent. In our simulation, since the initial estimate of the covariance (no matter ${\mathit{Q}}_{\mathrm{big}}$ or ${\mathit{Q}}_{\mathrm{small}}$) has large deviation from the true value, the parameter N should be small. Otherwise the estimation will reach convergence more slowly. All of the simulation results are based on $N=10$.

Now we compare the tracking performance while using different combinations of sensors. From the comparison of PCRLB for RMSE of position shown in Figure 6, it is easy to notice that ${\mathit{s}}_2,{\mathit{s}}_4$ is the best pair of sensors in most of the time and ${\mathit{s}}_1,{\mathit{s}}_2$ is the last choice. From about the 120th second to the end of the simulation, however, the performance of tracking the target with ${\mathit{s}}_2,{\mathit{s}}_4$ begins to degrade and ${\mathit{s}}_3,{\mathit{s}}_4$ turns to be the best combination. These results are corresponding to the illustration in Figure 7, which shows the specific sensor selection of the AFCIF at each time step. Another valuable information in Figure 6 is that the tracking performance using ${\mathit{s}}_1,{\mathit{s}}_4$, ${\mathit{s}}_2,{\mathit{s}}_4$ and ${\mathit{s}}_3,{\mathit{s}}_4$ are very close to that using all the sensors. The strategy of sensor selection helps reach the best tracking performance with as few sensors as possible, thus the sensor resources can be utilized reasonably and sufficiently.

In Figure 8 and Figure 9, we compare the performance on estimating process noise covariance of AFCIF with sensor selection and AFCIFs with fixed combinations of sensors. Once again, sensor selection strategy is proved to make the estimation of target state more accurate and this also facilitates the estimation of the process noise covariance.

Table 1. Relative Computation Times of the algorithms.

Algorithm	Relative Computation Time
FCIF	1
AFCIF	1.7

shows the computation time of the algorithms relative to that of FCIF. AFCIF runs a litter slower, since it incorporates the procedures of selecting the best pair of sensors and estimating the process noise covariance. While considering the prominent improvement over FCIF, this added computation burden of AFCIF is ignorable.

8. Conclusions

In this paper, we have proposed an adaptive fifth-degree cubature information filter (AFCIF) for multi-sensor BOT. Our main contributions are summarized, as follows:

(1)

A sensor selection strategy based on the observability theory is developed for multi-sensor BOT to choose the optimal combination of sensors to track the target at each sample interval. With the given sensors at different locations, this strategy can help the tracking system reach the best possible observability, which will enhance the filtering stability and accuracy. By using this strategy, the sensor resource can also be made the best use of.

(2)

A novel nonlinear estimation algorithm is constructed within the information filtering framework. Using the newly introduced fifth-degree cubature rule, this algorithm can approximate the multidimensional integrals existing in Bayesian filtering with considerable accuracy.

(3)

In order to deal with the state estimation of nonlinear system that is corrupted by Gaussian noise with unknown covariance, a recursive covariance estimation procedure derived through the covariance matching principle is embedded into the proposed algorithm. Thus, the statistical property of process noise, which is not usually available in reality, can be estimated and modified in time. As a result, the estimated covariance will get close to the true value over time, which can improve the estimation performance of the algorithm.

The efficacy of the proposed algorithm is verified by the simulation results in a multi-sensor BOT scenario. Under the circumstance of unknown process noise covariance, the target state can still be well estimated. Through the sensor selection strategy, the best combination of sensors with the lowest PCRLB and the highest degree of observability can always be selected, which could further enhance the estimation performance on target state and process noise covariance.