State Estimation for a Tractor Semi-trailer System using a Minimum-Energy Filter

In this article we apply a second-order minimum-energy filter based on Lie groups to the problem of parking a truck with a semi-trailer in a docking station. The use of the filter, that exploits the geometry of Lie groups to estimate the truck and trailer pose, is useful to improve the precision of the state and thus perform better controls. We consider two different types of measurements: the first consists of GPS-like devices that detect the positions of the front wheels of the truck and the rear wheels of the trailer, and the second improves the measurement of the rear wheels with the measurement of the pose of the trailer with a LIDAR sensor. The accuracy of the LIDAR is useful for having a better estimate when parking in reverse. We show two simulations with two different datasets.