A decoupled non-linear observer to estimate the 3D pose of a vehicle

This paper deals with the online observation of the pan and tilt angles of the pose of wheeled vehicles in 3D environements with gentle slopes such as roads in highlands or agricultural terrains. Taking into account the third dimension in the localization process is of prior importance in natural terrains or even in urban environments when the feedback comes from the comparison between vision data with their prediction computed from a 3D Geographical Information System. The position localization data are supposed to be given by à combination of GPS measurement together with a numerical model of the terrain. The paper introduces a kinematical model of wheeled vehicles on slopes and shows that, under the asumption that the slopes are sufficiently gentle, the evolution of the set of variables defined in the local tangent plane are decoupled from the ones tied to the local normal. Focusing the interest onto those elevation variables and after an observability study, an observer is designed that estimates both slope and cross-slope angles from the heigth measurements. Its stabilty is studied by using a suitable Lyapunov function and a tuning rule of the parameters of the observer is given. Results are given from real experimental data.