Abstract
In the problem of target tracking, different types of biases can enter into the measurement collected by sensors due to various reasons. In order to accurately track the target, it is essential to estimate and correct the measurement bias. Considering practical backgrounds, the bias is assumed to be locally stationary Gaussian distributed and an iterative estimation algorithm is proposed. Firstly, a mechanism is established to detect whether the bias switches between different Gaussian distributions. Secondly, the expectation maximization algorithm with the assistance of extended Kalman filtering and smoothing is proposed to iteratively estimate the bias and target state in an offline manner. Simulations show the proposed algorithm can suppress the impact of the measurement bias on target tracking.
1. Introduction
As an emerging type of wireless service, positioning or tracking a moving target using the measurements collected by a sensor network has drawn considerable attention over the past two decades [1]. The problem could be solved in two steps. First, sensors receive the signal transmitted by the target to be tracked and then calculate the measurement, e.g., time of arrival (TOA) and time difference of arrival (TDOA). Second, the fusion center fuses the complementary information provided by the measurements of all the sensors and deduces target trajectory. In practice, the measurement calculated by sensors can be biased, which is caused by systematic error (e.g., spatial misregistration), inaccurate sensor calibration, environmental constraints (e.g., non-line-of-sight (NLOS) propagation), or other sources [2]. The bias inherent in the measurements will enter into the fusion process and lead to false tracks. Therefore, the bias effect must be eliminated using estimation and correction methods.
There are mainly two directions for dealing with the bias. First, the bias is corrected and then the unbiased measurements are fused to deduce target trajectory, e.g., in the application of the airport surveillance data fusion system [3, 4]. Second, the biased measurements are directly fused into tracking system and the bias is estimated together with target trajectory. We will focus on the latter in the sequel.
Estimation of measurement bias has been investigated in a great deal of references [5, 6]. The algorithm for bias estimation can be divided into two categories, that is, online and offline. The online algorithm estimates the bias in real time, but it cannot produce a satisfactory bias estimate in some cases. By contrast, the offline algorithm estimates the bias more precisely but with a certain time delay, since it usually requires batch processing of an amount of data.
The offline algorithm can be found in [2, 7, 8]. The least squares (LS) [2] and the maximum likelihood (ML) [7] are used to estimate measurement biases, based on which target state is estimated subsequently. Alternatively, the expectation maximization (EM) algorithm is incorporated with the Kalman filter to give simultaneous estimation of measurement biases and target state in a linear state-space model [8].
The online algorithm can be found in [9–11]. In [9], the unscented Kalman filter is used to estimate measurement biases and target state together, after the measurement biases are augmented into the state vector of a nonlinear state-space model. In [10], the marginalized particle filter is used. Measurement biases are treated as nuisance parameters and are marginalized out. After target state is estimated using the particle filter, the measurement biases are estimated using the Kalman filter. In [11], a recursive joint estimation algorithm is proposed by carefully coupling measurement biases and target state. The algorithm is qualified to deal with different types of state-space models and measurement biases.
The aforementioned algorithms assume that the bias is constant. In contrast, the bias is treated as a random variable with limited fluctuation in different filtering frameworks [12–14]. In our previous work [15], the TOA measurement is employed such that a nonlinear state-space model is used to formulate the tracking problem [16]. The measurement bias is assumed to be strictly stationary Gaussian distributed and incorporated into two different state-space models. Then, the measurement bias and target state are estimated based on the EM algorithm [17] that uses the output of extended Kalman filter (EKF) and smoother.
In this paper, we extend the work in [15] and consider a more complicated case where the bias is assumed to be a random variable with a locally stationary Gaussian distribution. For this, the random variable follows a Gaussian distribution in a time interval, but in different time intervals the Gaussian distribution has different forms, i.e., with different means and variances. This kind of time interval is referred to here as stationary time interval, due to stationary statistics observed in it. An example is given in Figure 1. It is obvious that the observation period can be divided into four stationary time intervals in which the random variable follows four Gaussian distributions, respectively.
In real scenarios, there are lots of physical factors which could cause measurement biases to follow a locally stationary Gaussian distribution. For example, in some outdoor scene, target signal arrives at a sensor via a NLOS propagation path. In one time interval, the sensor receives the target signal by reflection on an obstacle between the sensor and target. The similar propagation paths at successive time steps lead to the measurement biases of the sensor approximately following a Gaussian distribution in this time interval. However, in another time interval, the signal is reflected on another obstacle, which corresponds to a different propagation path and leads to a different Gaussian distribution for the measurement biases.
In order to estimate the measurement bias which follows a locally stationary Gaussian distribution using the EM algorithm, we have to cope with two issues in addition to the work in [15]. First, the stationary time intervals in which the bias follows different Gaussian distributions should be divided appropriately. In other words, the time slots should be detected when Gaussian distributions switch from one to another. Second, the EM estimation should be adapted to cope with the measurement biases that have asynchronous stationary time intervals. By addressing these two issues, the proposed algorithm can iteratively estimate the measurement bias and target state.
The remainder of the paper is organized as follows. In Section 2, we formulate the problem and describe briefly the proposed algorithm. In Section 3, two EM-based estimation processes are given in detail, to estimate the measurement bias in two distinct state-space models. In Section 4, we present the mechanism of bias switch detection. Simulation results and conclusions are given in Sections 5 and 6, respectively. In the appendix, the equations of EKF and related smoother are given.
2. Problem Formulation
We assume that the target to be tracked is radioactive and moving in a two-dimensional plane according to a Gauss-Markov random force model. A number of static sensors are used to localize the target using TOA measurements that are biased. In this case, the problem can be formulated by the following state-space model:where is the time step. denotes the state vector of the target, including instantaneous position and velocity . denotes the measurement vector with length of M. M is the number of sensors involved. denotes the additive bias vector of the measurement. Herein, denotes matrix transpose. In (1a), the matrix and are defined aswhere denotes an identity matrix of size 2, denotes a null matrix of dimension , and denotes the sampling interval. In (1b), is a nonlinear function mapping the target state to the TOA measurement; i.e.,where is the propagation velocity of the signal, is the target position at time step , and is the position of the th sensor.
As a nonlinear state-space model, (1a) and (1b) are known as the linear state equation and nonlinear measurement equation, respectively. Without loss of generality, we assume that the driving noise vector , the measurement noise vector , and the bias vector are uncorrelated with each other and are Gaussian distributed as follows:where covariance matrices , and are square diagonal matrices. is a column vector of length M. and can be determined by the properties of the target and sensors. They are usually known to us. is an unknown square matrix of order M. Therefore, the existing problem is to estimate the target state and the measurement bias (or and ), using the measurement .
The proposed estimation procedure in the paper is summarized in Figure 2. Suppose that we focus on a time period, e.g., , which is exactly the fixed interval of extended Rauch-Tung-Striebel (RTS) smoother. In other words, the window size of extended RTS smoother is . In this time period, we implement bias switch detection and EKF using the measurement at each time step. Next, extended RTS smoother is conducted using the measurements at all time steps. Then, the EM estimation is conducted independently in the stationary time intervals in which the bias follows Gaussian distribution. Note that the stationary time intervals are divided by bias switch detection. The steps above are repeated until the estimation convergence is achieved. At last, we obtain the estimates of and for all . It is worth mentioning that if the total observation period consists of time steps, the procedure in Figure 2 should be conducted in time periods continuously. Here, denotes the operation of rounding the value to the nearest integer greater than or equal to it.
Note that bias switch detection is closely related to the formulations of bias models, EKF and EM estimation. In order to improve readability of bias switch detection, the EM estimation will be introduced in advance.
3. EM-Based Estimation
In [15], the measurement bias is incorporated into two different state-space models, for which two EM-based estimation processes are, respectively, given based on the results of EKF and extended RTS smoother. However, they are only suitable for strictly stationary bias. In Sections 3.1 and 3.2, we adapt the EM-based estimation processes for these two models (i.e., Model 1 and Model 2) to the locally stationary bias.
Suppose that the stationary time intervals in which the bias has different Gaussian distributions are divided based on bias switch detection presented in Section 4. In each stationary time interval, the bias is estimated individually based on the EM-based estimation process. Depending on the characteristics of sensors, the biases contributed by different sensors vary in an asynchronous manner. In other words, the biases of different sensors have asynchronous stationary time intervals (see Figure 4). Hence, the EM-based estimation process should be conducted in an asynchronous way, for different sensors.
In this section, denote by the time frame of interest, and denote by the jth stationary time interval for the bias of the ith sensor in this time frame. Here, with being the number of time intervals for the bias of the ith sensor.
3.1. Estimation Process for Model 1
In Model 1, the measurement bias is considered as a part of the state. The state-space model is given as follows:where is the noise of and follows a Gaussian distribution asIn Model 1, (5a) and (5b) are known as the evolution equations of the state. The entire state vector can be written as . If we define the following matrix and vector:Model 1 can be rewritten aswhere , , and
Based on the state-space model in (8a) and (8b), we need to estimate (including the target state and the measurement bias ) and (the covariance matrix of the bias noise ). To do this, the EKF and extended RTS smoother are applied to estimate , using the measurements in —see and in (A.1a), (A.1b), (A.1c), (A.1d), (A.1e), and (A.1f) and (A.3) in Appendix A—while the EM is applied to estimate , using the measurements in . In the EM algorithm, is treated as a vector of hidden variables while is treated as a matrix of unknown parameters. Since is assumed to be diagonal, it can be written aswhere denotes a diagonal matrix with the elements of the main diagonal taken from the values between parentheses. Therefore, the unknown parameters are given aswhich can be estimated using the EM algorithm in two iterative steps:(i)E-step: calculate function ,(ii)M-step: estimate parameters .
and are the EM estimate in the previous and current iteration, respectively. The function is given aswithwhere and is contained in . Following the result in [18] and linearizing the model in (8a) and (8b) using the EKF, we can derive the estimate in the nth iteration:with where . denotes the ith element of the main diagonal of a matrix, and denotes the ith element of a vector. (a part of ), , and are the estimates of extended RTS smoother; see (A.3).
As aforementioned, the stationary time interval is different or asynchronous for the biases of different sensors. Therefore, the EM estimation should be adapted to cope with the biases having asynchronous stationary time intervals. More precisely, the stationary time interval for the EM estimation, i.e., in (15), is different for different values of i and j, which is determined by bias switch detection. In Section 5, we give an example of asynchronous execution of the EM estimation based on Figure 4. Such adaption is validated by the assumption of uncorrelated biases between sensors; see (4c) and (15). Note that the extended RTS smoother is conducted in ; see (13a) and (13b).
The EM-based estimation of and (i.e., ) is summarized as follows:(i)assume some initial value , set , and run the E-step and M-step,(1)E-step: calculate , and using in (A.3),(2)M-step: update in (14),(ii)set and repeat the E-step and M-step until the estimate is stable.
3.2. Estimation Process for Model 2
Model 2 is formulated as follows:where In (17a), denotes the bias of the ith sensor in the jth stationary time interval, and the vector is constant in each stationary time interval. In (17b), the mixed noise follows withThe measurement bias defined in (1a) and (1b) is divided into two parts in Model 2; that is, deals with the mean of while deals with the variance of . In Model 2, , , and are to be estimated. We can employ the EKF and extended RTS smoother to estimate , using the measurements in ; see in (B.1a), (B.1b), (B.1c), (B.1d), (B.1e), and (B.1f) and in (B.2) in Appendix B. The unknown parameters of Model 2 contain and , which are estimated via the EM using the measurements in . According to (17a) and (18), we can writeBy using the EM algorithm, we can obtain the estimate in the nth iteration:where , , andIn (20a) and (20b), and are a posteriori estimates of extended RTS smoother; see (B.2).
The EM-based estimation of and (including and ) is summarized as follows:(i)assume some initial value , set , and run the E-step and M-step,(1)E-step: calculate and using in (B.2),(2)M-step: update in (20a) and (20b),(ii)set and repeat the E-step and M-step until the estimate is stable.
Note that the discussion about the difference in treating the measurement bias between two models can be found in [15].
4. Bias Switch Detection
Recall that the measurement bias has a locally stationary Gaussian distribution in our study case. In order to divide the stationary time intervals, it is essential to detect if the measurement bias of each sensor switches between different Gaussian distributions at each time step, which is referred to here as bias switch detection. Since we assume that the biases between different sensors are uncorrelated (see (4c)), the detection procedure can be processed independently for different sensors. For brevity, the detection procedure is presented only for the measurement bias of the ith sensor at time step k. At the previous time step (i.e., k-1), there exist two cases, i.e., whether the measurement is biased or not (bias presence or absence).
Firstly, let us consider the case with bias presence at time step k-1. There are three possibilities for the measurement bias at time step k: first, keeping the same Gaussian distribution as that at the previous time step; second, switching to another Gaussian distribution; third, switching to zero (or bias absence). The task is to detect which of the three possibilities above occurs, which can be conducted in two steps: first, to detect whether bias switches or not, that is, to distinguish the first possibility above from the remaining two; second, if the bias switches, to detect whether the bias switches to another Gaussian or zero, that is, to distinguish between the second and third possibilities above.
In the first step of detection, we need to check the following inequality:where and denote the true measurement and the predicted measurement of the ith sensor, respectively. denotes the standard deviation of the predicted measurement. and can be derived using the EKF prediction, which is given in (A.1a), (A.1b), (A.1c), (A.1d), (A.1e), and (A.1f) for Model 1 and (B.1a), (B.1b), (B.1c), (B.1d), (B.1e), and (B.1f) for Model 2. In Model 1, is the predicted measurement vector with the covariance matrix in (A.1c). In Model 2, is the predicted measurement vector with the covariance matrix in (B.1c). denotes the bias estimate vector in the nth EM iteration in Model 2 (see (20a)). Following this, we can obtainIf in (22) holds, we can conclude that the measurement bias of the ith sensor switches, and then we need to conduct the second step of detection. Otherwise, the bias does not switch and the detection stops.
In the second step of detection, we check the following inequality:where and denote the predicted measurement and its standard deviation, respectively, provided that the bias is absent. In order to cope with bias absence, the bias of the ith sensor and its variance in two models has to be set as 0. In Model 1, both the ith element of and the ith element on the main diagonal of should be set as 0. Similarly, in Model 2, the ith element of should be set as 0 and the ith element on the main diagonal of should be replaced by the ith element on the main diagonal of . Following this, the formulations of the EKF for bias absence are not the same as those given in (A.1a), (A.1b), (A.1c), (A.1d), (A.1e), and (A.1f) and (B.1a), (B.1b), (B.1c), (B.1d), (B.1e), and (B.1f). Based on the modified EKF prediction, and can be obtained for both models using the form in (23a) and (23b). If in (24) holds, we can conclude that the measurement bias switches to another Gaussian distribution as compared to the one at the previous time step. Otherwise, the bias switches to zero; namely, it vanishes.
Secondly, let us consider the case where the measurement of the ith sensor is unbiased at time step k-1, that is, bias absence. At time step k, there are two possibilities. One is that the bias is still absent. The other is that the bias appears. To distinguish these two possibilities, we can resort to checking the inequality in (24). If holds, we can conclude that the bias appears with a Gaussian distribution. Otherwise, the bias is zero as that at the previous time step.
If the bias between two consecutive time steps is found to switch between the case of bias absence and the case of bias presence, or between two distinct Gaussian distributions, we have to reset the mean and variance of the bias before implementing the EKF in the flowchart of Figure 2. It is done as follows. When the bias switches from the case of bias presence to the case of bias absence, the bias and its variance are reset as 0. When the bias switches the case of bias absence to the case of bias presence or switches between two Gaussian distributions, the bias is reset as (see (24)) and its variance is set as a predefined value (see our simulation part).
After the procedure of bias switch detection is completed, the stationary time intervals in which the bias has different Gaussian distributions are divided. The EM estimation is executed independently and asynchronously to estimate the biases of different sensors in each stationary time interval; see (15) for Model 1 and (20a) and (20b) for Model 2 in Section 3.
Note that the proposed detection resembles the interacting multiple model (IMM) method [16] in some sense, if different Gaussian distributions are thought as different models. However, the proposed detection does not need the exact information of all the models involved whereas the IMM does. Correspondingly, the latter provides more robust results than the former. In our case, the model information is not available a priori so that the IMM is not applicable. The underlying principle of the proposed detection is to measure the distance between the true measurement value and the predicted measurement value. The distances indicate closeness to the models (or Gaussian distributions) and guide one to select the correct model. As potential future work, it is interesting to study how to improve the proposed detection using the curve fitting approach [19], and vice versa.
5. Simulations
In our simulations, we consider a sensor network with four static sensors; see Figure 3. The four sensors (marked with triangle) are located at [-90m, -25m], [m, m], [m, m], and [m, m], respectively. Following a Gauss-Markov random force model, the target starts from [-50m, m] with an initial velocity of about 10 m/s and its trajectory is plotted in solid line in Figure 3. The sensors record the signal sent from the target with a sampling interval of s. The driving noise covariance matrix is given as . The measurement noise covariance matrix is set as . is the propagation velocity of the signal. The time steps amount to 600 in our observation period. The artificial bias added to the measurements has a mean selected from (100, 150, 180, 200, 220, 250, 3000) and a variance selected from (100, 225, 400, 25). Since the TOA measurement is used, the unit of the measurement bias is the second in time. Note that the constant and the unit of the bias are omitted in the subsequent figures for brevity.
The EKF is initialized as follows. The initial mean of covariance matrix of the target state is set as and . The initial position is randomly selected from Gaussian distribution . The initial velocity is uniformly selected from and it satisfies. The initial covariance matrix of the measurement bias is set as . The EM iteration stops once the Euclidean distance between the estimates of the state in two consecutive iterations is smaller than a predefined threshold 0.1. The allowed maximal iteration number is set as 50.
In Figure 3, the estimated trajectories based on the IMD [20], Model 1, and Model 2 are given. The IMD algorithm detects if the bias exists in the measurement and uses the unbiased measurement. It cannot make use of the information provided by the bias. When the measurement is frequently corrupted by the bias, the IMD fails to track the target trajectory. In contrast, the EM-based estimation in Model 1 and Model 2 not only detects bias switch, but also estimates the bias value. By doing this, it can track the target trajectory more accurately.
Figure 4 shows the simulated values of the measurement bias of all four sensors for 600 time steps of observation period. Assume that the window size L of extended RTS smoother is set as 100 time steps. The procedure in Figure 2 will be conducted 6 times sequentially. Here, we take the first 100 time steps as an example to explain the procedure in Figure 2. We conduct bias switch detection, EKF, and extended RTS smoother for all 100 measurements. Suppose that the time slots for bias switch are correctly detected. The stationary time intervals for the EM estimation of the biases of four sensors are not the same. For the bias of Sensor 1, the EM estimation is conducted in the time interval . However, the EM estimation is not conducted in due to bias absence. For the bias of Sensor 2, the EM estimation is conducted in . For the bias of Sensor 3, there is no bias so that the EM estimation is not conducted. For the bias of Sensor 4, the EM estimation is conducted in . The above steps iterate until the bias estimation converges.
Figures 5 and 6 show the root mean square error (RMSE) of target position estimates and bias mean estimates based on the IMD and two models w.r.t. the time step k, respectively. The window size L is selected to be 50 time steps. The results are based on 400 Monte Carlo runs. It is seen that the EM estimation based on Model 1 outperforms that based on Model 2, in terms of estimation accuracy of both target position and bias mean. Moreover, we can find in Figure 5 that the RMSE of the position estimates in both models is highly affected by the length of L, since the RMSE smoothness is blocked by a frame of length L. In contrast, the RMSE of the bias mean estimates varies unregularly and seems not to be closely related to L, as shown in Figure 6. This is due to the fact that the bias mean estimates are affected by not only the window size but also the length of the stationary time interval in which the EM estimation is conducted (see in (15), (20a), and (20b)).
Figures 7 and 8 show the RMSE of position estimates and bias mean estimates w.r.t. the window size L, respectively. The results are based on 400 Monte Carlo runs. Generally speaking, the RMSE of the estimates in two models decreases as the window size L increases. This is due to the fact that the window size L equals the measurement size used in the extended RTS smoother. Larger value of L leads to a more accurate result (i.e., target position) of the extended RTS smoother. Furthermore, accurate position estimates contribute to accurate bias estimation results, since estimation of the target position and the bias is coupled together. In addition, we can find that the RMSE decrement is not obvious when L increases from 50 to 100. This can be explained as follows. Bias mean estimates rely on the measurement size used in the EM estimation, which equals the length of stationary time interval, namely, (see (15), (20a), and (20b)). The value that depends on bias statistics is independent of the window size L, which becomes a bottleneck in improving estimation accuracy of bias (and further target position), especially when L is large.
It is found in our simulation that the EM estimation based on Model 1 is superior to that based on Model 2. In Model 2, the measurement bias is separated into two parts; i.e., one is bias mean and the other is bias variance. The correlation of the two parts is neglected and they are estimated separately. It could result in unexpected estimation error, especially under our bias assumption. In contrast, in Model 1, the bias is treated in a piece as a random variable, which is supposed to be more suitable. As future work, we plan to collect real biased measurements under the condition of NLOS propagation based on our previous work [14] and use them to test the performance of the proposed algorithm and some existing algorithms.
6. Conclusions
We propose an EM-based algorithm to estimate the measurement bias which follows a locally stationary Gaussian distribution. Based on the two state-space models that formulate the impact of biases on target tracking in different ways, we use the mechanism of bias switch detection to detect if the bias switches from one Gaussian distribution to another or, more precisely, to divide the stationary time intervals. Next, based on the results of the EKF and extended RTS smoother, we use the EM-based estimation processes to independently estimate the biases in asynchronous stationary time intervals. Eventually, the proposed algorithm estimates the measurement bias and target state iteratively. It is able to suppress the impact of the measurement bias on target tracking, which is validated by simulation results.
Appendix
A. EKF and Extended RTS Smoother in Model 1
The state in Model 1 is estimated by the EKF and extended RTS smoother [21, 22]. The EKF has the following prediction and update equations:where and are the a priori estimates of the state and its covariance matrix; and are the a posteriori estimates. In (A.1d), denotes the Kalman gain. In (A.1c), and is the partial derivative of on ; i.e.,Note that in the (n+1)th EM iteration, (contained in ) in (A.1b) should be replaced by the EM estimate in (14).
Furthermore, following the above EKF equations, the backward recursion equations for the fixed-interval extended RTS smoother are obtained aswhere denotes the smoother gain, and are the a posteriori smoothed estimates of the state and the covariance matrix, and denotes the a posteriori smoothed estimate of the cross-covariance matrix.
B. EKF and Extended RTS Smoother in Model 2
The EKF in Model 2 is given byThe fixed-interval extended RTS smoother in Model 2 is given by
Data Availability
The MATLAB codes used to support the findings of this study are available from the corresponding author upon request.
Conflicts of Interest
The authors declare that there are no conflicts of interest regarding the publication of this paper.
Acknowledgments
The research was supported by the National Natural Science Foundation of China (No. 61801255) and the K. C. Wong Magna Fund of Ningbo University.