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

摘要

The present invention provides a method and apparatus for estimation of vehicle forward path geometry utilizing an adaptive Kalman filter bank and a two-clothoid road model. The invention provides that each of a plurality of Kalman filters, utilizing the latest available measurement vector Y_k at time k, estimates the state vector X_k and error covariance matrix P_k. The outputs of filter 504a, 504b, and 504c denoted as as X_k^j and P_k^j, are provided to a plurality of weighting elements, which calculate weight factors, W_k^j 506a, 506b, and 506c for each filter. The weight factor of each filter is the probability that the upcoming road geometry matches the road model hypothesized in the filter. After being assigned a weighted value, the weighted value road models are fused in a fusion element 508, and a weighted output road geometry model is provided.