EP1537440B1

Road curvature estimation and automotive target state estimation system

Abstract

This record has no abstract on file.

EP1537440B1, drawing sheet 1 — Sheet 1 of 118

Term

Term ended

Expired 15 July 2023, 3.2 years ago.

28 claims: 2 independent, 26 dependent

1
A method of estimating a state of a target vehicle (36) on a roadway (34), comprising:a. generating (506) an estimate (C 0 , C 1 ) of a curvature (C) of the roadway (34);b. estimating (510) an unconstrained state of the target vehicle (36) and estimating an associated covariance thereof, wherein said unconstrained state is representative of a two-dimensional position, velocity and acceleration of the target vehicle (36);c. establishing at least one prospective constraint of the target vehicle (36), wherein at least one prospective constraint is responsive to said estimate (C 0 , C 1 ) of the curvature (C) of the roadway (34) and said at least one prospective constraint is representative of a possible lane (38, 40) of the roadway (34) in which the target vehicle (36) is likely traveling;d. estimating (514) at least one constrained state and associated covariance thereof of the target vehicle (36) corresponding to said at least one prospective constraint of the target vehicle (36), wherein said at least one constrained state is representative of a two-dimensional position, velocity and acceleration of the target vehicle (36) when traveling along a corresponding said possible lane (38, 40) of the roadway (34);e. determining a most likely state of the target vehicle (36) to be the most likely state of said unconstrained state of the target vehicle (36) and a most probable of said at least one constrained state of the target vehicle (36);and f. if said most probable of said at least one constrained state of the target vehicle (36) is the determined most likely state, then fusing (528) the unconstrained state and covariance thereof of the target vehicle (36) with said most probable of said at least one constrained state and covariance thereof so as to generate a corresponding fused state estimate and an associated fused covariance thereof, and outputting (530), as the estimated state or covariance of the target vehicle (36), at least one of the fused state and the associated fused covariance thereof of the target vehicle (36);otherwise outputting (526), as the estimated state or covariance of the target vehicle (36), at least one of the unconstrained state and the associated unconstrained covariance thereof of the target vehicle (36).
23
A system for estimating a state of a target vehicle (36) on a roadway (34), comprising:a. a road curvature estimation subsystem (42) for estimating a curvature (C) of a roadway (34) upon which a host vehicle (12) is traveling;b. a target state estimation subsystem (44) operatively coupled to said host vehicle (12), wherein said target state estimation subsystem (44) is adapted to track the target vehicle (36) on the roadway (34), wherein said target state estimation subsystem (44) is configured to estimate (510) an unconstrained state of the target vehicle (36) and estimate an associated covariance thereof, wherein said unconstrained state is representative of a two-dimensional position, velocity and acceleration of the target vehicle (36);and c. at least one processor (26) operatively coupled to or a part of said road curvature estimation subsystem (42) and said target state estimation subsystem (44), wherein said processor (26) is adapted to determine if the target vehicle (36) is likely traveling in a particular lane (38, 40) of the roadway (34) and to determine in which of a plurality of lanes (38, 40) the target vehicle (36) is likely traveling, responsive to said curvature (C) estimated by said road curvature estimation subsystem (42), and responsive to a measure of target kinematics from said target state estimation subsystem (44), wherein said at least one processor (26) is configured to: i. establish at least one prospective constraint of the target vehicle (36), wherein at least one prospective constraint is responsive to said estimate (C 0 , C 1 ) of the curvature (C) of the roadway (34) and said at least one prospective constraint is representative of a possible lane (38, 40) of the roadway (34);ii. estimate (514) at least one constrained state and associated covariance thereof of the target vehicle (36) corresponding to said at least one prospective constraint of the target vehicle (36), wherein said at least one constrained state is representative of a two-dimensional position, velocity and acceleration of the target vehicle (36) when traveling along said possible lane (38, 40) of the roadway (34);iii. determine a most likely state of the target vehicle (36) to be the most likely state of said unconstrained state of the target vehicle (36) and a most probable of said at least one constrained state of the target vehicle (36);and iv. estimate and output a probability that the target vehicle (36) is in said particular lane (38, 40) of the roadway (34) associated with said most probable of said at least one constrained state;otherwise estimate and output that the target vehicle (36) is not in any particular lane of the roadway (34).

Independent claims2

74 paragraphs in 3 sections, as filed

0001<patcit id="pcit0001" dnum="DE19637245A"><text>DE 196 37 245 A</text></patcit> describes a regulating method involving using a forward looking sensor system, e.g. a radar system for a road vehicle moving on a multi lane highway. The radar system has sensing beams that distinguish between the three lanes and identify vehicles on those lanes. The on-board controller executes a programme to provide speed regulation based upon which lanes have vehicles ahead. Additionally, a minimum value of acceleration must be met for the vehicles ahead.

0002<patcit id="pcit0002" dnum="DE19855400A"><text>DE 198 55 400 A</text></patcit> describes a method and device for determining the future course of a first vehicle that is fitted with a distance sensor, whereby the at least relative positions of at least one travelling vehicle ahead are determined with respect to the first vehicle at a given or selectable moment in time with the aid of said distance sensor. The at least relative positions thus determined are stored in at least one memory. These relative positions that are stored in said memory are used to create a trajectory for the vehicle ahead. The future course of the first vehicle is determined at least by means of the trajectory of the travelling vehicle ahead. The trajectory of the travelling vehicle ahead is projected towards the position of the first vehicle.

0003<patcit id="pcit0003" dnum="DE10118265A"><text>DE 101 18 265 A</text></patcit> describes a method involving measuring the angular rate of at least one preceding vehicle with respect to the actual vehicle with the aid of the position determination device and forming a track change indicating signal by comparing the measured angular rate with the vehicle's own yaw rate.

0004<patcit id="pcit0004" dnum="EP0915350A"><text>EP 0 915 350 A</text></patcit> describes an arrangement having a sensor of road measurement data, an object position sensing arrangement and a vehicle motion sensor. An estimating device estimates the curvature of the road and/or the transverse position of a detected object relative to the road from the received sensor data using an estimating algorithm containing a dynamic vehicle motion model.

0005According to the present invention, there is provided a method and a system of estimating a state of a target vehicle on a roadway as defined in appended claims 1 and 23.

BRIEF DESCRIPTION OF THE DRAWINGS

0006In the accompanying drawings: <ul id="ul0001" list-style="none"><li><figref idref="f0001">FIG. 1</figref> illustrates a block diagram of hardware associated with a predictive collision sensing system;</li><li><figref idref="f0001">FIG. 2</figref> illustrates a coverage pattern of a radar beam used by the predictive collision sensing system;</li><li><figref idref="f0001">Fig. 3</figref> depicts a driving scenario for purposes of illustrating the operation of the predictive collision sensing system;</li><li><figref idref="f0002">Fig. 4</figref> illustrates a block diagram of the hardware and an associated signal processing algorithm of the predictive collision sensing system;</li><li><figref idref="f0003">Fig. 5</figref> illustrates a flow chart of an associated signal processing algorithm of the predictive collision sensing system;</li><li><figref idref="f0004">Fig. 6</figref> illustrates a geometry used for determining curvature parameters of a roadway;</li><li><figref idref="f0004">Fig. 7</figref> illustrates the geometry of an arc;</li><li><figref idref="f0004 f0006">Figs. 8a-d</figref> illustrates an example of the estimation of target position, lateral velocity, and road curvature parameters for a straight roadway;</li><li><figref idref="f0006">Figs. 9a</figref>-b illustrate an example of the target state RMS errors from unconstrained and constrained filtering on the straight roadway, corresponding to <figref idref="f0004">Figs. 8a</figref>-d;</li><li><figref idref="f0007">Figs. 10a</figref>-d illustrate an example of the estimation of target position, lateral velocity, and road curvature parameters for a curved roadway;</li><li><figref idref="f0009 f0010">Figs. 11a-b</figref> illustrate an example of the target state RMS errors from unconstrained and constrained filtering for the curved roadway, corresponding to <figref idref="f0007">Figs. 10a</figref>-d;</li><li><figref idref="f0010 f0012">Figs. 12a-d</figref> illustrate an example of the estimation of target position, lateral velocity, and associated RMS errors for a straight roadway involving a lane change; and</li><li><figref idref="f0012">Figs. 13a</figref>-d illustrates an example of the estimation of target position, lateral velocity, and their RMS errors for a curved roadway involving a lane change.</li></ul>

DESCRIPTION OF EMBODIMENT(S)

0007Referring to <figref idref="f0001">Fig. 1</figref>, a predictive collision sensing system 10 incorporated in a host vehicle 12, comprises a radar system 14 for sensing objects external to the host vehicle 12, and a set of sensors, including a yaw rate sensor 16, e.g. a gyroscopic sensor, and a speed sensor 18, for sensing motion of the host vehicle 12. The yaw rate sensor 16 and speed sensor 18 respectively provide measurements of the yaw rate and speed of the host vehicle 12. The radar system 14, e.g. a Doppler radar system, comprises an antenna 20 and a radar processor 22, wherein the radar processor 22 generates the RF signal which is transmitted by the antenna 20 and which is reflected by objects in view thereof. The radar processor 22 demodulates the associated reflected RF signal that is received by the antenna 20, and detects a signal that is responsive to one or more objects that are irradiated by the RF signal transmitted by the antenna 20. For example, the radar system 14 provides target range, range rate and azimuth angle measurements in host vehicle 12 fixed coordinates. Referring to <figref idref="f0001">Fig. 2</figref>, the antenna 20 is adapted to generate a radar beam 23 of RF energy that is, for example, either electronically or mechanically scanned across an azimuth range, e.g. +/- γ, e.g. +/- 50 degrees, responsive to a beam control element 24, and which has a distance range, e.g. about 100 meters, from the host vehicle 12 that is sufficiently far to enable a target to be detected sufficiently far in advance of a prospective collision with the host vehicle 12 so as to enable a potentially mitigating action to be taken by the host vehicle 12 so as to either avoid the prospective collision or mitigate damage or injury as a result thereof. The radar processor 22, yaw rate sensor 16, and speed sensor 18 are operatively connected to a signal processor 26 that operates in accordance with an associated predictive collision sensing algorithm to determine whether or not a collision with an object, e.g. a target vehicle 36 (illustrated in <figref idref="f0001">Fig. 3</figref>), is likely, and if so, to also determine an action to be taken responsive thereto, for example, one or more of activating an associated warning system 28 or safety system 30 (e.g. frontal air bag system), or using a vehicle control system 32 (e.g. an associated braking or steering system) to take evasive action so as to either avoid the prospective collision or to reduce the consequences thereof.

0008Referring to <figref idref="f0001">Fig. 3</figref>, the host vehicle 12 is shown moving along a multiple lane roadway 34, either straight or curved, and there is also shown a target vehicle 36 moving in an opposite direction, towards the host vehicle 12. Generally, there can be any number of target vehicles 36 that can fit on the roadway 34, each moving in the same or opposite direction as the host vehicle 12. These target vehicles 36 can either be in the host lane 38 or in a neighboring lane 40 either adjacent to or separated from the host lane 38, but generally parallel thereto. For purposes of analysis, it is assumed that the host vehicle 12 moves along the center line 41 of its lane 38 steadily without in-lane wandering, and the road curvatures of all the parallel lanes 38, 40 are the same. Road curvature is assumed small such that the differences between the heading angles of the host vehicle 12 and any detectable target vehicles 36 are smaller than 15 degrees.

0009Referring to <figref idref="f0002">Fig. 4</figref>, the predictive collision sensing system 10 uses the measurements of speed Uh and yaw rate ωh of the host vehicle 12 from the speed sensor 18 and the yaw rate sensor 16 respectively therein; and the measurements of target range r, range rate ṙ and azimuth angle η for all target vehicles 36 from the radar system 14 mounted on the host vehicle 12; along with the corresponding error covariance matrices of all these measurements, to estimate each target's two dimensional position, velocity and acceleration [x,ẋ,ẍ,y,ẏ,ÿ] in the host fixed coordinate system at every sampling instance, preferably with an error as small as possible. The predictive collision sensing system 10 comprises 1) a road curvature estimation subsystem 42 for estimating the curvature of the roadway 34 using measurements from the host vehicle motion sensors, i.e. the yaw rate sensor 16 and speed sensor 18; 2) an unconstrained target state estimation subsystem 44 for estimating the state of a target illuminated by the radar beam 23 and detected by the radar processor 22; 3) a constrained target state estimation subsystem 46 for estimating the state of the constraint on the target, assuming that the target is constrained to be on the roadway 34, either in the host lane 38 or in a neighboring lane 40, for each possible lane 38, 40; 4) a target state decision subsystem 48 for determining whether the best estimate of the target state is either the unconstrained target state, or a target state constrained by one of the constraints; and 5) a target state fusion subsystem 50 for fusing the unconstrained target state estimate with the appropriate constraint identified by the target state decision subsystem 48 so as to generate a fused target state. The best estimate of target state - either the unconstrained target state or the fused target state -- is then used by a decision or control subsystem for determining whether or not the host vehicle 12 is at risk of collision with the target, and if so, for determining and effecting what the best course of action is to mitigate the consequences thereof, e.g. by action of either the warning system 28, the safety system 30, or the vehicle control system 32, or some combination thereof. When possible, the use of the geometric structure of the roadway 34 as a constraint to the target kinematics provides for a more accurate estimate of the target state, which thereby improves the reliability of any actions taken responsive thereto.

0010Referring also to <figref idref="f0003">Fig. 5</figref>, illustrating a method 500 of detecting the state, i.e. kinematic state variables, of a target in view of the host vehicle 12, the steps of which are, for example, carried out by the signal processor 26, in steps (502) and (504), the speed Uh and yaw rate ωh of the host vehicle 12 relative to the roadway 34 are respectively read from the speed sensor 18 and the yaw rate sensor 16 respectively. Then, in step (506), the curvature parameters and associated covariance thereof of the roadway 34 are estimated using first 52 and second 54 Kalman filters that respectively estimate the state (i.e. kinematic state variables of the host vehicle 12) and associated covariance thereof of the host vehicle 12, and then the curvature parameters and associated covariance thereof of the roadway 34, as described hereinbelow, wherein the curvature parameters and associated covariance thereof of the roadway 34 are then subsequently used by the constrained target state estimation subsystem 46 to generate associated constraints on the possible location of a prospective target vehicle 36.

0011A well-designed and constructed roadway 34 can be described by a set of parameters, including curvature, wherein the curvature of a segment of the roadway 34 is defined as: <maths id="math0001" num="(1)"><math display="block"><mrow><mi>C</mi><mo>=</mo><mfrac><mn>1</mn><mi>R</mi></mfrac></mrow></math><img file="EP1537440B1_D0001.tif" /></maths> where R is the radius of the segment. In general, for a piece of smooth roadway 34, the curvature variation can be described as a function of a distance l along the roadway 34 by a so-called clothoid model, i.e.: <maths id="math0002" num="(2)"><math display="block"><mrow><mi>C</mi><mo>=</mo><msub><mi>C</mi><mn>0</mn></msub><mo>+</mo><mfrac><mi mathvariant="italic">dC</mi><mi mathvariant="italic">dl</mi></mfrac><mi>l</mi><mo>=</mo><msub><mi>C</mi><mn>0</mn></msub><mo>+</mo><msub><mi>C</mi><mn>1</mn></msub><mi>l</mi></mrow></math><img file="EP1537440B1_D0002.tif" /></maths> where C1 = 1/A2 and A is referred to as the clothoid parameter.

0012Referring to <figref idref="f0004">Fig. 6</figref>, the heading angle θ defining the heading direction is given by: <maths id="math0003" num="(3)"><math display="block"><mrow><mi>θ</mi><mo>=</mo><msub><mi>θ</mi><mn>0</mn></msub><mo>+</mo><msubsup><mrow><mo>∫</mo></mrow><mn>0</mn><mi>l</mi></msubsup><mi>C</mi><mfenced><mi>τ</mi></mfenced><mi mathvariant="italic">dτ</mi><mn>.</mn></mrow></math><img file="EP1537440B1_D0003.tif" /></maths>

0013Substituting equation (2) into equation (3) gives <maths id="math0004" num="(4)"><math display="block"><mrow><mi mathvariant="normal">Δ</mi><mi>θ</mi><mo>=</mo><mi>θ</mi><mo>−</mo><msub><mi>θ</mi><mn>0</mn></msub><mo>=</mo><msub><mi>C</mi><mn>0</mn></msub><mi>l</mi><mo>+</mo><msub><mi>C</mi><mn>1</mn></msub><msup><mi>l</mi><mn>2</mn></msup><mo>/</mo><mn>2</mn></mrow></math><img file="EP1537440B1_D0004.tif" /></maths>

0014Referring to <figref idref="f0004">Fig. 6</figref>, the equation of the roadway 34, i.e. the road equation, in x-y coordinates is given by: <maths id="math0005" num="(5)"><math display="block"><mrow><mi>x</mi><mo>=</mo><msub><mi>x</mi><mn>0</mn></msub><mo>+</mo><msubsup><mrow><mo>∫</mo></mrow><mn>0</mn><mn>1</mn></msubsup><mi>cos</mi><mi>θ</mi><mfenced><mi mathvariant="normal">τ</mi></mfenced><mi>d</mi><mi mathvariant="normal">τ</mi></mrow></math><img file="EP1537440B1_D0005.tif" /></maths> and <maths id="math0006" num="(6)"><math display="block"><mrow><mi>y</mi><mo>=</mo><msub><mi>y</mi><mn>0</mn></msub><mo>+</mo><mrow><msubsup><mrow><mo>∫</mo></mrow><mn>0</mn><mn>1</mn></msubsup><mrow><mi>sin</mi><mi>θ</mi></mrow><mfenced><mi>τ</mi></mfenced></mrow><msub><mrow><mover><mrow><mi>d</mi><msup><mrow><mi>τ</mi><mn>.</mn></mrow><mrow><msubsup><mi>r</mi><mi>k</mi><mi>h</mi></msubsup></mrow></msup></mrow><mrow><msubsup><mi>r</mi><mi>k</mi><mi>h</mi></msubsup></mrow></mover></mrow><mrow><msubsup><mi>r</mi><mi>k</mi><mi>h</mi></msubsup></mrow></msub></mrow></math><img file="EP1537440B1_D0006.tif" /></maths>

0015Assuming the heading angle θ to be within 15 degrees, i.e. |θ|<15°, equations (5) and (6) can be approximated by: <maths id="math0007" num="(7)"><math display="block"><mrow><mi mathvariant="normal">Δ</mi><mi>x</mi><mo>=</mo><mi>x</mi><mo>−</mo><msub><mi>x</mi><mn>0</mn></msub><mo>≈</mo><mi>l</mi></mrow></math><img file="EP1537440B1_D0007.tif" /></maths><maths id="math0008" num="(8)"><math display="block"><mrow><mi mathvariant="normal">Δ</mi><mi>y</mi><mo>=</mo><mi>y</mi><mo>−</mo><msub><mi>y</mi><mn>0</mn></msub><mo>≈</mo><msub><mi>C</mi><mn>0</mn></msub><msup><mi>l</mi><mn>2</mn></msup><mo>/</mo><mn>2</mn><mo>+</mo><msub><mi>C</mi><mn>1</mn></msub><msup><mi>l</mi><mn>3</mn></msup><mo>/</mo><mn>6</mn><mo>≈</mo><msub><mi>C</mi><mn>0</mn></msub><mfrac><mrow><mi mathvariant="normal">Δ</mi><msup><mi>x</mi><mn>2</mn></msup></mrow><mn>2</mn></mfrac><mo>+</mo><msub><mi>C</mi><mn>1</mn></msub><mfrac><mrow><mi mathvariant="normal">Δ</mi><msup><mi>x</mi><mn>3</mn></msup></mrow><mn>6</mn></mfrac></mrow></math><img file="EP1537440B1_D0008.tif" /></maths>

0016Accordingly, the roadway 34 is modeled by an incremental road equation in terms of curvature coefficients: C0 and C1. This incremental road equation describes a broad range of road shapes as follows: 1) Straight roadway 34:C0=0 and C1=0; 2) circular roadway 34:C1=0; and 3) a general roadway 34 with an arbitrary shape for which the change in heading angle θ is less than 15 degrees: C0>0.

0017The road curvature parameters C0 and C1 are estimated using data from motion sensors (yaw rate sensor 16 and speed sensor 18) in the host vehicle 12, based upon the assumption that the host vehicle 12 moves along the center line 41 of the roadway 34 or associated host lane 38.

0018The road curvature parameters C0 and C1 can be calculated from data of ω, ω̇, U, U̇ responsive to measurements of yaw rate ωh and speed Uh of the host vehicle 12 from the available host vehicle 12 motion sensors. However, generally the measurements of yaw rate ωh and speed Uh, from the yaw rate sensor 16 and speed sensor 18 respectively, are noisy. A host state filter implemented by a first Kalman filter 52 is beneficial to generate estimates of ω, ω̇, U, U̇ from the associated noisy measurements of yaw rate ωh and speed Uh; after which a curvature filter implemented by a second Kalman filter 54 is used to generate smoothed estimates of the curvature parameters C0 and C1. The dynamics of the host vehicle 12 for the host state filter follows a predefined set of kinematic equations (constant velocity in this case) given by: <maths id="math0009" num="(9)"><math display="block"><mrow><msubsup><mrow><munder><mi>x</mi><mrow><mo>̲</mo></mrow></munder></mrow><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow><mi>h</mi></msubsup><mo>=</mo><msubsup><mi mathvariant="bold">F</mi><mi>k</mi><mi>h</mi></msubsup><mo>⋅</mo><msubsup><mrow><munder><mi>x</mi><mrow><mo>̲</mo></mrow></munder></mrow><mi>k</mi><mi>h</mi></msubsup><mo>+</mo><msubsup><mrow><munder><mi>w</mi><mrow><mo>̲</mo></mrow></munder></mrow><mi>k</mi><mi>h</mi></msubsup><mo>,</mo><mspace width="1em" /><msubsup><mrow><munder><mi>w</mi><mrow><mo>̲</mo></mrow></munder></mrow><mi>k</mi><mi>h</mi></msubsup><mo>∼</mo><mi>N</mi><mfenced separators=","><mn>0</mn><msubsup><mi mathvariant="bold">Q</mi><mi>k</mi><mi>h</mi></msubsup></mfenced></mrow></math><img file="EP1537440B1_D0009.tif" /></maths><maths id="math0010" num="(10)"><math display="block"><mrow><msubsup><mrow><munder><mi>z</mi><mrow><mo>̲</mo></mrow></munder></mrow><mi>k</mi><mi>h</mi></msubsup><mo>=</mo><msubsup><mi mathvariant="bold">H</mi><mi>k</mi><mi>h</mi></msubsup><mo>⋅</mo><msubsup><mrow><munder><mi>x</mi><mrow><mo>̲</mo></mrow></munder></mrow><mi>k</mi><mi>h</mi></msubsup><mo>+</mo><msubsup><mrow><munder><mi>v</mi><mrow><mo>̲</mo></mrow></munder></mrow><mi>k</mi><mi>h</mi></msubsup><mo>,</mo><mspace width="2em" /><msubsup><mrow><munder><mi>v</mi><mrow><mo>̲</mo></mrow></munder></mrow><mi>k</mi><mi>h</mi></msubsup><mo>∼</mo><mi>N</mi><mfenced separators=","><mn>0</mn><msubsup><mi mathvariant="bold">R</mi><mi>k</mi><mi>h</mi></msubsup></mfenced></mrow></math><img file="EP1537440B1_D0010.tif" /></maths> where <maths id="math0011" num="(11)"><math display="block"><mrow><msubsup><mi mathvariant="bold">F</mi><mi>k</mi><mi>h</mi></msubsup><mo>=</mo><mfenced open="[" close="]"><mtable><mtr><mtd><mn>1</mn></mtd><mtd><mi>T</mi></mtd><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd></mtr><mtr><mtd><mn>0</mn></mtd><mtd><mn>1</mn></mtd><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd></mtr><mtr><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd><mtd><mn>1</mn></mtd><mtd><mi>T</mi></mtd></mtr><mtr><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd><mtd><mn>1</mn></mtd></mtr></mtable></mfenced><mo>,</mo><msubsup><mi mathvariant="bold">H</mi><mi>k</mi><mi>h</mi></msubsup><mo>=</mo><mfenced open="[" close="]"><mtable><mtr><mtd><mn>1</mn></mtd><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd></mtr><mtr><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd><mtd><mn>1</mn></mtd><mtd><mn>0</mn></mtd></mtr></mtable></mfenced><mo>,</mo><mspace width="1em" /><msubsup><mrow><munder><mi>x</mi><mrow><mo>̲</mo></mrow></munder></mrow><mi>k</mi><mi>h</mi></msubsup><mo>=</mo><mrow><mo>[</mo><mtable><mtr><mtd><mi>U</mi></mtd></mtr><mtr><mtd><mover><mrow><mi>U</mi></mrow><mrow><mo>˙</mo></mrow></mover></mtd></mtr><mtr><mtd><mi>ω</mi></mtd></mtr><mtr><mtd><mover><mi>ω</mi><mrow><mo>˙</mo></mrow></mover></mtd></mtr></mtable><msub><mrow><mo>]</mo></mrow><mi>k</mi></msub></mrow><mi>and</mi><mspace width="1em" /><msubsup><mrow><munder><mi>z</mi><mrow><mo>̲</mo></mrow></munder></mrow><mi>k</mi><mi>h</mi></msubsup><mo>=</mo><msub><mfenced open="[" close="]"><mtable><mtr><mtd><msup><mi>U</mi><mi>h</mi></msup></mtd></mtr><mtr><mtd><msup><mrow><mi>ω</mi></mrow><mi>h</mi></msup></mtd></mtr></mtable></mfenced><mi>k</mi></msub></mrow></math><img file="EP1537440B1_D0011.tif" /></maths> and where T is the sampling period, superscript (·)h is used to indicate that the filter is the host filter, and Uh and ωh are host vehicle 12 speed and yaw rate measurements. The first Kalman filter 52 is implemented to estimate the host state <maths id="math0012"><math display="inline"><mrow><msubsup><mrow><mover><mrow><munder><mi>x</mi><mrow><mo>̲</mo></mrow></munder></mrow><mrow><mo>^</mo></mrow></mover></mrow><mrow><mi>k</mi><mo>|</mo><mi>k</mi></mrow><mi>h</mi></msubsup></mrow></math><img file="EP1537440B1_D0012.tif" /></maths> and its error covariance <maths id="math0013"><math display="inline"><mrow><msubsup><mi mathvariant="bold">P</mi><mrow><mi>k</mi><mo>|</mo><mi>k</mi></mrow><mi>h</mi></msubsup><mo>,</mo></mrow></math><img file="EP1537440B1_D0013.tif" /></maths> as illustrated in <figref idref="f0002">Fig. 4</figref>.

0019The estimate of the host state from the first Kalman filter 52, i.e. the host state filter, is then used to generate a synthetic measurement that is input to the second Kalman filter 54, i.e. curvature coefficient filter, wherein the associated Kalman filters 52, 54 operate in accordance with the Kalman filtering process described more fully in the Appendix hereinbelow. The relationship between the road curvature parameters C0, C1 and the host state variables ω, ω̇, U, U̇ is derived as follows: <ul id="ul0002" list-style="none" compact="compact"><li>From equation (4), the radius R of road curvature is expressed generally as a function R(l) of the distance l along the roadway, as is illustrated in <figref idref="f0004">Fig. 7</figref>. Taking the time derivative on both sides of equation (4) yields: <maths id="math0014" num="(12)"><math display="block"><mrow><mover><mi>θ</mi><mrow><mo>˙</mo></mrow></mover><mo>=</mo><msub><mi>C</mi><mn>0</mn></msub><mo>⋅</mo><mi>i</mi><mo>+</mo><msub><mi>C</mi><mn>1</mn></msub><mo>⋅</mo><mi>l</mi><mo>⋅</mo><mi>i</mi><mo>=</mo><mfenced><msub><mi>C</mi><mn>0</mn></msub><mo>+</mo><msub><mi>C</mi><mn>1</mn></msub><mo>⋅</mo><mi>l</mi></mfenced><mo>⋅</mo><mi>i</mi><mn>.</mn></mrow></math><img file="EP1537440B1_D0014.tif" /></maths></li></ul>

0020Noting that θ̇ = ω, the yaw rate of the host vehicle 12, and that i=U, the speed of the host vehicle 12, and substituting the clothoid model of equation (2) in equation (12), yields: <maths id="math0015" num="(13)"><math display="block"><mrow><mi>ω</mi><mo>=</mo><mi>C</mi><mo>⋅</mo><mi>U</mi></mrow></math><img file="EP1537440B1_D0015.tif" /></maths> or <maths id="math0016" num="(14)"><math display="block"><mrow><mi>C</mi><mo>=</mo><mfrac><mi>ω</mi><mi>U</mi></mfrac><mn>.</mn></mrow></math><img file="EP1537440B1_D0016.tif" /></maths>

0021Clothoid parameter C0 is given as the value of curvature C at l=0, or <maths id="math0017" num="(15)"><math display="block"><mrow><msub><mi>C</mi><mn>0</mn></msub><mo>=</mo><mi>C</mi><msub><mrow><mo>|</mo></mrow><mrow><mi>l</mi><mo>=</mo><mn>0</mn></mrow></msub><mo>=</mo><mfrac><mi>ω</mi><mi>U</mi></mfrac><mn>.</mn></mrow></math><img file="EP1537440B1_D0017.tif" /></maths>

0022Taking the derivative on both sides of equation (14) yields <maths id="math0018" num="(16)"><math display="block"><mrow><mover><mi>C</mi><mrow><mo>˙</mo></mrow></mover><mo>=</mo><mfrac><mrow><mover><mi>ω</mi><mrow><mo>˙</mo></mrow></mover></mrow><mi>U</mi></mfrac><mo>−</mo><mfrac><mrow><mi>ω</mi><mo>⋅</mo><mover><mi>U</mi><mrow><mo>˙</mo></mrow></mover></mrow><mrow><msup><mi>U</mi><mn>2</mn></msup></mrow></mfrac><mn>.</mn></mrow></math><img file="EP1537440B1_D0018.tif" /></maths>

0023Using the definition of C1, from equation (2), C1 may be expressed in terms of the host state as follows: <maths id="math0019" num="(17)"><math display="block"><mrow><msub><mi>C</mi><mn>1</mn></msub><mo>=</mo><mfrac><mi mathvariant="italic">dC</mi><mi mathvariant="italic">dl</mi></mfrac><mo>=</mo><mfrac><mi mathvariant="italic">dC</mi><mi mathvariant="italic">dt</mi></mfrac><mo>⋅</mo><mfrac><mi mathvariant="italic">dt</mi><mi mathvariant="italic">dl</mi></mfrac><mo>=</mo><mfrac><mrow><mover><mi mathvariant="italic">C</mi><mrow><mo>˙</mo></mrow></mover></mrow><mi mathvariant="italic">U</mi></mfrac><mo>=</mo><mfrac><mrow><mover><mi mathvariant="italic">ω</mi><mrow><mo>˙</mo></mrow></mover></mrow><mrow><msup><mi mathvariant="italic">U</mi><mn mathvariant="italic">2</mn></msup></mrow></mfrac><mo>−</mo><mfrac><mrow><mi mathvariant="italic">ω</mi><mo>⋅</mo><mover><mi mathvariant="italic">U</mi><mrow><mo>˙</mo></mrow></mover></mrow><mrow><msup><mi mathvariant="italic">U</mi><mn>3</mn></msup></mrow></mfrac><mn>.</mn></mrow></math><img file="EP1537440B1_D0019.tif" /></maths>

0024The system equations for the second Kalman filter 54, i.e. the curvature filter, that generates curvature estimates Ĉ0<sub2>k|k</sub2> and Ĉ1<sub2>k|k</sub2>, are given by <maths id="math0020" num="(18)"><math display="block"><mrow><msubsup><mrow><munder><mi>x</mi><mrow><mo>̲</mo></mrow></munder></mrow><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow><mi>C</mi></msubsup><mo>=</mo><msubsup><mi mathvariant="bold">F</mi><mi>k</mi><mi>C</mi></msubsup><mo>⋅</mo><msubsup><mrow><munder><mi>x</mi><mrow><mo>̲</mo></mrow></munder></mrow><mi>k</mi><mi>C</mi></msubsup><mo>+</mo><msubsup><mrow><munder><mi>w</mi><mrow><mo>̲</mo></mrow></munder></mrow><mi>k</mi><mi>C</mi></msubsup><mo>,</mo><mspace width="1em" /><msubsup><mrow><munder><mi>w</mi><mrow><mo>̲</mo></mrow></munder></mrow><mi>k</mi><mi>C</mi></msubsup><mo>∼</mo><mi>N</mi><mfenced separators=","><mn>0</mn><msubsup><mi mathvariant="bold">Q</mi><mi>k</mi><mi>C</mi></msubsup></mfenced></mrow></math><img file="EP1537440B1_D0020.tif" /></maths><maths id="math0021" num="(19)"><math display="block"><mrow><msubsup><mrow><munder><mi>z</mi><mrow><mo>̲</mo></mrow></munder></mrow><mi>k</mi><mi>C</mi></msubsup><mo>=</mo><msup><mi mathvariant="bold">H</mi><mi>C</mi></msup><mo>⋅</mo><msubsup><mrow><munder><mi>x</mi><mrow><mo>̲</mo></mrow></munder></mrow><mi>k</mi><mi>C</mi></msubsup><mo>+</mo><msubsup><mi>v</mi><mi>k</mi><mi>C</mi></msubsup><mo>,</mo><mspace width="1em" /><msubsup><mi>v</mi><mi>k</mi><mi>C</mi></msubsup><mo>∼</mo><mi>N</mi><mfenced separators=","><mn>0</mn><msubsup><mi mathvariant="bold">R</mi><mi>k</mi><mi>C</mi></msubsup></mfenced></mrow></math><img file="EP1537440B1_D0021.tif" /></maths> where <maths id="math0022" num="(19a)"><math display="block"><mrow><msubsup><mi mathvariant="bold">F</mi><mi>k</mi><mi>C</mi></msubsup><mo>=</mo><mfenced open="[" close="]"><mtable><mtr><mtd><mn>1</mn></mtd><mtd><mi mathvariant="normal">Δ</mi><mi>t</mi><mo>⋅</mo><mover><mi>U</mi><mrow><mo>^</mo></mrow></mover></mtd></mtr><mtr><mtd><mn>0</mn></mtd><mtd><mn>1</mn></mtd></mtr></mtable></mfenced><mo>,</mo><mspace width="1em" /><msubsup><mi mathvariant="bold">H</mi><mi>k</mi><mi>h</mi></msubsup><mo>=</mo><mfenced open="[" close="]"><mtable><mtr><mtd><mn>1</mn></mtd><mtd><mn>0</mn></mtd></mtr><mtr><mtd><mn>0</mn></mtd><mtd><mn>1</mn></mtd></mtr></mtable></mfenced><mo>,</mo><mspace width="1em" /><msubsup><mrow><munder><mi>x</mi><mrow><mo>̲</mo></mrow></munder></mrow><mi>k</mi><mi>C</mi></msubsup><mo>=</mo><mrow><mo>[</mo><mtable><mtr><mtd><msub><mi>C</mi><mn>0</mn></msub></mtd></mtr><mtr><mtd><msub><mi>C</mi><mn>1</mn></msub></mtd></mtr></mtable><msub><mrow><mo>]</mo></mrow><mi>k</mi></msub></mrow><mspace width="1em" /><mo>,</mo></mrow></math><img file="EP1537440B1_D0022.tif" /></maths> Δt is the update time period of the second Kalman filter 54, and the values of the elements of the measurement vector <maths id="math0023"><math display="inline"><mrow><msubsup><mrow><munder><mi>z</mi><mrow><mo>̲</mo></mrow></munder></mrow><mi>k</mi><mi>C</mi></msubsup></mrow></math><img file="EP1537440B1_D0023.tif" /></maths> are given by the corresponding values of the state variables -- i.e. the clothoid parameters C0 and C1 -- of the curvature filter.

0025The measurement, <maths id="math0024"><math display="inline"><mrow><msubsup><mrow><munder><mi>z</mi><mrow><mo>̲</mo></mrow></munder></mrow><mi>k</mi><mi>C</mi></msubsup><mo>,</mo></mrow></math><img file="EP1537440B1_D0024.tif" /></maths> is transformed from the estimated state <maths id="math0025"><math display="inline"><mrow><msup><mrow><msub><mfenced open="[" close="]" separators=",,,"><mover><mi>U</mi><mrow><mo>^</mo></mrow></mover><mover><mrow><mover><mi>U</mi><mrow><mo>˙</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover><mover><mi>ω</mi><mrow><mo>^</mo></mrow></mover><mover><mrow><mover><mi>ω</mi><mrow><mo>˙</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mfenced><mi>k</mi></msub></mrow><mi>T</mi></msup></mrow></math><img file="EP1537440B1_D0025.tif" /></maths> as follows: <maths id="math0026" num="(20)"><math display="block"><mrow><mrow><msubsup><mrow><munder><mi>z</mi><mrow><mo>̲</mo></mrow></munder></mrow><mi>k</mi><mi>C</mi></msubsup></mrow><mo>=</mo><msub><mfenced open="[" close="]"><mtable><mtr><mtd><mfrac><mrow><mover><mi>ω</mi><mrow><mo>^</mo></mrow></mover></mrow><mrow><mover><mi>U</mi><mrow><mo>^</mo></mrow></mover></mrow></mfrac></mtd></mtr><mtr><mtd><mfrac><mrow><mover><mrow><mover><mi>ω</mi><mrow><mo>˙</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mrow><msup><mrow><mover><mi>U</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>2</mn></msup></mrow></mfrac><mo>−</mo><mfrac><mrow><mover><mi>ω</mi><mrow><mo>^</mo></mrow></mover><mo>⋅</mo><mover><mrow><mover><mi>U</mi><mrow><mo>˙</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mrow><msup><mrow><mover><mi>U</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>3</mn></msup></mrow></mfrac></mtd></mtr></mtable></mfenced><mrow><mi>k</mi><mrow><mo>|</mo><mi>k</mi></mrow></mrow></msub></mrow></math><img file="EP1537440B1_D0026.tif" /></maths> and the associated covariance of the measurements is given by: <maths id="math0027" num="(21)"><math display="block"><mrow><msubsup><mi mathvariant="bold">R</mi><mi>k</mi><mi>C</mi></msubsup><mo>=</mo><msubsup><mi mathvariant="bold">J</mi><mi>k</mi><mi>C</mi></msubsup><msubsup><mi mathvariant="bold">P</mi><mrow><mi>k</mi><mrow><mo>|</mo><mi>k</mi></mrow></mrow><mi>h</mi></msubsup><msup><mfenced><msubsup><mi mathvariant="bold">J</mi><mi>k</mi><mi>C</mi></msubsup></mfenced><mi>T</mi></msup></mrow></math><img file="EP1537440B1_D0027.tif" /></maths> where <maths id="math0028" num="(22)"><math display="block"><mrow><msubsup><mi mathvariant="bold">J</mi><mi>k</mi><mi>C</mi></msubsup><mo>=</mo><mrow><mfrac><mrow><mo>∂</mo><mfenced open="[" close="]"><mtable><mtr><mtd><msub><mi>C</mi><mn>0</mn></msub></mtd></mtr><mtr><mtd><msub><mi>C</mi><mn>1</mn></msub></mtd></mtr></mtable></mfenced></mrow><mrow><mo>∂</mo><msup><mrow><munder><mi>x</mi><mrow><mo>̲</mo></mrow></munder></mrow><mi>h</mi></msup></mrow></mfrac></mrow><mrow><mo>|</mo><msup><mrow><munder><mi>x</mi><mrow><mo>̲</mo></mrow></munder></mrow><mi>h</mi></msup><mo>=</mo><msubsup><mrow><munder><mrow><mover><mi>x</mi><mrow><mo>^</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow><mi>h</mi></msubsup></mrow><mo>=</mo><msub><mfenced open="[" close="]"><mtable><mtr><mtd><mo>−</mo><mfrac><mrow><mover><mi>ω</mi><mrow><mo>^</mo></mrow></mover></mrow><mrow><msup><mrow><mover><mi>U</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>2</mn></msup></mrow></mfrac></mtd><mtd><mn>0</mn></mtd><mtd><mfrac><mn>1</mn><mrow><mover><mi>U</mi><mrow><mo>^</mo></mrow></mover></mrow></mfrac></mtd><mtd><mn>0</mn></mtd></mtr><mtr><mtd><mo>−</mo><mfrac><mrow><mn>2</mn><mo>⋅</mo><mover><mrow><mover><mi>ω</mi><mrow><mo>˙</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mrow><msup><mrow><mover><mi>U</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>3</mn></msup></mrow></mfrac><mo>+</mo><mfrac><mrow><mn>3</mn><mo>⋅</mo><mover><mi>ω</mi><mrow><mo>^</mo></mrow></mover><mo>⋅</mo><mover><mrow><mover><mi>U</mi><mrow><mo>˙</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mrow><msup><mrow><mover><mi>U</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>4</mn></msup></mrow></mfrac></mtd><mtd><mo>−</mo><mfrac><mrow><mover><mi>ω</mi><mrow><mo>^</mo></mrow></mover></mrow><mrow><msup><mrow><mover><mi>U</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>3</mn></msup></mrow></mfrac></mtd><mtd><mo>−</mo><mfrac><mrow><mover><mrow><mover><mi>U</mi><mrow><mo>˙</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mrow><msup><mrow><mover><mi>U</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>3</mn></msup></mrow></mfrac></mtd><mtd><mfrac><mn>1</mn><mrow><msup><mrow><mover><mi>U</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>2</mn></msup></mrow></mfrac></mtd></mtr></mtable></mfenced><mrow><mi>k</mi><mrow><mo>|</mo><mi>k</mi></mrow></mrow></msub><mn>.</mn></mrow></math><img file="EP1537440B1_D0028.tif" /></maths>

0026It should be understood that other systems and methods for estimating the curvature parameters of the roadway 34 may be substituted in the road curvature estimation subsystem 42 for that described above. For example, the curvature parameters of the roadway may also be estimated from images of the roadway 34 by a vision system, either instead of or in conjunction with the above described system based upon measurements of speed Uh and yaw rate ωh from associated motion sensors. Furthermore, it should be understood that yaw rate can be either measured or determined in a variety of ways, or using a variety of means, for example, but not limited to, using a yaw gyro sensor, a steering angle sensor, a differential wheel speed sensor, or a GPS-based sensor; a combination thereof; or functions of measurements therefrom (e.g. a function of, inter alia, steering angle rate).

0027Referring again to <figref idref="f0003">Fig. 5</figref>, in step (508), the measurements of target range r, range rate ṙ, and azimuth angle η are read from the radar processor 22, and are used as inputs to an extended Kalman filter 56, i.e. the main filter, which, in step (510), generates estimates of the unconstrained target state -- i.e. the kinematic state variables of the target - which estimates are relative values in the local coordinate system of the host vehicle 12 (i.e. the host-fixed coordinate system) which moves with therewith. In step (512), the unconstrained target state, i.e. the target velocity and acceleration, is transformed to absolute coordinates of the absolute coordinate system fixed on the host vehicle 12 at the current instant of time as illustrated in <figref idref="f0001">Fig. 3</figref>, so as to be consistent with the absolute coordinate system in which the road constraint equations are derived and for which the associated curvature parameters are assumed to be constant, when used in the associated constraint equations described hereinbelow in order to generate estimates of the constrained target state. The absolute coordinate system superimposes the moving coordinate system in space at the current instant, so that the transformation in step (512) is realized by adding the velocities and accelerations of the host vehicle 12 to the corresponding target estimates, in both x and y directions.

0028The result from the coordinate transformation in step (512) of the output from the extended Kalman filter 56 is then partitioned into the following parts, corresponding respectively to the x and y position of the target vehicle 36 relative to the host vehicle 12, wherein the superscript 1 refers to the unconstrained target state of the target vehicle 36:<maths id="math0029" num="(23)"><math display="block"><mrow><msubsup><mrow><munder><mrow><mover><mi>X</mi><mrow><mo>^</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo><mi>k</mi></mrow></mrow></msub></mrow><mn>1</mn></msubsup><mo>=</mo><msub><mrow><mfenced open="[" close="]"><mtable><mtr><mtd><msubsup><mrow><mover><mrow><mrow><munder><mi>x</mi><mrow><mo>̲</mo></mrow></munder></mrow></mrow><mrow><mo>^</mo></mrow></mover></mrow><mi>t</mi><mn>1</mn></msubsup></mtd></mtr><mtr><mtd><msubsup><mrow><mover><mrow><mrow><munder><mi>y</mi><mrow><mo>̲</mo></mrow></munder></mrow></mrow><mrow><mo>^</mo></mrow></mover></mrow><mi>t</mi><mn>1</mn></msubsup></mtd></mtr></mtable></mfenced><mspace width="1em" /></mrow><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub><mspace width="2em" /><mi>and</mi><mspace width="2em" /><msubsup><mi mathvariant="bold">P</mi><mrow><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo><mi>k</mi></mrow></mrow></msub></mrow><mn>1</mn></msubsup><mo>=</mo><msub><mfenced open="[" close="]"><mtable><mtr><mtd><msub><mi mathvariant="bold">P</mi><mrow><msub><mi>x</mi><mi>t</mi></msub></mrow></msub></mtd><mtd><msubsup><mi mathvariant="bold">P</mi><mrow><msub><mrow><mi>x</mi><mi>y</mi></mrow><mi>t</mi></msub></mrow><mn>1</mn></msubsup></mtd></mtr><mtr><mtd><msubsup><mi mathvariant="bold">P</mi><mrow><msub><mrow><mi>y</mi><mi>x</mi></mrow><mi>t</mi></msub></mrow><mn>1</mn></msubsup></mtd><mtd><msubsup><mi mathvariant="bold">P</mi><mrow><msub><mi>y</mi><mi>t</mi></msub></mrow><mn>1</mn></msubsup></mtd></mtr></mtable></mfenced><mrow><mi>k</mi><mrow><mo>|</mo><mi>k</mi></mrow></mrow></msub><mn>.</mn></mrow></math><img file="EP1537440B1_D0029.tif" /></maths>

0029Referring again to <figref idref="f0003">Fig. 5</figref>, following steps (506) and (512), in steps (514) through (524) described more fully hereinbelow, various constraints on the possible trajectory of the target vehicle 36 are applied and tested to determine if the target vehicle 36 is likely traveling in accordance with one of the possible constraints. For example, the constraints are assumed to be from a set of lanes that includes the host lane 38 and possible neighboring lanes 40, and a target vehicle 36 that is likely traveling in accordance with one of the possible constraints would likely be traveling on either the host lane 38 or one of the possible neighboring lanes 40. In step (524), the hypothesis that the target vehicle 36 is traveling on either the host lane 38 or one of the possible neighboring lanes 40 is tested for each possible lane. If the hypothesis is not satisfied for one of the possible lanes, then, in step (526), the state of the target is assumed to be the unconstrained target state, which is then used for subsequent predictive crash sensing analysis and control responsive thereto. Otherwise, from step (524), in step (528), the target state is calculated by the target state fusion subsystem 50 as the fusion of the unconstrained target state with the associated state of the constraint that was identified in step (524) as being most likely.

0030Prior to discussing the process of steps (514) through (524) for determining whether the target is likely constrained by a constraint, and if so, what is the most likely constraint, the process of fusing the unconstrained target state with state of a constraint will first be described for the case of a target vehicle 36 moving in the same lane as the host vehicle 12. The constraints are assumed to be active in y-direction only, consistent with the assumptions that the host vehicle 12 moves along the center line 41 of its lane 38 steadily without in-lane wandering and that the road curvatures of all the parallel lanes 38, 40 are the same, and given that the absolute coordinate system is fixed on the host vehicle 12 at the current instant of time. Assuming the target vehicle 36 is moving in the same lane 38 as the host vehicle 12, and using the road constraint equation with the estimated coefficients, in step (514), the constraint state variables are then given in terms of the lateral kinematic variable as: <maths id="math0030" num="(24)"><math display="block"><mrow><msub><mrow><munder><mrow><mover><mrow><mover><mi>y</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo><mi>k</mi></mrow></mrow></msub></mrow></msub><mo>=</mo><msub><mfenced open="[" close="]"><mtable><mtr><mtd><mover><mrow><mi>y</mi></mrow><mrow><mo>‾</mo></mrow></mover></mtd></mtr><mtr><mtd><mover><mrow><mrow><mover><mrow><mi>y</mi></mrow><mrow><mo>˙</mo></mrow></mover></mrow></mrow><mrow><mo>‾</mo></mrow></mover></mtd></mtr><mtr><mtd><mover><mrow><mrow><mover><mi>y</mi><mrow><mo>¨</mo></mrow></mover></mrow></mrow><mrow><mo>‾</mo></mrow></mover></mtd></mtr></mtable></mfenced><mrow><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo><mi>k</mi></mrow></mrow></msub></mrow></msub><mo>=</mo><mfenced open="[" close="]"><mtable><mtr><mtd><msub><mrow><mover><mi>C</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>0</mn></msub><msup><mfenced><msup><mrow><mover><mi>x</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup></mfenced><mn>2</mn></msup><mo>/</mo><mn>2</mn><mo>+</mo><msub><mrow><mover><mi>C</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msub><msup><mfenced><msup><mrow><mover><mi>x</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup></mfenced><mn>3</mn></msup><mo>/</mo><mn>6</mn></mtd></mtr><mtr><mtd><msub><mrow><mover><mi>C</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>0</mn></msub><msup><mrow><mover><mi>x</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup><msup><mrow><mover><mrow><mover><mi>x</mi><mrow><mo>˙</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup><mo>+</mo><msub><mrow><mover><mi>C</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msub><msup><mfenced><msup><mrow><mover><mi>x</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup></mfenced><mn>2</mn></msup><msup><mrow><mover><mrow><mover><mi>x</mi><mrow><mo>˙</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup><mo>/</mo><mn>2</mn></mtd></mtr><mtr><mtd><msub><mrow><mover><mi>C</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>0</mn></msub><msup><mfenced><msup><mrow><mover><mrow><mover><mi>x</mi><mrow><mo>˙</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup></mfenced><mn>2</mn></msup><mo>+</mo><msub><mrow><mover><mi>C</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>0</mn></msub><msup><mrow><mover><mi>x</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup><msup><mrow><mover><mrow><mover><mi>x</mi><mrow><mo>¨</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup><mo>+</mo><msub><mrow><mover><mi>C</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msub><msup><mrow><mover><mi>x</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup><msup><mfenced><msup><mrow><mover><mrow><mover><mi>x</mi><mrow><mo>˙</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup></mfenced><mn>2</mn></msup><mo>+</mo><msub><mrow><mover><mi>C</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msub><msup><mfenced><msup><mrow><mover><mi>x</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup></mfenced><mn>2</mn></msup><msup><mrow><mover><mrow><mover><mi>x</mi><mrow><mo>¨</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup><mo>/</mo><mn>2</mn></mtd></mtr></mtable></mfenced></mrow></math><img file="EP1537440B1_D0030.tif" /></maths> and <maths id="math0031" num="(25)"><math display="block"><mrow><msub><mrow><mover><mi mathvariant="bold">P</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mi>y</mi><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo><mi>k</mi></mrow></mrow></msub></mrow></msub><mo>=</mo><msubsup><mi mathvariant="bold">A</mi><mi>k</mi><mn>1</mn></msubsup><msub><mi mathvariant="bold">P</mi><mrow><mi>x</mi><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo><mi>k</mi></mrow></mrow></msub></mrow></msub><msup><mfenced><msubsup><mi mathvariant="bold">A</mi><mi>k</mi><mn>1</mn></msubsup></mfenced><mi mathvariant="normal">T</mi></msup><mo>+</mo><msubsup><mi mathvariant="bold">A</mi><mi>k</mi><mn>2</mn></msubsup><msubsup><mi mathvariant="bold">P</mi><mrow><mi>k</mi><mrow><mo>|</mo><mi>k</mi></mrow></mrow><mi>C</mi></msubsup><msup><mfenced><msubsup><mi mathvariant="bold">A</mi><mi>k</mi><mn>2</mn></msubsup></mfenced><mi mathvariant="normal">T</mi></msup></mrow></math><img file="EP1537440B1_D0031.tif" /></maths> where <maths id="math0032" num="(26)"><math display="block"><mrow><msubsup><mi mathvariant="bold">A</mi><mi>k</mi><mn>1</mn></msubsup><mo>=</mo><mfenced open="[" close="]"><mtable><mtr><mtd><msub><mrow><mover><mi>C</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>0</mn></msub><msup><mrow><mover><mi>x</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup><msub><mrow><mo>+</mo><mover><mi>C</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msub><msup><mfenced><msup><mrow><mover><mi>x</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup></mfenced><mn>2</mn></msup><mo>/</mo><mn>2</mn></mtd><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd></mtr><mtr><mtd><msub><mrow><mover><mi>C</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>0</mn></msub><msup><mrow><mover><mrow><mover><mi>x</mi><mrow><mo>˙</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup><mo>+</mo><msub><mrow><mover><mi>C</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msub><msup><mrow><mover><mi>x</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup><msup><mrow><mover><mrow><mover><mi>x</mi><mrow><mo>˙</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup></mtd><mtd><msub><mrow><mover><mi>C</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>0</mn></msub><msup><mrow><mover><mi>x</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup><msub><mrow><mo>+</mo><mover><mi>C</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msub><msup><mfenced><msup><mrow><mover><mi>x</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup></mfenced><mn>2</mn></msup><mo>/</mo><mn>2</mn></mtd><mtd><mn>0</mn></mtd></mtr><mtr><mtd><msub><mrow><mover><mi>C</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>0</mn></msub><msup><mrow><mover><mrow><mover><mi>x</mi><mrow><mo>¨</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup><mo>+</mo><msub><mrow><mover><mi>C</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msub><msup><mfenced><msup><mrow><mover><mrow><mover><mi>x</mi><mrow><mo>˙</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup></mfenced><mn>2</mn></msup><mo>+</mo><msub><mrow><mover><mi>C</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msub><msup><mrow><mover><mi>x</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup><msup><mrow><mover><mrow><mover><mi>x</mi><mrow><mo>¨</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup></mtd><mtd><msub><mrow><mn>2</mn><mover><mi>C</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>0</mn></msub><mover><mrow><mover><mi>x</mi><mrow><mo>˙</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover><mo>+</mo><mn>2</mn><msub><mrow><mover><mi>C</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msub><msup><mrow><mover><mi>x</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup><mover><mrow><mover><mi>x</mi><mrow><mo>˙</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mtd><mtd><msub><mrow><mover><mi>C</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>0</mn></msub><msup><mrow><mover><mi>x</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup><msub><mrow><mo>+</mo><mover><mi>C</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msub><msup><mfenced><msup><mrow><mover><mi>x</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup></mfenced><mn>2</mn></msup><mo>/</mo><mn>2</mn></mtd></mtr></mtable></mfenced></mrow></math><img file="EP1537440B1_D0032.tif" /></maths> and <maths id="math0033" num="(27)"><math display="block"><mrow><msubsup><mi mathvariant="bold">A</mi><mi>k</mi><mn>2</mn></msubsup><mo>=</mo><mfenced open="[" close="]"><mtable><mtr><mtd><msup><mrow><mrow><mfenced><msup><mrow><mover><mi>x</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup></mfenced></mrow></mrow><mn>2</mn></msup><mo>/</mo><mn>2</mn></mtd><mtd><msup><mfenced><msup><mrow><mover><mi>x</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup></mfenced><mn>3</mn></msup><mo>/</mo><mn>6</mn></mtd></mtr><mtr><mtd><msup><mrow><mover><mi>x</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup><mo>⋅</mo><msup><mrow><mover><mrow><mover><mi>x</mi><mrow><mo>˙</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup></mtd><mtd><msup><mrow><mrow><mfenced><msup><mrow><mover><mi>x</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup></mfenced></mrow></mrow><mn>2</mn></msup><mo>⋅</mo><msup><mrow><mover><mrow><mover><mi>x</mi><mrow><mo>˙</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup><mo>/</mo><mn>2</mn></mtd></mtr><mtr><mtd><msup><mrow><mrow><mfenced><msup><mrow><mover><mrow><mover><mi>x</mi><mrow><mo>˙</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup></mfenced></mrow></mrow><mn>2</mn></msup><mo>+</mo><msup><mrow><mover><mi>x</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup><mo>⋅</mo><msup><mrow><mover><mrow><mover><mi>x</mi><mrow><mo>¨</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup></mtd><mtd><msup><mrow><mover><mi>x</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup><mo>⋅</mo><msup><mfenced><msup><mrow><mover><mrow><mover><mi>x</mi><mrow><mo>˙</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup></mfenced><mn>2</mn></msup><mo>+</mo><msup><mfenced><msup><mrow><mover><mi>x</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup></mfenced><mn>2</mn></msup><mo>⋅</mo><msup><mrow><mover><mrow><mover><mi>x</mi><mrow><mo>¨</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup><mo>/</mo><mn>2</mn></mtd></mtr></mtable></mfenced><mn>.</mn></mrow></math><img file="EP1537440B1_D0033.tif" /></maths>

0031In step (528), the two y-coordinate estimates, one from the main filter and the other from the road constraint, are then fused as follows: <maths id="math0034" num="(28)"><math display="block"><mrow><msubsup><mi mathvariant="bold">P</mi><mrow><mi>y</mi><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo><mi>k</mi></mrow></mrow></msub></mrow><mi>f</mi></msubsup><mo>=</mo><msup><mrow><mo>⌊</mo><msup><mfenced><msub><mrow><mover><mi mathvariant="bold">P</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mi>y</mi><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo><mi>k</mi></mrow></mrow></msub></mrow></msub></mfenced><mrow><mo>−</mo><mn>1</mn></mrow></msup><mo>+</mo><msup><mfenced><msubsup><mi mathvariant="bold">P</mi><mrow><mi>y</mi><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo><mi>k</mi></mrow></mrow></msub></mrow><mn>1</mn></msubsup></mfenced><mrow><mo>−</mo><mn>1</mn></mrow></msup><mo>⌋</mo></mrow><mrow><mo>−</mo><mn>1</mn></mrow></msup></mrow></math><img file="EP1537440B1_D0034.tif" /></maths><maths id="math0035" num="(29)"><math display="block"><mrow><msubsup><mrow><munder><mrow><mover><mi>y</mi><mrow><mo>^</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo><mi>k</mi></mrow></mrow></msub></mrow><mi>f</mi></msubsup><mo>=</mo><msubsup><mi mathvariant="bold">P</mi><mrow><mi>y</mi><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo><mi>k</mi></mrow></mrow></msub></mrow><mi>f</mi></msubsup><mfenced open="[" close="]"><msup><mfenced><msub><mrow><mover><mi mathvariant="bold">P</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mi>y</mi><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo><mi>k</mi></mrow></mrow></msub></mrow></msub></mfenced><mrow><mo>−</mo><mn>1</mn></mrow></msup><msub><mrow><munder><mrow><mover><mrow><mover><mi>y</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo><mi>k</mi></mrow></mrow></msub></mrow></msub><mo>+</mo><msup><mfenced><msubsup><mi mathvariant="bold">P</mi><mrow><mi>y</mi><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo><mi>k</mi></mrow></mrow></msub></mrow><mn>1</mn></msubsup></mfenced><mrow><mo>−</mo><mn>1</mn></mrow></msup><msubsup><mrow><munder><mrow><mover><mi>y</mi><mrow><mo>^</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo><mi>k</mi></mrow></mrow></msub></mrow><mn>1</mn></msubsup></mfenced></mrow></math><img file="EP1537440B1_D0035.tif" /></maths>

0032Finally, the composed estimate of the target state is <maths id="math0036" num="(30)"><math display="block"><mrow><msub><mrow><mover><mrow><munder><mi>X</mi><mrow><mo>̲</mo></mrow></munder></mrow><mrow><mo>^</mo></mrow></mover></mrow><mrow><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo><mi>k</mi></mrow></mrow></msub></mrow></msub><mo>=</mo><mfenced open="[" close="]"><mtable><mtr><mtd><msubsup><mrow><munder><mrow><mover><mi>x</mi><mrow><mo>^</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo><mi>k</mi></mrow></mrow></msub></mrow><mn>1</mn></msubsup></mtd></mtr><mtr><mtd><msubsup><mrow><munder><mrow><mover><mi>y</mi><mrow><mo>^</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo><mi>k</mi></mrow></mrow></msub></mrow><mi>f</mi></msubsup></mtd></mtr></mtable></mfenced></mrow></math><img file="EP1537440B1_D0036.tif" /></maths> and <maths id="math0037" num="(31)"><math display="block"><mrow><msub><mi mathvariant="bold">P</mi><mrow><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo><mi>k</mi></mrow></mrow></msub></mrow></msub><mo>=</mo><msub><mfenced open="[" close="]"><mtable><mtr><mtd><msub><mi mathvariant="bold">P</mi><mrow><msub><mi>x</mi><mi>t</mi></msub></mrow></msub></mtd><mtd><msub><mrow><mover><mi mathvariant="bold">P</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><msub><mrow><mi>x</mi><mi>y</mi></mrow><mi>t</mi></msub></mrow></msub></mtd></mtr><mtr><mtd><msubsup><mrow><mover><mi mathvariant="bold">P</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><msub><mrow><mi>x</mi><mi>y</mi></mrow><mi>t</mi></msub></mrow><mi>ʹ</mi></msubsup></mtd><mtd><msubsup><mi mathvariant="bold">P</mi><mrow><msub><mi>y</mi><mi>t</mi></msub></mrow><mi>f</mi></msubsup></mtd></mtr></mtable></mfenced><mrow><mi>k</mi><mrow><mo>|</mo><mi>k</mi></mrow></mrow></msub><mn>.</mn></mrow></math><img file="EP1537440B1_D0037.tif" /></maths> where <maths id="math0038" num="(32)"><math display="block"><mrow><msub><mrow><mover><mi mathvariant="bold">P</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><msub><mi mathvariant="italic">xy</mi><mi>t</mi></msub></mrow></msub><mo>=</mo><msub><mi mathvariant="bold">P</mi><mrow><msub><mi>x</mi><mi>t</mi></msub></mrow></msub><msup><mfenced><msubsup><mi mathvariant="bold">A</mi><mi>k</mi><mn>1</mn></msubsup></mfenced><mi>ʹ</mi></msup></mrow></math><img file="EP1537440B1_D0038.tif" /></maths>

0033In step (530), this composed estimate would then be output as the estimate of the target state if the target vehicle 36 were to be determined from steps (514) through (524) to be traveling in the host lane 38.

0034Returning to the process of steps (514) through (524) for determining whether the target is likely constrained by a constraint, and if so, what is the most likely constraint; according to the assumption that targets follow the same roadway 34, if the target vehicle 36 were known to travel in a particular lane, it would desirable to use estimated road parameters for that lane as a constraint in the main filter of estimating target kinematics. However, the knowledge of which lane the target vehicle 36 is currently in is generally not available, especially when the target is moving on a curved roadway 34. Since the road equation (8) is only for the host lane 38 in the host-centered coordinate system, constrained filtering would require knowing which lane the target is in, and different constraint equations would be needed for different lanes. Ignoring the difference of road curvature parameters among these parallel lanes, i.e. assuming the curvature of each lane to be the same, the road equation for an arbitrary lane can be written as: <maths id="math0039" num="(33)"><math display="block"><mrow><mi>y</mi><mo>=</mo><mi>m</mi><mi>B</mi><mo>+</mo><msub><mrow><mover><mi>C</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>0</mn></msub><mfrac><mrow><msup><mi>x</mi><mn>2</mn></msup></mrow><mn>2</mn></mfrac><mo>+</mo><msub><mrow><mover><mi>C</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msub><mfrac><mrow><msup><mi>x</mi><mn>3</mn></msup></mrow><mn>6</mn></mfrac><mo>,</mo><mspace width="1em" /><mi mathvariant="italic">m</mi><mo>=</mo><mn>0</mn><mo>,</mo><mo>±</mo><mn>1</mn><mo>,</mo><mo>±</mo><mn>2</mn><mo>,</mo><mn>....</mn></mrow></math><img file="EP1537440B1_D0039.tif" /></maths> where B is the width of the lanes and m represents the lane to be described (m = 0 corresponds the host lane 38,m = 1 corresponds the right neighboring lane 40,m = -1 corresponds the left neighboring lane 40, and so on). Without the prior knowledge of the target lane position, each of the multiple constraints forming a multiple constraint system (analogous to the so-called multiple model system) is tested to determine identify which, if any, of the constraints are active. A multiple constraint (MC) system is subjected to one of a finite number NC of constraints. Only one constraint can be in effect at any given time. Such systems are referred to as hybrid -- they have both continuous (noise) state variables as well as discrete number of constraints.

0035The following definitions and modeling assumptions are made to facilitate the solution of this problem: <ul id="ul0003" list-style="none" compact="compact"><li>Constraint equations: <maths id="math0040" num="(34)"><math display="block"><mrow><msub><mrow><munder><mi>y</mi><mo>̲</mo></munder></mrow><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow></msub><mo>=</mo><msub><mrow><munder><mi>f</mi><mo>̲</mo></munder></mrow><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow></msub><mfenced><msub><mrow><munder><mi>x</mi><mo>̲</mo></munder></mrow><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow></msub></mfenced></mrow></math><img file="EP1537440B1_D0040.tif" /></maths> where f t<sub2>k</sub2> denotes the constraint at time tk in effect during the sampling period ending at tk.</li></ul>

0036Constraint: among the possible NC constraints <maths id="math0041" num="(35)"><math display="block"><mrow><msub><mrow><munder><mi>f</mi><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow></msub><mo>∈</mo><msubsup><mfenced open="{" close="}"><msup><mrow><munder><mi>f</mi><mrow><mo>̲</mo></mrow></munder></mrow><mi>j</mi></msup></mfenced><mrow><mi>j</mi><mo>=</mo><mn>1</mn></mrow><mrow><msup><mi>N</mi><mi>C</mi></msup></mrow></msubsup></mrow></math><img file="EP1537440B1_D0041.tif" /></maths><ul id="ul0004" list-style="none" compact="compact"><li><maths id="math0042"><math display="inline"><mrow><msubsup><mrow><mover><mrow><mover><mrow><munder><mi>y</mi><mrow><mo>̲</mo></mrow></munder></mrow><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mrow><msub><mi>l</mi><mrow><mi>k</mi><mo>|</mo><mi>k</mi></mrow></msub></mrow><mi>j</mi></msubsup><mo>:</mo></mrow></math><img file="EP1537440B1_D0042.tif" /></maths> state estimate at time tk using constraint <maths id="math0043"><math display="inline"><mrow><msubsup><mrow><munder><mi>f</mi><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>l</mi><mi>k</mi></msub></mrow><mi>j</mi></msubsup></mrow></math><img file="EP1537440B1_D0043.tif" /></maths></li><li><maths id="math0044"><math display="inline"><mrow><msubsup><mrow><mover><mi mathvariant="bold">P</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><msub><mi mathvariant="italic">yl</mi><mrow><mi>k</mi><mo>|</mo><mi>k</mi></mrow></msub></mrow><mi>j</mi></msubsup><mo>,</mo></mrow></math><img file="EP1537440B1_D0044.tif" /></maths><maths id="math0045"><math display="inline"><mrow><msubsup><mrow><mover><mi mathvariant="bold">P</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><msub><mi mathvariant="italic">xyl</mi><mrow><mi>k</mi><mo>|</mo><mi>k</mi></mrow></msub></mrow><mi>j</mi></msubsup><mo>:</mo></mrow></math><img file="EP1537440B1_D0045.tif" /></maths> covariance matrix at time tk under constraint <maths id="math0046"><math display="inline"><mrow><msubsup><mrow><munder><mi>f</mi><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>l</mi><mi>k</mi></msub></mrow><mi>j</mi></msubsup></mrow></math><img file="EP1537440B1_D0046.tif" /></maths></li><li><maths id="math0047"><math display="inline"><mrow><msubsup><mi>μ</mi><mrow><msub><mi>l</mi><mrow><mi>k</mi><mo>−</mo><mi>l</mi></mrow></msub></mrow><mi>j</mi></msubsup><mo>:</mo></mrow></math><img file="EP1537440B1_D0047.tif" /></maths> probability that the target is following constraint j at time tk-1</li></ul>

0037Constraint jump process: is a Markov chain with known transition probabilities <maths id="math0048" num="(36)"><math display="block"><mrow><mi>P</mi><mfenced open="{" close="}"><msub><mrow><munder><mi>f</mi><mo>̲</mo></munder></mrow><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow></msub><mo>=</mo><msup><mrow><munder><mi>f</mi><mo>̲</mo></munder></mrow><mi>j</mi></msup><mrow><mo>|</mo><msub><mrow><munder><mi>f</mi><mo>̲</mo></munder></mrow><mrow><msub><mi>t</mi><mrow><mi>k</mi><mo>−</mo><mn>1</mn></mrow></msub></mrow></msub><mo>=</mo><msup><mrow><munder><mi>f</mi><mo>̲</mo></munder></mrow><mi>i</mi></msup></mrow></mfenced><mo>=</mo><msub><mi>p</mi><mi mathvariant="italic">ij</mi></msub><mn>.</mn></mrow></math><img file="EP1537440B1_D0048.tif" /></maths>

0038To implement the Markov model - for systems with more than one possible constraint state -- it is assumed that at each scan time there is a probability pij that the target will make the transition from constraint state i to state j. These probabilities are assumed to be known a priori and can be expressed in the probability transition matrix as shown below.

New State

0039<maths id="math0049" num="(37)"><math display="block"><mrow><msub><mi>P</mi><mi mathvariant="italic">trans</mi></msub><mo>=</mo><mtable><mtr><mtd><mi>Pr</mi><mi mathvariant="italic">ior</mi></mtd></mtr><mtr><mtd><mi mathvariant="italic">State</mi></mtd></mtr></mtable><mtable><mtr><mtd><mn>1</mn></mtd></mtr><mtr><mtd><mn>2</mn></mtd></mtr><mtr><mtd><mn>3</mn></mtd></mtr></mtable><mtable><mtr><mtd columnalign="left"><mtable><mtr><mtd><mspace width="1em" /><mn>1</mn></mtd><mtd><mspace width="1em" /><mn>2</mn></mtd><mtd><mspace width="1em" /><mn>3</mn></mtd></mtr></mtable></mtd></mtr><mtr><mtd><mfenced open="[" close="]"><mtable><mtr><mtd><mrow><msub><mi>p</mi><mn>11</mn></msub></mrow></mtd><mtd><msub><mi>p</mi><mn>12</mn></msub></mtd><mtd><msub><mi>p</mi><mn>13</mn></msub></mtd></mtr><mtr><mtd><msub><mi>p</mi><mn>21</mn></msub></mtd><mtd><msub><mi>p</mi><mn>22</mn></msub></mtd><mtd><msub><mi>p</mi><mn>23</mn></msub></mtd></mtr><mtr><mtd><msub><mi>p</mi><mn>31</mn></msub></mtd><mtd><msub><mi>p</mi><mn>32</mn></msub></mtd><mtd><msub><mi>p</mi><mn>33</mn></msub></mtd></mtr></mtable></mfenced></mtd></mtr></mtable></mrow></math><img file="EP1537440B1_D0049.tif" /></maths>

0040The prior probability that fj is correct (fj is in effect) is <maths id="math0050" num="(38)"><math display="block"><mrow><mtable><mtr><mtd><mi>P</mi><mfenced><msup><mrow><munder><mi>f</mi><mrow><mo>̲</mo></mrow></munder></mrow><mi>j</mi></msup><mrow><mo>|</mo><msup><mi>Z</mi><mn>0</mn></msup></mrow></mfenced><mo>=</mo><msubsup><mi>μ</mi><mrow><msub><mi>t</mi><mn>0</mn></msub></mrow><mi>j</mi></msubsup></mtd><mtd><mi>j</mi><mo>=</mo><mn>1</mn><mo>,</mo><mo>⋯</mo><mo>,</mo><msup><mi>N</mi><mi>C</mi></msup></mtd></mtr></mtable></mrow></math><img file="EP1537440B1_D0050.tif" /></maths> where Z0 is the prior information and <maths id="math0051" num="(39)"><math display="block"><mrow><mstyle displaystyle="true"><mrow><munderover><mrow><mo>∑</mo></mrow><mrow><mi>j</mi><mo>=</mo><mn>1</mn></mrow><mrow><msup><mi>N</mi><mi>C</mi></msup></mrow></munderover></mrow></mstyle><mrow><msubsup><mi>μ</mi><mrow><msub><mi>t</mi><mn>0</mn></msub></mrow><mi>j</mi></msubsup></mrow><mo>=</mo><mn>1</mn></mrow></math><img file="EP1537440B1_D0051.tif" /></maths> since the correct constraint is among the assumed NC possible constraints.

0041The constrained target state estimation subsystem 46 provides for determining whether the target state corresponds to a possible constrained state, and if so, then provides for determining the most likely constrained state.

0042One way of determining this is the multiple model filtering algorithm proposed by Bar-Shalom, wherein NC parallel filters are run simultaneously in parallel.

0043In another way, a multiple constraint (MC) estimation algorithm mixes and updates NC constraint-conditioned state estimates using the unconstrained state estimate <maths id="math0052"><math display="inline"><mrow><msubsup><mrow><mover><mrow><munder><mi>y</mi><mrow><mo>̲</mo></mrow></munder></mrow><mrow><mo>^</mo></mrow></mover></mrow><mrow><msub><mi>l</mi><mrow><mi>k</mi><mo>|</mo><mi>k</mi></mrow></msub></mrow><mi mathvariant="normal">l</mi></msubsup></mrow></math><img file="EP1537440B1_D0052.tif" /></maths> as a measurement, along with the calculation of the likelihood function and probability associated with each constraint. In one embodiment of the multiple constraint (MC) estimation algorithm, the constrained state estimate output is a composite combination of all of the constraint-conditioned state estimates. If this constrained state estimate is valid, i.e. if the constrained state estimate corresponds to the unconstrained state estimate, then the target state is given by fusing the constrained and unconstrained state estimates; otherwise the target state is given by the unconstrained state estimate. This embodiment of the multiple constraint (MC) estimation algorithm comprises the following steps: <ol id="ol0001" compact="compact"><li>1.Estimation of state variables from multiple constraints: In step (514), using the multiple lane road equation (33) to replace the first row in equation (24), the multiple constraint state estimates are given by: <maths id="math0053" num="(40)"><math display="block"><mrow><msubsup><mrow><munder><mrow><mover><mrow><mover><mi>y</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub></mrow><mrow><mn>0</mn><mi>j</mi></mrow></msubsup><mo>=</mo><msub><mfenced open="[" close="]"><mtable><mtr><mtd><mover><mi>y</mi><mrow><mo>‾</mo></mrow></mover></mtd></mtr><mtr><mtd><mover><mrow><mover><mi>y</mi><mrow><mo>˙</mo></mrow></mover></mrow><mrow><mo>‾</mo></mrow></mover></mtd></mtr><mtr><mtd><mover><mrow><mover><mi>y</mi><mrow><mo>¨</mo></mrow></mover></mrow><mrow><mo>‾</mo></mrow></mover></mtd></mtr></mtable></mfenced><mrow><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub></mrow></msub><mo>=</mo><mfenced open="[" close="]"><mtable><mtr><mtd><msub><mi>B</mi><mi>j</mi></msub><mo>+</mo><msub><mrow><mover><mi>C</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>0</mn></msub><mo>⋅</mo><msup><mfenced><msup><mrow><mover><mi>x</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup></mfenced><mn>2</mn></msup><mo>/</mo><mn>2</mn><mo>+</mo><msub><mrow><mover><mi>C</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msub><mo>⋅</mo><msup><mfenced><msup><mrow><mover><mi>x</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup></mfenced><mn>3</mn></msup><mo>/</mo><mn>6</mn></mtd></mtr><mtr><mtd><msub><mrow><mover><mi>C</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>0</mn></msub><mo>⋅</mo><msup><mrow><mover><mi>x</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup><mo>⋅</mo><msup><mrow><mover><mrow><mover><mi>x</mi><mrow><mo>˙</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup><mo>+</mo><msub><mrow><mover><mi>C</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msub><mo>⋅</mo><msup><mfenced><msup><mrow><mover><mi>x</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup></mfenced><mn>2</mn></msup><mo>⋅</mo><msup><mrow><mover><mrow><mover><mi>x</mi><mrow><mo>˙</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup><mo>/</mo><mn>2</mn></mtd></mtr><mtr><mtd><msub><mrow><mover><mi>C</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>0</mn></msub><mo>⋅</mo><msup><mfenced><msup><mrow><mover><mrow><mover><mi>x</mi><mrow><mo>˙</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup></mfenced><mn>2</mn></msup><mo>+</mo><msub><mrow><mover><mi>C</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>0</mn></msub><mo>⋅</mo><msup><mrow><mover><mi>x</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup><mo>⋅</mo><msup><mrow><mover><mrow><mover><mi>x</mi><mrow><mo>¨</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup><mo>+</mo><msub><mrow><mover><mi>C</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msub><mo>⋅</mo><msup><mrow><mover><mi>x</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup><mo>⋅</mo><msup><mfenced><msup><mrow><mover><mrow><mover><mi>x</mi><mrow><mo>˙</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup></mfenced><mn>2</mn></msup><mo>+</mo><msub><mrow><mover><mi>C</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msub><mo>⋅</mo><msup><mfenced><msup><mrow><mover><mi>x</mi><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup></mfenced><mn>2</mn></msup><mo>⋅</mo><msup><mrow><mover><mrow><mover><mi>x</mi><mrow><mo>¨</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mn>1</mn></msup><mo>/</mo><mn>2</mn></mtd></mtr></mtable></mfenced></mrow></math><img file="EP1537440B1_D0053.tif" /></maths> where <maths id="math0054"><math display="inline"><mrow><msub><mi>B</mi><mi>j</mi></msub><mo>=</mo><mn>0</mn><mo>,</mo><mo>±</mo><mi>B</mi><mo>,</mo><mspace width="1em" /><mo>…</mo><mo>,</mo><mo>±</mo><mfrac><mrow><msup><mi>N</mi><mi>C</mi></msup><mo>−</mo><mn>1</mn></mrow><mn>2</mn></mfrac><mi>B</mi><mo>,</mo></mrow></math><img file="EP1537440B1_D0054.tif" /></maths> and B is the width of a lane. Stated in another way, the constraint state estimates correspond to the y locations of the centerlines of each possible lane in which the target vehicle 36 could be located. The associated covariance is given by: <maths id="math0055" num="(41)"><math display="block"><mrow><msubsup><mrow><mover><mi mathvariant="bold">P</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mi>y</mi><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub></mrow><mrow><mn>0</mn><mi>j</mi></mrow></msubsup><mo>=</mo><msubsup><mi mathvariant="bold">A</mi><mi>k</mi><mn>1</mn></msubsup><mo>⋅</mo><msub><mi mathvariant="bold">P</mi><mrow><mi>x</mi><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub></mrow></msub><mo>⋅</mo><msup><mfenced><msubsup><mi mathvariant="bold">A</mi><mi>k</mi><mn>1</mn></msubsup></mfenced><mi mathvariant="normal">T</mi></msup><mo>+</mo><msubsup><mi mathvariant="bold">A</mi><mi>k</mi><mn>2</mn></msubsup><mo>⋅</mo><msubsup><mi mathvariant="bold">P</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow><mi>C</mi></msubsup><mo>⋅</mo><msup><mfenced><msubsup><mi mathvariant="bold">A</mi><mi>k</mi><mn>2</mn></msubsup></mfenced><mi mathvariant="normal">T</mi></msup></mrow></math><img file="EP1537440B1_D0055.tif" /></maths> where <maths id="math0056"><math display="inline"><mrow><msubsup><mi mathvariant="bold">A</mi><mi>k</mi><mn>1</mn></msubsup></mrow></math><img file="EP1537440B1_D0056.tif" /></maths> and <maths id="math0057"><math display="inline"><mrow><msubsup><mi mathvariant="bold">A</mi><mi>k</mi><mn>2</mn></msubsup></mrow></math><img file="EP1537440B1_D0057.tif" /></maths> are given by equation (26) and equation (27), Pxt<sub2>k|k</sub2> is from equation (23) and <maths id="math0058"><math display="inline"><mrow><msubsup><mi mathvariant="bold">P</mi><mrow><mi>k</mi><mo>|</mo><mi>k</mi></mrow><mi>C</mi></msubsup></mrow></math><img file="EP1537440B1_D0058.tif" /></maths> is from the curvature filter.</li><li>2.Constraint-conditioned updating: In step (516), the state estimates and covariance conditioned on a constraint being in effect are updated, as well as the constraint likelihood function, for each of the constraints j = 1, ... NC. The updated state estimate and covariances corresponding to constraint j are obtained using measurement <maths id="math0059"><math display="inline"><mrow><msubsup><mrow><mover><mrow><munder><mi>y</mi><mrow><mo>̲</mo></mrow></munder></mrow><mrow><mo>^</mo></mrow></mover></mrow><mrow><msub><mi>l</mi><mrow><mi>k</mi><mo>|</mo><mi>k</mi></mrow></msub></mrow><mn>1</mn></msubsup><mo>,</mo></mrow></math><img file="EP1537440B1_D0059.tif" /></maths> as follows: <maths id="math0060" num="(42)"><math display="block"><mrow><msubsup><mrow><munder><mrow><mover><mrow><mover><mi>y</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub></mrow><mi>j</mi></msubsup><mo>=</mo><msubsup><mrow><munder><mrow><mover><mrow><mover><mi>y</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub></mrow><mrow><mn>0</mn><mi>j</mi></mrow></msubsup><mo>+</mo><msubsup><mrow><mover><mi mathvariant="bold">P</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mi>y</mi><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub></mrow><mrow><mn>0</mn><mi>j</mi></mrow></msubsup><mrow><msup><mfenced><msubsup><mrow><mover><mi mathvariant="bold">P</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mi>y</mi><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub></mrow><mrow><mn>0</mn><mi>j</mi></mrow></msubsup><mo>+</mo><msubsup><mi mathvariant="bold">P</mi><mrow><mi>y</mi><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub></mrow><mn>1</mn></msubsup></mfenced><mrow><mo>−</mo><mn>1</mn></mrow></msup></mrow><mfenced><msubsup><mrow><munder><mrow><mover><mi>y</mi><mrow><mo>^</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub></mrow><mrow><mo>−</mo><mn>1</mn></mrow></msubsup><mo>−</mo><msubsup><mrow><munder><mrow><mover><mrow><mover><mi>y</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub></mrow><mrow><mn>0</mn><mi>j</mi></mrow></msubsup></mfenced></mrow></math><img file="EP1537440B1_D0060.tif" /></maths><maths id="math0061" num="(43)"><math display="block"><mrow><msubsup><mrow><mover><mi mathvariant="bold">P</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mi>y</mi><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub></mrow><mi>j</mi></msubsup><mo>=</mo><msubsup><mrow><mover><mi mathvariant="bold">P</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mi>y</mi><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub></mrow><mrow><mn>0</mn><mi>j</mi></mrow></msubsup><mo>−</mo><msubsup><mrow><mover><mi mathvariant="bold">P</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mi>y</mi><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub></mrow><mrow><mn>0</mn><mi>j</mi></mrow></msubsup><mrow><msup><mfenced><msubsup><mrow><mover><mi mathvariant="bold">P</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mi>y</mi><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub></mrow><mrow><mn>0</mn><mi>j</mi></mrow></msubsup><mo>+</mo><msubsup><mi mathvariant="bold">P</mi><mrow><mi>y</mi><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub></mrow><mn>1</mn></msubsup></mfenced><mrow><mo>−</mo><mn>1</mn></mrow></msup></mrow><msubsup><mrow><mover><mi mathvariant="bold">P</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mi>y</mi><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub></mrow><mrow><mn>0</mn><mi>j</mi></mrow></msubsup><mn>.</mn></mrow></math><img file="EP1537440B1_D0061.tif" /></maths><maths id="math0062" num="(44)"><math display="block"><mrow><msubsup><mrow><mover><mi mathvariant="bold">P</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mi mathvariant="italic">xy</mi><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub></mrow><mi>j</mi></msubsup><mo>=</mo><msubsup><mrow><mover><mi mathvariant="bold">P</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mi mathvariant="italic">xy</mi><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub></mrow><mrow><mn>0</mn><mi>j</mi></mrow></msubsup><mo>−</mo><msubsup><mrow><mover><mi mathvariant="bold">P</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mi>y</mi><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub></mrow><mrow><mn>0</mn><mi>j</mi></mrow></msubsup><mrow><msup><mfenced><msubsup><mrow><mover><mi mathvariant="bold">P</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mi>y</mi><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub></mrow><mrow><mn>0</mn><mi>j</mi></mrow></msubsup><mo>+</mo><msubsup><mi mathvariant="bold">P</mi><mrow><mi>y</mi><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub></mrow><mn>1</mn></msubsup></mfenced><mrow><mo>−</mo><mn>1</mn></mrow></msup></mrow><msubsup><mrow><mover><mi mathvariant="bold">P</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mi mathvariant="italic">xy</mi><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub></mrow><mrow><mn>0</mn><mi>j</mi></mrow></msubsup></mrow></math><img file="EP1537440B1_D0062.tif" /></maths></li><li>3.Likelihood calculation: In step (518), the likelihood function corresponding to constraint j is evaluated at the value <maths id="math0063"><math display="inline"><mrow><msubsup><mrow><mover><mrow><munder><mi>y</mi><mrow><mo>̲</mo></mrow></munder></mrow><mrow><mo>^</mo></mrow></mover></mrow><mrow><msub><mi>l</mi><mrow><mi>k</mi><mo>|</mo><mi>k</mi></mrow></msub></mrow><mn>1</mn></msubsup></mrow></math><img file="EP1537440B1_D0063.tif" /></maths> of the unconstrained target state estimate, assuming a Gaussian distribution of the measurement around the constraint-conditioned state estimate for each of the constraints j = 1, ... NC, as follows: <maths id="math0064" num="(45)"><math display="block"><mrow><msubsup><mi mathvariant="normal">Λ</mi><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow><mi>j</mi></msubsup><mo>=</mo><mi>N</mi><mfenced><msubsup><mrow><munder><mi>y</mi><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub></mrow><mn>1</mn></msubsup><mo>;</mo><msubsup><mrow><munder><mrow><mover><mrow><mover><mi>y</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub></mrow><mrow><mn>0</mn><mi>j</mi></mrow></msubsup><mo>,</mo><msubsup><mrow><mover><mi mathvariant="bold">P</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mi>y</mi><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub></mrow><mrow><mn>0</mn><mi>j</mi></mrow></msubsup><mo>+</mo><msubsup><mi mathvariant="bold">P</mi><mrow><mi>y</mi><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub></mrow><mn>1</mn></msubsup></mfenced></mrow></math><img file="EP1537440B1_D0064.tif" /></maths> wherein the Gaussian distribution N( ; , ) has a mean value of <maths id="math0065"><math display="inline"><mrow><msubsup><mrow><mover><mrow><mover><mrow><munder><mi>y</mi><mrow><mo>̲</mo></mrow></munder></mrow><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mrow><msub><mi>l</mi><mrow><mi>k</mi><mo>|</mo><mi>k</mi></mrow></msub></mrow><mrow><mn>0</mn><mi>j</mi></mrow></msubsup></mrow></math><img file="EP1537440B1_D0065.tif" /></maths> and an associated covariance of <maths id="math0066"><math display="inline"><mrow><msubsup><mrow><mover><mi mathvariant="bold">P</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><msub><mi mathvariant="italic">yl</mi><mrow><mi>k</mi><mo>|</mo><mi>k</mi></mrow></msub></mrow><mrow><mn>0</mn><mi>j</mi></mrow></msubsup><mo>+</mo><msubsup><mi mathvariant="bold">P</mi><mrow><msub><mi>yl</mi><mrow><mi>k</mi><mo>|</mo><mi>k</mi></mrow></msub></mrow><mn>1</mn></msubsup><mn>.</mn></mrow></math><img file="EP1537440B1_D0066.tif" /></maths></li><li>4.Constraint probability evaluations: In step (520), the updated constraint probabilities are calculated for each of the constraints j = 1, ... NC, as follows: <maths id="math0067" num="(46)"><math display="block"><mrow><msubsup><mi>μ</mi><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow><mi>j</mi></msubsup><mo>=</mo><mfrac><mn>1</mn><mi>a</mi></mfrac><msubsup><mi mathvariant="normal">Λ</mi><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow><mi>j</mi></msubsup><msub><mrow><mover><mi>a</mi><mrow><mo>‾</mo></mrow></mover></mrow><mi>j</mi></msub></mrow></math><img file="EP1537440B1_D0067.tif" /></maths> where <o ostyle="single">a</o>j, the probability after transition that constraint j is in effect, is given by <maths id="math0068" num="(47)"><math display="block"><mrow><msub><mrow><mover><mi>a</mi><mrow><mo>‾</mo></mrow></mover></mrow><mi>j</mi></msub><mo>=</mo><mrow><mstyle displaystyle="true"><mrow><munderover><mrow><mo>∑</mo></mrow><mrow><mi>i</mi><mo>=</mo><mn>1</mn></mrow><mrow><msup><mi>N</mi><mi>C</mi></msup></mrow></munderover></mrow></mstyle><mrow><msub><mi>p</mi><mi mathvariant="italic">ij</mi></msub></mrow></mrow><mo>⋅</mo><msubsup><mi>μ</mi><mrow><msub><mi>t</mi><mrow><mi>k</mi><mo>−</mo><mn>1</mn></mrow></msub></mrow><mi>i</mi></msubsup></mrow></math><img file="EP1537440B1_D0068.tif" /></maths> and the normalizing constant is <maths id="math0069" num="(48)"><math display="block"><mrow><mi>a</mi><mo>=</mo><mrow><mstyle displaystyle="true"><mrow><munderover><mrow><mo>∑</mo></mrow><mrow><mi>j</mi><mo>=</mo><mn>1</mn></mrow><mrow><msup><mi>N</mi><mi>C</mi></msup></mrow></munderover></mrow></mstyle><msubsup><mi mathvariant="normal">Λ</mi><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow><mi>j</mi></msubsup><msub><mrow><mover><mi>a</mi><mrow><mo>‾</mo></mrow></mover></mrow><mi>j</mi></msub></mrow><mn>.</mn></mrow></math><img file="EP1537440B1_D0069.tif" /></maths></li><li>5.Overall state estimate andcovariance: In step (522), the combination of the latest constraint-conditioned state estimates and covariances is given by: <maths id="math0070" num="(49)"><math display="block"><mrow><msub><mrow><munder><mrow><mover><mrow><mover><mi>y</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub></mrow></msub><mo>=</mo><mrow><mstyle displaystyle="true"><mrow><munderover><mrow><mo>∑</mo></mrow><mrow><mi>j</mi><mo>=</mo><mn>1</mn></mrow><mrow><msup><mi>N</mi><mi>C</mi></msup></mrow></munderover></mrow></mstyle><msubsup><mi>μ</mi><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow><mi>j</mi></msubsup><mo>⋅</mo><msubsup><mrow><munder><mrow><mover><mrow><mover><mi>y</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub></mrow><mi>j</mi></msubsup></mrow></mrow></math><img file="EP1537440B1_D0070.tif" /></maths><maths id="math0071" num="(50)"><math display="block"><mrow><msub><mrow><mover><mi mathvariant="bold">P</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mi>y</mi><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub></mrow></msub><mo>=</mo><mstyle displaystyle="true"><mrow><munderover><mrow><mo>∑</mo></mrow><mrow><mi>j</mi><mo>=</mo><mn>1</mn></mrow><mrow><msup><mi>N</mi><mi>C</mi></msup></mrow></munderover></mrow></mstyle><msubsup><mi>μ</mi><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow><mi>j</mi></msubsup><mo>⋅</mo><mfenced open="[" close="]"><msubsup><mrow><mover><mi mathvariant="bold">P</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mi>y</mi><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub></mrow><mi>j</mi></msubsup><mo>+</mo><mfenced><msubsup><mrow><munder><mrow><mover><mrow><mover><mi>y</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub></mrow><mi>j</mi></msubsup><mo>−</mo><msub><mrow><munder><mrow><mover><mrow><mover><mi>y</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub></mrow></msub></mfenced><mo>⋅</mo><mrow><msup><mfenced><msubsup><mrow><munder><mrow><mover><mrow><mover><mi>y</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub></mrow><mi>j</mi></msubsup><mo>−</mo><msub><mrow><munder><mrow><mover><mrow><mover><mi>y</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub></mrow></msub></mfenced><mi>ʹ</mi></msup></mrow></mfenced><mn>.</mn></mrow></math><img file="EP1537440B1_D0071.tif" /></maths><maths id="math0072" num="(51)"><math display="block"><mrow><msub><mrow><mover><mi mathvariant="bold">P</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mi mathvariant="italic">xy</mi><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub></mrow></msub><mo>=</mo><mstyle displaystyle="true"><mrow><munderover><mrow><mo>∑</mo></mrow><mrow><mi>j</mi><mo>=</mo><mn>1</mn></mrow><mrow><msup><mi>N</mi><mi>C</mi></msup></mrow></munderover></mrow></mstyle><msubsup><mi>μ</mi><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow><mi>j</mi></msubsup><mo>⋅</mo><msubsup><mrow><mover><mi mathvariant="bold">P</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mi mathvariant="italic">xy</mi><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub></mrow><mi>j</mi></msubsup></mrow></math><img file="EP1537440B1_D0072.tif" /></maths></li></ol>

0044The output of the estimator from step (522) in the above algorithm is then used as the constrained estimates in the fusion process described by equations (28) and (29), and the result of equation (51), instead of the result of equation (32), is used in equation (31).

0045When the target vehicle 36 is not following the roadway 34 or is changing lanes, imposing the road constraint on target kinematic state variables will result in incorrect estimates that would be worse than using the associated unconstrained estimates. However, noise related estimation errors might cause a correct road constraint to appear invalid. Accordingly, it is beneficial to incorporate a means that can keep the constraints in effect when they are valid, e.g. when the target vehicle 36 follows a particular lane; and lift them off promptly when they are invalid, e.g. when the target vehicle 36 departs from its lane. The unconstrained target state estimate plays a useful role in road constraint validation, since it provides independent target state estimates.

0046One approach is to test the hypothesis that the unconstrained target state estimate satisfies the road constraint equation, or equivalently, that the constrained estimate and the unconstrained estimate each correspond to the same target. The optimal test would require using all available target state estimates in history through time tk and is generally not practical. A practical approach is the sequential hypothesis testing in which the test is carried out based on the most recent state estimates only. In accordance with the notation used hereinabove, the difference between the constrained and unconstrained target state estimates (y direction only) is denoted: <maths id="math0073" num="(52)"><math display="block"><mrow><msub><mrow><munder><mrow><mover><mi>δ</mi><mrow><mo>^</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow></msub><mo>=</mo><msubsup><mrow><munder><mrow><mover><mi>y</mi><mrow><mo>^</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo><mi>k</mi></mrow></mrow></msub></mrow><mn>1</mn></msubsup><mo>−</mo><msub><mrow><munder><mrow><mover><mrow><mover><mi>y</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo><mi>k</mi></mrow></mrow></msub></mrow></msub></mrow></math><img file="EP1537440B1_D0073.tif" /></maths> as the estimate of <maths id="math0074" num="(53)"><math display="block"><mrow><msub><mrow><munder><mi>δ</mi><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow></msub><mo>=</mo><msubsup><mrow><munder><mi>y</mi><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow><mn>1</mn></msubsup><mo>−</mo><msub><mrow><munder><mrow><mover><mi>y</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow></msub></mrow></math><img file="EP1537440B1_D0074.tif" /></maths> where <maths id="math0075"><math display="inline"><mrow><msubsup><mrow><munder><mi>y</mi><mo>̲</mo></munder></mrow><mrow><msub><mi>l</mi><mi>k</mi></msub></mrow><mn>1</mn></msubsup></mrow></math><img file="EP1537440B1_D0075.tif" /></maths> is the true target state and <o ostyle="single">y</o>t<sub2>k</sub2> is the true state of a target moving along the roadway 34 (or a lane). In step (524), the "same target" hypothesis is tested, i.e. <maths id="math0076" num="(54)"><math display="block"><mrow><msub><mi>H</mi><mn>0</mn></msub><mo>:</mo><msub><mrow><munder><mi>δ</mi><mo>̲</mo></munder></mrow><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow></msub><mo>=</mo><mn mathvariant="bold">0</mn></mrow></math><img file="EP1537440B1_D0076.tif" /></maths> vs. <maths id="math0077" num="(55)"><math display="block"><mrow><msub><mi>H</mi><mn>1</mn></msub><mo>:</mo><msub><mrow><munder><mi>δ</mi><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow></msub><mo>≠</mo><mn mathvariant="bold">0</mn></mrow></math><img file="EP1537440B1_D0077.tif" /></maths>

0047The main filter error <maths id="math0078" num="(56)"><math display="block"><mrow><msubsup><mrow><munder><mrow><mover><mi>y</mi><mrow><mo>˜</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow><mn>1</mn></msubsup><mo>=</mo><msubsup><mrow><munder><mi>y</mi><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow><mn>1</mn></msubsup><mo>−</mo><msubsup><mrow><munder><mrow><mover><mi>y</mi><mrow><mo>^</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub></mrow><mn>1</mn></msubsup></mrow></math><img file="EP1537440B1_D0078.tif" /></maths> is assumed independent of the error <maths id="math0079" num="(57)"><math display="block"><mrow><msub><mrow><munder><mrow><mover><mrow><mover><mi>y</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mo>˜</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow></msub><mo>=</mo><msub><mrow><munder><mrow><mover><mi>y</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow></msub><mo>−</mo><msub><mrow><munder><mrow><mover><mrow><mover><mi>y</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub></mrow></msub></mrow></math><img file="EP1537440B1_D0079.tif" /></maths> which is from the constraints. The covariance of the difference δ̂t<sub2>k</sub2> is, under hypothesis H0, given by: <maths id="math0080" num="(58)"><math display="block"><mrow><mtable columnalign="left" width="auto"><mtr><mtd><msubsup><mi mathvariant="bold">P</mi><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow><mi>δ</mi></msubsup></mtd><mtd><mo>=</mo><mi>E</mi><mfenced><msub><mrow><munder><mrow><mover><mi>δ</mi><mrow><mo>˜</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow></msub><msub><mrow><munder><mrow><mover><mi>δ</mi><mrow><mo>˜</mo></mrow></mover><mi>ʹ</mi></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow></msub></mfenced><mo>=</mo><mi>E</mi><mfenced open="[" close="]"><mfenced><msubsup><mrow><munder><mrow><mover><mi>y</mi><mrow><mo>˜</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow><mn>1</mn></msubsup><mo>−</mo><msub><mrow><munder><mrow><mover><mrow><mover><mi>y</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mo>˜</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow></msub></mfenced><mrow><msup><mfenced><msubsup><mrow><munder><mrow><mover><mi>y</mi><mrow><mo>˜</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow><mn>1</mn></msubsup><mo>−</mo><msub><mrow><munder><mrow><mover><mrow><mover><mi>y</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mo>˜</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow></msub></mfenced><mi>ʹ</mi></msup></mrow></mfenced></mtd></mtr><mtr><mtd><mspace width="1em" /></mtd><mtd><mo>=</mo><msubsup><mi mathvariant="bold">P</mi><mrow><mi>y</mi><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub></mrow><mn>1</mn></msubsup><mo>+</mo><msub><mrow><mover><mi mathvariant="bold">P</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mi>y</mi><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub></mrow></msub></mtd></mtr></mtable></mrow></math><img file="EP1537440B1_D0080.tif" /></maths>

0048Assuming that the estimation errors are Gaussian, the test of H0 vs. H1 is as follows: <maths id="math0081" num="(59)"><math display="block"><mrow><mi>Accept</mi><mspace width="1em" /><msub><mi>H</mi><mn>0</mn></msub><mspace width="1em" /><mi>if</mi><mspace width="1em" /><msub><mi>ρ</mi><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow></msub><mo>=</mo><msub><mrow><munder><mrow><mover><mi>δ</mi><mrow><mo>^</mo></mrow></mover><mi>ʹ</mi></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow></msub><msup><mfenced><msubsup><mi mathvariant="bold">P</mi><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow><mi>δ</mi></msubsup></mfenced><mrow><mo>−</mo><mn>1</mn></mrow></msup><msub><mrow><munder><mrow><mover><mi>δ</mi><mrow><mo>^</mo></mrow></mover><mi>ʹ</mi></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow></msub><mo>≤</mo><mi>γ</mi></mrow></math><img file="EP1537440B1_D0081.tif" /></maths>

0049The threshold is chosen such that <maths id="math0082" num="(60)"><math display="block"><mrow><mi>P</mi><mfenced><msub><mi>ρ</mi><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow></msub><mo>></mo><mi>γ</mi><mrow><mo>|</mo></mrow><msub><mi>H</mi><mn>0</mn></msub></mfenced><mo>=</mo><mi>α</mi></mrow></math><img file="EP1537440B1_D0082.tif" /></maths> where α is a predefined error tolerance value. Note that based on the above Gaussian error assumption, ρt<sub2>k</sub2> has a chi-square distribution with ny degrees of freedom. The choice of this threshold is a significant design factor and should be based on specific application need. In road vehicle collision prediction, a target in the host lane 38 is regarded to be on a collision course and is considered more dangerous than a target in one of the neighboring lanes 40. Thus it is desirable to have a high threshold (a low error tolerance value) for a target in host lane 38 since constrained filtering can provide accurate target state estimates while a "changing lane" maneuver of such a target will not pose a threat to the host vehicle 12. On the other hand, targets in neighboring lanes 40 are usually regarded as passing-by vehicles. Though constrained filtering may further reduce false alarm rate, a "changing lane" maneuver of such a target (into the host lane 38) would pose a real threat to the host vehicle 12. Thus it is desirable to have a low threshold (a high error tolerance value) for a target in a neighboring lane if false alarm rate is already low enough.

0050Based on the above analysis, the hypothesis testing scheme efficiently uses different threshold values for targets in different lanes, with the multiple constraint filtering algorithm providing the knowledge of which lane the target is most likely in currently. Assuming that there are NC possible lanes on the roadway 34, and each lane is described by a constraint equation, the constraint equation with the highest probability <maths id="math0083"><math display="inline"><mrow><msubsup><mi>μ</mi><mrow><msub><mi>l</mi><mi>k</mi></msub></mrow><mi>j</mi></msubsup></mrow></math><img file="EP1537440B1_D0083.tif" /></maths> for a target corresponds to the lane that this target is most likely in at time tk (the current time). Denoting this most likely lane as lt, then <maths id="math0084" num="(61)"><math display="block"><mrow><msubsup><mi>μ</mi><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow><mrow><msub><mi>l</mi><mi>t</mi></msub></mrow></msubsup><mo>=</mo><munder><mi>max</mi><mi>j</mi></munder><mfenced open="{" close="}"><msubsup><mi>μ</mi><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow><mi>j</mi></msubsup><mo>,</mo><mi>j</mi><mo>=</mo><mn>1</mn><mo>,</mo><mo>⋯</mo><mo>,</mo><mi>r</mi></mfenced><mn>.</mn></mrow></math><img file="EP1537440B1_D0084.tif" /></maths>

0051The difference between the unconstrained state estimates and lane lt constrained state estimates (y direction only), denoted as: <maths id="math0085" num="(62)"><math display="block"><mrow><msubsup><mrow><munder><mrow><mover><mi>δ</mi><mrow><mo>^</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow><mrow><msub><mi>l</mi><mi>t</mi></msub></mrow></msubsup><mo>=</mo><msubsup><mrow><munder><mrow><mover><mi>y</mi><mrow><mo>^</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub></mrow><mn>1</mn></msubsup><mo>−</mo><msubsup><mrow><munder><mrow><mover><mrow><mover><mi>y</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub></mrow><mrow><msub><mi>l</mi><mi>t</mi></msub></mrow></msubsup></mrow></math><img file="EP1537440B1_D0085.tif" /></maths> is the estimate of <maths id="math0086" num="(63)"><math display="block"><mrow><msubsup><mrow><munder><mi>δ</mi><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow><mrow><msub><mi>l</mi><mi>t</mi></msub></mrow></msubsup><mo>=</mo><msubsup><mrow><munder><mi>y</mi><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow><mn>1</mn></msubsup><mo>−</mo><msubsup><mrow><munder><mrow><mover><mi>y</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow><mrow><msub><mi>l</mi><mi>t</mi></msub></mrow></msubsup></mrow></math><img file="EP1537440B1_D0086.tif" /></maths> where <maths id="math0087"><math display="inline"><mrow><msubsup><mrow><munder><mi>y</mi><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>l</mi><mi>k</mi></msub></mrow><mn>1</mn></msubsup></mrow></math><img file="EP1537440B1_D0087.tif" /></maths> is the true target state and <maths id="math0088"><math display="inline"><mrow><msubsup><mrow><mover><mrow><munder><mi>y</mi><mrow><mo>̲</mo></mrow></munder></mrow><mrow><mo>‾</mo></mrow></mover></mrow><mrow><msub><mi>l</mi><mi>k</mi></msub></mrow><mrow><msub><mi>l</mi><mi>l</mi></msub></mrow></msubsup></mrow></math><img file="EP1537440B1_D0088.tif" /></maths> is the true state of a target moving along lane lt.

0052The test for the "same target" hypothesis is then given by: <maths id="math0089" num="(64)"><math display="block"><mrow><msub><mi>H</mi><mn>0</mn></msub><mo>:</mo><mspace width="1em" /><msubsup><mrow><munder><mi>δ</mi><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow><mrow><msub><mi>l</mi><mi>t</mi></msub></mrow></msubsup><mo>=</mo><mn mathvariant="bold">0</mn></mrow></math><img file="EP1537440B1_D0089.tif" /></maths> vs. <maths id="math0090" num="(65)"><math display="block"><mrow><msub><mi>H</mi><mn>1</mn></msub><mo>:</mo><mspace width="1em" /><msubsup><mrow><munder><mi>δ</mi><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow><mrow><msub><mi>l</mi><mi>t</mi></msub></mrow></msubsup><mo>≠</mo><mn mathvariant="bold">0</mn></mrow></math><img file="EP1537440B1_D0090.tif" /></maths>

0053The constrained estimation error is given by: <maths id="math0091" num="(66)"><math display="block"><mrow><msubsup><mrow><munder><mrow><mover><mrow><mover><mi>y</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mo>˜</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow><mrow><msub><mi>l</mi><mi>t</mi></msub></mrow></msubsup><mo>=</mo><msub><mrow><munder><mrow><mover><mi>y</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow></msub><mo>−</mo><msubsup><mrow><munder><mrow><mover><mrow><mover><mi>y</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mo>^</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub></mrow><mrow><msub><mi>l</mi><mi>t</mi></msub></mrow></msubsup></mrow></math><img file="EP1537440B1_D0091.tif" /></maths>

0054Assuming that the estimation errors are independent and Gaussian, the test of H0 vs. H1 becomes: <maths id="math0092" num="(67)"><math display="block"><mrow><mi>Accept</mi><mspace width="1em" /><msub><mi>H</mi><mn>0</mn></msub><mspace width="1em" /><mi>if</mi><mspace width="1em" /><msubsup><mi>ρ</mi><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow><mrow><msub><mi>l</mi><mi>t</mi></msub></mrow></msubsup><mo>=</mo><msup><mfenced><msubsup><mrow><munder><mrow><mover><mi>δ</mi><mrow><mo>^</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow><mrow><msub><mi>l</mi><mi>t</mi></msub></mrow></msubsup></mfenced><mi>ʹ</mi></msup><msup><mfenced><msubsup><mi mathvariant="bold">P</mi><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow><mrow><msub><mi>δ</mi><mi>t</mi></msub></mrow></msubsup></mfenced><mrow><mo>−</mo><mn>1</mn></mrow></msup><msubsup><mrow><munder><mrow><mover><mi>δ</mi><mrow><mo>^</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow><mrow><msub><mi>l</mi><mi>t</mi></msub></mrow></msubsup><mo>≤</mo><msub><mi>γ</mi><mrow><msub><mi>l</mi><mi>t</mi></msub></mrow></msub></mrow></math><img file="EP1537440B1_D0092.tif" /></maths> where <maths id="math0093" num="(68)"><math display="block"><mrow><mtable columnalign="left" width="auto"><mtr><mtd><msubsup><mi mathvariant="bold">P</mi><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow><mrow><msub><mi>δ</mi><mi>t</mi></msub></mrow></msubsup></mtd><mtd><mo>=</mo><mi>E</mi><mfenced open="[" close="]"><mfenced><msubsup><mrow><munder><mi>δ</mi><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow><mrow><msub><mi>l</mi><mi>t</mi></msub></mrow></msubsup><mo>−</mo><msubsup><mrow><munder><mrow><mover><mi>δ</mi><mrow><mo>^</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow><mrow><msub><mi>l</mi><mi>t</mi></msub></mrow></msubsup></mfenced><mrow><msup><mfenced><msubsup><mrow><munder><mi>δ</mi><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow><mrow><msub><mi>l</mi><mi>t</mi></msub></mrow></msubsup><mo>−</mo><msubsup><mrow><munder><mrow><mover><mi>δ</mi><mrow><mo>^</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow><mrow><msub><mi>l</mi><mi>t</mi></msub></mrow></msubsup></mfenced><mi>ʹ</mi></msup></mrow></mfenced><mo>=</mo><mi>E</mi><mfenced open="[" close="]"><mfenced><msubsup><mrow><munder><mrow><mover><mi>y</mi><mrow><mo>˜</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow><mn>1</mn></msubsup><mo>−</mo><msubsup><mrow><munder><mrow><mover><mrow><mover><mi>y</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mo>˜</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow><mrow><msub><mi>l</mi><mi>t</mi></msub></mrow></msubsup></mfenced><mrow><msup><mfenced><msubsup><mrow><munder><mrow><mover><mi>y</mi><mrow><mo>˜</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow><mn>1</mn></msubsup><mo>−</mo><msubsup><mrow><munder><mrow><mover><mrow><mover><mi>y</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mo>˜</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow><mrow><msub><mi>l</mi><mi>t</mi></msub></mrow></msubsup></mfenced><mi>ʹ</mi></msup></mrow></mfenced></mtd></mtr><mtr><mtd><mspace width="1em" /></mtd><mtd><mo>=</mo><msubsup><mi mathvariant="bold">P</mi><mrow><mi>y</mi><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub></mrow><mn>1</mn></msubsup><mo>+</mo><msubsup><mrow><mover><mi mathvariant="bold">P</mi><mrow><mo>‾</mo></mrow></mover></mrow><mrow><mi>y</mi><msub><mi>t</mi><mrow><mi>k</mi><mrow><mo>|</mo></mrow><mi>k</mi></mrow></msub></mrow><mrow><msub><mi>l</mi><mi>t</mi></msub></mrow></msubsup></mtd></mtr></mtable></mrow></math><img file="EP1537440B1_D0093.tif" /></maths> and the threshold is such that <maths id="math0094" num="(69)"><math display="block"><mrow><mi>P</mi><mfenced><msubsup><mi>ρ</mi><mrow><msub><mi>t</mi><mi>k</mi></msub></mrow><mrow><msub><mi>l</mi><mi>t</mi></msub></mrow></msubsup><mo>></mo><msub><mi>γ</mi><mrow><msub><mi>l</mi><mi>t</mi></msub></mrow></msub><mrow><mo>|</mo></mrow><msub><mi>H</mi><mn>0</mn></msub><mo>,</mo><msub><mi>l</mi><mi>t</mi></msub></mfenced><mo>=</mo><msub><mi>α</mi><mrow><msub><mi>l</mi><mi>t</mi></msub></mrow></msub></mrow></math><img file="EP1537440B1_D0094.tif" /></maths> where <maths id="math0095" num="(70)"><math display="block"><mrow><msub><mi>γ</mi><mrow><msub><mi>l</mi><mi>t</mi></msub></mrow></msub><mo>∈</mo><msubsup><mfenced open="{" close="}"><msub><mi>γ</mi><mi>j</mi></msub></mfenced><mrow><mi>j</mi><mo>=</mo><mn>1</mn></mrow><mi>r</mi></msubsup><mspace width="1em" /><mi>and</mi><mspace width="1em" /><msub><mi>α</mi><mrow><msub><mi>l</mi><mi>t</mi></msub></mrow></msub><mo>∈</mo><msubsup><mfenced open="{" close="}"><msub><mi>α</mi><mi>j</mi></msub></mfenced><mrow><mi>j</mi><mo>=</mo><mn>1</mn></mrow><mi>r</mi></msubsup></mrow></math><img file="EP1537440B1_D0095.tif" /></maths>

0055Such a lane adaptive hypothesis testing scheme provides for a prompt switch of the target state estimation output to the unconstrained estimate when the target vehicle 36 leaves its current lane, while the estimation accuracy of a target in host lane 38 is substantially improved by constrained filtering.

0056In another embodiment of the multiple constraint (MC) estimation algorithm, the constrained state estimate used for the hypothesis testing is the most likely of the separate constrained target state estimates (i.e. in accordance with a "winner take all" strategy), rather than a composite combination of all of the constrained target state estimates. If this most likely constrained state estimate is valid, i.e. if the most likely constrained state estimate corresponds to the unconstrained state estimate, then the target state is given by fusing the most likely constrained state estimate and the unconstrained state estimate; otherwise the target state is given by the unconstrained state estimate.

0057In yet another embodiment of the multiple constraint (MC) estimation algorithm, hypothesis tests are made for each of the constrained state estimates. If none of the hypotheses are satisfied, then the target state is given by the unconstrained state estimate. If one of the hypotheses is satisfied, then the target state is given by fusing the corresponding constrained state estimate and the unconstrained state estimate. If more than one hypothesis is satisfied, then the most likely constrained state may be identified by voting results from a plurality of approaches, or by repeating the hypothesis tests with different associated thresholds.

0058Generally, the number of constraints (i.e. the number of roadway lanes) can vary with respect to time, as can associated parameters therewith, for example, the width of the lanes of the roadway, so as to accommodate changes in the environment of the host vehicle 12. For example, the host vehicle 12 in one trip could travel on a one-lane road, a two-lane road with opposing traffic, a three-lane road with a center turn lane, a four line road two lanes of opposing traffic, or on a multi-lane divided freeway.

0059Road vehicle tracking simulations using constrained and unconstrained filtering were carried out for four scenarios. In all scenarios, the host vehicle 12 was moving at 15.5 m/s and a target vehicle 36 is approaching on the same roadway 34 at a speed of 15.5 m/s. The initial position of the target was 125 meters away from the host in the x direction, and the lane width for all lanes was assumed to be 3.6 meters. The measurement variance of the vehicle speed sensor was 0.02 m/s and the variance of the gyroscope yaw rate measurement was 0.0063 rad/s. The variances of radar range, range rate and azimuth angle measurements were 0.5 m, 1 m/s, and 1.5° respectively. Simulation results were then generated from 100 Monte-Carlo runs of the associated tracking filters.

0060In the first scenario, the host vehicle 12 and the target vehicle 36 were moving on a straight roadway 34 (C0 = 0 and C1 = 0) and the target vehicle 36 was moving toward the host vehicle 12 in the same lane. <figref idref="f0004">Figs. 8a</figref>-d illustrate the target state estimation and road curvature estimation results of the unconstrained and constrained filtering schemes, and <figref idref="f0006">Fig. 9a</figref>-b illustrate the average target vehicle 36 lateral position, velocity and acceleration RMS errors of the unconstrained and constrained filtering schemes. The estimation errors from constrained filtering were substantially reduced. Before 48 radar scans, when the target vehicle 36 was farther than 65 meters away from the host vehicle 12, constrained filtering resulted in a more than 40 percent reduction of error in target lateral velocity estimation, and a more than 60 percent reduction of error in lateral acceleration estimation. When the target vehicle 36 was less than 65 meters away from the host vehicle 12, which is a more relevant condition for collision prediction, more than 50 percent of lateral position estimation error, and more than 90 percent of lateral velocity and acceleration estimation errors, were reduced by constrained filtering.

0061In the second scenario, the host vehicle 12 and the target vehicle 36 were moving on a curved roadway 34 (C0 = -10-5 and C1 = -3×10-5) and the target vehicle 36 was moving toward the host vehicle 12 in the same lane. <figref idref="f0007">Figs. 10a</figref>-d illustrate the target state estimation and curvature estimation results of the unconstrained and constrained filtering schemes, and <figref idref="f0009 f0010">Figs. 11a-b</figref> illustrate the average target vehicle 36 lateral position, velocity and acceleration RMS errors of the unconstrained and constrained filtering schemes. The estimation errors from constrained filtering were substantially reduced after about 48 radar scans, when the target vehicle 36 was less than 65 meters away from the host vehicle 12. Estimation errors were the same for constrained and unconstrained filtering before 20 radar scans, when the target vehicle 36 was about 100 meters away from the host vehicle 12. For the target vehicle 36 located between 100 and 65 meters away from the host vehicle 12, constrained filtering resulted in about a 30 percent reduction in errors of lateral velocity and acceleration estimation, and when the target vehicle 36 was less than 65 meters away from the host vehicle 12, more than 50 percent of lateral position estimation error and more than 90 percent of lateral velocity and acceleration estimation errors were reduced by constrained filtering. The lack of improvement for constrained filtering when the target vehicle 36 was far away resulted from estimation errors of road curvature parameters, which caused constraint errors proportional to the distance between host vehicle 12 and the target vehicle 36. This is more evident in the curved roadway 34 case, where curvature estimation error was larger and caused more lane position ambiguity of a distant target vehicle 36.

0062In the third scenario, the host vehicle 12 and the target vehicle 36 were moving on a straight roadway 34 (C0 = 0 and C1 = 0) and the target vehicle 36 was initially approaching in the left neighboring lane. At t = 2.2 second (55 radar scans), the target vehicle 36 began to diverge from its lane and turns toward the host lane 38, which resulted in a collision at t = 4 seconds (100 radar scans). <figref idref="f0010 f0012">Figs. 12a-d</figref> illustrate the target state estimation results and the lateral position and velocity RMS errors of the unconstrained and constrained filtering schemes. The error tolerance levels for constraint validity hypothesis testing (equation (69)) were chosen as α ≈ 1 for the host lane 38 and α = 0.5 for all neighboring lanes 40. Whereas constrained filtering without validation produces substantially lower estimation errors before the target vehicle 36 turns away, the associated target state estimation result was incorrect and its RMS errors were much larger than that of unconstrained filtering after the target vehicle 36 began to turn away from its lane (the left neighboring lane), implying that the road constraints, which become invalid after the target vehicle 36 began to diverge from its lane, were not promptly lifted off. On the other hand, the performance of constrained filtering with validation was substantially close to that of unconstrained filtering, producing slightly lower estimation errors before the target vehicle 36 turns away, and exhibiting target state estimation results and RMS errors that were the same as unconstrained filtering after the target vehicle 36 began to turn away from its lane, implying that road constraints were promptly lifted off after the target vehicle 36 began to diverge from its lane.

0063The fourth scenario was similar to the third scenario, the only difference being that the vehicles were on a curved roadway 34 (C0 = -10-5 and C1 = -3×10-5) instead of a straight one. The target vehicle 36 began to diverge at t = 2.2 s and results in a collision at t = 4 s. <figref idref="f0012 f0014">Figs. 13a-d</figref> illustrate the target state estimation results and the lateral position and velocity RMS errors of the unconstrained and constrained filtering schemes. The error tolerance levels were the same as in the third scenario, and the results and observations were also similar to that of the third scenario. Road constraints were promptly lifted off by the proposed constraint validation after the target vehicle 36 began to diverge from its lane. In general, the overall improvement by constrained filtering in estimation accuracy of target vehicle 36 lateral kinematics was substantial, given the fact that estimation accuracy of target vehicle 36 lateral kinematics was often limited by poor radar angular resolution.

0064Accordingly, simulation results of road vehicle tracking on both straight and curved roadways 34 show that the predictive collision sensing system 10 could substantially reduce the estimation errors in target vehicle 36 lateral kinematics when the target vehicles 36 were in the host lane 38. When a target vehicle 36 maneuvers from a neighboring lane into the host lane 38, the predictive collision sensing system 10 promptly detects this maneuver and lifts off the road constraint to avoid an otherwise incorrect constrained result. In view of the fact that poor radar angular resolution often results in poor lateral kinematics estimation, the predictive collision sensing system 10 has provided for a substantial improvement in estimation accuracy of target vehicle 36 lateral kinematics, which is beneficial for an early and reliable road vehicle collision prediction.

0065While specific embodiments have been described in detail in the foregoing detailed description and illustrated in the accompanying drawings, those with ordinary skill in the art will appreciate that various modifications and alternatives to those details could be developed in light of the overall teachings of the disclosure. Accordingly, the particular arrangements disclosed are meant to be illustrative only and not limiting as to the scope of the invention, which is to be given the full breadth of the appended claims and any and all equivalents thereof.

APPENDIX - DESCRIPTION OF KALMAN FILTERING

0066A Kalman filter is used to estimate, from a set of noisy measurements, the state and associated measurements of a dynamic system subject to noise.

0067The system dynamics are defined by: <maths id="math0096" num="(A-1)"><math display="block"><mrow><msub><mrow><munder><mi>x</mi><mrow><mo>̲</mo></mrow></munder></mrow><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub><mo>=</mo><msub><mi mathvariant="bold">F</mi><mi>k</mi></msub><mo>⋅</mo><msub><mrow><munder><mi>x</mi><mrow><mo>̲</mo></mrow></munder></mrow><mi>k</mi></msub><mo>+</mo><msub><mrow><munder><mi>w</mi><mrow><mo>̲</mo></mrow></munder></mrow><mi>k</mi></msub><mo>,</mo><mspace width="3em" /><msub><mrow><munder><mi>w</mi><mrow><mo>̲</mo></mrow></munder></mrow><mi>k</mi></msub><mo>∼</mo><mi>N</mi><mfenced separators=","><mn>0</mn><msub><mi mathvariant="bold">Q</mi><mi>k</mi></msub></mfenced></mrow></math><img file="EP1537440B1_D0096.tif" /></maths> where xk is the system state vector, Fk is the system matrix and wk an associated vector of noise variables corresponding to each state variable, each noise variable having a mean value of zero, and a variance given by the corresponding element of the associated variance vector, Qk.

0068The dynamics of the associated system measurements are given by: <maths id="math0097" num="(A-2)"><math display="block"><mrow><msub><mrow><munder><mi>z</mi><mrow><mo>̲</mo></mrow></munder></mrow><mi>k</mi></msub><mo>=</mo><msub><mi mathvariant="bold">H</mi><mi>k</mi></msub><mo>⋅</mo><msub><mrow><munder><mi>x</mi><mrow><mo>̲</mo></mrow></munder></mrow><mi>k</mi></msub><mo>+</mo><msub><mrow><munder><mi>v</mi><mrow><mo>̲</mo></mrow></munder></mrow><mi>k</mi></msub><mo>,</mo><mspace width="3em" /><msub><mrow><munder><mi>v</mi><mrow><mo>̲</mo></mrow></munder></mrow><mi>k</mi></msub><mo>∼</mo><mi>N</mi><mfenced separators=","><mn>0</mn><msub><mi mathvariant="bold">R</mi><mi>k</mi></msub></mfenced></mrow></math><img file="EP1537440B1_D0097.tif" /></maths> where zk is the system measurement vector, Hk is the measurement matrix and vk an associated vector of noise variables corresponding to each measurement variable, each noise variable having a mean value of zero, and a variance given by the corresponding element of the associated variance vector, Rk. The values of the elements of the associated covariance matrix Rk can be determined a priori from analysis of the representative measurements of the associated system for associated representative sets of operating conditions. The values of the elements of the associated covariance matrix Qk account for modeling errors. Generally, the associated matrices Fk, Qk, Hk, Rk can vary over time.

0069Given a measurement zk at time k, and initial values of the state xk-1|k-1 and associated covariance Pk-1|k-1 at time k-1, the Kalman filter is used to estimate the associated state xk|k and associated covariance Pk|k at time k.

0070The first step in the filtering process is to calculate estimates of the state xk|k-1 and associated covariance Pk|k-1 at time k based upon estimates at time k-1, as follows: <maths id="math0098" num="(A-3)"><math display="block"><mrow><msub><mrow><munder><mi>x</mi><mrow><mo>̲</mo></mrow></munder></mrow><mrow><mi>k</mi><mrow><mo>|</mo><mi>k</mi><mo>−</mo><mn>1</mn></mrow></mrow></msub><mo>=</mo><msub><mi mathvariant="bold">F</mi><mi>k</mi></msub><mo>⋅</mo><msub><mrow><munder><mi>x</mi><mrow><mo>̲</mo></mrow></munder></mrow><mrow><mi>k</mi><mo>−</mo><mn>1</mn><mrow><mo>|</mo><mi>k</mi><mo>−</mo><mn>1</mn></mrow></mrow></msub></mrow></math><img file="EP1537440B1_D0098.tif" /></maths><maths id="math0099" num="(A-4)"><math display="block"><mrow><msub><mi mathvariant="bold">P</mi><mrow><mi>k</mi><mrow><mo>|</mo><mi>k</mi><mo>−</mo><mn>1</mn></mrow></mrow></msub><mo>=</mo><msub><mi mathvariant="bold">F</mi><mi>k</mi></msub><mo>⋅</mo><msub><mi mathvariant="bold">P</mi><mrow><mi>k</mi><mo>−</mo><mn>1</mn><mrow><mo>|</mo><mi>k</mi><mo>−</mo><mn>1</mn></mrow></mrow></msub><mo>⋅</mo><msubsup><mi mathvariant="bold">F</mi><mi>k</mi><mi>T</mi></msubsup><mo>+</mo><msub><mi mathvariant="bold">Q</mi><mi>k</mi></msub></mrow></math><img file="EP1537440B1_D0099.tif" /></maths>

0071The next step is to predict the measurement ẑk and associated covariance matrix Sk at time k, as follows: <maths id="math0100" num="(A-5)"><math display="block"><mrow><msub><mrow><mover><mrow><munder><mi>z</mi><mrow><mo>̲</mo></mrow></munder></mrow><mrow><mo>^</mo></mrow></mover></mrow><mi>k</mi></msub><mo>=</mo><msub><mi mathvariant="bold">H</mi><mi>k</mi></msub><mo>⋅</mo><msub><mrow><munder><mi>x</mi><mrow><mo>̲</mo></mrow></munder></mrow><mrow><mi>k</mi><mrow><mo>|</mo><mi>k</mi><mo>−</mo><mn>1</mn></mrow></mrow></msub></mrow></math><img file="EP1537440B1_D0100.tif" /></maths><maths id="math0101" num="(A-6)"><math display="block"><mrow><msub><mi mathvariant="bold">S</mi><mi>k</mi></msub><mo>=</mo><mi>cov</mi><mfenced><msub><mrow><munder><mrow><mover><mi>z</mi><mrow><mo>^</mo></mrow></mover></mrow><mrow><mo>̲</mo></mrow></munder></mrow><mi>k</mi></msub></mfenced><mo>=</mo><msub><mi mathvariant="bold">H</mi><mi>k</mi></msub><mo>⋅</mo><msub><mi mathvariant="bold">P</mi><mrow><mi>k</mi><mrow><mo>|</mo><mi>k</mi><mo>−</mo><mn>1</mn></mrow></mrow></msub><mo>⋅</mo><msubsup><mi mathvariant="bold">H</mi><mi>k</mi><mi mathvariant="normal">T</mi></msubsup><mo>+</mo><msub><mi mathvariant="bold">R</mi><mi>k</mi></msub></mrow></math><img file="EP1537440B1_D0101.tif" /></maths>

0072The next step is to calculate a gain matrix Gk used for updating the state vector xk|k and associated covariance matrix Pk|k, as follows: <maths id="math0102" num="(A-7)"><math display="block"><mrow><msub><mi mathvariant="bold">G</mi><mi>k</mi></msub><mo>=</mo><msub><mi mathvariant="bold">P</mi><mrow><mi>k</mi><mrow><mo>|</mo><mi>k</mi><mo>−</mo><mn>1</mn></mrow></mrow></msub><mo>⋅</mo><msubsup><mi mathvariant="bold">H</mi><mi>k</mi><mi mathvariant="normal">T</mi></msubsup><mo>⋅</mo><msubsup><mi mathvariant="bold">S</mi><mi>k</mi><mrow><mo>−</mo><mn>1</mn></mrow></msubsup></mrow></math><img file="EP1537440B1_D0102.tif" /></maths>

0073Finally, the state vector xk|k and associated covariance matrix Pk|k are estimated at time k, responsive to the associated measurement zk, as follows: <maths id="math0103" num="(A-8)"><math display="block"><mrow><msub><mrow><munder><mi>x</mi><mrow><mo>̲</mo></mrow></munder></mrow><mrow><mi>k</mi><mrow><mo>|</mo><mi>k</mi></mrow></mrow></msub><mo>=</mo><msub><mrow><munder><mi>x</mi><mrow><mo>̲</mo></mrow></munder></mrow><mrow><mi>k</mi><mrow><mo>|</mo><mi>k</mi><mo>−</mo><mn>1</mn></mrow></mrow></msub><mo>+</mo><msub><mi mathvariant="bold">G</mi><mi>k</mi></msub><mo>⋅</mo><mfenced><msub><mrow><munder><mi>z</mi><mo>̲</mo></munder></mrow><mi>k</mi></msub><mo>−</mo><msub><mrow><munder><mrow><mover><mi>z</mi><mrow><mo>^</mo></mrow></mover></mrow><mo>̲</mo></munder></mrow><mi>k</mi></msub></mfenced></mrow></math><img file="EP1537440B1_D0103.tif" /></maths><maths id="math0104" num="(A-9)"><math display="block"><mrow><msub><mi mathvariant="bold">P</mi><mrow><mi>k</mi><mrow><mo>|</mo><mi>k</mi></mrow></mrow></msub><mo>=</mo><msub><mi mathvariant="bold">P</mi><mrow><mi>k</mi><mrow><mo>|</mo><mi>k</mi><mo>−</mo><mn>1</mn></mrow></mrow></msub><mo>−</mo><msub><mi mathvariant="bold">G</mi><mi>k</mi></msub><mo>⋅</mo><msub><mi mathvariant="bold">S</mi><mi>k</mi></msub><mo>⋅</mo><msubsup><mi mathvariant="bold">G</mi><mi>k</mi><mi mathvariant="normal">T</mi></msubsup></mrow></math><img file="EP1537440B1_D0104.tif" /></maths>

Contents3

Every citation, both ways

Citations
Document	Relation	Office	Cited during
EP4070167B1	Cited by	European Patent Office (EPO)	Examiner
WO0221156A2	Cites	World Intellectual Property Organization (WIPO)	Examiner
EP0915350A2	Cites	European Patent Office (EPO)	–
WO0221156A2	Cites	World Intellectual Property Organization (WIPO)	–
DE4407757A1	Cites	Germany	–
DE10118265A1	Cites	Germany	–
DE19637245A1	Cites	Germany	–
DE19855400A1	Cites	Germany	–
US5710565A	Cites	United States of America	–
US5926126A	Cites	United States of America	–
US5955967A	Cites	United States of America	–
US2002044081A1	Cites	United States of America	–
US2002049539A1	Cites	United States of America	–
US6202027B1	Cites	United States of America	–
US6282478B1	Cites	United States of America	–
US6292752B1	Cites	United States of America	–
US6300865B1	Cites	United States of America	–

31 members in 6 offices

This recordMember, by publication datePriority claim

Priority claims3

Priority claims
Document	Office	Kind	Date
396211P	United States of America	–	2002-07-15
39621102	United States of America	P	2002-07-15
0322182	United States of America	W	2003-07-15

Members31

Family members, oldest first
Document	Office	Kind
WO2004008648A2	World Intellectual Property Organization (WIPO)	A2	2004-01-22
AU2003251943A1	Australia	A1	2004-02-02
AU2003251943A8	Australia	A8	2004-02-02
WO2004008648A3	World Intellectual Property Organization (WIPO)	A3	2004-09-02
EP1537440A2	European Patent Office (EPO)	A2	2005-06-08
WO2005062984A2	World Intellectual Property Organization (WIPO)	A2	2005-07-14
US2005179580A1	United States of America	A1	2005-08-18
CN1668938A	China	A	2005-09-14
US2005225477A1	United States of America	A1	2005-10-13
JP2005539288A	Japan	A	2005-12-22
US7034742B2	United States of America	B2	2006-04-25
EP1714108A2	European Patent Office (EPO)	A2	2006-10-25
WO2005062984A3	World Intellectual Property Organization (WIPO)	A3	2007-02-22
JP2007516906A	Japan	A	2007-06-28
CN100365430C	China	C	2008-01-30
US2008183419A1	United States of America	A1	2008-07-31
US7522091B2	United States of America	B2	2009-04-21
US7626533B2	United States of America	B2	2009-12-01
EP1714108A4	European Patent Office (EPO)	A4	2010-01-13
JP2010280378A	Japan	A	2010-12-16
JP4823520B2	Japan	B2	2011-11-24
EP1537440A4	European Patent Office (EPO)	A4	2012-03-28
JP2012131495A	Japan	A	2012-07-12
JP2012131496A	Japan	A	2012-07-12
JP4990629B2	Japan	B2	2012-08-01
JP5265787B2	Japan	B2	2013-08-14
JP2013209085A	Japan	A	2013-10-10
JP5323766B2	Japan	B2	2013-10-23
JP5848137B2	Japan	B2	2016-01-27
JP5864473B2	Japan	B2	2016-02-17
EP1537440B1This record	European Patent Office (EPO)	B1	2016-04-06

33 legal events, as 3 offices reported them to INPADOC

Over the term

Point at a mark for the event

Events

Legal events, newest first
	Event	Code	Office
2021-05-31	Lapsed in a contracting state [announced via postgrant information from national office to epo]LapsedPG25	PG25	EP
2021-02-02	Application deemed withdrawn, or ip right lapsed, due to non-payment of renewal feeWithdrawnR119	R119	DE
2019-10-31	Annual fee paid to national office [announced via postgrant information from national office to epo]GrantedPGFP	PGFP	EP
2017-05-31	Lapsed in a contracting state [announced via postgrant information from national office to epo]LapsedPG25	PG25	EP
2017-03-22	Gb: european patent ceased through non-payment of renewal feeCeasedGBPC	GBPC	EP
2017-03-15	No opposition filedOpposition26N	26N	EP
2017-02-10	No opposition filed within time limitOppositionORIGINAL CODE: 0009261PLBE	PLBE	EP
2017-02-10	Information on the status of an ep patent application or granted ep patentGrantedSTATUS: NO OPPOSITION FILED WITHIN TIME LIMITSTAA	STAA	EP
2017-01-10	No opposition filed against granted patent, or epo opposition proceedings concluded without decisionGrantedR097	R097	DE
2016-05-19	Dpma publication of mentioned ep patent grantGrantedR096	R096	DE
2016-04-06	Designated contracting statesAK	AK	EP
2016-04-06	European patent grantedGrantedFG4D	FG4D	GB
2016-03-04	(expected) grantORIGINAL CODE: 0009210GRAA	GRAA	EP
2016-02-12	Grant fee paidORIGINAL CODE: EPIDOSNIGR3GRAS	GRAS	EP
2015-11-18	Intention to grant announcedINTG	INTG	EP
2015-10-28	Information provided on ipc code assigned before grantRIC1	RIC1	EP
2015-10-28	Information provided on ipc code assigned before grantRIC1	RIC1	EP
2015-10-28	Information provided on ipc code assigned before grantRIC1	RIC1	EP
2015-10-28	Information provided on ipc code assigned before grantRIC1	RIC1	EP
2015-10-28	Information provided on ipc code assigned before grantRIC1	RIC1	EP
2015-10-20	Despatch of communication of intention to grant a patentORIGINAL CODE: EPIDOSNIGR1GRAP	GRAP	EP
2012-07-11	First examination report despatched17Q	17Q	EP
2012-03-28	Supplementary search report drawn up and despatchedA4	A4	EP
2012-03-28	Information provided on ipc code assigned before grantRIC1	RIC1	EP
2012-03-28	Information provided on ipc code assigned before grantRIC1	RIC1	EP
2012-03-28	Information provided on ipc code assigned before grantRIC1	RIC1	EP
2012-03-28	Information provided on ipc code assigned before grantRIC1	RIC1	EP
2005-11-02	Request for extension of the european patent (deleted)DAX	DAX	EP
2005-11-02	Designated contracting states (corrected)RBV	RBV	EP
2005-06-08	Request for examination filed17P	17P	EP
2005-06-08	Designated contracting statesAK	AK	EP
2005-06-08	Request for extension of the european patentAX	AX	EP
2005-04-22	Public reference made under article 153(3) epc to a published international application that has entered the european phaseORIGINAL CODE: 0009012PUAI	PUAI	EP