Observable dynamics and coordinate systems for …

1 Observable dynamics and coordinate systems forautomotive target trackingRichard AltendorferDriver Assistance SystemsTRW AutomotiveEmail: We investigate several coordinate systems and dy-namical vector fields for target tracking to be used in driverassistance systems . We show how to express the discrete dynamicsof maneuvering target vehicles in arbitrary coordinates startingfrom the target s and the own (ego) vehicle s assumed dynamicalmodel in global coordinates . We clarify the notion of egocompensation and show how non-inertial effects are to beincluded when using a body-fixed coordinate system for targettracking. We finally compare the tracking error of differentcombinations of target tracking coordinates and dynamical vectorfields for simulated INTRODUCTIOND river assistance systems (DAS) such as adaptive cruisecontrol (ACC) or lane departure warning (LDW) need toperceive the environment using exteroceptive sensors (e.)

2 Or laser for ACC and camera for LDW). As DAS be-come more sophisticated and move from comfort applicationsto safety-critical applications such as automatic emergencybraking, the requirements regarding perception are becomingmore stringent. An essential part of exteroception is the targetdynamics used in the temporal prediction of observers witha predictor-corrector structure. In this paper different targetdynamics in different coordinate systems will be derivedand assessed with respect to satisfactory modeling of targetdynamics and modeling of target dynamics without an explicit es-timation of the target driver s intent for DAS is generallyapproximated by assuming non-maneuver or maneuver models[1], [2] of varying degrees of complexity [3] where the targetdriver s actions like accelerating or steering are subsumed into(Gaussian) noise processes: tg=ft( tg,utg, tg)with tgthe target state vector in global coordinates ,utgtheinput (control) vector, and tga multi-dimensional stochasticprocess.

3 Those models are usually formulated with respect toa global, inertial reference frame1tangential to the earth ssurface. This also applies to the modeling of the ownship( ego ) dynamics , egog=fego( egog,uegog, egog)howeverthe corresponding observer can be fed with proprioceptivemeasurements such as yaw rate, lateral acceleration, or longi-tudinal velocity coming from vehicle stability control (VSC)1 The effect of the earth s rotation around its polar axis as well as its rotationaround the sun, etc on the vehicle motion can safely be neglected, hence anearth fixed reference frame will be called inertial in this Since in both target and ego modeling the input isusually zero, we will suppress the input vectorsuin thesevector fields from now of the target dynamics is based on exteroceptionby radar, laser, or video sensors that provide measurements relative to the ego vehicle.

4 Hence somewhere in the estimationprocess a transformation from relative to global coordinatesmust be dynamical system for the combinedtarget and ego observer would then have the following form tg=ft( tg, tg) egog=fego( egog, egog) t=ht( tg,utg,wt) ego=hego( egog,wego)(1)where thew s are stochastic measurement processes. Since themeasurements tare relative to the ego vehicle, the outputfunctionhtmust contain a control vectorutg= egogin orderto map the relative measurements onto the global target state tg. As all relative measurements contain some sort of positioninformation, the ego state egogmust hence also estimate theposition of the ego would be an appropriate approach if the ego state egogwere fully Observable which implies absolute positionmeasurements by e. g. GPS receivers. If egogwere not fullyobservable its covariance would grow without bounds whichwould cause the covariance matrix of tgto grow infinitelyby propagation through the output functionht.

5 This meansthat the dynamical system not stochastically Observable a necessary condition for the convergence of an extendedKalman filter (EKF), see e. g. [4]. Since most vehicles arenot equipped with a GPS receiver, their position can only beestimated by dead-reckoning and is therefore if vehicles are equipped with a GPS receiver e. g. fromtheir navigation system , the GPS position (without differentialcorrections) is only accurate to about [5]. Thiserror would then be propagated to unacceptably large positioncovariances in tg. While the covariance of therelativepositionin this case might remain bounded as suggested in [6] it2 Modeling the target dynamicsa prioriin coordinates relative to the egovehicle trel=f( trel, trel)is rather unattractive and not considered heresince e. g. a constant acceleration model forfwould imply that the targetpermanently moves with a constant acceleration plus noise relative to the egovehicle irrespective of the actual state and motion of the ego [ ] 21 Sep 2014 Fig.

6 1. Inertial and ego-fixed coordinate frames. The origin of the ego-fixedframe and the reference point of the target vehicle are common choices butare by no means not advisable to work with unobservable, non-convergentsystems whose ever-growing covariances will also invalidatethe propagation of covariance by linearization as in an we propose formulating the target dynamics inrel-ative, i. e. in ego vehicle fixed coordinates . The goal is toreplace the combined dynamical system (1) with a system thatcontains treland egogwhere the unobservable states of egogare not used for the estimation of trel. In the next section it willbe shown how to derive the relative target dynamics startingfrom the global target and ego dynamics in eq. 1. While theuse of relative target dynamics for automotive target tracking isnot new (see e. g. [7], [8], [9]), in this paper a general, system -theoretic framework for the rigorous derivation of relativetarget dynamics starting from arbitrary dynamical vector fieldsfor global target and ego dynamics is presented.

7 This includesthe derivation of the discrete dynamics with process andinput noise covariance matrices as needed for an EKF asobserver. The derivation is illustrated by three different choicesof coordinate systems and vector fields and their accuracyin tracking targets is assessed by numerical simulation. Theobservability of the combined target and ego dynamics is RELATIVE TARGET VEHICLE DYNAMICSA. DerivationIn order to derive the discrete target dynamics in relativecoordinates we first need to obtain its continuous vector requires a definition of the state vectors for the globaltarget and ego dynamics and their continuous vector fieldsas well as a definition of the relative coordinates . The vectorfields are given by egog=fego( egog, egog) tg=ft( tg, tg)(2)where we assume that no control inputs are necessary forfegoandft. We now define a new state by trel=m( tg, egog)(3)wheremis in general a nonlinear function that depends uponthe choice of the coordinates of tg, egog(Cartesian, polar, etc).

8 If both tgand egogare in Cartesian coordinates and the newcoordinates are also Cartesian coordinates for a ego body fixedsystem we get the more intuitive expressionm( tg, egog) =M( egog)( tg egog)(4)whereM( egog)contains a rotation to the ego-fixed coordinatesystem as well as corrections due to the fact that the ego-fixedsystem is a non-inertial system and thus experiences pseudo-forces, see app. taking the time derivative of eq. (4)we get a vector field for trel trel= M( tg egog) +M(ft( tg, tg) fego( egog, egog))However, the goal of this computation is to replace tgwith trel; therefore we need to use eq. (4) again in orderto eliminate tg: trel= MM 1 trel(5)+M(ft(M 1 trel+ egog, tg) fego( egog, egog))The combined system of differential equations for treland egogreads trel= MM 1 trel+M(ft(M 1 trel+ egog, tg) fego( egog, egog)) egog=fego( egog, egog)(6)This system of differential equations governs the dynamics ofthe target vehicle relative to the ego vehicle and are basedupon: the dynamics of the target vehicle with respect to thegroundft, the dynamics of the ego vehicle with respect tothe groundfego, and the definition of the relative coordinatesm.

9 This procedure separates dynamical models for individualvehicle dynamics (ego or target) from the relative dynamicsused for tracking in an arbitrary coordinate system . The egocompensation at the level of continuous dynamics is not aseparate step but is intertwined with the relative SolutionIn this paper, all stochastic differential equations are chosento be solved by the discrete-time counterpart method [1]where the continuous stochastic process g(t)is replaced bya discrete stochastic process g,kwhich is constant from onetime step to the abuse of notation, here galsodenotes this constant the ego dynamics is decoupled from the relative targetdynamics, it can be solved first, see app. B, and its solution egog(t) =Fegog( egog(t0),t t0, egog)can be inserted intothe differential equation for trelto arrive at a time-dependentdifferential equation: trel= MM 1 trel+M(ft(M 1 trel+ egog(t), tg) egog(t))=frel( trel, egog(t), egog(t), tg, egog)(7)3 From now on, we suppress the dependence ofMon egogin our , the discrete-time equivalent method [1] can be employedusing the power spectral density of the continuous stochastic solution of this differential equation - if it exists - can becast into the notation of discrete time systems to be used laterfor application of the EKF.

10 Egog k+1=Fegog( egog k, tk, egog k) trel k+1=Ftrel( trel k, egog k, tk, trel k)(8)where tk=tk+1 tkis the time difference from oneiteration to the next and trel k= ( tg k, egog k)>is the effectivediscrete stochastic process for the relative target that egog k+1does not have inputs but trel k+1has theego state egog kas its input or control the following we will assume that the ego estimationusing proprioceptive measurements from VSC sensors is sep-arate from the exteroception and only outputs the ego state egog kand its covariance matrixPegog the use of eq. (8) in an EKF, we define the matricesAk= trelFtrel( trel k, egog k, tk, trel k)Bk= egogFtrel( trel k, egog k, tk, trel k)Gk= trelFtrel( trel k, egog k, tk, trel k)which are used for the propagation of the state (Ak), input(Bk), and process noise (Gk) covariances by linearization. Theinput noise and process noise covariance matrices are thenQinputk=BkPegog kB>kQprocessk=GkVtrel kG>kwhereVtrel k= cov( trel k).