Statically Fused Converted Measurement Kalman Filters

This chapter presents a state estimation method without using of nonlinear recursive filters when Doppler measurement is incorporated into the tracking system. The commonly used motions, such as the constant velocity (CV), constant acceleration (CA), and constant turn (CT), are represented in a pseudo-state space, defined from the product of target true range and range rate, by linear pseudo-state equations. Then the linear converted Doppler measurement Kalman filter (CDMKF) is presented to extract pseudo-state from the converted Doppler measurement, constructed by the product of the range and Doppler measurements. The output of the CDMKF is fused statically with that of the converted position measurement (range and one or two angles) Kalman filter (CPMKF) to produce target Cartesian state estimates. The accuracy and consistence of the estimator can be both guaranteed, since linear Kalman filters are both used to extract information from position and Doppler measurements.


Introduction
In real tracking applications, most sensors report target parameters in sensor (e.g., polar) coordinates, while target motion is usually modeled in Cartesian coordinates. In Doppler radars, the measurements consist of range, one or two angles, and range rate. Tracking in Cartesian coordinates using polar measurements is a nonlinear estimation problem. To handle the range and angle measurements, it is preferred to convert the measurements to a linear form in Cartesian coordinates to avoid nonlinear filtering. This results in the converted position measurement Kalman filtering (CPMKF) method [1]. The statistic property of the converted position measurement errors has been explored in [1][2][3][4][5][6][7][8][9][10]. The Doppler or rangerate measurement, which is the only measurement containing target velocity information, is not processed in the CPMKF method.
Due to the high nonlinearity of the Doppler measurement, the filtering process becomes more complex when Doppler is also included as a part of measurement vector. Various nonlinear filtering methods are utilized to handle Doppler measurements [11][12][13][14][15][16][17][18]. The first-order extended Kalman filter (EKF) is used to process the position and Doppler measurements simultaneously [12,16]. A sequential processing approach, which can not only improve the filtering accuracy but also reduce the computational complexity [19,20], is used with the first-order EKF [18] to process position and range-rate measurements. Since the EKF is based on a Taylor series expansion, large errors in the posterior mean and covariance approximation may lead to performance degradations and possible divergence with the highly nonlinear range-rate measurements. The unscented Kalman filter (UKF), which overcomes some of the shortcomings of the EKF based on a deterministic sampling approach, is used to process the Doppler measurements sequentially in [15,17]. On the other hand, the converted Doppler measurements, constructed by the product of range and Doppler measurement, are used to replace the original Doppler measurement in [13,14,21]. Nonlinear recursive filtering methods still have to be employed to extract target information from the nonlinear converted measurements directly, although the nonlinearity is reduced largely [13]. The second-order EKF is utilized to process the converted Doppler measurements and the converted position measurements simultaneously in [21] and sequentially in [13,14]. The measurement errors amplified by large ranges may result in performance degradation of those converted Doppler measurement-based methods. A linear denoising filter is presented in [22,23] to reduce the noise contained in the converted Doppler measurement, but only the constant velocity (CV) motion is investigated. In [3], based on the estimated angle cosine and sine, Doppler is treated as an approximate linear function of velocity components to allow the use of linear Kalman filter. Since the angle cosine and sine are all nonlinear functions of target position components, the process is actually nonlinear, and estimation performance may rely on the quality of angle cosine and sine critically.
Although performance improvements have been observed in the existing literature, which use the Doppler measurements in state estimation, performance cannot be guaranteed due to the utilization of nonlinear recursive filtering methods. One of the major advantages of the CPMKF is that the nonlinear approximations (i.e., measurement conversion and statistic evaluation) are performed outside the filtering recursion and nonlinear filters are avoided [24]. Whereas, in the existing nonlinear filtering methods [13,17,18], the nonlinear approximation of the Doppler measurements or the converted Doppler measurement is performed inside the dynamic filtering recursion. The accumulations of approximation errors may lead to unsatisfactory performance.
In order to rectify these flaws, a new method is proposed in [27][28][29] and summarized in this chapter. In the proposed method, the use of nonlinear filtering approaches is also avoided while dealing with the Doppler measurements. First, the pseudo-state vectors, of which the converted Doppler measurements [13,14,22,23] are linear functions, are defined. The pseudo-state vector consists of the converted Doppler (i.e., the product of range and range rate) and its derivatives. The timeevolving equations of the pseudo-states are derived and proven to be linear for the three commonly utilized motion models, the constant velocity (CV), constant turn (CT), and the constant acceleration (CA) models. Then, the converted Doppler measurement Kalman filter (CDMKF), a linear filter, is presented to produce the pseudo-state estimates and filter the noise contained in the converted Doppler measurements. Finally, the CDMKF is carried out along with the CPMKF to establish a new state estimation method, statically fused converted measurement Kalman filters (SF-CMKF). Cartesian state estimates and pseudo-state estimates are provided by CPMKF and CDMKF, respectively, and are then fused by a static estimator to produce final state estimates. The quadratic nonlinearity of the pseudo-states is processed by expanding the pseudo-states up to the second order around the estimates from the CPMKF. The correlation between the CPMKF and CDMKF, caused by the common range measurement and process noise, is involved in the static minimum mean squared error (MMSE) estimator to derive correct fusion of the states from the two linear Kalman filters. This filtering scheme actually converts the dynamic nonlinear estimation problem to a linear dynamic estimation following with a static nonlinear fusion problem, where nonlinearity approximations are carried out outside the filtering recursions and the static fusion part produces estimates only for overall outputting. Therefore, nonlinear filtering is also avoided even when range-rate measurements are used for estimation.

Problem formulation
A target's motion is modeled in a two-dimensional (2D) Cartesian system by a discrete time-state equation as where X k ð Þ is a state vector consisting of position components and corresponding velocity components (or acceleration components) along x and y directions, Φ is the state transition matrix, and Γ denotes the noise input matrix.
For CV model, For CA model, follows the Gauss distribution with zero mean and known covariance Q ¼ diag q; q ½ . Here, and denote process noises in x and y directions, respectively, and q denotes the process noise intensity. is the known turn rate for CT model [25].
A 2D Doppler radar is assumed to report measurements of targets in polar coordinates, including range, range rate, and azimuth. The measurement equation can be expressed as where , , and are the target's true range, azimuth, and range rate, respectively. , , and are the corresponding measurement noises, which are all assumed to be zero-mean Gaussian noises with known variances , , and , respectively. It is assumed that the measurement noises are mutually independent with the exception that and are statistically correlated [26] with correlation coefficient , i.e., .

Measurement conversion
In this chapter, the CPMKF is utilized to extract information from position (range and azimuth) measurements, and the CDMKF is used to produce pseudostate estimates from range and range-rate measurements. Fusion of the two converted measurement Kalman filters yields final Cartesian state estimation. For appropriate filtering, the actual statistic, mean and covariance, of the converted measurements has to be evaluated.
The converted position measurements constructed from range and azimuth measurements can be written as is the product of true range and range rate, expressed by.
ð8Þ and is the corresponding error. It can be seen from (8), the converted Doppler measurements are nonlinear with respect to Cartesian states. In conventional tracking approaches, which estimate Cartesian states by recursive filtering directly from measurements, nonlinear recursive filters have to be employed, resulting in unsatisfied performance and possible divergence. In this chapter, the converted Doppler measurements are processed first, by a linear filter, to produce pseudo-state estimates, instead of Cartesian state estimates directly.
The converted measurements in (6) are biased [1,8] due to nonlinear transformations. Both additive [1] and multiplicative [8] debiasing approaches are presented to counter this bias. Some modifications have also been proposed to deal with large errors [4] and correlation between the measurement noise and the covariance [2,3,5,7]. A detailed discussion on the bias issue is beyond the scope of this chapter. In [1], the additive debiasing method is used with converted position measurements. A linearization method based on Taylor series expansion, which may result in large errors, is proposed in [18]. To facilitate a fair comparison against existing works [14,15] dealing with Doppler measurements, the position measurement conversion is implemented here based on the additive debiasing [1] technique, as in [14,15].
Using the nested conditioning method [1,24], which was proven to be effective and consistent, the calculation of the converted Doppler measurement errors was investigated in [14,15]. The objective of the nested conditioning method is to find the mean and covariance conditioned on the unknown ideal measurement first and then to find their expectations conditioned on the noisy measurement.
Denote the bias and covariance of the converted position measurements as respectively. Then the debiased converted position measurements can be obtained as The expressions of the elements in (9) and (10) Similarly, one can get the bias and variance of the converted Doppler measurements as [14]. The covariance between the converted position and Doppler measurements is given as For details of the derivations of the measurement errors presented above, see [1,13,14].

Statically fused converted measurement Kalman filters
The converted position measurements, given by (10) and (11), are preferably processed by the standard linear Kalman filter to outperform practical nonlinear filters (EKF and UKF). The converted Doppler measurements, given by (18) and (19), are also processed by a linear Kalman filter to extract pseudo-state. The outputs of these two linear filters are then fused by a static MMSE estimator to yield the final Cartesian state estimates. This results in the statically fused converted measurement Kalman filter, which is illustrated in Figure 1. This method, abbreviated as SF-CMKF, produces superior performance in both accuracy and consistency.

Pseudo-state equation
Finding the proper representation of a certain motion in the generic state space that corresponds to the converted Doppler is critical to this method. In this section, the pseudo-state equation, consistent with the CV, CA, and CT model in 2D Cartesian state space, is derived first, and then the CDMKF and SF-CMKF filtering procedures are formulated.

Pseudo-state equation for CV model
The CV model in 2D Cartesian coordinates can be described by The converted Doppler (8) is considered to be a generic position to define a pseudo-state that is linear with respect to measurement (7). Since the converted Doppler is quadratic in Cartesian states, it has limited derivatives for the CV model. Taking derivatives of the converted Doppler up to the second order using (21), we have It shows that the CV motion in 2D Cartesian coordinates can be described completely by and its first-order derivative in the state space corresponding to the generic position, converted Doppler. Then the pseudo-state vector can be defined as It can be seen from (25)-(31) that the pseudo-states evolve linearly in time, disturbed by the noise part. Based on the assumption that , , and are white Gaussian with zero mean and mutually independent, we have that the noises and are white with zero mean; the corresponding covariance can be obtained as For CV, CA, and CT models, the cross-covariance between and is proven to be zero. Therefore, the disturbance part in the pseudo-state equation can be treated as white. Additionally, the pseudo-state equation can be considered as linear. In this case, the MMSE can be used to produce a best linear estimation.

Converted doppler measurement Kalman filter
The debiased converted Doppler measurements in (19) guarantee the observability of the pseudo-states in (23) In the above, denotes the zero-mean Gaussian measurement noise with known variance given by (18). The CDMKF is derived under the linear minimum mean squared error (LMMSE) estimation frameworks as follows: Applying the expectation operator on the pseudo-state (25), (34), and (44) conditioned on the converted Doppler measurements up to time step , we can obtain the predicted pseudo-state as where is the estimation error at time step . Then the state prediction covariance is The predicted measurement can be obtained similarly by taking expectation (62) conditioned on the measurements until time step as ð66Þ Subtracting the above from (62), the measurement prediction error can be written as Then the measurement prediction covariance is given by And the cross-covariance between the pseudo-state and the measurement is The filter gain is calculated by Then the updated pseudo-state at time step is given as And the updated covariance is Eqs. (63)-(72) summarize the filtering procedure of the CDMKF with the key matrix parameters defined by the pseudo-state equation. The whiteness of the process noise and the linearity of the state equations guarantee that the CDMKF is a best linear estimator in the sense of MMSE. This provides a new method, rather than a nonlinear filter, to mitigate the noises in Doppler measurements and extract target information in pseudo-state space.
Note that the Cartesian state at time is required to determine the matrix in (30), (40), and (50). In practice, the true state is not available. The solution is to replace the true state by the state estimates at time step from the CPMKF. Also, the covariance for CV model in (32) can be approximated using components of as ð73Þ and the covariance for CA model in (42) can be approximated as The process noise is identical to those in (30), (40), and (50). Here, is the debiased converted position measurement in (11). The converted measurement error is zero mean with known covariance in (10). The subscript is utilized to indicate the matrixes or variables are related to the CPMKF. The implementation procedure of CPMKF is as below:

Correlation between CDMKF and CPMKF
As shown in Figure 1, the range measurements are commonly used in CDMKF and CPMKF. Therefore, the pseudo-state produced by the CDMKF and the Cartesian states from the CPMKF are not independent. The correlation should be handled appropriately in the fusion procedure.
According to the formulations in the CDMKF, we can rewrite the state estimate from CDMKF at time step as Then the corresponding estimation error is Similarly, the filtering error of CPMKF at time step can be obtained as Multiplying (84) by the transpose of (83) and taking expectation, the covariance between the parallel two filters can be updated recursively as In the above, is the cross-covariance between the converted position and Doppler measurements at time step . It can be given by (20). The above equation is a Lyapunov-type equation. The initial condition can be obtained from the initial covariance between the converted measurements.

Static fusion of CDMKF and CPMKF
The challenges, when combing the estimates from the CDMKF and CPMKF to obtain final state estimates, are not only the nonlinearity but also the correlation between the CDMKF and CPMKF. Since the pseudo-states from the CDMKF are quadratic in Cartesian states, the nonlinearity can be dealt with by expanding the pseudo-states up to the second order in a Taylor series. In the meantime, the crosscovariance between these two filters should also be taken into account. A static estimator is derived based on the framework of linear MMSE estimator to fuse the outputs from the two filters, with both the nonlinearity and the dependence handled simultaneously.
The problem is to estimate target states at time step using the pseudo-state estimates produced by the CDMKF and the Cartesian state estimates from the CPMKF. The prior mean of the state is ð86Þ given the state estimates of the CPMKF. A "measurement" ð87Þ is constructed to update the state of interest. The error is assumed zero mean, with known covariance , and is correlated to the estimation error of the CPMKF ð88Þ with cross-covariance in (85). To obtain the prior mean of measurement , the nonlinear function between pseudo-states from CDMKF and Cartesian states is expanded as a Taylor series around with terms up to the second order as Since the calculations are all performed at a single time step , the time subscript is omitted for simplicity. Here, denotes the Cartesian basis vector. In the above, ð90Þ is the Jacobian of the vector , evaluated at . Also, is the Hessian of the component of , and stands for the higher-order terms, which are all zero for the components with quadratic nonlinearity.
The prior mean of the measurement can be obtained by taking expectation on (89) conditioned on the estimates from the CPMKF as Then the covariance between the states to be estimated and the "measurement" is, incorporating their dependence, The covariance of the measurement is The items with in (93) and (94) arise from the correlation between the two converted measurement filters, and the other items are the same as the measurement update of the second-order EKF.
The static nonlinear estimates are obtained as The covariance associated with this combined estimate is The final target states are then evaluated by (95) from the pseudo-state estimates of the CDMKF and the state estimates of the CPMKF.

Initialization
In the SF-CMKF, there are three estimators that need to be initialized: (1) the CPMKF, (2) the CDMKF, and (3) the static fuser.
The two-point differencing method, which uses measurements from two consecutive time steps and to estimate the "position" components and the corresponding "velocity" components and the measurement covariance to approximate state covariance , is used. The initial state has the form as where is the remaining components. The corresponding covariance is given by ð98Þ ð99Þ where is the vector consisting of maximum values of the remaining components.
The initialization of the three estimators can be implemented all based on (97) and (98), using correct measurements and measurement covariance.
To initialize the CPMKF, we should use the converted position measurements in (11) and the converted position measurement covariance in (10). To initialize the CDMKF, the converted Doppler measurements in (19) and the corresponding covariance in (18) are used to calculate (97) and (98).
The initialization of the static estimator in the fusion step can be implemented similarly to (98) as ð100Þ where is the cross-covariance (20) between the converted position measurements and the converted Doppler measurements. Here, matrix is replaced by , which is because there is no correlation between the states that are initialized to zero.

Conclusions
Tracking with position and Doppler measurements, where the range and Doppler measurement errors may be correlated, was considered in this chapter.
First, the converted Doppler measurement Kalman filter, a linear filtering approach, can be used to improve state estimation results with the converted Doppler measurements, which are constructed by the product of the range and Doppler measurements. This estimation produces pseudo-state estimate. The pseudo-state equations for three commonly used target motion models, the CV, CA, and CT models, were presented, and the filtering procedures were derived.
Based on the converted Doppler measurement Kalman filter and the converted position measurement Kalman filter, a novel tracking filter was proposed to estimate target states from position and Doppler measurements. In this approach, the two converted measurement Kalman filters are used to produce recursive state estimates individually, and their outputs are fused outside the filtering recursion by a static nonlinear estimator, with nonlinearity and correlation handled properly. Since nonlinear operations are all shifted outside the filtering recursions and the two dynamic filters are the best linear MMSE estimators, the proposed method mitigates the effects of nonlinear filtering methods.