Adaptive Kalman filter method for accurate estimation of forward path geometry of an automobile

[0002] The present invention is generally related to vehicular forward path geometry estimation, and more specifically to a method and apparatus for accurate estimation of vehicle forward path geometry utilizing an adaptive Kalman filter bank and a two-clothoid road model.

[0003] Vehicular land and road detection systems have been the subject of significant research. Applications such as adaptive cruise control, collision avoidance systems, and vehicle guidance systems require knowledge of the geometry of the road to be most effective. It is difficult for a collision avoidance system to accurately characterize a stationary or slower moving object as non-hazardous unless there is some certainty that the stationary object is not in the vehicle's path. Previous approaches to estimation of forward road geometry model the road in the forward view as either a constant curvature or a linearly varying curvature. These road geometry estimation approaches then fit parameters of these models via a least-squares fitting technique or other mathematical technique. The linearly-varying curvature model is usually referred to as a single-clothoid model and is completely described by two coefficients: c₀, the local curvature at host vehicle position, and c₁, the rate of change of curvature with distance. Both the constant curvature and single-clothoid road models do not adequately represent road geometry, especially when there are abrupt changes in curvature in the look-ahead range. Such sharp changes in curvature are common on many roads and freeways, particularly where a straightaway section abruptly transitions to a curved section and vice-versa. Thus, methods based on these simple models are generally inadequate in describing forward road geometry.

[0004] To overcome the problem of inaccurate road geometry estimation with these simple models, an averaged curvature road model has been proposed. This approach works well on roads with low curvature and very smooth curvature changes, but still fails on roads with significant curvature changes or discontinuities in c₁. To reduce these errors further, more complex road models were proposed that split the road into multiple clothoid segments. The transition between segments is estimated using ad-hoc methods such as the generalized likelihood ratio test. In this approach, the geometry of the segments is not dynamic, and is updated using measurements that fall in the corresponding segment. A dynamic model is required for vehicle dynamics only, as the road model remains spatially fixed, while the vehicle is moving through the road segments. One of the fundamental limitations of this approach is that road geometry estimation accuracy is strongly dependent on reliable detection of transition points between segments making it very sensitive to noise in the measurements. Thus they are often inaccurate in moderate and high noise measurement conditions. In addition, the segmentation process is computationally expensive because the likelihood ratio test has to be performed for each new measurement.

[0005] A naïve approach of using a road model based on a higher-order polynomial to solve these problems with previous methods will lead to over-fitting road geometry and a statistically insignificant estimate of the coefficient of this higher order term. This will also make this approach more sensitive to noise. Thus, it would be desirable to develop a model that overcomes the problems associated with previous approaches. Ideally such a model would be simple to implement and would not be as computationally intensive or ad-hoc as the previous multi-clothoid models.

[0006] Previous approaches use a conventional Kalman filter with fixed process model parameters that are chosen to provide a compromise between noise and filter lag. In other words, process noise parameters in the conventional filters is chosen so that the filter lag (time taken by filter to catch up with new geometry) is reduced during transitions in road geometry at the expense of introducing some noise during slowly changing road geometry conditions. Thus, the filter performance is moderate during all road geometry conditions. However, the estimation errors may still be unacceptably large for systems that rely on the forward geometry information.

[0007] The single-clothoid road model could result in large estimation errors on roads with transitions in curvature rate parameter c₁in the look-ahead range. To overcome the problem of suddenly changing c₁parameters, artisans have used an averaged curvature model. This approach is suitable on some ideal roads with low curvature and very smooth curvature changes, but performs poorly on roads with significant jumps in c₁. To reduce these errors, the road model was split into a number of segments and each segment was modeled as a clothoid. However, this solution assumes that clothoid coefficients c₀and c₁are constant instead of being time-dependent. As a consequence, measurements only improve the accuracy of estimation of segment parameters, but do not drive a dynamic system of the road course. This approach requires accurate estimation of the transition between road segments. Artisans have estimated transition points by applying spatially recursive estimation and the generalized likelihood ratio method. One of the fundamental limitations of this approach is that road geometry estimation accuracy is strongly dependent on reliable detection of transition points which is a very difficult estimation problem. In addition, the segmentation process is computationally expensive because the likelihood ratio test has to be performed for each new measurement. It would thus be desirable to provide a geometry estimation scheme that is not strongly dependent on reliable detection of transition points and is not computationally expensive. It would also be desirable to avoid the need to use compromised, static process model parameters.

[0008] The present invention provides an apparatus for dynamically predicting road geometry. The apparatus comprises a bank of filters configured to receive current environmental data from at least one sensor. After receiving the data, the bank of filters performs an estimation using the data, resulting in filter output from each filter. A weighting element is configured to receive the filter output from the bank of filters, and to ascribe a weighted value to each filter output based on a relevance of the filter. The weighting element thereby provides weighted outputs.

[0009] The apparatus further comprises a fusing element configured to fuse the weighted outputs into a single output; whereby the output describes a vehicle forward path estimate.

[0010] In another aspect, the bank of filters of the apparatus is further configured to receive a previous vehicle path estimate and to recursively perform the estimation based on the current environmental data and the previous vehicle path estimate. In a still further aspect, each filter in the bank of filters is a Kalman filter. In yet another aspect, the bank of filters includes three Kalman filters.

[0011] In a further aspect, the configuration of the bank of filters is based on a polynomial road model, which in a still further aspect is a two-clothoid road model. In this case, each filter may be tuned to a different class of geometry within the two-clothoid road model. In a more specific aspect, the different classes of geometry include a constant geometry road section, an upcoming transition in the far range, and an upcoming transition in the near range.

[0012] In yet another aspect, the weighting element ascribes weighted values based on the posterior probabilities of a fit between the filter and the forward vehicle path estimate.

[0013] In a further aspect, the sensor is a device selected from a group consisting of imaging devices, GPS/map systems, laser sensors, and radars.

[0014] In another aspect, the output of the apparatus is configured for use by other automotive systems selected from a group consisting of driver assistance systems and vehicle safety systems.

[0015] Each of the operations of the apparatus discussed above typically corresponds to a software module for performing the function on a computer. In other embodiments, the means or modules may be incorporated onto a computer readable medium to provide a computer program product. Also, the means discussed above also correspond to steps in a method for estimating a vehicle forward path.

[0016] The objects, features, and advantages of the present invention will be apparent from the following detailed description of the preferred aspect of the invention with references to the following drawings, where:

[0017] FIG. 1 is a block diagram depicting an aspect of the present invention;

[0018] FIG. 2 is a flow chart depicting a method according to the present invention for predicting the geometry of the road in front of a host vehicle;

[0019] FIGS. 3(a), 3(b), and 3(c) are diagrams of three different road models used in the adaptive Kalman filter scheme according to the present invention;

[0020] FIG. 4 is an illustrative depiction of a host vehicle and a curved road portion ahead of the vehicle;

[0021] FIG. 5 is a block diagram showing the adaptive Kalman filter system of the present invention, implementing three parallel Kalman filters;

[0022] FIG. 6 is a block diagram depicting the components of a computer system used in the present invention; and

[0023] FIG. 7 is an illustrative diagram of a computer program product embodying the present invention.

[0024] The present invention is generally related to forward path geometry estimation and more specifically to a method and apparatus for accurate estimation of forward path geometry of a vehicle, based on a bank of Kalman filters configured to adaptively respond to changes in road geometry and consistently identify which road model the existing path represents. The following description, taken in conjunction with the referenced drawings, is presented to enable one of ordinary skill in the art to make and use the invention and to incorporate it in the context of particular applications. Various modifications, as well as a variety of uses in different applications, will be readily apparent to those skilled in the art, and the general principles defined herein may be applied to a wide range of aspects. Thus, the present invention is not intended to be limited to the aspects presented, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein. For instance, although only three road models and three Kalman filters have been described, it is considered to be within the scope of the invention to incorporate additional road models and filters. Furthermore, it should be noted that unless explicitly stated otherwise, the figures included herein are illustrated diagrammatically and without any specific scale, as they are provided as qualitative illustrations of the concept of the present invention.

[0025] One aspect of the present invention provides an apparatus for dynamically predicting road geometry. The apparatus is shown in FIG. 1 and includes at least one sensor 100, which is configured to collect environmental data and provide the data to a bank of filters 102. The filters 102, in a preferred aspect are Kalman filters. The filters 102 are configured to recursively estimate the road geometry according to a plurality of road models, and provide the estimated geometries to a weighting element 104. The weighting element 104, in turn, ascribes a weight to the output of each filter and provides the weighted values to a fusion element 106 which fuses the weighted values and provides a single road geometry prediction based on the weighted filter outputs. This prediction output has application in many vehicle systems 108 and vehicle subsystems 110 such as: autonomous vehicles, adaptive cruise control, and automotive collision warning systems.

[0026] Another aspect of the present invention, shown in FIG. 2, provides a method for dynamically predicting road geometry comprised of the following steps: The first is collecting environmental data utilizing at least one sensor 200. The sensor may include a GPS/MAP combination or video footage, or a combination of both. The second step is to provide the collected environmental data to a bank of filters 202. In one aspect the bank of filters is comprised of Kalman filters. In the third step the road geometry is recursively estimated utilizing environmental data provided by the sensors or sensors and is constrained to fit a plurality of road models 204. Fourth, the estimated road model geometries are provided to a weighting element, which ascribes a weight value to the output of each filter 208. In the fifth step the weighted values are provided to a fusion element, which fuses the weighted values 210; and finally provides a single, fused, road geometry prediction output 212. This method provides for improved accuracy in estimating forward road geometry with stable estimates during constant geometry scenarios and better tracking during abrupt geometry transitions.

[0027] Another aspect of the invention adaptively tunes process model parameters such that the filter responds to road geometry changes quickly during sharp changes and provides stable and low noise estimates otherwise. This increased accuracy in road geometry estimation translates to improved detection of targets in the host vehicle's forward path and improves systems using this information, such as autonomous vehicles, adaptive cruise control and automotive collision warning systems.

[0028] The present invention finds application in automotive applications such as adaptive cruise control and collision warning applications. Additionally, the invention finds application in the enhancement of a variety of vehicle subsystems such as vehicle stability, vision-based systems for detecting vehicles and pedestrians on the road, etc. Vehicle stability systems, in particular, can directly benefit from forward geometry information. Vision-based systems could benefit greatly by using this information to focus on only the area in the cameras field-of-view that corresponds to the forward path of the host-vehicle, thereby enhancing target identification and reducing the spurious signal reporting rate.

[0029] Information about road geometry in the forward path of an automobile can be obtained by using an onboard vision system or by using a GPS/MAP system, or by detecting electromagnetic radiation emitting beacons along the roadside, which are configured to provide data that can be correlated to a map database, or any of a plurality of other systems. The sensor system detects and identifies lane markers on the road and fits them to a polynomial road model. The actual vision system could be responsive to electromagnetic radiation in any number of forms, such as a camera responsive to radiation in the infrared region, the visible region, or both. The GPS/MAP system combines information about host vehicle position using a satellite based locating system such as the global positioning system (GPS) with a road map database. The system uses GPS data, to place the vehicle on the correct road and then extracts forward road information about this road from the MAP database. Both these methods provide noisy information about forward road geometry and do not generally find application as an exclusive means for forward road geometry prediction. It is assumed that the host vehicle is equipped with at least one of the above sensor systems, or an equivalent system, for providing forward road geometry information. Furthermore, the collected road geometry data is in the form of noisy offsets of a number of road points starting at, and extending up to, a finite distance (referred to as the look-ahead view of the sensor system) in front of the host vehicle. The invention uses the noisy information provided by the sensors to estimate forward geometry more accurately and with much finer sampling.

[0030] A useful model, finding application in the identification of the horizontal course of a road is a clothoid, which is a curve whose curvature changes linearly with distance along the road. Starting with curvature c₀, curvature at distance x along the curve is given by:

[0031] c(x)=c₀+c₁x.  Eqn. (1)

[0032] The coefficient c₀and c₁(rate of change of curvature with distance) are called the clothoid coefficients. Typical highway geometries are approximated by a set of successive clothoids with different coefficients. For example, two segments of constant curvature (c₁=0) could be joined by a transition section where curvature changes linearly with distance (c₁not equal 0). Thus, transition between segments can have jumps in the clothoid coefficient c₁, even when the curvature c(x) itself is steady across the segment transition. A clothoid can be approximated by a third order polynomial that relates the lateral offset of the curve with longitudinal distance along it (when the change in road heading angle over the distance along the curve is small). Denoting x as longitudinal distance, and y as lateral offset (within a segment), then under small angle approximations: $\begin{array}{cc}y\approx \frac{c_0{x}^2}{2}+\frac{c_1{x}^3}{6}.& \mathrm{Eqn}.\left(2\right)\end{array}$

[0033] Since the road geometry changes as the host vehicle moves along the road, the clothoid coefficients are time-varying and dependent on the speed of the vehicle. One approach to estimating model parameters at each time instant is to perform non-recursive least-squares fitting on the data points (offset value of road points). The main problem with non-recursive least-squares estimation is that it can be very sensitive to stochastic errors (e.g., sensor noise) resulting in oscillating estimates from one time step to the next. One possible solution is to use a recursive estimation method. However, since vehicle motion along the road makes forward road geometry a quantity that varies dynamically, a recursive least-squares method cannot be used with adequate results; rather a dynamic recursive estimation approach (Kalman filter) is utilized. Kalman filters perform recursive estimation using both a model-based update of state variables (clothoid coefficients) and an update of the state estimates using a weighted version of the new measurements. Recursive filtering can provide more accuracy and improved robustness to stochastic errors (e.g., measurement noise)—in part because the Kalman filter acts as a sort of “low-pass” filter.

[0034] One aspect of the present invention includes a novel road model where the forward lane geometry is divided into two regions or segments with different c₁parameters: one from 0-d₁in front of the host vehicle (near-range segment) and the second from d₁-d₂in front of the host vehicle (far-range segment). Nominal values for d₁and d₂are 50 m and 100 m, respectively. The curvature is assumed to be continuous at the transition between the two segments. Instead of estimating the transition point, it is assumed that the transition point is at a fixed distance, d₁, in front of the host vehicle. This approach provides better results than those obtained while using a single clothoid road model and solves the shortcomings encountered with the use of a multi-clothoid model based on the detection of transition points. Certain components of this aspect are described in the U.S. patent application Ser. No. 09/994,515 which is hereby incorporated by reference for all purposes.

[0035] Referring now to FIG. 3, which presents a two-clothoid road model the near-range segment 300 extends from the host vehicle 302 up to distance d₁304. The far-range segment 306 extends from d₁304 up to d₂308 in front of the host vehicle 302. The curvature of the road at the host vehicle is c₀, and the rate of change of curvature in the near and far range segments 300 and 306 are c₁and c₂, respectively. For small angle approximations, the lateral offset y of road edge as a function of look-ahead distance x is given by: $\begin{array}{cc}y=c_0\frac{x^2}{2}+c_1\frac{x^3}{6},0<x\le d_{1}y=c_0\frac{x^2}{2}+c_1\left(\frac{d_1^3}{6}+\frac{x\left(x-d_1\right)d_1}{2}\right)+c_2\frac{{\left(x-d_1\right)}^3}{6},d_1<x\le d_2,& \mathrm{Eqn}.3\end{array}$

[0036] the Kalman filter formulation for two-clothoid road model. The two-clothoid road model using a Kalman filter produces better results than have been obtained using conventional approaches. According to the two-clothoid process dynamics model, as the vehicle 302 drives through the clothoid with speed v, the coefficients describing the curvature model of the clothoid change according to the motion of the vehicle 302. For a road with jumps in curvature rate in the look-ahead range, the time-derivative of this rate of change, at the transition, is an impulse. However, since these transitions cannot be reliably determined using the existing noisy measurements, the time derivative of c₁and c₂is approximated by white noise. This leads to the following dynamic model of the road in the horizontal plane: $\begin{array}{cc}\left[\begin{array}{c}{\stackrel{.}{c}}_0\\ {\stackrel{.}{c}}_1\\ {\stackrel{.}{c}}_2\end{array}\right]=\left[\begin{array}{ccc}0& v\left(t\right)& 0\\ 0& 0& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}{c}_0\\ c_1\\ c_2\end{array}\right]+\left[\begin{array}{c}0\\ w_1\left(t\right)\\ w_2\left(t\right)\end{array}\right],& \mathrm{Eqn}.4\end{array}$

[0037] where,

[0038] v(t) is vehicle speed;

[0039] w₁(t) is the continuous driving white noise term for c₁; and

[0040] w₂(t) is the continuous driving white noise term for c₂.

[0041] The vertical curvature in the above model has been ignored, but can be easily appended to the above state-space representation. Consider a sample time interval T, the predicted state vector estimate at time k+1 using the estimate at time k is given by:

[0042] X_k+1=Φ_kX_k+W_k;  Eqn. 5

[0043] $\begin{array}{cc}{X}_k=\mathrm{state}\mathrm{vector}=\left[\begin{array}{c}{c}_{0,k+1}\\ c_{1,k+1}\\ c_{2,k+1}\end{array}\right]& \mathrm{Eqn}.6\\ {\Phi }_k=\mathrm{State}\mathrm{transitio}n\mathrm{matrix}=\left[\begin{array}{ccc}1& v\left(k\right)T& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]& \mathrm{Eqn}.7\\ W_k=\mathrm{Process}\mathrm{noise}\mathrm{vector}=\left[\begin{array}{c}v\left(k\right){\mathrm{Tw}}_1\left(k\right)\\ w_1\left(k\right)\\ w_2\left(k\right)\end{array}\right]& \mathrm{Eqn}.8\end{array}$

[0044] where w₁(k) and w₂(k) are scalar, zero-mean, white-noise sequences with variances σ₁²and σ₂², respectively.

[0045] The process noise covariance Q(k) is thus defined as: $\begin{array}{cc}Q\left(k\right)=\left[\begin{array}{ccc}{\left(v\left(k\right)T\right)}^2{\sigma }_1^2& v\left(k\right)T{\sigma }_1^2& 0\\ v\left(k\right)T{\sigma }_1& {\sigma }_1^2& 0\\ 0& 0& {\sigma }_2^2\end{array}\right].& \mathrm{Eqn}.9\end{array}$

[0046] The measurement vector Y_kis comprised of noisy lateral offsets of a number of road points available from some sensor system onboard the host vehicle (vision, GPS, etc.). Let us assume that measurements are available for N road points: n points with lateral offsets y₁, y₂, . . . , y_nat distances x₁, x₂, . . . , x_n, respectively, in the near-range segment, and f points with lateral offsets y_n+1, y_n+2, . . . , y_n+fat distances X_n+1, X_n+2, . . . , x_n+f, respectively, in the far-range segment (N=n+f). Using Equation (3), we can denote the measurement equation in a matrix notation:

[0047] Y_k=H_kX_k+N_k,  Eqn. 10

[0048] $\begin{array}{cc}{Y}_k=\mathrm{Measurement}\mathrm{vector}=\left[\begin{array}{c}{y}_1\\ y_2\\ ⋮\\ y_N\end{array}\right],& \mathrm{Eqn}.11\\ H_k=\mathrm{Measurement}\mathrm{transfer}\mathrm{function}=\left[\begin{array}{ccc}\frac{1}{2}{x}_1^2& \frac{1}{6}{x}_1^3& 0\\ \frac{1}{2}{x}_2^2& \frac{1}{6}{x}_2^3& 0\\ ⋮& ⋮& ⋮\\ \frac{1}{2}{x}_n^2& \frac{1}{6}{x}_n^3& 0\\ \frac{1}{2}{x}_{n+1}^2& \frac{1}{6}{d}_1^3+\frac{1}{2}\left(x_{n+1}-d_1\right)\left(x_{n+1}\right)d_1& \frac{1}{6}{\left(x_{n+1}-d_1\right)}^3\\ \frac{1}{2}{x}_{n+1}^2& \frac{1}{6}{d}_1^3+\frac{1}{2}\left(x_{n+2}-d_1\right)\left(x_{n+2}\right)d_1& \frac{1}{6}{\left(x_{n+2}-d_1\right)}^3\\ \frac{1}{2}{x}_{n+f}^2& \frac{1}{6}{d}_1^3+\frac{1}{2}\left(x_{n+f}-d_1\right)\left(x_{n+f}\right)d_1& \frac{1}{6}{\left(x_{n+f}-d_1\right)}^3\end{array}\right]& \mathrm{Eqn}.12\\ N_k=\mathrm{Measurement}\mathrm{noise}\mathrm{vector}=\left[\begin{array}{c}{n}_1\\ n_2\\ ⋮\\ n_N\end{array}\right],& \mathrm{Eqn}.13\end{array}$

[0049] The estimation approach set forth above, uses the above process and measurement model equations to recursively estimate the road model parameters. In this example a Kalman filter is used.

[0050] One aspect of the present invention provides an adaptive Kalman filter protocol wherein a bank of Kalman filters dynamically provide estimates of road geometry. The invention dynamically selects the Kalman filter, from the filter bank, which is best suited to existing road conditions. A Kalman filtering protocol not utilizing the bank of Kalman filters has a simple Kalman filter with parameters σ₁²and σ₂²of the process noise covariance matrix Q, which are assigned a fixed value. Typical value of σ₁²for the single-clothoid road model, as might be found in an existing system is on the order of 5e⁻⁶. This value is selected to provide a compromise between noise in the estimated geometry and filter delay. The Kalman filter then uses the process and measurement model equations described above to recursively estimate the road model parameters, which is a sequence of time and measurement update steps as set forth in TABLE 1. The estimated state vector after each measurement update by this Kalman filter best describes the upcoming road geometry.

[0051] As the vehicle approaches a transition point (change in road geometry), the variances of c₁and c₂should ideally be increased to better track the rapidly changing geometry. When the geometry in the sensor range (0-d₂) is constant, then variances of c₁and c₂should be lowered to reduce the noise in the estimated results. Thus, improved estimation accuracy will be obtained by varying the values of σ₁²and σ₂²adaptively depending on the type of upcoming road geometry (constant or varying).

[0052] Typical roadways are laid as either (1) constant geometry clothoid segments with curvature continuity across the transition between these segments or (2) constant geometry segments (straight or circular) with abrupt curvature discontinuity at the transition between these segments. As used herein, the letter “D” shall denote the distance to the nearest transition point in front of the host vehicle. As depicted in FIG. 4, the near-range path in front of the host vehicle 400 is 0-d₁402 and far-range path in front of the host vehicle is d₁-d₂404. Under the assumption that all constant geometry segment lengths are >d₂406, upcoming road geometry on virtually any roadway can be described by one of the following three road models:

[0053] Model 1: Constant Geometry Road Section (D>d2)

[0054] Referring again to FIG. 3(a), the transition point 310 is beyond the sensor look-ahead range 308. There is no change in geometry within the sensor look-ahead range, i.e., D>d2. In this scenario, both c₁and c₂are constant and the process noise should be set to very low values. Stated alternatively the Kalman filter parameters σ₁²and σ₂²of the noise covariance matrix, Q, should both be set to values that are at or below the values provided in the situation where d1<D<d2 or where D<d1.

[0055] Model 2: Upcoming Transition in Far-Range (d1<D<d2)

[0056] When a host vehicle is close enough to a transition point 310, as depicted in FIG. 3(b), such that the transition point 310 is within the far-range 308, then the far-range coefficient c₂will change rapidly while near-range coefficient c₁remains constant. In such a situation the σ₁²value is relatively low, and the σ₂²is relatively high.

[0057] Model 3: Upcoming Transition in Near-Range (D<d1)

[0058] In the third model, depicted in FIG. 3(c), as the vehicle approaches the transition point 310, the transition point 310 will eventually enter the near-range region 300 of the vehicle. Thus, this model always follows model 2. During upcoming transitions in road geometry in the near-range (distance to transition point D<d₁), near-range coefficient c₁will change rapidly while far-range coefficient c₂has already adapted to the new range geometry and so is constant. In this situation, σ₁²is relatively high and σ₂²is relatively low.

[0059] If the upcoming road geometry model was known a priori to be model 1, 2, or 3, then the process noise covariance matrix Q parameters σ₁²and σ₂²could be set accordingly, resulting in good filter performance. However, the upcoming road geometry model is generally not known a priori. According to the present invention, the upcoming road geometry is modeled as a discrete random variable with a finite number of possible realizations (in this example either model 1, 2 or 3). The adaptive Kalman filter protocol accordingly implements 3 parallel Kalman filters (one for each model) operating on the same measurement sequence simultaneously as shown in FIG. 5. A sensor measurement sequence y₁, y₂. . . y_k(element 500) is provided to a bank of Kalman filters 502, all of which are identical (same (φ, H, R), except for the road model used in each. According to this aspect there are three road models.

[0060] Each Kalman filter uses the process and measurement model equations to recursively estimate the road model parameters using the Kalman filter, which is a sequence of time and measurement update steps (see TABLE 1). The outputs of each Kalman filter after the measurement update step, utilizing the latest available measurement vector Y_kat time k, are the estimated state vector x_kand error covariance matrix P_k. In FIG. 5, the outputs of filters 504a, 504b, and 504chave been denoted as j (using road model j, where j=1,2 or 3) as x_k^jand P_k^j. The weight factors W_k^j506a, 506b, and 506cfor each filter are next calculated using the outputs of each filter 504a, 504b, and 504c. The weight factor of each filter is the probability that the upcoming road geometry matches the road model hypothesized in the filter. Stated alternatively, the weight factor for the filter with the correct hypothesis (correct road model) approaches unity. After being assigned a weighted value, the weighted road models are fused in a fusion element 508, and a weighted output road geometry model is provided. One of the main objectives of the adaptive filter scheme is to optimally estimate the state vector x_kbased on the filter weight factors (or probability of each model). In this aspect, the fused state and covariance matrix estimates are simply a weighted combination of the individual filter estimates, where each estimate is weighted according to its filter weight. Since the weight factors are updated after each measurement step, the filter adapts to the changing road geometry and is self-learning in that respect.

[0061] Calculation of weight factors: As mentioned before, the weight factors are the α posteriori probabilities of the corresponding road model being correct. Three process (road) models have been postulated and a Kalman filter is set up for each. The present invention contemplates that additional road models could be postulated and corresponding Kalman filters could be added to the filter bank. Based on the road model likelihood functions, the probability of each road model being correct needs to be calculated. Detailed derivation of α posteriori probabilities for a general adaptive Kalman filter scheme can be found in the literature. The present invention gives the basic equations as applicable to the proposed invention. The measurement sequence up to current time k is, herein defined as, E_k={Y₁, Y₂, . . . Y_k}. Letting M_jbe the event that model j is correct with prior probability P{M_j}=W₀^j, where 0 denotes starting time index time and j denotes filter number (j=1, 2, or 3). The likelihood of the measurements up to time k under the assumption of model j is: ${\lambda }_k^j=p\left(E_k/M_j\right)=\prod _{i=1}^{k}p\left[v_i^j\right].$

[0062] Here v_i^jis a part of the innovation from filter j at time i. Under the Gaussian assumption, the probability density function (PDF) of the innovation is:

[0063] p[v_i^j]=det(2πS_i^j)^−½exp(−½v_i^j(S_i^j)⁻¹v_i^j),

[0064] where S_i^j=HP_i^j(−)H^T+R.

[0065] H is the measurement transfer function as described above, P(−) is the extrapolated error covariance matrix (see TABLE 1) and S is the measurement prediction covariance matrix. Using Bayes' rule, the weight factor or posterior probability that model j is correct at time k is: $W_k^j=P\left(M_j/E_k\right)=\frac{{\lambda }_k^j{W}_0^j}{\sum _{l=1}^3{\lambda }_k^l{W}_0^l}.$

[0066] The above derivation is exact under the following assumptions:

[0067] A. The correct model is among the set of road models considered.

[0068] B. The same model has been in effect from the initial time.

[0069] Assumption A requires that the three road models chosen here exhaustively describe any road geometry (assuming each segment length is >d₂, which is a reasonable assumption). Even though the choice of process noise is done in an ad-hoc manner and may not match the true variance of the transition or of the constant geometry segment, one of the models still “best” describes the upcoming road geometry amongst all three models (i.e., has the highest weight factor). While estimation errors with this “best” road model will be larger than the “optimal” model (where exact variances were known somehow), the errors will be smaller than the conventional Kalman filter approach. Assumption B is not true as road geometry is constantly changing. This violation can be addressed by sliding-window approach, where the likelihood function is calculated using a finite-sequence of innovations. In this aspect, an ad-hoc approach of choosing L immediately previous innovations is used (where L is the length of the sliding-window). Thus the likelihood of measurements up to time k under assumption of model j becomes: ${\lambda }_k^j=p\left(E_k/M_j\right)=\prod _{i=k-I-1}^{k}p\left[v_i^j\right].$

[0070] Fused state vector and error covariance estimates are then obtained using: ${\hat{x}}_k^{\mathrm{fused}}=\sum _{j=1}^3{W}_k^j{\hat{x}}_k^j\mathrm{and}$ $P_k^{\mathrm{fused}}=\sum _{j=1}^3{W}_k^j{P}_k^j.$

[0071] The fused estimate {circumflex over (x)}_k^fusedis used as the best description of upcoming road geometry (in forward range 0-d₂).

[0072] The present invention has three principal “physical” embodiments. The first is an apparatus for dynamically predicting road geometry, typically in the form of a computer system operating software of in the form of a “hard-coded” instruction set. The second physical embodiment is a method, typically in the form of software, operated using a data processing system (computer). The third principal physical embodiment is a computer program product. The computer program product generally represents computer readable code stored on a computer readable medium such as an optical storage device, e.g., a compact disc (CD) or digital versatile disc (DVD), or a magnetic storage device such as a floppy disk or magnetic tape. Other, non-limiting examples of computer readable media include hard disks and flash-type memories. These embodiments will be described in more detail below.

[0073] A block diagram depicting the components of a computer system used in the present invention is provided in FIG. 6. The data processing system 600 comprises an input 602 for receiving sensor data relevant to predicting road geometry. Note that the input 602 may include multiple “ports” for receiving data as well as user input. The output 604 is connected with the processor for providing output. Output may be provided directly to the user through a display or may be provided to other devices or other programs for use therein. The input 602 and the output 604 are both coupled with a processor 606, which may be a general-purpose computer processor or a specialized processor designed specifically for use with the present invention. The processor 606 is coupled with a memory 608 to permit storage of data and software to be manipulated by commands to the processor.

[0074] An illustrative diagram of a computer program product embodying the present invention is depicted in FIG. 7. The computer program product 700 is depicted as an optical disk such as a CD or DVD. However, as mentioned previously, the computer program product generally represents computer readable code stored on any compatible computer readable medium.