A New Approach To Estimate The Collision Probability For Automotive Applications

A New A ppr oach T o Estimate The Collision Pr obability F or A utomotiv e A pplications Richard Altendorfer and Christoph W ilkmann Abstract —W e re visit the computation of a probability of collision in the context of automotive collision a voidance (also referr ed to as conflict detection in other contexts). After r eviewing existing approaches to the definition and computation of a collision probability we argue that the question “What is the probability of collision within the next thr ee seconds?” can be answered on the basis of a collision probability rate. Using r esults on le vel cr ossings for vector stochastic processes we derive a general expr ession for the upper bound of the distribution of the collision pr obability rate. This expression is valid for arbitrary prediction models including process noise. W e demonstrate in several examples that distributions ob- tained by large-scale Monte-Carlo simulations obey this bound and in many cases approximately saturate the bound. W e derive an approximation f or the distribution of the collision probability rate that can be computed on an embedded platform. An upper bound of the probability of collision is then obtained by one-dimensional numerical integration over the time period of interest. A straightforward application of this method applies to the collision of an extended object with a second point-like object. Using an abstraction of the second object by salient points of its boundary we propose an application of this method to two extended objects with arbitrary orientation. Finally , the distribution of the collision probability rate is compared to approximations of time-to-collision distributions for one-dimensional motions that have been obtained pre viously . I . I N T RO D U C T I O N The implementation of a collision mitigation or collision av oidance system requires the computation of a measure of criticality in order to assess the current traf fic situation as well as its evolution in the short-term future. There are many criticality measures available, for example time-to-go (TTG) or time-to-collision (TTC) [1],[2], or the brake threat number [3]. All those measures are based on models of v arying degrees of complexity of touching or penetrating the boundary of the potential colliding object, e. g. both the TTC = − x (0) ˙ x (0) (for a constant velocity model) and the brake threat number a req = − ˙ x 2 (0) 2 x (0) are based on the one-dimensional collision ev ent x ( t ) = 0 . In this paper we focus on this underlying collision event – the boundary penetration – in a fully probabilistic manner, i. e. we propose a ne w approach to compute the collision probability for automotive applications. The use of this collision probability for decision making in collision mitigation or av oidance systems is not subject of this in vestigation. There are two different approaches to computing a collision probability for automoti ve applications that are known to the authors: Richard Altendorfer and Christoph W ilkmann are with Advanced Driv er Assistance Systems, ZF , Germany . { Richard.Altendorfer, Christoph.Wilkmann } @zf.com 1) probability of the spatial overlap of the host vehicle with the colliding vehicle’ s probability distribution, see [4], [5], and 2) probability of penetrating a boundary around the host vehicle, see [6]. There is currently no satisfying way to compute an automotive collision probability over a time period: there is a heuristic proposal to pick the maximal collision probability over that period as the collision probability for that time period [1], and there are calculations relying on strong assumptions (e. g. constant velocity models) that directly compute the collision probability ov er a time period [6]. On the other hand in the field of collision risk modeling for air traffic scenarios (for a recent overvie w see [7]) a special case of the general mathematical result on crossings of multi- dimensional stochastic processes [8] has been re-derived in [9] and applied to air traffic specific setups [10],[11],[12]. This allows for the computation of a collision probability ov er an extended period of time for aircraft modeled as axis-aligned cuboids or cylinders. Another approach based on a result for a one-dimensional stochastic process with particular dynamics has been suggested in [13]. In the follo wing, based on the formalism in [8] we will deriv e an expression for the upper bound of the probability of penetrating a boundary around the host vehicle in a time period ∆ T = [ t 1 , t 2 ] . This will be the result of the temporal integration of an upper bound of the probability rate for which we deriv e a general expression valid for arbitrary prediction models including process noise. Inclusion of process noise is crucial for collision avoidance systems since it allows to encode the uncertainty in the relative motion of the host and the colliding vehicle. This uncertainty is particularly relev ant in safety-critical applications with typical prediction times or TTCs of . 5 s where it is unknown whether the colliding vehicle keeps its motion, accelerates or slo ws down, or whether the host vehicle dri ver perceiv es the risk and slows down, for example. The basis of our deriv ations are the time-dependent distri- butions p t ( x, y , ˙ x, ˙ y, . . . ) , t ∈ ∆ T . Those distributions charac- terize a non-stationary vector stochastic process that represents the predicted relati ve state ξ − ( t ) of the colliding vehicle. The stochastic process can be the result of a dynamical system whose flow f can depend upon the state ξ , a time-dependent control input u ( t ) , process noise ν ( t ) , and time t : f ( ξ , u ( t ) , ν ( t ) , t ) (1) In the remainder of this paper the time dependence of ξ − ( t ) and its elements will be suppressed, howe ver the temporal dependence of probability distributions will be indicated by p → p t where appropriate. The expressions deriv ed in this article are designed to be ex ecuted on embedded, automotive platforms; hence we do not resort to methods that include Monte-Carlo simulations as in [11] or [1]. Instead we use lar ge-scale Monte-Carlo simulations of potentially colliding trajectories as collision probability ground truth in order to assess the accuracy of our results. The Monte-Carlo outcome is represented by a histogram of the number of collisions that occur within a time interval with respect to time. Therefore simulating colliding trajectories naturally leads to the concept of a collision prob- ability rate which is central to this article. The main contributions of this paper are as follows: 1) the incorporation of the mathematical theory of level crossings of multi-dimensional stochastic processes de- veloped in [8] for the definition and computation of a general collision probability and deriv ation of upper bounds of the collision probability rate as well as the collision probability based on the entry intensity from [8] (section IV -A) 2) deri vation of approximate closed-form formulae for the collision probability rate for automotiv e applications (section IV -C and app. C) 3) a nov el numerical study of the collision probability rate with special emphasis on the accuracy of our approx- imate formulae as well as on the upper bound and its saturation (section V) 4) proposal of an adaptive method to ef ficiently sample the collision probability rate (section V -D) 5) application of the computation of collision probability to a probabilistic treatment of two extended objects with arbitrary orientation by representativ e salient points of an object’ s geometry (sections IV -B and V -E) 6) comparison of the distribution of the collision proba- bility rate to approximations of time-to-collision dis- tributions for one-dimensional motions from existing approaches (section VI) Note that while some of the contributions are specific to a two- dimensional setup and hence readily applicable to automo- tiv e problems others are generally applicable to collisions of higher-dimensional objects with piecewise smooth boundaries. In the following two sections we will critically revie w existing approaches to computing a collision probability , first on the basis of spatial ov erlap and second with respect to boundary penetration. I I . C O L L I S I O N P R O BA B I L I T Y F R O M 2 D S PA T I A L OV E R L A P This is the probability of the spatial overlap between the host v ehicle and the colliding v ehicle that has been in vestigated in [10], [4], [5]. First, an instantaneous ov erlap probability is computed which inv olves integrals of the type P I O ( t ) = Z Z Z ( ψ ,y,x ) ∈ D p t ( x, y , ψ ) dxdy dψ (2) where the state variables differ according to the model used (2D or 3D, with or without orientation angle ψ ). The collision volume D can be restricted to the vehicle boundary or can include a safe distance. Note that e ven in the simplest case of only 2D position and Gaussian distribution the resulting two-dimensional integral, i. e. the cumulati ve distribution function of a biv ariate Gaussian, cannot be solved in closed form; howe ver , numerical approximation schemes exist [14]. A problem of deriving an instantaneous collision probability 2 3 4 5 6 7 8 t [s] 0 0.05 0.1 0.15 0.2 0.25 0.3 P(t) [] P(t) from 2D overlap TTC x Fig. 1. Example of an instantaneous collision probability ov er time deriv ed from a collision defined by spatial overlap as described in section II. This is based on the first scenario described in sec. V -A with initial condition in front of the host vehicle. from 2D spatial overlap is that this approach directly yields a collision probability for a specific time, see fig. 1. Hence it does not allow to answer the question “What is the probability of collision within the next three seconds?” because inte gration of the collision probability ov er time does not yield a collision probability over a certain time period as already pointed out in [6]. In particular , time is not a random variable that can be marginalized over and an integral over a time interval ∆ T : R ∆ T P I O ( t ) dt has dimension of time and does not constitute a probability . A heuristic proposal to solve this problem has been to pick the maximal collision probability over a time period as the collision probability for that period [13],[1]. This proposal has also been used in the definition of an ov erlap probability in [10] where it was shown in a Monte-Carlo simulation that the overlap probability and a collision probability based on boundary crossings - discussed in the next section - are rather unrelated since they differ by two to three orders of magnitude. Another issue is that an instantaneous collision probability based on the ov erlap of a spatial probability distribution with the area of the host vehicle is determined by those sample trajectories whose current end points, i. e. the position at the current time, lie within the area of the host vehicle. But this is independent of when the trajectory has crossed the host vehicle boundary hence all end points except those exactly on the boundary (whose contribution to the two-dimensional integral is zero) correspond to a collision event in the past and therefore too late for collision avoidance, see also fig. 1 for an example where the maximum of the instantaneous collision probability from spatial overlap occurs after the TTC in x- direction. Also, by only considering trajectories with current end points within the area of the host vehicle other colliding trajectories with current end points outside the host vehicle area that hav e already entered and exited the boundary are unaccounted for . What we are actually interested in is the probability of the colliding object touching and/or penetrating the boundary of the host vehicle. This requires a different approach than integration over state space as in eq. (2) since the integral over a lo wer-dimensional subspace would always be zero. Some existing approaches that consider a boundary instead of a state space v olume for the computation of a collision probability are revie wed in the next section. I I I . C O L L I S I O N P R O BA B I L I T Y AT B O U N D A RY A probabilistic approach to computing the probability of penetrating a boundary - instead of the probability of a spatial ov erlap - has been proposed in [6]. Their method is based on the probability density of the time to cross a straight, axis- aligned boundary assuming a constant velocity model. The deriv ed collision probability refers to a time period and not just a time instant. It is only applicable to straight paths or combinations of piecewise straight paths and does not take into account more complex geometries such as a rectangle. It relies on a separation of longitudinal and lateral motion. Another limitation is that the stochastic nature of their conflict detection approach only comes from the distribution of the initial condition of their state – process noise is not considered. A somewhat complementary approach is taken in [13] for aircraft conflict detection in the sense that process noise is incorporated whereas the uncertainty of the initial condition is not. They propose two different algorithms, one for mid- range and one for short-range conflict detection. F or mid-range conflict detection their measure of criticality is an instanta- neous probability of conflict and similar to the 2D spacial ov erlap discussed in the previous section. It is computed by a specific Monte-Carlo scheme. On the other hand their short-range conflict detection is based on the penetration of a spherical boundary around the aircraft as criticality measure. The dynamics is a constant velocity model perturbed by Brownian motion. The many strong assumptions, in particular constant velocity motion, specific Brownian noise model, and decoupling into one-dimensional motions make this approach hard to generalize. The approaches discussed above are limited to constant ve- locity models with assumptions on the coupling of longitudinal and lateral motion, they either incorporate specific process noise or no process noise at all or exclude the uncertainty of the initial condition. Additionally , they all rely on a time- to-go or TTC as a prerequisite quantity - either probabilistic or non-probabilistic. As we will show in the next section, such a temporal collision measure is not necessary for the computation of a collision probability . Instead, we show that a fundamental quantity to compute the collision probability for stochastic processes is the collision probability rate . The mathematical foundation for this approach was provided in [8] in terms of a stochastic intensity for piece wise smooth boundaries. Collision probability rates have already been used in the context of air traffic collision risk modeling: In [9] an intensity specific to a hyperrectangular boundary has been re-deri ved although [8] was kno wn to the authors. A subsequent publication ([11]) also includes definitions and theorems independent of established definitions and theorems from [8] for those hyperrectangles. Further applications to air traffic collision risk modeling are presented in [10] and [12]. In the following section we define a collision probability and derive upper bounds of the collision probability and collision probability rate based on standard concepts from the theory of lev el crossings and the intensity for more general boundaries from [8]. I V . C O L L I S I O N P R O BA B I L I T Y R AT E A T B O U N D A R Y A. Derivation of an upper bound for the collision pr obability rate W e hav e seen that simulating colliding trajectories naturally giv es us a probability rate and that a collision probability rate allows us to perform temporal integration to arriv e at a collision probability for an extended period of time. An expression for the upper bound of the collision probability rate will be derived on the basis of a theorem on boundary crossings of stochastic vector processes. For sake of lucidity of arguments we restrict ourselves to one of the four straight boundaries of the host vehicle, see fig. 2; extension to the other boundaries is straightforward. W e start with the prediction of the pdf of a state vector that at least contains relativ e position and its deriv ativ e, i. e. ξ = ( x y ˙ x ˙ y · · · ) > for a two-dimensional geometry , of a colliding object from an initial condition at t = 0 to a future time t where process noise ν ( t ) is explicitly incorporated: prediction : p 0 ( x, y , ˙ x, ˙ y, . . . ) t,ν ( t ) 7− → p t ( x, y , ˙ x, ˙ y, . . . ) Note that we do not make any assumptions on the used prediction model as well as noise model or explicit temporal dependencies, hence the stochastic dynamical system that giv es rise to this pdf could also explicitly depend upon time or a time-dependent control input u ( t ) . In order to cast the following expressions into a more readable format we define a probability distribution that only depends upon relati ve position and its deri vati ve by marginalization (see app. A for marginalization of Gaussian densities, for example) of the predicted pdf over the other variables: 1 p t ( x, y , ˙ x, ˙ y ) := Z other v ar . p t ( x, y , ˙ x, ˙ y, other v ar . ) d (other v ar . ) Giv en the pdf p t ( x, y , ˙ x, ˙ y ) what we are looking for is an expression for dP + C dt (Γ f ront , t ) i. e. the collision probability rate dP + C dt with dimension [ s − 1 ] at time t for the front boundary Γ f ront . The superscript + is used to denote that this probability rate is referring to boundary crossings from outside to inside. 1 The state vector ξ = ( x y ˙ x ˙ y ¨ x ¨ y ) > specified in app. B is an obvious extension of the minimal state vector above with corresponding white noise jerk model described in eq. (26) and is used as an example to illustrate the computation of collision probability rate. It is howe ver by no means specific to the results stated in this paper . 1) An Intuitive Motivation: W e start with the probability of the colliding object being inside an infinitesimally thin strip at the boundary Γ f ront (see fig. 2) dP + C (Γ f ront , t ) = Z ˙ y ∈ R Z ˙ x ≤ 0 Z y ∈ I y p t ( x 0 , y , ˙ x, ˙ y ) dxdy d ˙ xd ˙ y Here, since we are only interested in colliding trajectories, i. e. trajectories that cross the boundary from outside to inside, we do not fully marginalize ov er ˙ x but restrict the x − velocity to negati ve values at the boundary . A collision probability rate can now be obtained by dividing the unintegrated differential dx by dt ; in that way the “flow” of the target vehicle through the host vehicle boundary is described at x 0 with velocity ˙ x ≤ 0 : dP + C dt (Γ f ront , t ) ' − Z ˙ y ∈ R Z ˙ x ≤ 0 Z y ∈ I y p t ( x 0 , y , ˙ x, ˙ y ) ˙ x dy d ˙ xd ˙ y (3) Here, since the velocity is restricted to negativ e values a minus sign is required to obtain a positive rate. This e xpression constitutes the expected value of the velocity component perpendicular to the boundary ev aluated at the boundary . 2) Derivation based upon the theory of level cr ossings: This intuiti ve deri vation can be amended as well as generalized in a mathematically rigorous way by in v oking a result on crossings of a surface element by a stochastic vector process stated in [8] and generalized in [15] and [16]. First we need to set up the notations and definitions for entries and exits (level crossings) across the boundary of a region. Let ζ ( t ) be a continuously dif ferentiable n − dimensional vector stochastic process with values x ∈ R n . The probability densities p t ( x ) and p t ( ˙ x , x ) exist where ˙ x ∈ R n are the values of ˙ ζ ( t ) . 2 Let the region S ∈ R n be bounded by the smooth surface ∂ S defined by the smooth function g as ∂ S = { x : g ( x ) = 0 } and let Γ ⊆ ∂ S be a subset of that surface. Let n Γ ( x ) be the surface normal at x directed towards the interior of the region. A sample function x ( t ) of ζ ( t ) has an entry (exit) across the boundary Γ at t 0 if g ( x ) > 0 ( g ( x ) < 0) ∀ t ∈ ( t 0 − , t 0 ) and g ( x ) < 0 ( g ( x ) > 0) ∀ t ∈ ( t 0 , t 0 +  ) for some  > 0 . For a temporal interval ∆ T = [ t 1 , t 2 ] the number of entries/exits across Γ in this interval is denoted by N ± (Γ , t 1 , t 2 ) . The importance of this mathematical setup is that using the number of entries a collision probability ov er ∆ T can be defined 3 as P + C (Γ , t 1 , t 2 ) := P  g ( x ( t 1 )) ≥ 0 , N + (Γ , t 1 , t 2 ) ≥ 1  + P ( g ( x ( t 1 )) < 0) | {z } =0 = P  N + (Γ , t 1 , t 2 ) ≥ 1  (4) i. e. the probability that the stochastic process enters the boundary in ∆ T at least once with initial value outside the boundary . The probability that the process is outside 2 Further assumptions on the stochastic process and its probability densities apply [8]. 3 This definition is motiv ated by the probability distribution of the maximum of a continuous process, see e. g. [17]. the boundary at initial time t 1 should be one in automotiv e applications where a collision probability is to be computed for a time interval that begins at a time when the collision has not happened yet. The first moment of N + (Γ , t 1 , t 2 ) can be used to obtain an upper bound for P ( N + (Γ , t 1 , t 2 ) ≥ 1) : 4 P ( N + (Γ , t 1 , t 2 ) ≥ 1) ≤ E  N + (Γ , t 1 , t 2 )  (5) This becomes obvious by writing out the expressions above: P ( N + ≥ 1) = ∞ X k =1 P ( N + = k ) ≤ ∞ X k =0 k P ( N + = k ) = E  N +  (6) It also shows that if the probabilities for two or more entries are much smaller than for one entry then E { N + (Γ , t 1 , t 2 ) } is not just an upper bound but a good approximation to P ( N + (Γ , t 1 , t 2 ) ≥ 1) . It remains to compute the first moments for entry and exit which can be obtained via temporal integration of the entry/exit intensities µ ± as defined (see e. g. [17]) below: t 2 Z t 1 µ ± (Γ , t ) dt := E  N ± (Γ , t 1 , t 2 )  (7) By combining eqs. (4) and (5) and ev aluating the temporal deriv ati ve with respect to t 2 at t 1 we obtain dP + C dt (Γ , t 1 ) ≤ µ + (Γ , t 1 ) (8) i. e. we have deriv ed an upper bound for the collision proba- bility rate . This upper bound can be further ev aluated using the explicit expression for the entry/exit intensities µ ± from [8]: µ ± (Γ , t ) = Z x ∈ Γ E n h n Γ ( x ) , ˙ ζ ( t ) i ±    ζ ( t ) = x o p t ( x ) ds Γ ( x ) (9) where h· , ·i is the scalar product, ds Γ ( x ) is an infinitesimal surface element of Γ at x and ( · ) + := max( · , 0) and ( · ) − := − min( · , 0) . Equation (9) holds for general non-Gaussian as well as non-stationary stochastic processes. In order to apply eq. (9) to the front boundary Γ f ront as in fig. 2 we need to perform the following identifications: 5 ζ ( t ) = ( x, y ) > Γ f ront = { ( x, y ) : x − x 0 = 0 ∧ y ∈ I y } g Γ f r ont ( x ) = x − x 0 n Γ f r ont ( x ) = ( − 1 , 0) > ds Γ f r ont ( x ) = dy (10) Hence we obtain for the intermediate expectation operator 4 Using Markov’ s generalized inequality also a lower bound can be derived in terms of the first and second factorial moments [8]. 5 From now on we now do not distinguish anymore between a stochastic process and its sample values. H os t v eh i c l e Fr on t boun dar y : fron t  righ t  le ft  re ar  0 x y I L y R y x y dx In fini t es i mall y thi n s tr i p Fig. 2. Horizontal view of the host vehicle rectangle with local Cartesian coordinate system and coordinate origin at the middle of the front boundary characterized by x = 0 and y ∈ [ y L , y R ] = I y . E n h n Γ f r ont ( x ) , ˙ ζ ( t ) i +    ζ ( t ) = x o = − Z ˙ y ∈ R Z ˙ x ≤ 0 ˙ x p t ( ˙ x, ˙ y | x, y ) d ˙ xd ˙ y (11) and the entry intensity becomes µ + (Γ f ront , t ) = − Z y ∈ I y   Z ˙ y ∈ R Z ˙ x ≤ 0 ˙ x p t ( ˙ x, ˙ y | x 0 , y ) d ˙ xd ˙ y   p t ( x 0 , y ) dy = − Z ˙ y ∈ R Z ˙ x ≤ 0 Z y ∈ I y ˙ x p t ( x 0 , y , ˙ x, ˙ y ) dy d ˙ xd ˙ y (12) This shows that the intuitiv e deriv ation of the collision prob- ability rate (eq. (3)) results in the correct expression for the upper bound. It should be noted, ho wev er, that the application of the formalism above to a rectangular boundary of the host vehicle is just an example. By the theorem stated above the formula can be applied to any subsets of smooth surfaces, in- cluding higher dimensional ones for three-dimensional objects, for example. The computation above applies to the front boundary of the host vehicle. Since the results in [8] are also valid for piecewise smooth boundaries 6 the entry intensities of the four boundaries can be added. Hence the total entry intensity is giv en by µ + (Γ host vehicl e , t ) = µ + (Γ f ront , t ) + µ + (Γ rig ht , t ) + µ + (Γ lef t , t ) + µ + (Γ rear , t ) (13) W ith these expressions the collision probability rate and colli- sion probability for the surface subset Γ within a time interval ∆ T = [ t 1 , t 2 ] are bounded by dP + C dt (Γ host vehicl e , t 1 ) ≤ µ + (Γ host vehicl e , t 1 ) (14) P + C (Γ host vehicl e , t 1 , t 2 ) ≤ t 2 Z t 1 µ + (Γ host vehicl e , t ) dt (15) 6 This corrects a contrary statement in [7]. The results of [8] have been applied to polyhedral [18] and other regions S with a piecewise smooth surface ∂ S , for an overview see [19]. In summary the upper bounds are due to the approximation of the probability of one or more boundary entries by the expected number of boundary entries (inequality (5)). Note that the stochastic process ξ representing the state of the colliding object needs to contain 2D relative position ( x y ) > and 2D relative velocity ( ˙ x ˙ y ) > . In many AD AS applications the target vehicle dynamics for moving as well as stationary vehicles is modeled directly in relativ e coordinates. For state vectors that do not contain the 2D relativ e velocity but other quantities such as the velocity ov er ground (see e. g. [20]), a probabilistic transformation to relativ e velocities must be performed first. B. Entry intensity for two extended vehicles In pre vious sections the colliding vehicle was modeled as a point distrib ution corresponding to a single reference point (for example the middle of the vehicle’ s rear b umper). This allo wed the direct application of the theory of boundary crossings of a point process. Using strong assumptions about the two colliding objects’ shape and orientation this can also be applied to two extended objects: the collision models in [12] assume either a partially isotropic shape (cylinder) or use axis-aligned cuboids and can hence be reduced to the collision of an extended object and a point distribution as described in [12]. Note that the collision of a rectangular host vehicle with a circular object (this could serve as the approximate horizontal shape of a pedestrian) can also be reduced to the collision of the circle midpoint with an enlarged “rectangle” with rounded edges as shown in fig. 3. Hos t v ehi cle Hos t v ehi cle enl ar g ed , r ounded ֞ P oin t object Cir cular obje ct Fig. 3. Equiv alent representations of objects regarding collision modeling. While in many AD AS applications the object state is modeled by a single reference point and possibly additional attributes, for many collision avoidance applications the object exten- sion/geometry is crucial, e. g. for scenarios with fractional ov erlap. In [21] a method to compute the collision probability between a rectangular host and rectangular object vehicle was deriv ed using our method described above. By assuming both the host trajectory and the object orientation to be deterministic the collision between two oriented rectangles was reduced to the collision of a point and an octagon. Howe ver , for example in EuroNCAP-relev ant junction scenar - ios where an oncoming object turns into the host vehicle’ s path the latter assumption might not be v alid. Instead, in order to represent the extended geometry of the colliding object, we model the object’ s dynamical state (see app. B) relativ e to a specific reference point, for example the middle of the rear bumper or the rear axle, and then transform the state probability distrib ution to its boundary (see app. D). The entry intensity can be obtained for every point on the H os t v eh i c l e R ear l ef t c orn er R ear ri gh t c orn er F ron t r i gh t c orn er Sali en t poi n ts : F ron t l ef t c orn er Fig. 4. Horizontal view of the host vehicle and colliding object vehicle with reference point and salient points at the four corners. colliding object’ s boundary which results in a family of entry intensities. W e approximate this family by the entry intensities of a small number of representative salient points of the colliding vehicle’ s two-dimensional geometry as in fig. 4. The collision probability for the extended colliding object can then be approximately determined by the collision probability of the “riskiest” salient point which we define to be the one where the collision probability exceeds a certain threshold the earliest. A fully worked example of this approach is given in sec. V -E. C. Implementation for Gaussian distrib utions For further computations - especially in the Gaussian case - it will be conv enient to marginalize ov er ˙ y and rewrite eq. (12) in terms of a conditional probability: µ + (Γ f ront , t ) = − p t ( x 0 ) Z y ∈ I y Z ˙ x ≤ 0 ˙ x p t ( ˙ x, y | x 0 ) d ˙ xdy (16) For general distribution functions the integral in eq. (16) cannot be computed in closed form and numerical integration methods must be used. Even in the biv ariate Gaussian case there is no explicit solution known to the authors. Howe ver , by a T aylor-e xpansion with respect to the off-diagonal element of the inv erse cov ariance matrix of p ( y , ˙ x | x 0 ) as detailed in app. C, the integral can be factorized into one-dimensional Gaussians and solved in terms of the standard normal one- dimensional cumulativ e distribution function Φ . T o zeroth order the integration yields: µ + (Γ f ront , t ) = − N ( x 0 ; µ x , σ x ) ·  µ ˙ x | x 0 Φ  − µ ˙ x | x 0 ˜ σ ˙ x | x 0  − ˜ σ 2 ˙ x | x 0 N (0; µ ˙ x | x 0 , ˜ σ ˙ x | x 0 )  · ·  Φ  y R − µ y | x 0 ˜ σ y | x 0  − Φ  y L − µ y | x 0 ˜ σ y | x 0  + O  Σ − 1 12  ! (17) Here, if Σ ∈ R 2 × 2 is the covariance matrix of p ( ˙ x, y | x 0 ) , then ˜ σ ˙ x | x 0 = q | Σ | Σ yy and ˜ σ y | x 0 = q | Σ | Σ ˙ x ˙ x , see app. C where the integration has also been carried out to first order in Σ − 1 12 . Expression (17) can be computed on an embedded platform using the complementary error function av ailable in the C math library . 7 In the next section an extensi ve numerical study using the abov e formulae and Monte-Carlo simulations is presented. V . N U M E R I C A L S T U D Y A. The collision pr obability ground truth: lar ge-scale Monte- Carlo simulations Here, we want to in vestigate two examples of possible collision scenarios, one where the target vehicle is currently in front of the host vehicle and one where it is on the front right side. In order to obtain ground truth data for the future collision probability Monte-Carlo simulations are performed. The target vehicle on a possibly colliding path with the host vehicle is modeled by the state vector ξ = ( x y ˙ x ˙ y ¨ x ¨ y ) > and the dynamical system as specified in appendix B. The target vehicle is chosen to be detected by a radar sensor mounted at the middle of the front bumper of the host vehicle. Note ho wever that this state vector as well as the dynamical system specified in appendix B constitute just an example – the central results in section IV -A hold for general non- stationary as well as non-Gaussian stochastic processes. In particular , the absence of assumptions on the stationarity of the stochastic process means that processes deri ved from more general dynamical system – including systems with explicit time dependence or time-dependent control inputs u ( t ) – are cov ered. The starting point for an individual simulation is a sample point in state space ξ − i where the target vehicle is some distance away from the host vehicle - either directly in front or coming from the right side, see fig. 5. This sample point is drawn from a multiv ariate distribution characterized by its mean vector and cov ariance matrix which is usually the output of a probabilistic filter that takes into account the history of all previous sensor measurements that have been associated with this object. Instead of arbitrarily picking specific values for this initial cov ariance matrix we take its v alues from steady state at this mean vector using the discrete algebraic Riccati equation for typical radar detection measurements. 8 An instance ξ − i of an initial state of the target vehicle is drawn as a sample of N ( ξ − ; µ − ξ , P − ∞ ) . This state is predicted using the stochastic differential equation (26) until it crosses the host vehicle boundary or a certain time limit is exceeded. Hence: collision e vent = crossing of the target vehicle path with the host vehicle boundary . The time until the crossing is recorded and a new simulation with a new sample of initial conditions is started. Examples of colliding trajectories starting from an 7 Φ is related to the error function erf and complementary error function erfc by Φ( x ) = 1 2 erfc  − x √ 2  = 1 2 − 1 2 erf  − x √ 2  . 8 Strictly speaking there is no steady state at those points since the system is non-linear and the relative speed is not zero. Ne vertheless the solution of the Riccati equation is still representativ e if the filter settles within a smaller time period than the time period in which the state changes significantly . initial position in front of the host vehicle are depicted in fig. 5. W e have performed simulations of N traj = 3 · 10 6 trajectories for the two starting points. The result is represented by a histogram of the number of collisions that occur within a histogram bin, i. e. time interval, with respect to time. Hence simulating colliding trajectories naturally leads to a collision probability rate which is by construction the distribution of the TTC. An example is giv en in fig. 6 where the bins are normalized by the total number of trajectories N traj and the chosen bin width of dt = 0 . 05 s to obtain a collision probability rate. In addition, the collision probability rate integrated by simple midpoint quadrature from 0 to time t is shown. In this example the probability of collision with the target vehicle exceeds 60% within the first 6 s . The asymptotic value of the collision probability as t → ∞ indicates the overall probability of collision over all times. W ith the setup explained above the following questions can be addressed: • Is the expression for calculating the entry intensity from eq. (9) consistent with the results from large scale Monte- Carlo simulations? • Ho w does the approximation (17) perform in comparison with the numerical integration of the deri ved expression (16) for the entry intensity? • Can the computational effort be reduced by increasing ∆ t and still accurately calculating the entry intensity? • Does the entry intensity still reproduce results from Monte-Carlo simulations after non-linear transformation from a reference point to representativ e salient points of the colliding vehicle’ s geometry? B. Is the upper bound of the collision pr obability rate corrob- orated by Monte-Carlo simulation? In order to address the first question, large scale Monte- Carlo simulations as described in sec. V -A hav e been per- formed. Entry intensities were calculated based on 3 · 10 6 sample trajectories for each of the two initial conditions N i ( ξ − ; µ − ξ i , P − ∞ ) , where µ − ξ i is shown in table I, and P − ∞ is calculated using the discrete Riccati equation with matrices defined in appendix B. The two initial conditions ( i ∈ { f , f r } ) describe a starting point directly in front of the host vehicle, and in front to the right at an angle of 45 degrees with respect to the host vehicle. The inclusion of process noise as well as the inclusion of acceleration in the state vector as specified in appendix B allows for multiple entries and enables us to assess the influence of multiple entries on the accuracy of the upper bounds deriv ed abov e. 9 T able II shows the number of collisions di vided into the respectiv e boundaries of the host vehicle where the impact or boundary crossing occurred for the two different simulations. The resulting histograms of the collision probability rates are shown in fig. 7 together with the entry intensity obtained by numerical integration of the biv ariate Gaussian in (16) as well as the difference between 9 This is in contrast to the aviation-specific numerical study in [11] where during the part of a trajectory where a collision could occur a constant velocity model without process noise was used and hence multiple entries were excluded. T ABLE I M E AN O F I N I TI A L C O N DI T I O NS F O R M O N T E - C A R LO S I MU L A T I O NS µ − ξ Scenario front ( f ) front right ( f r ) µ − x [ m ] 10 10 µ − y [ m ] 0 10 µ − ˙ x  m s  − 2 − 2 µ − ˙ y  m s  0 . 4 − 1 . 6 µ − ¨ x h m s 2 i − 0 . 2 − 0 . 001 µ − ¨ y h m s 2 i 0 . 0 − 0 . 01 T ABLE II N U MB E R O F C O L L IS I O NS AT H O S T V E HI C L E B O UN DA R I E S F O R 3 · 10 6 S I MU L A T E D T R AJ E C TO R I E S W I T H D I FFE R E N T I N I TI A L C O N D IT I O NS . Γ Scenario front ( f ) front right ( f r ) Γ f r ont 1 . 50 · 10 6 8 . 31 · 10 5 Γ rig ht 4 . 25 · 10 5 1 . 39 · 10 6 Γ lef t 0 0 Γ rear 0 0 Γ host vehicle 1 . 93 · 10 6 2 . 22 · 10 6 the simulation and the calculation. The difference is calculated by ev aluating the entry intensity at the same time as the mid points of the histogram bins. As can be seen in fig. 7, the entry intensity obtained by numerical integration of the exact expression (eq. (16)) accurately reproduces the collision probability rate from Monte-Carlo simulations. In order to illustrate the increase in accuracy as a function of the number of simulated trajectories, fig. 8 shows the differences between simulation and numerical integration with increasing amount of simulated trajectories for collisions at the right side of the host vehicle in the front scenario. The reason why the entry intensity approximates the observed collision probability rates so well is the very lo w occurrence of higher order entries, i. e. entries where the trajectory enters the bound- ary more than once (see statistics of a Monte-Carlo simulation in table III). In the absence of higher order entries the expected number of entries becomes equal to the probability of entering the boundary at least once, see eq. (6). Since the corresponding time interval is arbitrary this equality propagates to an equality of the rates (compare to eq. (8)). In this context, we want to point out a subtlety concerning the number of entries regarding the entire vehicle boundary Γ host vehicl e versus entries through one of the boundary segments such as Γ rig ht . In Monte-Carlo simulations we have observed trajectories as sho wn in fig. 9a where the trajectory first enters the front boundary , exits the right boundary and then enters the right boundary again. W ith respect to the entire vehicle boundary Γ host vehicl e this is a second entry – howe ver with respect to the individual right boundary segment Γ rig ht this is a first entry . This is illustrated in fig. 9b where the entry intensity and Monte- Carlo histogram for Γ rig ht are plotted. Only by taking into account all entries for Γ rig ht , i. e. entries of Γ rig ht that are first crossings of Γ rig ht , as well as entries of Γ rig ht that are second or higher crossings does the entry intensity for Γ rig ht match the histogram from Monte-Carlo simulation. -10 -5 0 5 10 y [m] -5 0 5 10 x [m] shape of initial x-y covariance sample collision trajectories (a) The target is coming from the front ( µ x , µ y ) = (10 , 0) m . The parameters for the time-dependent input as specified in app. B are b 1 = − 0 . 2 ms − 3 , b 2 = − 0 . 3 ms − 3 , ω = 0 . 5 s − 1 . -10 -5 0 5 10 y [m] -5 0 5 10 x [m] shape of initial x-y covariance sample collision trajectories (b) The target is coming from the front right ( µ x , µ y ) = (10 , 10) m . The parameters for the time-dependent input as specified in app. B are b 1 = − 0 . 4 ms − 3 , b 2 = − 0 . 5 ms − 3 , ω = 0 . 5 s − 1 . Fig. 5. Samples of simulated colliding trajectories for vehicles initially coming from the front (a) and from the front right (b) side. 0 1 2 3 4 5 6 7 8 t [s] 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 collision probability rate [s -1 ], collision probability [] collision probability rate collision probability Fig. 6. Collision probability rate as a function of time for ( µ x , µ y ) = (10 , 0) m based upon N traj = 3 · 10 6 trajectories. Also sho wn is the collision probability obtained by integrating over time. This should be contrasted with the shape of the instantaneous collision probability in fig. 1. T ABLE III A B SO L U T E F R E QU E N C Y H A N D R E L A T I VE F R EQ U E NC Y P O F T HE N U MB E R O F E N T RI E S N + O F C O LL I D I NG T R AJ E C TO R I E S F O R Γ host vehicle BA S E D O N 1 · 10 7 S I MU L A T E D T R AJ E C TO R I E S F O R ∆ T = [0 , 8 s ] . X H ( X ) P ( X ) P ( X ) P ( N + ≥ 1) N + = 1 4 , 493 , 419 0 . 4493 0 . 9981 N + = 2 8 , 772 0 . 0009 0 . 0019 N + ≥ 1 4 , 502 , 191 0 . 4502 1 C. Does the appr oximation by T aylor -expansion accurately r epr oduce the exact r esult? In order to be able to compute the entry intensity ef ficiently on an embedded platform, an approximation of the exact expression (eq. (12)) was deriv ed in eq. (17). Fig. 10a,b shows the dif ferences between this approximation as well as a higher-order approximation where the pdf is T aylor-e xpanded to linear order with respect to the of f-diagonal element of the inv erse covariance matrix around 0 (see app. C) and the numerical integration of (16). As can be seen, the higher-order approximation reduces the error to a large extent while it can be still calculated efficiently on an embedded platform using the complementary error function. In Fig. 10c,d the dif ferences between the numerical inte gration of (16) and the method described in [10] is shown in addition where it can be seen that the deviation is much bigger compared to the approximations deriv ed in (17). D. An adaptive method to sample the entry intensity over ∆ T The approximations abov e of the exact expression of the entry intensity were ev aluated at small time increments of ∆ t = 0 . 05 s . Thus, the calculation over the entire time period of interest (e.g 8 s as used above) and for every relev ant object could induce a substantial computational burden. In order to reduce this effort, we propose an adaptive method to sample the entry intensity function with variable – i. e. in general larger – time increments ∆ t over the time period of interest while still capturing the characteristics of this function, in particular its shape around the maximum. The sampling starting point is based upon the non-probabilistic TTCs for single, straight boundaries using a one-dimensional constant acceleration model. Those TTCs for penetrating the front, left, and right boundaries can then be used as initial condition for the start of the sampling iteration of the entry intensity . 10 T o reproduce the entry intensity without substantial loss of information but with lower computational effort, the follo wing algorithm is proposed: 10 Due to the low probability of penetration the non-probabilistic TTC for the rear boundary is not considered for the determination of the sampling starting point. 2 3 4 5 6 7 8 t [s] 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 collision probability rate [s -1 ] Monte-Carlo histogram (ground truth) numerical integration of exact expression (a) Front scenario total collision probability rate and entry intensity 2 3 4 5 6 7 8 t [s] 0 0.2 0.4 0.6 0.8 1 collision probability rate [s -1 ] Monte-Carlo histogram (ground truth) numerical integration of exact expression (b) Front-Right scenario total collision probability rate and entry intensity 2 3 4 5 6 7 8 t [s] -8 -6 -4 -2 0 2 4 6 collision probability rate [s -1 ] 10 -3 numerical integration - Monte-Carlo histogram (c) Front scenario: difference between total collision probability rate and entry intensity 2 3 4 5 6 7 8 t [s] -8 -6 -4 -2 0 2 4 6 collision probability rate [s -1 ] 10 -3 numerical integration - Monte-Carlo histogram (d) Front-Right scenario: difference between total col- lision probability rate and entry intensity Fig. 7. The histogram resulting from Monte-Carlo simulation is shown together with the entry intensity obtained by numerical integration of the biv ariate Gaussian for front (a) and front-right (b) scenario. The differences between simulation and numerical integration are calculated by ev aluating the numerical integration at the same time as the mid points of the histogram bins and shown in (c) and (d). The process noise PSD for both coordinates is ˜ q x = ˜ q y = 0 . 0101 m 2 s − 5 . 1 2 3 4 5 6 7 8 t [s] 0 0.002 0.004 0.006 0.008 0.01 0.012 collision probability rate [s -1 ] Monte-Carlo histogram (ground truth) numerical integration of exact expression (a) Simulation based on 1 · 10 5 trajectories. 1 2 3 4 5 6 7 8 t [s] 0 0.002 0.004 0.006 0.008 0.01 0.012 collision probability rate [s -1 ] Monte-Carlo histogram (ground truth) numerical integration of exact expression (b) Simulation based on 1 · 10 6 trajectories. 1 2 3 4 5 6 7 8 t [s] 0 0.002 0.004 0.006 0.008 0.01 0.012 collision probability rate [s -1 ] Monte-Carlo histogram (ground truth) numerical integration of exact expression (c) Simulation based on 1 · 10 7 trajectories. Fig. 8. The collision probability rate for the right side of the host vehicle for the front scenario is shown comparing the results from Monte-Carlo simulation with increasing amount of simulated trajectories (a)-(c) with the entry intensity obtained by numerical integration of the bivariate Gaussian distribution (eq. (16)). The process noise PSD for both coordinates is ˜ q x = ˜ q y = 1 . 0125 m 2 s − 5 . • Calculate the times of penetrating the front, left and right boundaries based upon the non-probabilistic TTCs described abov e. • Calculate the entry intensity for each time. Pick the time with the maximum entry intensity as a starting point. • Mov e left and right from this starting point with equally spaced ∆ t 1 > ∆ t and calculate the entry intensity at these time points. Stop on each side if the entry intensity has reached a lower threshold of ( dP + C /dt ) low . • While moving left and right, check if the slope of the entry intensity has changed its sign. • On ev ery slope sign change, calculate the entry intensity around this time interval with decreased ∆ t 2 < ∆ t 1 . Examples of this implementation can be found in fig. 11 for the front and front-right scenarios. It can be seen that the entry intensity as well as the entry intensity integrated over -3 -2 -1 0 1 2 3 y [m] -5 -4 -3 -2 -1 0 1 2 3 4 5 x [m] trajectory with single entry trajectory with dual entry (a) Observed simulated trajectories 2 3 4 5 6 7 8 t [s] 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 collision probability rate [s -1 ] 10 -3 previous exits (ground truth) no previous exits (ground truth) entry intensity (b) Single and multiple entry histograms Fig. 9. Illustration of multiple entries into Γ host vehicle . In (a), observed simulated a trajectory entering the entire vehicle boundary Γ host vehicle once and a trajectory entering twice are shown. The entry intensity of the right side Γ rig ht of the host vehicle for the front scenario is shown in (b) together with the Monte-Carlo histogram where entries by trajectories that have previously exited Γ rig ht from inside Γ host vehicle are marked in dark gray . Trajectories with no previous exit from inside are marked in light gray . a certain time period can be determined with considerably fewer sampling points while still capturing the shape of the functions to be approximated. E. Salient points of colliding vehicle’s geometry In this section, we in vestigate a family of entry intensities by a parsimonious sampling in terms of sev eral representativ e salient points of the colliding vehicle’ s two-dimensional ge- ometry , i.e. the four corner points of a vehicle’ s rectangular shape incorporating width and length information, see IV -B. This enables the approximate estimation of the collision proba- bility between two vehicles modeled as extended objects with arbitrary orientation in the horizontal plane by the collision probability of the “riskiest” salient point. The first step of the computation is the prediction of the reference point’ s state distribution to a certain time as before. But then it needs to be transformed to representative salient points as described in app. D. In order to apply the approximate formulae for Gaussian distributions as in sec. IV -C the transformation is performed by the usual second order linearization, i. e. using the full nonlinear transformation for the mean and its Jacobian for the cov ariance matrix propagation. For this in vestigation, three approaches are compared in fig. 12: first the numerical integration of the resulting 2d Gaussian distribution as well as two closed-form approximations derived in app. C by T aylor- expansion. Contrary to the in vestigations in sec. V -B and V -C ev en the numerical integration of the 2d Gaussian distrib u- tion cannot fully match the Monte-Carlo simulations due to the Gaussian approximation of the non-Gaussian transformed predicted distributions. Also both closed-form approximations to the 2d Gaussian integral sho w de viations to the Monte-Carlo simulation which describes the front scenario with process noise PSD for both coordinates of ˜ q x = ˜ q y = 0 . 0101 m 2 s − 5 and input gain B set to zero. The closed-form approximations by T aylor-expansion with respect to the off-diagonal element of the cov ariance matrix and the in verse covariance matrix show similar accuracy with respect to the Monte-Carlo sim- ulations except for the salient point in fig. 12d where the former expansion is fav ored. Ne vertheless in these cases both T aylor-e xpansions approximately capture both the shape and the location of the maximum of the intensity distributions. The collision probability for the extended colliding object can then be approximated by the collision probability of the riskiest salient point which is the one where the collision probability exceeds a certain threshold the earliest. In the example above the riskiest salient point would be the rear left corner , see fig. 13. Clearly , using additional salient points such as the mid points of the vehicle’ s faces would improve the accuracy of this approximation at the expense of increased computational effort. Also, salient points can be used for more complicated, non-rectangular object boundaries. In the next section we turn our attention to time-to-collision which is an often used characteristic of collision scenarios. V I . W H A T I S T H E T T C ? The time-to-collision (TTC) is a stochastic process de- scribing the first time of contact or collision and is hence characterized by a distribution. There have been various ap- proaches to approximating the TTC-distribution. In [2], the TTC is computed as the mean of the time distribution of reaching the x 0 boundary of the car as a function of the initial conditions assuming a constant speed model; process noise is not considered. This is also presented in [1]; in addition the time distrib ution for reaching the x 0 boundary as a function of the initial conditions assuming a constant acceleration model is calculated by Monte-Carlo-simulation and its mean values depending upon the initial condition setup is giv en - again, process noise for this motion model is not considered. As a notable exception, in [3] the covariance of the distribution of TTC (or the related time-to-go in [6]) has 2 3 4 5 6 7 8 t [s] -3 -2 -1 0 1 2 collision probability rate [s -1 ] 10 -5 approx. - num. integration higher order approx. - num. integration (a) Front scenario 2 3 4 5 6 7 8 t [s] -1 -0.5 0 0.5 1 1.5 2 2.5 collision probability rate [s -1 ] 10 -3 approx. - num. integration higher order approx. - num. integration (b) Front-Right scenario 2 3 4 5 6 7 8 t [s] -0.01 0 0.01 0.02 0.03 0.04 0.05 collision probability rate [s -1 ] approx. - num. integration higher order approx. - num. integration integration by averaging - num. integration (c) Front scenario 2 3 4 5 6 7 8 t [s] -0.2 -0.15 -0.1 -0.05 0 0.05 0.1 0.15 0.2 collision probability rate [s -1 ] approx. - num. integration higher order approx. - num. integration integration by averaging - num. integration (d) Front-Right scenario Fig. 10. Differences between numerical integration of the biv ariate Gaussian in the expression of the entry intensity in eq. (16) and two approximations (a,b) as well as the method described in [10] (c,d). been augmented by standard error propagation and clever use of the implicit function theorem to include the ef fect of process noise. Nevertheless their TTC is still based on a reduction to a one-dimensional, longitudinal motion. As will be shown below these restricted temporal quantities do not fully capture the characteristics of horizontal plane collision scenarios. What is required is a distribution of the TTC that takes into account process noise as well as two- or higher- dimensional geometries. In the following figures entry intensities are plotted together with initial condition TTC-distributions from Monte-Carlo simulations similar to [1]. These Monte-Carlo simulations are based on TTC values for the front boundary Γ f ront ( x -direction) and the right boundary Γ rig ht ( y -direction) as solutions of the constant acceleration equations x 0 = x ( T T C f ront ) = x (0) + ˙ x (0) T T C f ront + ¨ x (0) 2 T T C 2 f ront y R = y ( T T C rig ht ) = y (0) + ˙ y (0) T T C rig ht + ¨ y (0) 2 T T C 2 rig ht (18) As an extension of the one-dimensional Monte-Carlo setup in [1] the following conditions and constraints need to be considered for consistent TTC-histograms for one-dimensional boundaries embedded in two-dimensional space - for arbitrary initial conditions and v alues of x 0 , y R all real, positiv e solutions of the quadratic equations above need to be considered - a real, positiv e solution for T T C f ront is only valid if ( x ( T T C f ront ) , y ( T T C f ront )) ∈ Γ f ront , and a real solution for T T C rig ht is only valid if ( x ( T T C rig ht ) , y ( T T C rig ht )) ∈ Γ rig ht - the trajectory must enter the boundary from outside, e. g. for T T C rig ht it is checked that y ( T T C rig ht −  ) > y R for a small  > 0 Since time-dependent input cannot be handled in Monte-Carlo simulations only based on stochastic initial conditions we restrict the dynamical model in this section for comparison to a constant acceleration model, i. e. the input gain B in app. B is set to zero. As one central result of this section, we show in fig. 14 that initial condition TTC-histograms from Monte-Carlo simula- tions described above (referred to as ‘TTC initial condition MC-histograms’) match the corresponding entry intensities when process noise is zero. If contributions of higher order en- tries are negligible as discussed in sec. V -B the entry intensity accurately approximates the collision probability rate which is the probability density of the TTC. It is also note worthy that in this case the entry intensity in its approximate version from sec. IV -C affords a closed-form expression for a distribution that hitherto had to be obtained by Monte-Carlo simulation. Also shown are the approximate Gaussian distributions ac- 2 3 4 5 6 7 8 t [s] 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 collision probability rate [s -1 ] starting point with increment of 0.5s with increment of 0.2s around max linear-order approximation linear-order approximation with reduced samples (a) Front scenario entry intensity 2 3 4 5 6 7 8 t [s] 0 0.2 0.4 0.6 0.8 1 1.2 collision probability rate [s -1 ] starting point with increment of 0.5s with increment of 0.2s around max linear-order approximation linear-order approximation with reduced samples (b) Front-Right scenario entry intensity 2 3 4 5 6 7 8 t [s] 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 collision probability [] Monte-Carlo simulation linear-order approximation linear-order approximation with reduced samples (c) Front scenario integrated entry intensity 2 3 4 5 6 7 8 t [s] 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 collision probability [] Monte-Carlo simulation linear-order approximation linear-order approximation with reduced samples (d) Front-Right scenario integrated entry intensity Fig. 11. Examples for reducing the number of calculations to determine the entry intensity and the integrated entry intensity . (a) and (c) show the results for the front scenario and (b) and (d) for the front-right scenario. The parameters in these examples are ∆ t 1 = 0 . 5 s , ∆ t 2 = 0 . 2 s and ( dP + C /dt ) low = 0 . 01 . In doing so the number of calculations for the entry intensity could be reduced from 120 (using a fixed sampling increment of ∆ t = 0 . 05 s ) to 13 for the front scenario and to 12 for the front-right scenario, respectively . cording to the method using the implicit function theorem from [3] extended to a constant acceleration model to maintain com- parability (referred to as ‘TTC implicit function Gaussians’). Their mean values coincide with the deterministic expressions of eq. (18) due to the usual first-order approximation of non- Gaussian densities. 11 Next, we turn on process noise and show how the ‘TTC initial condition MC-histograms’ now deviate from the entry intensity . In fig. 15 the entry intensity is plotted with ‘TTC initial condition MC-histograms’ and ‘TTC implicit function Gaussians’ for x - and y -directions for an initial position at the front, right side. The maxima and in particular the shapes of the ‘TTC implicit function Gaussians’ in x - and y -direction are significantly different from the shapes and maxima of the entry intensity . Like wise, the ‘TTC initial condition MC-histograms’ do not resemble the entry intensity and reach their maxima at later times. Since the bulk of the colliding trajectories go through two sides - front and right (see also fig. 5b) - only a collision model that takes into account process noise and the full geometry of the host vehicle can yield accurate results. In fig. 16 an entry intensity restricted to the x -direction is plotted together with a ‘TTC initial condition MC-histogram’ and ‘TTC implicit function Gaussian. ’ The initial position is straight in front 11 Note that the augmented TTC-computation in [3] does not alter the mean but only the cov ariance. of the vehicle hence almost all trajectories pass through the front boundary . Nevertheless the entry intensity is lower and shifted to the left of the ‘TTC initial condition MC-histogram. ’ Also the maximum of the entry intensity occurs before the maxima of the ‘TTC initial condition MC-histogram’ and the ‘TTC implicit function Gaussian. ’ In general the shapes and maxima of the ‘TTC initial condition MC-histogram’ and ‘TTC implicit function Gaussian’ do not match the shapes and maxima of the entry intensity . These differences increase as the process noise increases as can be seen in fig. 17. This is due to the fact that the time of the maximum is strongly influenced by the factor p t ( x 0 ) in eq. (16); an increased level of process noise leads to a faster spreading of p t ( x 0 ) and hence the maximum is reached earlier . W e hav e check ed that the entry intensities in this section accurately match the ground truth histograms as in sec. V -B. The above discussion shows that temporal collision characteristics are encoded by the distribution of the entry intensity which incorporates the full geometry of the host vehicle as well as process noise during prediction. A scalar quantity called TTC could then be obtained as one of the characteristic properties of this distribution such as the mode or the mean or the median, or as a property of the integrated collision probability rate, for example the time when the collision probability exceeds a certain threshold. 2 3 4 5 6 7 8 t [s] 0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 collision probability rate [s -1 ] Monte-Carlo histogram (ground truth) entry intensity numerical entry intensity Taylor inverse entry intensity Taylor (a) Front left corner of colliding vehicle 2 3 4 5 6 7 8 t [s] 0 0.02 0.04 0.06 0.08 0.1 0.12 0.14 collision probability rate [s -1 ] Monte-Carlo histogram (ground truth) entry intensity numerical entry intensity Taylor inverse entry intensity Taylor (b) Front right corner of colliding vehicle 2 3 4 5 6 7 8 t [s] 0 0.1 0.2 0.3 0.4 0.5 0.6 collision probability rate [s -1 ] Monte-Carlo histogram (ground truth) entry intensity numerical entry intensity Taylor inverse entry intensity Taylor (c) Rear left corner of colliding vehicle 2 3 4 5 6 7 8 t [s] 0 0.05 0.1 0.15 collision probability rate [s -1 ] Monte-Carlo histogram (ground truth) entry intensity numerical entry intensity Taylor inverse entry intensity Taylor (d) Rear right corner of colliding vehicle Fig. 12. Collision probability rate and entry intensity of four corner points in the front scenario with process noise PSD for both coordinates of ˜ q x = ˜ q y = 0 . 0101 m 2 s − 5 and input gain B set to zero. Results for the entry intensity are given for numerical integration of the approximate 2d Gaussian distribution as well as two approximations to this integration as detailed in app. C. It can be observed that the non-Gaussian nature of the probability distributions transformed to salient points entail deviations with respect to Monte-Carlo simulations. This is due to the second order linearization with respect to the non-linear transformations derived in app. D for Gaussian densities. 2 3 4 5 6 7 8 t [s] 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 collision probability [] Front left corner Front right corner Rear left corner Rear right corner Relevant threshold Fig. 13. Collision probability for the colliding vehicle’ s salient points. In this example the rear left corner is considered the “riskiest” salient point, as it reaches a certain threshold the earliest. V I I . C O N C L U S I O N S As detailed in our literature revie w a common approach to compute a collision probability for automoti ve applications is via temporal collision measures such as time-to-collision or time-to-go. In this paper , howe ver , we hav e pursued a different 2 3 4 5 6 7 8 t [s] 0 0.05 0.1 0.15 0.2 0.25 0.3 collision probability rate [s -1 ], TTC prob. density [s -1 ] TTC initial condition MC-histogram right TTC implicit function Gaussian right entry intensity right TTC initial condition MC-histogram front TTC implicit function Gaussian front entry intensity front Fig. 14. Entry intensities, ‘TTC initial condition MC-histograms, ’ and ‘TTC implicit function Gaussians’ for an initial condition at the front, right side of the vehicle: ( x, y ) = (10 , 10) m . For comparability , process noise had to be set to zero in the computation of the entry intensities. approach, namely the in vestigation of a collision probability rate without temporal collision measures as an intermediate or prerequisite quantity . A collision probability rate then affords the provision of a collision probability ov er an extended period 2 3 4 5 6 7 8 t [s] 0 0.05 0.1 0.15 0.2 0.25 0.3 collision probability rate [s -1 ], TTC prob. density [s -1 ] TTC initial condition MC-histogram right TTC initial condition MC-histogram front TTC implicit function Gaussian right TTC implicit function Gaussian front entry intensity total Fig. 15. Entry intensity , ‘TTC initial condition MC-histogram, ’ and ‘TTC implicit function Gaussian’ for an initial condition at the front, right side of the vehicle: ( x, y ) = (10 , 10) m . The process noise PSD for both coordinates is ˜ q x = ˜ q y = 0 . 0405 m 2 s − 5 . 2 3 4 5 6 7 8 t [s] 0 0.1 0.2 0.3 0.4 0.5 0.6 collision probability rate [s -1 ], TTC prob. density [s -1 ] TTC initial condition MC-histogram front TTC implicit function Gaussian front entry intensity front Fig. 16. Entry intensity , ‘TTC initial condition MC-histogram, ’ and ‘TTC implicit function Gaussian’ for an initial condition in front of the vehicle: ( x, y ) = (10 , 0) m . The process noise PSD for both coordinates is ˜ q x = ˜ q y = 0 . 0101 m 2 s − 5 . of time by temporal integration. An expression for an upper bound of the collision probability rate has been deriv ed based on the theory of level crossings for vector stochastic processes. The condition under which the upper bound is saturated, i. e. is a good approximation of the collision probability rate has been discussed. While the expression was exemplified by an application of Gaussian distrib utions on a two-dimensional rectangular surface, the formalism holds for general non- stationary as well as non-Gaussian stochastic processes and can be applied to any subsets of multidimensional piece wise- smooth surfaces. The ground truth collision probability rate distribution has been obtained by Monte-Carlo simulations and approximated by our derived bound for the collision probability rate. W e hav e also implemented an approximation of the collision probability rate bound that can be computed in closed form on an embedded platform. This approximate formula provided 2 3 4 5 6 7 8 t [s] 0 0.1 0.2 0.3 0.4 0.5 0.6 collision probability rate [s -1 ], TTC prob. density [s -1 ] TTC initial condition MC-histogram front TTC implicit function Gaussian front entry intensity front Fig. 17. Entry intensity , ‘TTC initial condition MC-histogram, ’ and ‘TTC implicit function Gaussian’ for an initial condition in front of the vehicle: ( x, y ) = (10 , 0) m . The process noise PSD for both coordinates has been increased to ˜ q x = ˜ q y = 1 . 0125 m 2 s − 5 . bounds of the collision probability rate distributions that are almost indistinguishable from distributions obtained by nu- merical integration for the scenarios considered in this paper . A straightforward application of this method characterizes the collision of an extended object with a second point-like object. The case of two extended objects with circular boundaries or rectangular boundaries with identical, fixed, axis-aligned orientation can also be reduced to the collision of an extended object with a second point-like object as illustrated in [12]. In vehicle-to-vehicle collision scenarios these are unrealistic assumptions. Using an abstraction of the second object by salient points of its boundary we have shown ho w to augment the method to cover the case of two extended objects with arbitrary shape and orientation. In our discussion of approaches to computing a TTC we illustrated the correspondence between classical TTC- distributions derived by Monte-Carlo simulations based on stochastic initial conditions and the entry intensity . W e also showed that those classical one-dimensional TTC-distributions do not properly represent collision statistics in case of two- dimensional geometries and presence of process noise. The distribution of the collision probability rate is by construction the distribution of the TTC. Point estimators derived from this distrib ution (e. g. the mode, mean, or median) or its temporal integral – the collision probability – could be inv estigated as input signals to collision av oidance decision making in the context of a complete collision av oidance system. V I I I . AC K N O W L E D G E M E N T S Helpful clarifications by Prof. Georg Lindgren are gratefully acknowledged. A P P E N D I X A. P artitioned Gaussian densities In many calculations in stochastic estimation there is a need to marginalize over certain elements of a state vector or to obtain lower dimensional distributions by conditioning with respect to certain elements. For these calculations the original state vector ξ can be rearranged or partitioned such that x r denotes the remaining state vector and x m denotes the states to be marginalized ov er or which are used for conditioning. ξ =  x r x m  (19) Hence the mean vector µ and cov ariance matrix Σ can be partitioned into µ =  µ r µ m  , Σ =  Σ rr Σ rm Σ > rm Σ mm  (20) The following two well-known results on multiv ariate Gaus- sians are used in this paper: a) Mar ginalization: The probability density of ξ marginalized with respect to x m is p ( x r ) = Z x m p ( ξ ) dx m = N ( x r ; µ r , Σ rr ) (21) b) Conditioning: The probability density of ξ condi- tioned on x m is p ( ξ | x m ) = p ( x r | x m ) = N  x r ; µ r | m , Σ r | m  (22) with µ r | m = µ r + Σ rm Σ − 1 mm ( x m − µ m ) (23) Σ r | m = Σ rr − Σ rm Σ − 1 mm Σ > rm (24) B. Dynamical system The example kinematics is characterized by a six- dimensional state vector containing target vehicle coordinates relativ e to the host vehicle ξ =  x y ˙ x ˙ y ¨ x ¨ y  > (25) The continuous dynamics is gi ven by a continuous white noise jerk model with additional time-dependent control input u ( t ) : ˙ ξ = F ξ + Lν + B u (26) where F =         0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0         , L = B =         0 0 0 0 0 0 0 0 1 0 0 1         and u ( t ) =  b 1 sin( ω t ) b 2 sin( ω t )  Process noise ν is characterized by the jerk power spectral density (PSD) ˜ Q = diag ( ˜ q x , ˜ q y ) . The discrete dynamics, i. e. the solution of this differ - ential equation, can be obtained by standard linear system techniques. The cov ariance matrix of discrete-time equiv alent process noise is given by (see e. g. [22]) Q ( t k +1 , t k ) = t k +1 Z t k Φ( t k +1 , τ ) L ˜ QL > Φ > ( t k +1 , τ ) dτ where Φ is the transition matrix of the homogeneous dif feren- tial equation. The closed-form expression for this cov ariance matrix reads Q (∆ t k ) =    ∆ t 5 k 20 ∆ t 4 k 8 ∆ t 3 k 6 ∆ t 4 k 8 ∆ t 3 k 3 ∆ t 2 k 2 ∆ t 3 k 6 ∆ t 2 k 2 ∆ t k    ⊗  ˜ q x 0 0 ˜ q y  with ∆ t k = t k +1 − t k . Note that the dynamical system above is an example to illustrate the application of the results in sec. IV -A to compute the collision probability between two vehicles that can both be moving; other in general non-linear dynamical systems and state v ectors can be used as long as they contain relative position and its first deriv ativ e. C. Evaluation of the 2D integr al for the entry intensity In this article integrals of the form Z ˙ x ≤ 0 Z y ∈ I y ˙ x p t ( y , ˙ x | x 0 ) dy d ˙ x (27) as in eq. (16) for the entry intensity appear . W e are not aware of a closed-form solution if the cov ariance matrix of p t ( y , ˙ x | x 0 ) is not diagonal. In [10] the 1D integral with respect to ˙ x was computed in closed form for a Gaussian pdf and the remaining spatial integral was replaced by the integrand at mid-point times the integration interval. As can be seen in figures 10(c,d) this approximation does not accurately repro- duce the Monte-Carlo ground truth due to the considerable variation of the spatial distribution across the host vehicle rectangle. As an alternative approximation, we T aylor-e xpand the 2D pdf with respect to the off-diagonal element of the cov ariance matrix around 0 to a certain order and then integrate the factorized 1D distributions. For a general 2D Gaussian pdf p ( x 1 , x 2 ) = N ( ξ ; µ, Σ) with ξ = ( x 1 , x 2 ) > and mean µ and cov ariance matrix Σ the T aylor -expansion to linear order with respect to Σ 12 reads N ( ξ ; µ, Σ) = N  x 1 ; µ 1 , p Σ 11  N  x 2 ; µ 2 , p Σ 22  + Σ 12  x 1 − µ 1 Σ 11 N  x 1 ; µ 1 , p Σ 11   · ·  x 2 − µ 2 Σ 22 N  x 2 ; µ 2 , p Σ 22   + O  (Σ 12 ) 2  which leads to the following integral x 2 u Z x 2 l x 1 u Z x 1 l x 1 p ( x 1 , x 2 ) dx 1 dx 2 =  µ 1 Φ  x 1 − µ 1 √ Σ 11  − Σ 11 N ( x 1 ; µ 1 , p Σ 11 )  x 1 u x 1 l · ·  Φ  x 2 − µ 2 √ Σ 22  x 2 u x 2 l + Σ 12  Φ  x 1 − µ 1 √ Σ 11  − x 1 N ( x 1 ; µ 1 , p Σ 11 )  x 1 u x 1 l · · h −N ( x 2 ; µ 2 , p Σ 22 ) i x 2 u x 2 l + O  (Σ 12 ) 2  The quality of the approximation depends asymptotically upon the size of Σ 12 . An alternativ e T aylor-expansion would be an expansion with respect to the off-diagonal element of the in verse covariance matrix. Its off-diagonal element Σ − 1 12 :=  Σ − 1  12 = − Σ 12 | Σ | has the determinant of Σ in the denom- inator , hence for large determinants (i. e. large uncertainties as expected for long prediction times) this approximation is expected to be more accurate. For a general 2D Gaussian pdf p ( x 1 , x 2 ) = N ( ξ ; µ, Σ) with ξ = ( x 1 , x 2 ) > and mean µ and cov ariance matrix Σ the T aylor -expansion to linear order with respect to Σ − 1 12 reads N ( ξ ; µ, Σ) = N  x 1 ; µ 1 , q ˜ Σ 11  N  x 2 ; µ 2 , q ˜ Σ 22  − Σ − 1 12  ( x 1 − µ 1 ) N  x 1 ; µ 1 , q ˜ Σ 11  · ·  ( x 2 − µ 2 ) N  x 2 ; µ 2 , q ˜ Σ 22  + O  (Σ − 1 12 ) 2  with ˜ Σ 11 = | Σ | Σ 22 , ˜ Σ 22 = | Σ | Σ 11 . This leads to the following integral x 2 u Z x 2 l x 1 u Z x 1 l x 1 p ( x 1 , x 2 ) dx 1 dx 2 = " µ 1 2 erf x 1 − µ 1 p 2 ˜ Σ 11 ! − ˜ Σ 11 N  x 1 ; µ 1 , q ˜ Σ 11  # x 1 u x 1 l · · " 1 2 erf x 2 − µ 2 p 2 ˜ Σ 22 !# x 2 u x 2 l − Σ − 1 12 " x 1 ˜ Σ 11 N  x 1 ; µ 1 , q ˜ Σ 11  − ˜ Σ 11 2 erf x 1 − µ 1 p 2 ˜ Σ 11 !# x 1 u x 1 l · ·  ˜ Σ 22 N  x 2 ; µ 2 , q ˜ Σ 22  x 2 u x 2 l + O  (Σ − 1 12 ) 2  If the cov ariance matrix of p t ( y , ˙ x | x 0 ) is diagonal, i. e. Σ 12 = 0 , the integrand factorizes into Gaussians and can be integrated in a straightforward manner . D. State vector transformation to salient points In order to transform the state distribution describing the object’ s reference point (such as the middle of the rear bumper or the middle of the rear axle) to other points such as the four corners the deterministic state transformation is needed, which can be used either by propagation of the mean and cov ariance using linear system techniques or by Monte-Carlo sampling. T ransformation to other points of an extended object requires knowledge of its orientation which can be deri ved in the Ackermann limit from the angle of the velocity vector . This is an appropriate setup if the vehicle’ s reference point is the middle of the rear axle and side-slip at the rear wheels can be neglected as appropriate for normal dri ving conditions. T aking into account the state vector as defined in eq. (25) and ∆ ෤ 𝑥 ∆ ෤ 𝑦 Sali en t poi n t: e. g. rear l ef t c orn er Fig. 18. Horizontal view of the object rectangle with local Cartesian coordinate system and coordinate origin at the middle of the rear axle. The translation to the rear left corner as a salient point of the object’ s geometry is also drawn. translating the state along (∆ ˜ x ∆ ˜ y ) > in the object’ s local coordinate system (see fig. 18) the position transformation reads  x y  sal =  x y  ref + R  ∆ ˜ x ∆ ˜ y  with R =  cos α − sin α sin α cos α  and α = arctan ˙ y ˙ x the orientation angle as explained above. Then we have  ˙ x ˙ y  sal =  ˙ x ˙ y  ref + ˙ α R 0  ∆ ˜ x ∆ ˜ y   ¨ x ¨ y  sal =  ¨ x ¨ y  ref − ˙ α 2 R  ∆ ˜ x ∆ ˜ y  + ¨ α R 0  ∆ ˜ x ∆ ˜ y  with R 0 = d d α R and ˙ α = ˙ x ¨ y − ˙ y ¨ x ˙ x 2 + ˙ y 2 ¨ α = 2 ˙ x ˙ y ( ¨ x 2 − ¨ y 2 ) − ¨ x ¨ y ( ˙ x 2 − ˙ y 2 ) ( ˙ x 2 + ˙ y 2 ) 2 + ˙ x ... y − ˙ y ... x ˙ x 2 + ˙ y 2 Note that this transformation is non-linear , hence propagation of a multiv ariate Gaussian distribution by this transformation will result in a non-Gaussian distribution. In frameworks for Gaussian densities this can be handled by the usual second order linearization, i. e. using the full nonlinear transformation for the mean and its Jacobian for the cov ariance matrix propagation. R E F E R E N C E S [1] J. Jansson and F . Gustafsson, “ A framework and automotive application of collision avoidance decision making, ” Automatica , vol. 44, no. 9, pp. 2347–2351, 2008. [2] M. M. Muntzinger, S. Zuther , and K. Dietmayer, “Probability estimation for an automotiv e pre-crash application with short filter settling times, ” in Pr oceedings of IEEE Intelligent V ehicles Symposium , 2009, pp. 411– 416. [3] J. E. Stellet, J. Schumacher , W . Branz, and J. M. Z ¨ ollner , “Uncertainty propagation in criticality measures for driv er assistance, ” in Intelligent V ehicles Symposium (IV), 2015 IEEE . IEEE, 2015, pp. 1187–1194. [4] J. Jansson, “Collision avoidance theory: W ith application to automotiv e collision mitigation, ” Ph.D. dissertation, Link ¨ oping Uni versity Electronic Press, 2005. [5] A. Lambert, D. Gruyer , and G. Saint Pierre, “ A fast Monte Carlo algorithm for collision probability estimation, ” in Contr ol, Automation, Robotics and V ision, 2008. ICARCV 2008. 10th International Conference on . IEEE, 2008, pp. 406–411. [6] P .-J. Nordlund and F . Gustafsson, Pr obabilistic conflict detection for piecewise straight paths . Link ¨ oping Univ ersity Electronic Press, 2008. [7] M. Mitici and H. A. Blom, “Mathematical models for air traffic conflict and collision probability estimation, ” IEEE T ransactions on Intelligent T ransportation Systems , vol. 20, no. 3, pp. 1052 – 1068, 2019. [8] Y . K. Belyaev , “On the number of exits across the boundary of a region by a vector stochastic process, ” Theory of Probability & Its Applications , vol. 13, no. 2, pp. 320–324, 1968. [9] G. Bakker and H. A. Blom, “ Air traffic collision risk modelling, ” in Decision and Control, 1993., Pr oceedings of the 32nd IEEE Confer ence on . IEEE, 1993, pp. 1464–1469. [10] H. A. Blom and G. Bakker , “Conflict probability and incrossing prob- ability in air traffic management, ” in Pr oceedings of the 41st IEEE Confer ence on Decision and Control , vol. 3, 2002, pp. 2421–2426. [11] H. Blom, B. Bakker , M. Everdij, and M. V an Der Park, “Collision risk modeling of air traffic, ” in Eur opean Contr ol Confer ence (ECC), 2003 . IEEE, 2003, pp. 2236–2241. [12] ICA O, “ A unified framework for collision risk modelling in support of the manual on airspace planning methodology for the determination of separation minima (doc. 9689), circ. 319-an/181, ” International Civil A viation Organization, Montreal, Canada , 2009. [13] M. Prandini, J. Hu, J. L ygeros, and S. Sastry , “ A probabilistic approach to aircraft conflict detection, ” IEEE T ransactions on intelligent trans- portation systems , vol. 1, no. 4, pp. 199–220, 2000. [14] A. Genz, “Numerical computation of rectangular bivariate and trivariate normal and t probabilities, ” Statistics and Computing , vol. 14, no. 3, pp. 251–260, 2004. [15] Y . K. Belyaev and V . Nosko, “Characteristics of excursions above a high lev el for a gaussian process and its en velope, ” Theory of Probability & Its Applications , vol. 14, no. 2, pp. 296–309, 1969. [16] G. Lindgren, “Model processes in nonlinear prediction with application to detection and alarm, ” The Annals of Probability , pp. 775–792, 1980. [17] ——, Stationary stochastic processes: theory and applications . CRC Press, 2012. [18] D. V eneziano, C. A. Cornell, and M. Grigoriu, “V ector-process models for system reliability , ” Journal of the Engineering Mechanics Division , vol. 103, no. 3, pp. 441–460, 1977. [19] R. Illsley , “The moments of the number of exits from a simply connected region, ” Advances in Applied Probability , vol. 30, no. 1, pp. 167–180, 1998. [20] R. Altendorfer , “Observable dynamics and coordinate systems for auto- motiv e target tracking, ” in Pr oceedings of IEEE Intelligent V ehicles (IV) Symposium , 2009, pp. 741–746. [21] A. Philipp and D. Goehring, “ Analytic collision risk calculation for autonomous vehicle navigation, ” in 2019 International Confer ence on Robotics and Automation (ICRA) . IEEE, 2019, pp. 1744–1750. [22] Y . Bar-Shalom, X. R. Li, and T . Kirubarajan, Estimation with Applica- tions to Tr acking and Navigation . Wile y , 2001.