Innovative sequential track-to-track algorithm for telerobots tracking based on heterogeneous sensors

Abstract

In order to improve efficiency and accuracy of the existing telerobots tracking schemes, we present an innovative sequential track-to-track algorithm based on heterogeneous sensors in this article. Considering the effect of the difference of state estimation error on log-likelihood ratio, the modified sequential difference is first derived during the whole surveillance period. According to the chi-square test, the log-likelihood ratio under two hypotheses is further discussed using the weighted coefficient step by step. Subsequently, the extension for maneuvering telerobots tracking is derived based on the unscented Kalman filter. Finally, the numerical studies results indicate that the proposed sequential track-to-track algorithm has promising performance for tracking telerobots with various motion states.

Introduction

The intelligent robot technologies have been rapidly developed in the past decades. As a result, the teleoperation control for robotics has also become a challenging topic in the world.^1,2 In this case, it also raises a natural and interesting question: How to detect and track multi-telerobot in surveillance region efficiently? To answer it, we consider double telerobots tracking scheme using two heterogeneous sensors for simplification, that is, passive radar and infra-red detector, which can collect the current measurements to identify tracks of moving telerobots. Once two such tracks reflect a same telerobot, the state estimation results from heterogeneous sensors are automatically fused. ³ Therefore, how to associate correct pair of data available from different sensors in order to make an optimal decision has important significance.

By retrieving recent references, we have achieved various manipulation techniques and tracking methods. In previous studies,^4–6 a teleoperation control for robotic system based on the vision compressive sensing and adaptive fuzzy controllers in joint–space were integrated to complete robot performing manipulation. To guarantee stability of telerobot, the controlled discrete-time systems were both state and input couplings and non-affine functions to be included in the Lyapunov functional based on combination of wave variable and neural networks in previous studies.^7–9 By utilizing fuzzy logic system to approximate unknown nonlinear functions, an adaptive fuzzy controller was constructed for a class of uncertain nonlinear switching systems. In previous studies,^10–13 some proposed adaptive location methods reduced the amount of online adjustable parameters, especially the adaptive law guaranteed that all the signals were bounded and the system output converged to a small neighborhood of reference signals. Therefore, these methods above provide excellent solutions to telerobot manipulation and tracking.

As an attractive method, the track-to-track algorithm (T2TA) can be utilized to track moving targets with lower complexity. Hence, we plan to use an innovative idea to achieve telerobot location and tracking instead of fuzzy and/or neural methods. As we know, Bar-Shalom and Chen ¹⁴ derived the likelihood function for track-to-track application from multi-sensor, which formed a basis for cost function based on multi-dimensional assignment method for the first time. Subsequently, He and Zhang ¹⁵ proposed a sequential track correlation algorithm for distributed multi-sensor system and then obtained the promising tracking results. In Tian and Bar-Shalom, ¹⁶ an exact algorithm on calculating test statistics for the T2TA in multi-frame of data was derived, which adaptively led to a sliding window test. Aiming at cross-correlation, an exact hypothesis test for the T2TA using single or multi-frame of data was further presented in Liu et al. ¹⁷ On the other hand, ¹⁸ fused a variety of heterogeneous sensor information according to its characteristics under the concept of reliability. On the basis of uncertainty of telerobot information, the fuzzy-matter element analysis method was introduced to calculate entropy weight. In order to solve the interconnected fuzzy problem, an angle track interconnected function was defined to represent reliability of correct correlation in Xiu et al. ¹⁹ In addition, an optimal T2TA on the sequential modified grey association degree was proposed to compute state estimation covariance. ²⁰ Regarding extension to the T2TA, the additional non-kinematic information of multi-robot delivered by several environment perception sensors was applied in automotive region. ²¹ Although the mentioned studies have been done with heterogeneous sensors, the tracking accuracy should be further improved in information–fusion framework.

Since the state estimation error can be usually considered as a vital factor in telerobot manipulation, it restricts actual tracking performance to some extent. To our knowledge, few related works have been reported to deal with the improved sequential track-to-track algorithm (ST2TA) for telerobot tracking up to now. Therefore, it is our intention in this article to solve the following important problems: How to track two telerobots with heterogeneous sensors? How to improve tracking efficiency and prediction accuracy? For these goals, this article is to mainly analyze the effect of state estimation error on log-likelihood ratio. We present an improved ST2TA based on the modified sequential difference of state estimation error. The innovations of this article can be outlined as follows:

1. The modified log-likelihood ratio under two chi-square–based hypotheses is improved by combining cross-covariance of state estimation error with weighted coefficient during the whole surveillance period.

2. The proposed scheme is extended to maneuvering telerobots tracking based on the unscented Kalman filter (UKF) that can further reduce estimation error under the condition of the constant acceleration (CA) motion.

3. After illustrated numerical studies, the statistic results are compared in order to achieve promising performance for telerobots tracking under different dynamics.

The remainder of this article is organized as follows: in section “Problem statements,” the problem definition for telerobots tracking is formulated. In section “The standard ST2TA,” the principle of the standard ST2TA is briefly discussed. Subsequently, section “The proposed ST2TA” presents the recursion of the improved algorithm for telerobots tracking and then the extension and implementation of the proposed scheme are presented at length. In section “Numerical study and discussions,” the numerical studies are presented with results to verify tracking performance of the proposed ST2TA. Finally, we draw a conclusion with the next working plan in section “Conclusion.”

Problem statements

In the two-dimensional (2D) surveillance region, we have in hand the state equation for mth $(m=i,j)$ telerobot at time k^22–26

$$x_m\left(k\right)=F(k-1)x_m(k-1)+\Gamma \left(k\right)v_m\left(k\right)$$

(1)

where $F(k-1)$ is the state transition matrix, and $\Gamma (k)$ is the gain matrix associated with the process noise $v_m(k)$ that has zero mean and covariance $Q_m(k)$ .

Then, the measurement equation is given by^27,28

$$z_m\left(k\right)=H\left(k\right)x_m\left(k\right)+w_m\left(k\right)$$

(2)

where $H(k)$ is the measurement matrix, and $w_m(k)$ is the measurement noise with zero mean and covariance $R_m(k)$ .

Considering the Kalman filter (KF), we can obtain the state estimate as follows

$${\hat{x}}_m\left(k\right)=F(k-1){\hat{x}}_m(k-1)+K_m\left(k\right)[z_m\left(k\right)-H\left(k\right)F(k-1){\hat{x}}_m(k-1)]$$

(3)

where the filtering gain $K_m(k)$ is

$$K_m\left(k\right)={\hat{G}}_m\left(k\right){\left(H\left(k\right)\right)}^{\mathrm{T}}{[H\left(k\right){\hat{G}}_m\left(k\right){\left(H\left(k\right)\right)}^{\mathrm{T}}+R_m\left(k\right)]}^{-1}$$

(4)

where $(·{)}^{\mathrm{T}}$ denotes the transpose matrix.

After defining the covariance of state $G_m(k)$ , the state covariance estimation ${\hat{G}}_m(k)$ in equation (4) should satisfy the following recursion

$${\hat{G}}_m\left(k\right)=F(k-1)G_m(k-1){\left(F(k-1)\right)}^{\mathrm{T}}+\Gamma (k-1Q_m(k-1){\left(\Gamma (k-1)\right)}^{\mathrm{T}}$$

(5)

$$G_m\left(k\right)={\hat{G}}_m\left(k\right)-K_m\left(k\right)H\left(k\right){\hat{G}}_m\left(k\right)$$

(6)

Furthermore, the corresponding state estimation error ${\tilde{x}}_m(k)$ is

$$\begin{array}{c}{\tilde{x}}_m\left(k\right)=x_m\left(k\right)-{\widehat{x}}_m\left(k\right)\hfill \\ =F\left(k-1\right)x_m\left(k-1\right)+\Gamma \left(k\right)v_m\left(k\right)-F\left(k-1\right){\widehat{x}}_m\left(k-1\right)\hfill \\ -K_m\left(k\right)\left[\begin{array}{l}H\left(k\right)F\left(k-1\right)x_m\left(k-1\right)+H\left(k\right)\Gamma \left(k\right)v_m\left(k\right)\hfill \\ +w_m\left(k\right)-H\left(k\right)F\left(k-1\right){\widehat{x}}_m\left(k-1\right)\hfill \end{array}\right]\hfill \\ =\left[I-K_m\left(k\right)H\left(k\right)\right]\left[F\left(k-1\right){\tilde{x}}_m\left(k-1\right)+\Gamma \left(k\right)v_m\left(k\right)\right]-K\left(k\right)w_m\left(k\right)\hfill \end{array}$$

(7)

Remark 1

In general, the motion state information from local different sensors can be synthetically utilized to track telerobots. However, the independent measurement error and local state estimation error for the same telerobot are inevitable owing to the influence of process noise. Thus, each sensor should process its own measurements independently in order to reduce state estimates. ²⁹ Suppose that two tracks of telerobots are initialed in the ST2TA, then the state estimates from two heterogeneous sensors should be communicated together for completing multi-track fusion.

The standard ST2TA

For the telerobots i and j, we can obtain the cross-covariance of the difference of estimation error between two heterogeneous sensors ${\ell }_1$ and ${\ell }_2$ ^12,20,21

$$\begin{array}{c}\multicolumn{1}{c}{C_{\mathrm{ij}}^{{\ell }_1{\ell }_2}\left(k\right)=E\left[({\tilde{x}}_i^{{\ell }_1}\left(k\right)-{\tilde{x}}_j^{{\ell }_2}\left(k\right)){({\tilde{x}}_i^{{\ell }_1}\left(k\right)-{\tilde{x}}_j^{{\ell }_2}\left(k\right))}^{\mathrm{T}}\right]}\\ =P_i^{{\ell }_1}\left(k\right)+P_j^{{\ell }_2}\left(k\right)-P_{\mathrm{ij}}^{{\ell }_1{\ell }_2}\left(k\right)-{\left(P_{\mathrm{ij}}^{{\ell }_1{\ell }_2}\left(k\right)\right)}^{\mathrm{T}}\hfill \end{array}$$

(8)

where $E[·]$ denotes the mathematical expectation operation, and the covariance of state estimation error $P_i^{{\ell }_1}(k)$ for telerobot i from sensor ${\ell }_1$ and the covariance of state estimation error $P_j^{{\ell }_2}(k)$ for telerobot j from sensor ${\ell }_2$ are

$$P_i^{{\ell }_1}\left(k\right)=E\left[{\tilde{x}}_i^{{\ell }_1}\left(k\right){\left({\tilde{x}}_i^{{\ell }_1}\left(k\right)\right)}^{\mathrm{T}}\right]$$

(9)

$$P_j^{{\ell }_2}\left(k\right)=E\left[{\tilde{x}}_j^{{\ell }_2}\left(k\right){\left({\tilde{x}}_j^{{\ell }_2}\left(k\right)\right)}^{\mathrm{T}}\right]$$

(10)

Furthermore, the cross-covariance of state estimation error $P_{\mathrm{ij}}^{{\ell }_1{\ell }_2}(k)$ in equation (8) is

$$\begin{array}{c}{P}_{ij}^{{\ell }_1{\ell }_2}\left(k\right)=E\left[{\tilde{x}}_i^{{\ell }_1}\left(k\right){\tilde{x}}_j^{{\ell }_2}\left(k\right)\right]\hfill \\ =\left[I-K_i^{{\ell }_1}\left(k\right)H\left(k\right)\right]\hfill \\ \times \left[F\left(k-1\right)P_{ij}^{{\ell }_1{\ell }_2}\left(k-1\right){\left(F\left(k-1\right)\right)}^{\mathrm{T}}+Q_i^{{\ell }_1}\left(k\right){\left(Q_j^{{\ell }_2}\left(k\right)\right)}^{\mathrm{T}}\right]{\left[I-K_j^{{\ell }_2}\left(k\right)H\left(k\right)\right]}^{\mathrm{T}}\hfill \end{array}$$

(11)

According to a central track as well as a local track, the hypotheses of whether two tracks representing the same telerobot are defined as:

• H ₀: the tracking information from the same telerobot, that is, $i=j$ ;

• H ₁: the tracking information from different telerobots, that is, $i\ne j$ .

In order to accept one hypothesis above, we will consider that the likelihood function for state estimation error under H₀ assumption follows the Gaussian distribution in state space^15,30

$$f_0(\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}\left(k\right)|H_0)=\frac{1}{\sqrt{2\pi C_{\mathrm{ij}}^{{\ell }_1{\ell }_2}\left(k\right)}}\exp [-\frac{1}{2}{\left(\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}\left(k\right)\right)}^{\mathrm{T}}{\left(C_{\mathrm{ij}}^{{\ell }_1{\ell }_2}\left(k\right)\right)}^{-1}\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}\left(k\right)]$$

(12)

where the difference of estimation error $\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}(k)$ is

$$\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}\left(k\right)={\tilde{x}}_i^{{\ell }_1}\left(k\right)-{\tilde{x}}_j^{{\ell }_2}\left(k\right)$$

(13)

Under H₁ assumption, we have the uniform distribution in the state space

$$f_1(\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}\left(k\right)|H_1)={\mu }_{\mathrm{et}}^{{\ell }_1{\ell }_2}$$

(14)

where ${\mu }_{\mathrm{et}}^{{\ell }_1{\ell }_2}$ is the spatial density of extraneous tracks under the assumption that they are the Poisson distributed in state space and the true tracks are not homogeneously distributed.

According to equations (12) and (14), the standard log-likelihood ratio can be written as

$$\begin{array}{c}L(\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}(k))=\ln \frac{f_0(\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}(k)|H_0)}{f_1(\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}(k)|H_1)}\hfill \\ =-\frac{1}{2}{(\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}(k))}^T{(C_{i,j}^{l_1{l}_2}(k))}^{-1}\hfill \\ \multicolumn{1}{c}{\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}(k)-\ln \left({\mu }_{\mathrm{et}}^{l_1{l}_2}\sqrt{2\pi C_{i,j}^{l_1{l}_2}(k)}\right)}\end{array}$$

(15)

Therefore, we can conclude the following hypotheses:

Accept H₀, if the test statistic satisfies

$$L\left(\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}\left(k\right)\right)\le \epsilon \left(k\right)$$

(16)

In equation (16), the accepted significance threshold $\epsilon (k)$ is given by

$$P\{L\left({\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}\left(k\right)\right)>\epsilon \left(k\right)|H_0\}=\alpha$$

(17)

where $\alpha$ is the corresponding significance level.

Accept H₁, if the test statistic satisfies other conditions.

Remark 2

For the standard ST2TA, two heterogeneous sensors can achieve two telerobots tracking based on the statistical parameters $\epsilon (k)$ and $\alpha$ . As a result, the interpretation of equation (15) is relatively straightforward during the whole filtering process. Once the value of $\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}(k)$ between two tracks of telerobots becomes larger, the assignment metric $L(\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}(k))$ is more possibly rejected. Thus, the standard ST2TA is relatively unstable for telerobots tracking.

The proposed ST2TA

To deal with the inherent defect of the standard ST2TA, we present an innovative ST2TA in this section.

Principle of the proposed ST2TA

At time k, we first modify the sequential difference by applying the weighted coefficient $\lambda (\lambda \le 1)$ as follows

$$\begin{array}{c}\multicolumn{1}{c}{\widehat{\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}}\left(k\right)=({\tilde{x}}_i^{{\ell }_1}\left(k\right)-{\tilde{x}}_j^{{\ell }_2}\left(k\right))+\lambda \widehat{\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}}(k-1)}\\ =\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}\left(k\right)+\lambda \widehat{\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}}(k-1)\hfill \end{array}$$

(18)

where $\widehat{\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}}(k)$ can be represented by the sum of $\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}(k)$ and $\lambda \widehat{\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}}(k-1)$ .

Compared with equation (13), we find that equation (18) supplements the compensatory component $\lambda \widehat{\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}}(k-1)$ in order to correct the value of $\widehat{\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}}(k)$ , which is more reasonable in practical teleoperation control.

Similar to equation (18), we have the recursion of $\widehat{\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}}(k-1)$ at time $k-1$

$$\widehat{\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}}(k-1)=\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}(k-1)+\lambda \widehat{\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}}(k-2)$$

(19)

After plenty of iterations, we can get the recursion of $\widehat{\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}}(1)$ at time 1

$$\widehat{\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}}\left(1\right)=\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}\left(1\right)+\lambda \widehat{\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}}\left(0\right)$$

(20)

Then, equation (18) can be rewritten as follows

$$\begin{array}{c}\multicolumn{1}{c}{\widehat{\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}}\left(k\right)=\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}\left(k\right)+\lambda \widehat{\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}}(k-1)}\\ \multicolumn{1}{c}{=\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}\left(k\right)+\lambda \Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}(k-1)+{\lambda }^2\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}(k-2)}\\ \multicolumn{1}{c}{+\cdots +{\lambda }^{k-1}\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}\left(1\right)+{\lambda }^k\widehat{\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}}\left(0\right)}\\ \multicolumn{1}{c}{=\sum _{\tau =1}^k{\lambda }^{k-\tau }\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}\left(\tau \right)}\end{array}$$

(21)

Due to $\widehat{\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}}(0)=0$ at time 0, $\widehat{\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}}(k)$ can be represented as the sum of ${\lambda }^{t-\tau }\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}(\tau )$ from time 1 to time k. Consequently, we propose Proposition 1 to define the modified log-likelihood ratio.

Proposition 1

The modified log-likelihood ratio between hypotheses H₀ and H₁ is

$$\begin{array}{c}\multicolumn{1}{c}{L\left(\widehat{\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}}\left(k\right)\right)=-2\ln \frac{f_0(\widehat{\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}}\left(k\right)|H_0)}{f_1(\widehat{\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}}\left(k\right)|H_1)}}\\ \multicolumn{1}{c}{=\sum _{\tau =1}^k\left[{\left({\lambda }^{k-\tau }{\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}\left(\tau \right)\right)}^{\mathrm{T}}{\left(C_{\mathrm{ij}}^{{\ell }_1{\ell }_2}\left(\tau \right)\right)}^{-1}\left({\lambda }^{k-\tau }{\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}\left(\tau \right)\right)\right]}\\ \multicolumn{1}{c}{+\ln \left[2\pi {\mu }_{\mathrm{et}}^{2{\ell }_1{\ell }_2}\underset{\tau =1}{\overset{k}{\Pi }}{C}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}\left(\tau \right)\right]}\end{array}$$

(22)

Proof

According to equation (21), the likelihood function of hypothesis H₀ is

$$f_0\left(\stackrel{︹}{\Delta {\stackrel{~}{x}}_{ij}^{{\ell }_1{\ell }_2}}|H_0\right)={\displaystyle \prod _{\tau =1}^k\frac{1}{\sqrt{2\pi C_{ij}^{{\ell }_1{\ell }_2}\left(\tau \right)}}}\exp \left\{-\frac{1}{2}{\displaystyle \sum _{\tau =1}^k\left[{\left({\lambda }^{k-\tau }{\tilde{x}}_{ij}^{{\ell }_1{\ell }_2}\left(\tau \right)\right)}^{\mathrm{T}}{\left(C_{ij}^{{\ell }_1{\ell }_2}\left(\tau \right)\right)}^{-1}\left({\lambda }^{k-\tau }{\tilde{x}}_{ij}^{{\ell }_1{\ell }_2}\left(\tau \right)\right)\right]}\right\}$$

(23)

where the cross-covariance of the difference of estimation error is defined as

$$\begin{array}{c}\multicolumn{1}{c}{C_{\mathrm{ij}}^{{\ell }_1{\ell }_2}\left(\tau \right)=D\left[{\lambda }^{t-\tau }{\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}\left(\tau \right)\right]}\\ \multicolumn{1}{c}{={\lambda }^{2(t-\tau )}D\left[{\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}\left(\tau \right)\right]}\\ \multicolumn{1}{c}{={\lambda }^{2(t-\tau )}D[{\tilde{x}}_i^{{\ell }_1}\left(\tau \right)+{\tilde{x}}_j^{{\ell }_2}\left(\tau \right)]}\\ \multicolumn{1}{c}{={\lambda }^{2(t-\tau )}[P_i^{{\ell }_1}\left(\tau \right)+P_j^{{\ell }_2}\left(\tau \right)]}\end{array}$$

(24)

where $P_i^{{\ell }_1}(\tau )$ and $P_j^{{\ell }_2}(\tau )$ are similar to equations (9) and (10), respectively, and $D[·]$ denotes the covariance operation. According to equation (14), the likelihood function of hypothesis H₁ is

$$f_1(\widehat{\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}}\left(k\right)|H_1)={\mu }_{\mathrm{et}}^{{\ell }_1{\ell }_2}$$

(25)

To meet the needs of the chi-square test, we multiply $-2$ with the proposed log-likelihood ratio between equations (23) and (25) and then get equation (22). Therefore, $L(\widehat{\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}}(k))$ can be regarded as a measure of relative support for one hypothesis against another for telerobots tracking.

As we know, if a sample of size m follows the normal distribution, then the distribution of sample variance which allows a test to be made of whether the variance of the whole population has a pre-determined value based on the chi-square test.^15,16 Thus, we have the following hypotheses:

Accept H₀, if the test statistic satisfies the chi-square distribution ${\chi }_{(\alpha )}^2$

$$L\left(\widehat{\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}}\left(k\right)\right)={\chi }_{\left(\alpha \right)}^2$$

(26)

Accept H₁, if the test statistic satisfies other conditions.

Remark 3

In equation (21), $\widehat{\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}}(k)$ is equal to the sum of ${\lambda }^{t-\tau }\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}(\tau )$ during the whole surveillance period. In view of the computational efficiency, $\widehat{\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}}(k)$ can be derived from time $k-1$ to time k. Then, equation (21) reduces to

$$\widehat{\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}}\left(k\right)=\sum _{\tau =k-1}^k{\lambda }^{k-\tau }\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}\left(\tau \right)$$

(27)

In equation (27), we can only store tracking information at times $k-1$ and k, respectively. Of course, the value of $\widehat{\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}}(k)$ is relatively imprecise. By comparison, we find that the running time of equation (21) is slightly longer when having adequate space available.

Extension of the proposed ST2TA

In view of maneuvering telerobots tracking, we further extend the proposed ST2TA in this subsection. As we know, the KF can yield perfect tracking performance for non-maneuvering telerobots with the constant velocity (CV) motion model. However, the KF has worse precision when the telerobot is maneuvering to some extent, such as the CA dynamics. By comparison, the UKF is an enhanced version, whose essence is the unscented transform (UT) that is simple to approximate the Gaussian distribution than to approximate arbitrary nonlinear function. ³¹ Due to a set of sigma points by utilizing the deterministic sampling, the UKF is competent because the UT is easy to approximate maneuvering dynamics of telerobots.

At time $k-1$ , $G_m(k-1)$ is assumed to the initial covariance, then the sigma points are^32,33

$$\{\begin{array}{c}{\chi }_m^0(k-1)=x_m(k-1)\hfill \\ \multicolumn{1}{c}{{\chi }_m^{\alpha }(k-1)=x_m(k-1)+{\left[\sqrt{(\beta +\delta )G_m(k-1)}\right]}_{\alpha }^{\mathrm{T}}}\\ \multicolumn{1}{c}{{\chi }_m^{\alpha +\beta }(k-1)=x_m(k-1)-{\left[\sqrt{(\beta +\delta )G_m(k-1)}\right]}_{\alpha }^{\mathrm{T}}}\end{array}$$

(28)

where $\beta$ is the state dimension and $\delta$ is the regulation parameter of the covariance.

Subsequently, we get the weights corresponding to equation (28)

$$\{\begin{array}{c}{w}_m^0=\frac{\delta }{\chi +\delta }\hfill \\ \multicolumn{1}{c}{w_m^{\alpha }=\frac{1}{2(\beta +\delta )}}\\ \multicolumn{1}{c}{w_m^{\alpha +\beta }=\frac{1}{2(\beta +\delta )}}\end{array}$$

(29)

Then, the transformed set of the vectors can be written as

$${\zeta }_m^{\alpha }(k-1)=F(k-1){\chi }_m^{\alpha }(k-1)$$

(30)

The predicted state is given by

$${\hat{x}}_m(k-1)=\sum _{\alpha =0}^{2\beta }{w}_m^{\alpha }{\zeta }_m^{\alpha }(k-1)$$

(31)

The predicted covariance is

$${\hat{G}}_m(k-1)=\sum _{\alpha =0}^{2\beta }{w}_m^{\alpha }[{\zeta }_m^{\alpha }(k-1)-{\hat{x}}_m(k-1)]{[{\zeta }_m^{\alpha }(k-1)-{\hat{x}}_m(k-1)]}^{\mathrm{T}}$$

(32)

According to the measurement model, we have

$${\hat{z}}_m^{\alpha }(k-1)=H_m\left(k\right){\zeta }_m^{\alpha }(k-1)$$

(33)

The predicted observation can be written as

$${\hat{z}}_m(k-1)=\sum _{\alpha =0}^{2\beta }{w}_m^{\alpha }{\hat{z}}_m^{\alpha }(k-1)$$

(34)

If the measurement noise is additive and independent, the measurement covariance can be written as

$${\hat{G}}_{m,\mathrm{zz}}\left(k\right)=R_m\left(k\right)+\sum _{\alpha =0}^{\beta }{w}_m^{\alpha }[{\hat{z}}_m^{\alpha }(k-1)-{\hat{z}}_m(k-1)]{[{\hat{z}}_m^{\alpha }(k-1)-{\hat{z}}_m(k-1)]}^{\mathrm{T}}$$

(35)

Similarly, the cross-covariance between the state and the measurement is

$${\hat{G}}_{m,\mathrm{xz}}\left(k\right)=\sum _{\alpha =0}^{\beta }{w}_m^{\alpha }[{\zeta }_m^{\alpha }(k-1)-{\hat{x}}_m(k-1)]{[{\hat{z}}_m^{\alpha }(k-1)-{\hat{z}}_m(k-1)]}^{\mathrm{T}}$$

(36)

According to equations (35) and (36), we obtain the Kalman gain

$$K_m\left(k\right)={\hat{G}}_{m,\mathrm{xz}}\left(k\right){\hat{G}}_{m,\mathrm{zz}}^{-1}\left(k\right)$$

(37)

Therefore, the predicted state and covariance at time k can be written as

$${\hat{x}}_m\left(k\right)={\hat{x}}_m(k-1)+K_m\left(k\right)[z_m\left(k\right)-{\hat{z}}_m(k-1)]$$

(38)

$${\hat{G}}_m\left(k\right)={\hat{G}}_m(k-1)-K_m\left(k\right){\hat{G}}_{m,\mathrm{zz}}{\left[K_m\left(k\right)\right]}^{\mathrm{T}}$$

(39)

Remark 4

Note that the sigma points are propagated through nonlinear function, from which a new mean and covariance estimation are then formed. The result is a filter which more accurately estimates the true mean and covariance. It can be verified with the Taylor series expansion of the posterior statistics. In addition, the UKF filter removes the requirement to explicitly calculate complex Jacobian matrix. In the proposed ST2TA framework, for achieving satisfactory performance, we further combine the UKF with the chi-square test to tracking maneuvering telerobots instead of the standard KF.

Implementation of the proposed ST2TA

According to the above subsection, we describe the filtering process of the proposed ST2TA in one cycle as follows:

• ① Input the tracking information $x_i^{{\ell }_1}(k)$ and $x_j^{{\ell }_2}(k)$ ;

• ② if the telerobots execute the CV motion, then go to ⑤

• ③ otherwise, go to ⑥

• ④ end if

• ⑤ According to the KF method, compute the state estimates ${\hat{x}}_i^{{\ell }_1}(k)$ and ${\hat{x}}_j^{{\ell }_2}(k)$ using equation (3), the filtering gain $K_m(k)$ using equation (4), and the state covariance estimation ${\hat{G}}_m(k)$ using equation (5); then go to ⑦

• ⑥ According to the UKF method, compute the state estimates ${\hat{x}}_i^{{\ell }_1}(k)$ and ${\hat{x}}_j^{{\ell }_2}(k)$ using equation (38), the filtering gain $K_m(k)$ using equation (37), and the state covariance estimation ${\hat{G}}_m(k)$ using equation (39);

• ⑦ Compute the state estimation errors ${\tilde{x}}_i^{{\ell }_1}(k)$ and ${\tilde{x}}_j^{{\ell }_2}(k)$ using equation (7);

• ⑧ Compute the covariances of estimation error $P_i^{{\ell }_1}(k)$ and $P_j^{{\ell }_2}(k)$ using equations (9) and (10);

• ⑨ Compute the difference of state estimation error $\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}(k)$ using equation (13);

• ⑩ for $\tau =1,\dots ,k$

• ⑪ Compute the modified difference of estimation error $\widehat{\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}}(k)$ using equation (21);

• ⑫ Compute the cross-covariance of the difference of estimation error $C_{\mathrm{ij}}^{{\ell }_1{\ell }_2}(\tau )$ using equation (24);

• ⑬ Compute the likelihood function $f_0(\widehat{\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}}(k)|H_0)$ under hypothesis H₀ using equation (23);

• ⑭ Compute the likelihood function $f_1(\widehat{\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}}(k)|H_1)$ under hypothesis H₁ using equation (25);

• ⑮ Compute the modified log-likelihood ratio $L(\widehat{\Delta {\tilde{x}}_{\mathrm{ij}}^{{\ell }_1{\ell }_2}}(k))$ using equation (22);

• ⑯ end for

• ⑰ if the test satisfies equation (26), then accept H₀;

• ⑱ otherwise, accept H₁;

• ⑲ end if

• ⑳ Output tracking results.

Numerical study and discussions

In this section, two kinds of scenarios are done to evaluate tracking performance of the proposed ST2TA. The experimental environment was Intel™ Core™ i5, RAM 4 GB, Windows™ 7, and MATLAB™ V8.0. The scale of the 2D surveillance region is $[0,70]\times [-200,200]{\mathrm{m}}^2$ . The passive radar and the infra-red senor are used to track two telerobots in 30 s. Besides, the sampling interval is $T=0.1\mathrm{s}$ , and the covariances of measurement noise are $R_1(k)=\mathrm{diag}(0.01,0.01)$ and $R_2(k)=\mathrm{diag}(0.0001,0.0001)$ , where $\mathrm{diag}(·)$ denotes the diagonal matrix. Two telerobots (T1 and T2) move during the whole surveillance time. Their initial position coordinates are (0, 50) m and (10, –50) m, the velocity vectors are (2, 4) m/s and (1, –2) m/s, and the covariances of process noise are $Q_1(k)=\mathrm{diag}(0.001,0.001,0.001)$ and $Q_2(k)=\mathrm{diag}(0.0012,0.0012,0.0012)$ . During 100 simulation trials, the significance level is $\alpha =5\mathrm{\%}$ , and the weighted coefficient is $\lambda =1$ .

CV motion scenario

In this scenario, we define that $x_m(k)=[\mathcal{x}(k),\stackrel{·}{\mathcal{x}}(k),\mathcal{y}(k),\stackrel{·}{\mathcal{y}}(k){]}^{\mathrm{T}}$ is the vector of the planar position $(\mathcal{x}(k),\mathcal{y}(k))$ and the planar velocity $(\stackrel{·}{\mathcal{x}}(k),\stackrel{·}{\mathcal{y}}(k))$ . Then, we use the KF with the following matrices

$$F(k-1)=\left[\begin{array}{cccc}1\hfill & 1\hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 1\hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & 1\hfill & 1\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & 0\hfill & 1\hfill \end{array}\right],\Gamma \left(k\right)=\left[\begin{array}{cc}0.5\hfill & 0\hfill \\ \multicolumn{1}{c}{1}& 0\hfill \\ \multicolumn{1}{c}{0}& 0.5\hfill \\ \multicolumn{1}{c}{0}& 1\hfill \end{array}\right],\mathrm{and}H\left(k\right)={\left[\begin{array}{cc}1\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 0\hfill \\ \multicolumn{1}{c}{0}& 1\hfill \\ \multicolumn{1}{c}{0}& 0\hfill \end{array}\right]}^{\mathrm{T}}$$

Figure 1 shows the true tracks and estimates of two algorithms. Given that two different sensors have satisfactory detection probability, their estimated tracks approximate straight line versus time in this figure. Note that the track of T2 is distorted owing to the effect of process noise. As seen, the proposed ST2TA estimates two robots in accord with the true tracks. For comparison, the standard algorithm provides obvious position deviation to some extent, especially for T2 in the final stage. Figures 2 and 3 demonstrate the position estimation error. Note that the proposed ST2TA has smaller deviation in both x and y coordinates. However, the standard algorithm gives position estimates far away from the ground truth. For example, it is much unstable in y coordinate during 14th–23rd s. This reason can be explained that the standard algorithm ignores the effect from historic tracking information of two sensors on state estimation error. Finally, Figures 4 and 5 illustrate two kinds of statistic results. It can be seen that the proposed ST2TA has smaller standard deviation (STDEV) as well as sum of covariances (COVs). Again, the tracking performance of the standard algorithm is worse because it exaggerates abrupt position deviation without any time consuming. Regarding the latter statistic parameter, we can find that the value of T1 is twice as much as that of T2 because the velocity vector of T1 is double.

Figure 1.

Telerobots tracks and estimates in CV motion scenario.

Figure 2.

Position estimation error of T1 in CV motion scenario.

Figure 3.

Position estimation error of T2 in CV motion scenario.

Figure 4.

Statistic result of T1 in CV motion scenario.

Figure 5.

Statistic result of T2 in CV motion scenario.

CA motion scenario

In this scenario, two telerobots keep the CA motion with the accelerations (0.04, 0.02) m/s ² and (0.02, −0.01) m/s ² , respectively. The key parameters in the UKF are taken to $\beta =2$ and $\delta =0.001$ . In addition, the vector $x_m(k)=[\mathcal{x}(k),\stackrel{·}{\mathcal{x}}(k),\stackrel{··}{\mathcal{x}}(k),\mathcal{y}(k),\stackrel{·}{\mathcal{y}}(k),\stackrel{··}{\mathcal{y}}(k){]}^{\mathrm{T}}$ is defined to represent the planar position $(\mathcal{x}(k),\mathcal{y}(k))$ , the planar velocity $(\stackrel{·}{\mathcal{x}}(k),\stackrel{·}{\mathcal{y}}(k))$ , and the planar acceleration $(\stackrel{·}{\mathcal{x}}(k),\stackrel{·}{\mathcal{y}}(k))$ . The matrices in (1) and (2) are redefined as

$$F(k-1)=\left[\begin{array}{cccccc}1\hfill & 1\hfill & 0.5\hfill & 0\hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 1\hfill & 1\hfill & 0\hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & 1\hfill & 0\hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & 0\hfill & 1\hfill & 1\hfill & 0.5\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & 0\hfill & 0\hfill & 1\hfill & 1\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & 0\hfill & 0\hfill & 0\hfill & 1\hfill \end{array}\right],\Gamma \left(k\right)=\left[\begin{array}{cc}0.5\hfill & 0\hfill \\ \multicolumn{1}{c}{1}& 0\hfill \\ \multicolumn{1}{c}{1}& 0\hfill \\ \multicolumn{1}{c}{0}& 0.5\hfill \\ \multicolumn{1}{c}{0}& 1\hfill \\ \multicolumn{1}{c}{0}& 1\hfill \end{array}\right],\mathrm{and}H\left(k\right)={\left[\begin{array}{cc}1\hfill & 0\hfill \\ \multicolumn{1}{c}{1}& 0\hfill \\ \multicolumn{1}{c}{0}& 0\hfill \\ \multicolumn{1}{c}{0}& 1\hfill \\ \multicolumn{1}{c}{0}& 1\hfill \\ \multicolumn{1}{c}{0}& 0\hfill \end{array}\right]}^{\mathrm{T}}$$

For illustration purpose, Figures 6–10 successively provide the simulation results for tracking telerobots with the CA motion model. First, Figure 6 shows 2-telerobot tracks in x–y coordinate. As seen, T1 and T2 are maneuvering with different accelerations. According to 2D trajectories during the whole surveillance time, there is no crossing point among them in the region. Figures 7 and 8 demonstrate the position estimation error of telerobots in both x and y coordinates, respectively. Note that the proposed ST2TA effectively estimates the positions by combining modified log-likelihood ratio with the UKF. However, the standard ST2TA still gives serious position bias versus time because the telerobots maneuvers also restrict actual tracking performance. In Figures 9 and 10, the statistic results for two telerobots are illustrated. It can be observed that the performance of the standard ST2TA is worse because it overstates both the STDEV and the sum of COVs against time. In contrast, it can be verified that the proposed ST2TA achieves better tracking performance.

Figure 6.

Telerobots tracks and estimates in CA motion scenario.

Figure 7.

Position estimation error of T1 in CA motion scenario.

Figure 8.

Position estimation error of T2 in CA motion scenario.

Figure 9.

Statistic result of T1 in CA motion scenario.

Figure 10.

Statistic result of T2 in CA motion scenario.

From two scenarios, we can summarize that the proposed ST2TA is more accurate no matter what the various motion states of two telerobots. Furthermore, we find that the two kinds of statistic parameters (both the STDEV and the sum of COVs) from the proposed ST2TA are all reduced. Regarding the overall performance, the proposed ST2TA is more acceptable for telerobots tracking.

Conclusion

The challenges are to deal with the complicated and imprecise estimation for telerobots tracking. This article develops an innovative ST2TA based on heterogeneous sensors. In order to achieve the modified sequential difference of state estimation error, we derive the log-likelihood ratio under two hypotheses with the cross-covariance of state estimation error and weighted coefficient. What’s more, the UKF is extended to track maneuvering telerobots. The numerical study results indicate that the proposed ST2TA has remarkable improvement for telerobots tracking. As the future developments of this work, we plan to track strong maneuvering telerobots.