Second-order-optimal filtering on SE(2)×R2 for the Chaplygin sleigh

In the last decades many filters have been proposed to solve the problem of pose estimation that naturally arises in robotics, both for manipulators and mobile robots. The dynamics of mobile robots can be mathematically described by exploiting the theory of Lie groups embedding holonomic and nonholonomic constraints. In this paper we design a second-order-optimal filter for the so-called Chaplygin sleigh, that is a mechanical system with a nonholonomic constraint. In particular we examine the importance to know the exact dynamic equations, and to exploit the underline Lie group structure of the system. Moreover, we investigate the conditions that ensure the preservation of the nonholonomic constraint by properly choosing the affine connection which guarantees that the orthogonal velocity is equal to zero. In this work the sensing system consists of a GPS-like configuration (Global Positioning System) obtained by using two antennas attached to the planar rigid body and an INS-like unit (Inertial Navigation System) to measure the velocities.