Extended Kalman Doppler tracking and model determination for multi-sensor short-range radar

A tracking solution for collision avoidance in industrial machine tools based on short-range millimeter-wave radar Doppler observations is presented. At the core of the tracking algorithm there is an Extended Kalman Filter (EKF) that provides dynamic estimation and localization in realtime. The underlying sensor platform consists of several homodyne continuous wave (CW) radar modules. Based on In-phase-Quadrature (IQ) processing and down-conversion, they provide only Doppler shift information about the observed target. Localization with Doppler shift estimates is a nonlinear problem that needs to be linearized before the linear KF can be applied. The accuracy of state estimation depends highly on the introduced linearization errors, the initialization and the models that represent the true physics as well as the stochastic properties. The important issue of filter consistency is addressed and an initialization procedure based on data fitting and maximum likelihood estimation is suggested. Models for both, measurement and process noise are developed. Tracking results from typical three-dimensional courses of movement at short distances in front of a multi-sensor radar platform are presented.


Introduction
The fusion of electronics and mechanics is an ongoing process, which benefits from by downscaling and reduced production costs for various kinds of sensor technologies. For monitoring of machine states, all sorts of physical quantities are captured and analyzed. An even more sophisticated task is the prediction of future machine states and of temporary and instantaneous production steps, such as processes in milling machines. This leads to the notion of collision avoidance in automated machine tools in order to prevent damages and downtimes, which cause high maintenance and material costs (Wächter et al., 2014).
For these kinds of industrial machine tools, different approaches of systems for damage reduction are already under investigation, (e.g., Abele et al., 2012). However, for the time being, none of them is able to reliably predict the imminent risk and efficiently avoid collisions by proactive shutdown and deactivation.
In this contribution, the approach of a predictive 24 GHz Doppler surveillance and collision avoidance radar is described. Extensions and continuations of the fundamental investigations in Wächter et al. (2015), Azodi et al. (2013) and Azodi et al. (2014) are merged. A new signal processing stage for nonlinear target tracking based on Doppler shift estimates is introduced. It is essentially based on an Extended Kalman Filter (EKF). The algorithm is designed for short-range, single-target tracking. Appropriate process and measurement noise models are derived, test statistics are applied and trajectory estimation is shown. Recently, investigations on solvability, uniqueness of the solution, and the required minimum number of sensors were published by Shames et al. (2013). In the application discussed herein, the solution space for target position and velocity is restricted. Symmetries and ambiguities due to unfavorable sensor placement are reduced. Another approach, based on a two-stage filter, was proposed by Battistelli et al. (2013) in order to reduce the computational costs of a stand-alone particle filter.
The next sections are structured as follows: in Sect. 2, we introduce the model for mono-static Doppler radar on a moving observer platform and give the resultant equations. Section 3 contains the involved tracking algorithm, the underly-ing derived process and noise models, as well as an extensive study on appropriate filter initialization. Additionally, statistical tests for filter consistency are introduced. The methods are applied to simulation examples and experimental data described in Sect. 4. The final conclusion is given in Sect. 5.

Problem formulation and Doppler radar model
Within the scope of this work, we consider the following three-dimensional problem: The position of a single target in Cartesian coordinates shall be represented by and may be understood as the mass center of a point scatterer, which is assumed to be the dominant target compared to all other returns received from a cluttered environment. The observer platform comprises N sensors with states where the sensor positions are known, fixed and related to the origin of the global coordinate system, which lies in the center of the observer platform, see Fig. 1. The velocity vector of the platform is v = v x , v y , v z T and unknown. To get a state vector consisting of full three-dimensional position and velocity, we perform a transformation and assume the observer platform is fixed and the target is moving relative to the platform with just the reversed velocity. Hence, the full target state is A typical 2-D scenario (z = const) is illustrated in Fig. 1, with two sensors at the positions s (1) and s (2) , and a single moving target (scatterer) at position p with velocity v. In the following investigation, the radar sensors are assumed to have an isotropic radiation pattern, and they detect a target at distance s (n) − p with Doppler shift f (n) d . The system equation describes the evolution of the target state with time. The target state at time t k is given by position and velocity x = [p T , v T ] T . The kinematic state of the target can be deduced from the previous step t k−1 using the process matrix F of a constant velocity (CV) model. Small deviations from the true trajectory are modeled by zero-mean, white Gaussian noise w k , with corresponding process noise covariance matrix Q k . Then, the following linear discretetime dynamic model is obtained: The measurement equation relates the measurements z k to the state x k , where the dimension of z equals the number of sensors being involved. In mono-static radar systems the observed Doppler shift, corrupted by additive zero-mean, white Gaussian measurement noise n k , can be represented by where R k is the measurement noise covariance matrix. The measurement model is given by the nonlinear equation of the Doppler shift observed at the nth sensor where in contrast to Eq. (2), s (n) = s (n) x , s (n) y , s (n) z T is the sensor position in Cartesian coordinates, for all n = 1, . . ., N . · denotes the Euclidean norm. The radar wavelength λ = c/f c depends on the carrier frequency and the speed of light in the considered medium. Thus, λ has influence on the Doppler sensitivity, which is 160 Hz/(m s −1 ) in 24 GHz radar systems.
If only Doppler is estimated, Eq. (6) represents the measurement model, which is the only source of information in the localization algorithm. For state estimation, recursive Kalman filtering concepts are established as efficient algorithms. In this case, nonlinear approaches are necessary, like the EKF or the Unscented Kalman Filter (UKF), see e.g. Bar-Shalom et al. (2001), Anderson and Moore (1979), Julier and Uhlmann (2004). The application requires fast response on a millisecond scale (rapid estimation). Thus, it also necessitates short time for reaching a decision about shutdown. Additionally, due to the closely spaced sensor placement, the target observability is poor. Hence, a general maximum likelihood (ML) approach seems to be inappropriate here, as the number of required measurements might be very large in this nonlinear problem, to find an efficient, unbiased estimate. As a consequence, the integration time may be too long with re- Figure 2. Illustration of the radar system concept and fundamental signal processing blocks for single target multi-sensor tracking. This work is focused on the state estimation algorithm represented by the highlighted part. spect to the change rate of the state. The ML approach is used only for the initialization procedure to obtain reliable initial states close to the true target state.

State estimation via Kalman filtering
For the considered application the EKF is employed. Despite of the known flaws of the EKF, see e.g. Julier and Uhlmann (2004), first order linearization of the nonlinear Doppler function is assumed to be sufficient, since the scenarios considered here assume either constant velocity (CV, non-maneuvering) or constant acceleration within a limited range of values. If turning maneuvers and directional changes of motion have to be tracked, UKF is preferred (e.g. Smith, 2008). However, both EKF and UKF, require appropriate initialization of expected values and covariance matrices. Otherwise, tracking with these kinds of deterministic filters may lead to poor tracking accuracy or -in case of fatal ambiguities -to completely misdirected estimates.
In this section we introduce the adapted EKF algorithm, process and noise models, and give the initialization procedure based on maximum likelihood estimation. Furthermore, statistical methods for filter consistency tests are described.

Tracking filter algorithm
Taylor series expansion is used to linearize the measurement equation around the current target state x k . Second order terms and above are neglected. From Eq. (5) we get This will, from now on, serve as a linear measurement equation to establish the filtering procedure.
The system model consists of the linear equation given by Eq. (4). Now, the standard Kalman Filter equations can be applied for recursive state estimation, see Table 1 for the filter algorithm of an autonomous system (control input u ≡ 0), with F, Q and R being constant. Noise processes are assumed to have zero-mean Gaussian distribution, even though other distributions may be possible and may be tackled by the KF/EKF, which would lead to the best linear estimates. Table 1. Adapted EKF Algorithm. The initialization is based on MLE, with initial statex ML , the transition matrix F is linear and constant, and the measurement equation is linearized.

Measurement noise model
For experimental investigations low-cost CW radar sensor modules available from RFbeam Microwave GmbH (2014) were used. The signal processing chain consisting of radar sensor, analog-to-digital converter, digital low-pass filter and short-time frequency estimator is given in Fig. 2. The observed noise voltage of the down-converted, digitized IQ-signal at the mixer output typically has a variance of less than 1 µV 2 , with Gaussian probability distribution. The following frequency estimator based on FFT and spectral peak detection (Wächter et al., 2014(Wächter et al., , 2015 attains the Cramer-Rao lower bound (CRLB) for signal-to-noise ratios (S/N) larger than 0 dB. The frequency estimation uncertainty is Gaussian distributed with variances of less than 1 Hz 2 , dependent on the S/N and the length of the observation interval kT s . For mutually independent and uncorrelated sensors, the measurement noise covariance matrix R is described by where δ k denotes the Kronecker delta. The diagonal matrix R can be denoted as R = diag {σ 2 1 , σ 2 2 , . . ., σ 2 N }.

Process noise model
The linear drive system of machine tools are capable of fast traverse velocities up to 2 m s −1 and high accelerations up to 10 m s −12 . Also, the highly precise position encoder for synchronization of the drive ensures very low deviation from disturbance free motion (e.g. irregular motion and rattling).
In this contribution we consider only the non-maneuvering case of constant velocity (CV) withẍ = 0, see Bar-Shalom et al. (e.g. 2001, Ch. 6). Small, non-deterministic accelerations and deviations from the ideal rectilinear trajectory are modeled as a zero-mean Gaussian process, see Eq. (4), where Measurement and process noise are uncorrelated, as fully described by E{w k n T } = 0 ∀ k, .

Filter initialization
For initialization of a tracking filter, usually the expectation value of x 0 and the associated covariance matrix P 0 are used.
As prior information about the true mean x 0 is very limited, a non-Bayesian parameter estimation approach for position and velocity seems to be more appropriate, due to the fact that measurements and the corresponding true target state are very ambiguous and disturbed by noise (Bar-Shalom et al., 2001, p. 91f.). Restrictions due to the given geometry of the observed space in front of the sensor platform and the velocity interval from −|v max | to |v max | of the drive motor are considered. This leads to a non-convex, but constrained, sixdimensional optimization problem. The method of maximum likelihood estimation (MLE) is used in consideration of the following circumstances: -The a-priori knowledge about the true state is uncertain or not sufficient.
-The measurement function (Doppler equation) is highly nonlinear and ambiguous.
-The current target state is a deterministic constant.
-The length of the observation interval kT s does not interfere the constant parameter assumption.
-The measurement noise is a zero-mean Gaussian process with known covariance matrix R.
The MLE is given bŷ where Z k = {z 1 , . . ., z k } is the set of k observations gathered by N sensors. The likelihood function is given by where h(x) is the Doppler model given by Eq. (6), containing all unknown parameters. Here, the measurement noise is set to R = σ 2 I N for identical sensor properties.
Next, two extensive simulations are described. Figure 3 depicts the contour plots for MLE for a single measurement (left) and a set of ten sequential measurements z (right). For illustration, the zand v z -component were set to zero. In this example the velocity vector was preset and kept constant at √ 2) m s −1 throughout all simulation runs. The frequency estimation uncertainty was σ = 0.1 Hz. The colorbar represents the magnitude of the estimation error x −x . Since the whole geometry is symmetric, including a symmetric arrangement of ideal, isotropic sensors, the error has symmetric properties in the x-y-plane. This is influenced by the velocity components as well, e.g. if the sign of the v x -component is flipped, the pattern will be mirrored vertically. Broad areas with high error levels (top right and bottom left) indicate poor observability and solvability of the equation system due to high similarity of the obtained Doppler shift amongst the sensors. In Fig. 3 (right), significant accuracy improvements can be identified if several sequential Doppler estimates were exploited. The sampling interval between each estimate was 1 ms. Improvements can be identified in almost every region. Hence, more accurate initial estimates can be found by increasing the integration time. Of course, the resulting error characteristic shown in Fig. 3 depends on the velocity vector v. Hence, different velocities v result in different regions of poor observability.
A set of different random velocities is investigated next.

Filter consistency
Consistency of a state estimator means that the estimatesx are unbiased and the state estimation errors match the filtercalculated covariance matrix P k|k for a given finite number of measurements Z k ( Bar-Shalom et al., 2001, Ch. 5.4):  Magnitude of error (velocity) Figure 4. Averaged error magnitudes for increasing measurement noise variances σ 2 f . As shown in Fig. 3, a non-coherent ULA of five sensors is used, and sequential Doppler estimates Z k , with an integration time of 10 ms. For each grid point, the velocity vector was varied according to a set of 11 randomly defined, different approaching movements towards the ULA. This definition differs from parameter estimation, where consistency is an asymptotic property and an infinite set of samples is assumed.
The preferred measure for checking filter consistency in MC simulations is the covariance matrix P k|k , which is related to the estimate error x k|k = (x k −x k|k ). The normalized state estimation error squared (NEES) is defined by the quadratic form As x k|k is Gaussian, the NEES is the sum of the squares of n x independent zero-mean, unity-variance, Gaussian random variables. The filter is consistent, if the NEES has χ 2 n distribution with n x degrees of freedom (Bar-Shalom et al., 2001, Ch. 5.4). For illustration, Fig. 5 shows the χ 2 n -distribution for n = 2, 4 and 6. Furthermore, the confidence interval for n = 6 is highlighted. In three-dimensional scenarios, the (e.g.) 95 % confidence interval for NEES can be determined from the χ 2 6 distribution as χ 2 6 (0.025), χ 2 6 (0.975) = [1.237, 14.449] .
For observation of a set of N independent samples, the average NEES can be used as a statistical test for filter consistency: 0.25 2 4 6 8 10 12 14 16 x p(x) n = 2 n = 4 n = 6, (95% interval) Figure 5. Chi-Square distributions with 2, 4 and 6 degrees of freedom. For the case of n = 6, the 95 % confidence interval is given by [1.237, 14.449] (filled area). For optimized illustration, the axis are scaled appropriately.
NEES is restricted to simulations only, as it requires knowledge of the true state x k . An exploitation of the above quantity is impossible if real processes are observed, without knowledge of the true state. In this case a similar statistic can be evaluated, called the Normalized Innovation Squared: The residual covariance matrix S k = H k P k|k−1 H k T + R is part of the measurement update process, see Table 1. This can be applied in simulations, as well as for measurements. For analyzing a set of N independent samples, the average value is used, which is Usage of this quantity has several important benefits, e.g. outliers detection and gating capability, as demonstrated in the next section. In the simulated examples (see Fig. 6) the NIS and the 95 % confidence intervals will be given. It can be shown that E{χ 2 n } = n and var{χ 2 n } = 2n. Hence, the quantities NEES, NIS and the averages have to converge to n x resp. n z , if the filter models are correctly designed. These quantities were used in the following to check initialization and the running estimate in the simulations.

Simulation results
In this section, an illustrative simulation example, performed in Matlab, is described. The scenario is depicted in Fig. 6 (top), with the sensor platform inside the machine tool and six circularly arranged radars on the side front of it. The sensor radius is 20 cm. The global origin of the Cartesian coordinate system lies in the center of the platform. A single-target is given, moving towards the platform with constant velocity. This represents a typical situation of imminent danger, which has to be handled by a monitoring collision avoidance radar.
The initial target state x 0 is given in Table 2, together with the initial estimatex ML obtained from MLE. The initial state covariance matrix is set to P 0 = 0.1I 6 . These values can be derived from the results in Fig. 3. Additionally, the minor diagonal elements are overlaid by low-power additive noise (matrix still symmetric). This leads to improved convergence. Furthermore, the process and measurement noise matrices are defined as where G = T 2 /2, T 2 /2, T 2 /2, T , T , T T . The variances are set to σ 2 f = (0.4 Hz) 2 resp. σ 2 p = (0.4 m s −2 ) 2 . The sampling interval is T = 1 ms. The initial NEES = 0.25 n x Table 2. Comparison of true and estimated initial state in a simulation example. Sampling interval T = 1 ms, integration time 10 ms. The estimation error for position p 0 −p ML and velocity v 0 − v ML is 0.078 m resp. 0.059 m s −1 .
true initial estimated initial state: x 0 state:x ML has a quite low value. This comes along with low estimation error and the relative high initial covariance matrix. Better overall performance was observed during the MC simulations with larger elements in the initial covariance P 0 . Figure 6 (middle) gives an insight into the high tracking accuracy, with steadily decreasing estimation error. After 1 s, the final true position is p = (0.083, −0.014, 0.073) T m, which corresponds excellently with the estimated position of p = (0.083, −0.021, 0.077) T m. As can be identified from Fig. 6 (middle), the parameterẑ is the most erroneous one. However, this depends on the sensor configuration. If the sensors are rotated around the x axis, the estimation error z −ẑ becomes lower, whereas y −ŷ is slightly increased. The distance x is a crucial parameter in collision avoidance. It is estimated with sub-centimeter accuracy.
Estimated velocities are not depicted. The estimation properties of the velocities are similar to the coordinates: the estimation performance forv y is better compared tov z . This is again due to the sensor arrangement and could be improved by rotating the sensor arrangement. The parameterv x is estimated with very high accuracy.
In Fig. 6 (bottom) the NIS is depicted. This quantity is used for outlier tests, for the gating procedure, and for noise level increasing for the initialization period. Additionally, in Fig. 6 lower and upper bounds of the 95 % confidence intervals (χ 2 6 -distribution) are given as dashed lines. In this simulation, 95.4 % of all estimates (NIS) are within this acceptance interval. The time-average NIS avg has a value of 5.82, which matches excellently to the expectation of n z = 6.
A further statistical test for correct filter design is the mean-value of the innovation ν k , which has to be zero, with associated covariance matrix S k . In this simulation the obtained mean values were in the range −0.04 to 0.08 Hz.

Conclusions
This paper presented investigations and results on important issues of Doppler target tracking in short-range scenarios. The utilized Extended Kalman Filter (EKF) was designed considering the requirements for predictive 24 GHz Doppler radar processing aiming at collision warning and collision avoidance in industrial machine tools. The proposed procedure for filter initialization is based on Maximum Likelihood Estimation from several sequential measurements. It delivered reliable initial guesses close to the true target state. In combination with the known restrictions of the considered application, the utilization of a deterministic EKF was investigated. Simulation results confirmed the filter models and showed very high tracking accuracy.
As a next step, we plan to extend the algorithm to handle extended targets with more irregular shapes. In this case we expect that increased Doppler spreading due to multiple scattering centers will be observed and a data association problem will arise.