Fusion of cooperative localization data with dynamic object information using data communication for preventative vehicle safety applications

Cooperative sensors allow for reliable detection, classification and localization of objects in the vehicle’s surroundings – even without a line-of-sight contact to the object. The sensor principle is based on a communication signal between the vehicle and a transponder attached to the object of interest – a pedestrian, for example. Thereby, localization information is gathered by measuring the round-trip time-offlight (RTOF) and evaluating the angle-of-arrival (AOA) of the incident signal. After that, tracking algorithms are used to recover the kinematic state of the object providing a basis for situation assessment. This paper investigates possibilities and benefits of extending this principle by the communication of information from inertial sensors which are locally attached to the transponder. Furthermore, this paper presents a robust approach for fusing the localization data with dynamic object information using the Dempster-Shafer theory. The approach is evaluated by performing real-world experiments for the analysis of pedestrian accidents.


Introduction
Environment perception is still a major challenge for the realization of preventative vehicle safety applications.This includes detection, classification and tracking of surrounding objects such as other vehicles or vulnerable road users.Eventually, the quality and completeness of environment perception restricts the depth of intervention and determines the time at which an action can be taken.
The current performance of local perception sensors such as radars, lidars, or cameras already allows for the realization of a wide range of preventative vehicle safety applications.
However, local perception sensors always require a line-ofsight contact to the object of interest and suffer from uncertainties in the classification of the object type.
Motivated by these limitations, a different sensor concept based on the secondary radar principle and further referred to as "cooperative sensor" has been proposed in order to enhance the quality and completeness of environment perception (e.g., Shi et al., 2005;Andreone et al., 2006;Morhart and Biebl, 2008;Schwarz et al., 2011).The sensor consists of an on-board unit that is able to communicate with transponders which are attached to objects of interest in the vehicle's surrounding.Transponders cooperate with the vehicle by sending a dedicated response signal which is suited for measuring its relative position, e.g. by time-of-flight (TOF) or signal strength (RSSI), and allows a for unique target classification.Additionally, due to diffraction and transmission, localization is still possible in presence of a typical occlusion such as a parking vehicle (see e.g.Morhart and Biebl, 2008).
Particularly in urban scenarios, the past research activities have demonstrated a better localization performance in comparison to using the difference of GPS positions, currently done in V2X-communication.This is especially important when map-matching is not applicable -for example when tracking pedestrians.
This paper introduces the cooperative sensor concept of the current research project Ko-TAG.Section 3 presents a suitable algorithm for tracking cooperative pedestrians and derives its limitations.To overcome the limitations, Sect. 4 discusses possibilities of extending the transponder by inertial sensors, which has not been considered in this context yet.Therefore, the transponder becomes a sensor itself.In addition, the approach sets itself apart from convential tracking Published by Copernicus Publications on behalf of the URSI Landesausschuss in der Bundesrepublik Deutschland e.V.

H. Kloeden et al.: Fusion of cooperative localization data and inertial sensors
applications for navigation purposes (e.g., Foxlin, 2005).Based on the discussion, Sect. 5 derives a robust movement classificator using the Dempster-Shafer theory that is finally combined with the cooperative localization data in Sect.6 .The proposed method is evaluated in a real-world pedestrian accident scenario.A summary is given in Sect.7.

Cooperative sensor
The cooperative sensor developed in the research project Ko-TAG performs distance and angle measurements to localize transponders in the vehicle's surrounding.The following results are based on a system using an ISM carrier frequency at f c = 2.4 GHz, a bandwith of B = 83 MHz and a transmission power of P x = 20 dBm.Currently, an effort is being made to convey the system to the ITS band at f c = 5.9 GHz and a smaller bandwidth of B ≤ 50 MHz.A system verification at those frequencies has already been published which indicates a comparable system performance (Schaffer et al., 2011).Further information about the hardware and the localization principle can be found in various publications (e.g., Morhart and Biebl, 2008).

Distance measurements
The distance between a vehicle and a transponder is determined by round-trip time-of-flight (RTOF) measurements.Following the secondary radar principle, an interrogation pulse of the on-board unit is answered by a transponder after a defined guard time t w .Therefore, responses of closely spaced transponders are separated by Time Division Multiple Access (TDMA) by using different guard times.Finally, the distance r is calculated based on the propagation time t p between the interrogation pulse and the received answer r = 1/2 × (t p − t w ) × c, where c denotes the speed of light.
The measurement procedure is carried out by employing a signal correlation technique.Data bursts are encoded by a vehicle specific pseudo random code which is communicated to the transponder by the vehicle's interrogation pulse.Finally, the time-of-arrival is determined by correlating the received input signal with the vehicle's code and interpolating the correlation result.Furthermore, the use of orthogonal random codes ensures that communication from other vehicles does not interfere with the measurement procedure.
In free field conditions, the system enables reliable positioning within at least 200 m distance and when encoutering a sight obstruction between the vehicle and the target within about 30-50 m.The accuracy of the distance measurements depends on the situation: While cable measurements achieve an RMSE of about σ r = 0.05 m, multipath interference causes additional errors of about σ r = 0.30 m in line-of-sight (LOS) conditions and σ r = 1.50 m in non-line-of-sight (NLOS) conditions.

Angle measurements
If a valid transponder response is received, the angle-ofarrival (AOA) measurement device is triggered to indicate the time at which the transponder sends out a narrow band signal directly after the pseudo-noise code.The incident electromagnetic wave is spatially sampled at six antennas of an antenna array with an inter-patch distance of λ/2 installed at the front bumper of the vehicle.
Phases and amplitudes of the incident signal are used to determine the AOA using the Multiple Signal Classification (MUSIC) algorithm.The MUSIC spectrum is evaluated for incident angles from −60 to 60 degrees, whereas peaks in the spectrum indicate the AOA of the received signal.
In laboratory conditions, angle measurements can be taken with an accuracy of σ ϕ ≤ 1 • .However, in urban conditions, the accuracy is more susceptible to multipath reflections especially in combination with a low SNR.Typically, values of σ ϕ = 1-2.5 • are achieved in LOS scenarios and about σ ϕ = 3-6 • in NLOS scenarios within a distance of less than 100 m.

Communication
The measurement procedure includes the communication of a data package, mainly to transmit static information about the object such as its type or its dimensions.In the next hardware generation, the data package is extended to a size of 109 bit which enables the inclusion of dynamic object information.

Inertial sensors
To gather dynamic information, the next transponder generation is equipped with an inertial sensor system (INS) consisting of a 3-axis accelerometer and 3-axis gyroscope.The main reason for extending the transponder with inertial sensors comes from the necessity of identifying special situations including the cases "transponder not attached to a person", "person within a vehicle", or "free fall".However, this paper investigates the possibilities and benefits of further including dynamic information within the data package.To step ahead to the performance of the new transponders, the evaluation has been carried out by a separate unit of inertial sensors attached and synchronized to a recent transponder which allows for offline analysis.

Pedestrian tracking
The distance and angle measurements z = [r ϕ] T are input into the object tracking algorithm to filter the measurement noise and outliers as well as to recover the kinematic state of the object including its speed.Thereafter, the estimated state1 x = [x y v x v y ] T and its covariance P = cov( x − x) are the basis for situation assessment.

Tracking filter
Typically, tracking algorithms are realized on the basis of Bayesian filters that employ a motion model x k+1 = f (x k , w k ) to represent the object dynamics as well as a measurement model z k = h(x k , v k ) that relates the measurements z to the object state x.Herein, the random variables w and v denote the process and the measurement noise with a known probability distribution, respectively (Bar-Shalom and Li, 1993).
Further, using the common assumptions of normally distributed process noise w k ∼ N (0, Q k ) and measurement noise v k ∼ N (0, R k ) as well as a linear motion model the optimal filter with respect to a minimum mean squared error reduces to a standard Kalman filter with a known algorithmic solution.
The pedestrian motion is typically described by a constant velocity and direction, which leads to a linear model in the cartesian state space, where T denotes the sensor sampling time.The process noise originates from the assumption of a white noise acceleration with known power density q.Therefore, the motion model consists of a single scalar parameter q (Bar- Shalom and Li, 1993).
The measurement model describes the nonlinear conversion from polar into cartesian coordinates.However, the Kalman filter is commonly applied to a linearized formulation of the measurement model leading to a near-optimal solution referred to as Extended Kalman Filter (EKF) (Bar-Shalom and Li, 1993).The covariance of the measurements is known from the sensor evaluation (see Sect. 2).

Tracking limitations
With the above assumptions, the Kalman filter is parameterized with a single noise parameter q that embodies the tradeoff between filter dynamics and estimation accuracy (illustrated in Fig. 1).However, both characteristics are extremely important for situation assessment: reasonable accuracy -especially of the speed -is needed to predict the pedestrian movement over the desired time span to release a warning before a collision.On the other side, in order to reduce the number of false positives and negatives of the system, a reasonable dynamic behavior is needed to react on sudden movement changes.Step response of the tracking filter: Pedestrian suddenly starts movement towards the street (lateral speed: y ) at time t = 1.The higher the value of q, the faster the change of the speed estimate vy but the larger its uncertainty σ v y = cov( vy − v y ).
In literature, the problem of maneuvering objects is addressed by the means of adaptive state estimation (Bar-Shalom and Li, 1995).Using the input data or the model residuals, an indicator of the movement type is generated and utilized for adapting the motion model.However, even the more sophisticated methods -such as the interactive multiple model algorithm (IMM) -usually require a substantial time to generate a robust indicator.To further improve the filter dynamics, model or parameter switching has to be made extremly likely, which again causes an increase of the estimation uncertainties.
Consequently, the tracking filter needs additional information to optimize the trade-off between dynamics and accuracy.

Communication of dynamic information
This section investigates possibilities of using the dynamic information of an INS to overcome the tracking limitations.

Communication of dynamic states
The INS measures accelerations and yaw rates along the dimensions of the sensor coordinate system which depends on the relative position of the transponder with respect to the object.Therefore, in order to relate the data to the object, dynamic measurements have to be converted into the body coordinate system of the object.The roll and pitch angle are calculated by finding the gravitational vector in the acceleration data which is assumed to be orthogonal to the motion plane.Faster changes in the relative position are stabilized by using the gyroscope measurements.As a result, the angles can be determined with an accuracy of 1-2 • RMSE.However, estimating the yaw angle is generally harder and no robust approach suited for the application has been presented, yet2 .

H. Kloeden et al.: Fusion of cooperative localization data and inertial sensors
covariance ellipsis before EKF update speed vector v xy after EKF update covariance ellipsis after EKF update Fig. 2. Erroneous EKF update: with an initial speed of approx.zero, the yaw angle used for the EKF update is determined by noise only.After an update with v xy > 0, the speed vector points with high certainty to this arbitrary direction.
Consequently, the INS may provide the yaw rate ω z and the acceleration within the motion plane a xy .Furthermore, it is possible to estimate the speed of a pedestrian within the motion plane v xy by evaluating the step frequency from the acceleration data and assuming an average step length.The data introduces further dimensions in the measurement space and, therefore, requires an extension of the EKF update step, i.e.Eq. ( 2).However, the relationship v 2 xy = v 2 x + v 2 y to relate v xy to the state space x, for example, requires the knowledge of the yaw angle ψ = atan(v y /v x ) to identify the components v x and v y .In the EKF framework, the yaw angle is taken from the previous state estimate which may lead to inacceptable errors, explained in Fig. 2. As a result, the handling of nonlinearities requires a different filter topology than the standard tracking problem.
In contrast, the Unscented Kalman Filter (UKF) is able to handle such nonlinearites without leading to significant inconsistencies.However, when using the UKF or even more sophisticated filter topologies, the state estimate benefits from the dynamic data only after reliable information about the yaw angle is available.As the yaw angle is only determined by the cooperative localization data with a standard motion model with limited dynamics, the communication of dynamic states does not significantly contribute to the overall filter dynamics, especially when dealing with stopping vs. walking/running in the case of pedestrian tracking.

Communication of movement classification
Another possibility consists of using a movement classificator using features from the INS data and adapting an optimized motion model to the situation.The movement of a pedestrian, for example, can be classified as s ∈ S = {S, W, R}, whereas S: Standing, W : Walking, R: Running.Typically, the classifier is carried out in a Bayesian framework with features chosen in order to minimize the probathe yaw angle from motion characteristics which also suffers from a lack of robustness.
bility of false classification.The training causes the classifier to select low frequency features, e.g. the step frequency, which are very distinct in the different classes.While this leads to the aforementioned advantages, the use of low frequency features introduces a significant lag -typically in the order of seconds -that inhibits the improvement of the filter dynamics.On the other side, omitting those features leads to an inaccaptable probability of false classification (see Fig. 3).However, looking at the acceleration signal (e.g.Fig. 4), a maneuver can be suspected much earlier with a different concept of classification.

A fast and robust movement classificator
This section proposes a robust classificator that is suited for faster detection of movement changes.

Dempster-Shafer classificator
To achieve a faster movement classification, only high frequent features may be considered for classification.As a result, classes may not be separable in the reduced feature space, further illustrated in Fig. 3.However, instead of mapping every element of the feature space to a class F → S, the Dempster-Shafer theory suggests mapping every element of the feature space to the power set of the basis classes F → 2 S .Thereby, the classifier may express its uncertainty within a set of basis classes.
The classificator may be realized as follows: For every class A the two discriminators g Pl and g Bel define the regions of plausibility (Pl) and belief (Bel), with the aim that the Bel-region contains only samples that belong to A and the inverse of the Pl-region contains no samples that belong to A. The region inbetween Pl and Bel is considered to be unknown A = {A ∪ ¬A} with respect to class A.
While a strict criterion would lead to large "unknown" regions, the conditions need to be relaxed in order to minimize the relevant volume between Pl and Bel.A feasible cost criterion can be formulated by the use of slack variables ξ for every sample f i and the classification operator y A (g Pl , g Bel ) : F → {∅, A, ¬A, A }: Parameter 0 < α < 1 regulates the penalty of a sample being classified as "unknown" with respect to a misclassified sample, i.e. the volume of the "unknown" region vs. consistency.In addition, to avoid inconsistencies, β 1 is set to 3. Simulation example: while a linear Bayes classifier g By leads to a probability of false classification of about 30 %, the Dempster classificator reduces this probability to 0.5 % by assigning ambiguous regions to the unknown state A .Though offering a higher consistency, the Dempster classificator is only able to distinguish between A and ¬A with a probability of 67 %. a large value.Finally, Eq. ( 4) is used to find the optimized regions 3 for Bel and Pl.
m i = 1 which can be found by using the classification probabilities, e.g.m A = Pr(A|y A = A)δ, m ¬A = Pr(¬A|y A = A)δ, m A = 1− δ, while 0 < δ < 1 denotes a factor of mistrust.Finally, the classification results of the classes in S are combined using Dempster's rule of combination.
As a result, the classificator will be faster and more consistent than its Bayesian equivalent.However, during longer maneuvers, the classificator fails to persistently distinguish between related classes, e.g.R and W , that require the evaluation of low frequent features.Therefore, both classificators can be combined to yield a fast and robust result.

Fusion of Bayes and Dempster-Shafer
Likewise, using the classification probabilities of the confusion matrix, the result of a Bayesian movement classificator can be translated into a Dempster-Shafer mass function.However, when encoutering conflicting situations, the combination of the two classificators may lead to erroneous results when applying Dempster's rule.Such situations occur for example at the beginning of maneuvers in which the Bayesian classificator lags, while the Dempster classificator has already switched to a new state.Therefore, the masses m DS and m By are combined by using Yager's rule which penalizes conflicting situations by increasing the weight of the 3 In our problem we used a linear classifier in the feature space.Classes that are not linearly separable in the feature space can be transformed to a different feature space by using a kernel function unknown state: To reduce complexity, the fused result is mapped to an extended state space [0, 1] |2 S | → S + = {S, W, R, } including the unknown state .
Figure 4 shows the result of a test data sequence of the classificator fusion for pedestrian tracking.It can be observed that the movement state is assigned to "unknown" after about 200 ms and to "walking" after about 1 s.This behavior combines the advantages of a Bayesian classificator that enables the determination the exact type of motion in a long-term maneuver and a fast Dempster classificator that improves the filter dynamics and consistency.

Fusion of movement classication with localization information
The robust classificator is used for improving the tracking results of the cooperative sensor.

Extension of an IMM-Filter
The structure of an IMM enables the combination of localization information and movement classification on the basis of motion model probabilities.Using a Markov transition matrix and the model residuals, the basic structure of an IMM determines a suboptimal4 estimate of the motion model probabilities Pr(M i ) for a given set of models5 .Thereafter, the IMM probabilities P (M i ) are combined with the result of the movement classificator s + by considering the probability Pr(M i |s) (motion model M i best describes the object dynamics when s is in effect) and the probability Pr(s + |s) (accounts for misclassification).Using the updated model probabilities, the state and covariance can be calculated as a weighted mixture of every model.
To avoid information loops due to correlations in the classification error, the updated probabilities are not fed back to the IMM.Also, to account for data outages, one can buffer the last data package and predict it to the current fusion time by using a similar markov chain.

Evaluation in a pedestrian accident scenario
The proposed algorithm was realized in a test vehicle with a cooperative on board unit and real-time kinematic platform.Additionally, a pedestrian equipped with a transponder, inertial sensors and a DGPS-platform was situated on a sidewalk.The test sequence is comparable to the situation in Fig. 1 and consists of stopping, and walking and stopping in the lateral direction.
Figure 5 shows a typical plot of the estimated versus the true lateral speed v y with and without using the communication of dynamic states.With a very reliable detection of standing, the error and the uncertainty of v y is almost zero, whereas without communication the uncertainty equals σ v y = 0.3 m s −1 .While walking, the communication of the movement state and speed reduce the estimation uncertainty by a factor of 2 to about σ v y = 0.3 m s −1 .
Furthermore, the transition between standing and walking is detected about 400 ms earlier than in the case without communication.Likewise, the transition between walking and standing is detected about 250 ms earlier.
Figure 5 indicates that the transition from walking to standing is hardly lagged from the true pedestrian speed whereas the first transition lags by about 500 ms -although the transition was detected by the classficator fusion much earlier in time.The effect results from the unknown yaw angle that needs to be estimated from the localization data first.Herein, the choice of a better motion model makes the filter more sensitive to changes in direction, but still requires time to average over the noise.

Conclusions
The tracking filter of cooperative objects suffers from a tradeoff in filter dynamics and accuracy.However, both are needed to convey the early detection into an early warning for the driver.This paper discusses methods to overcome those limitations by the use of inertial sensor data.Based on the discussion, a fast and robust movement classificator is derived by combining a Bayesian and Dempster-Shafer classificator.The classification result is combined with the cooperative localization on the basis of motion model probabilities using an IMM-framework.Finally, a real world evaluation indicates improvements in both filter dynamics and accuracy.
Fig. 1.Step response of the tracking filter: Pedestrian suddenly starts movement towards the street (lateral speed: y ) at time t = 1.The higher the value of q, the faster the change of the speed estimate

Fig. 4 .
Fig. 4. Motion classification of the fused system output in comparison to the acceleration signals.The classificator combines the characteristics of a fast detection of movement changes as well as a reliable movement classification during long-term maneuvers.

Fig. 5 .
Fig. 5. Model probabilities Pr(M i ) and lateral speed estimate vy of a sequence recorded by the test vehicle.