Sub-full Model-Based Heterogeneous Sensor Fusion for Lateral State Estimation of Preceding Target Vehicles

Trajectory planning of an automated vehicle requires behavior knowledge of preceding target vehicles (PTVs), and lateral states, such as lateral velocity and yaw rate are key enablers for precise behavior description. However, lateral states of a PTV can hardly be measured directly by common onboard sensors. In addition, although these states can be transmitted via vehicle to vehicle (V2V), the accuracy is limited by the state holding process during the communication interval. Aiming at improved PTV lateral states, this article considers the estimation of these states using visual and V2V measurements, and proposes a three-stage fusion architecture consisting of input estimator, heterogeneous model-based local estimators, and fusion center. Specifically, to cope with the low rate of the received steering angle, the input estimator is constructed to obtain high rate control inputs. In the local estimator design, unlike the conventional modeling problems mixing control input errors with model errors in process noise, this paper separates them and develops a model adaption algorithm to compensate the time-varying covariance of the errors of the estimated control inputs. Then, to fuse the heterogeneous local estimates in the fusion center, a subfull model-based information matrix filter is designed to address this specific heterogeneous fusion problem, in which the local models are treated as submodels of a common full model. Hardware-in-the-loop experimental results show that the proposed method gives more accurate estimates in comparison with other approaches.