A translation invariance

In 2 dimensions, we use eq. Simplified rotations In 2 dimensions, the computations can be simplified since rotations commute. Thus, we wrap the computed angle difference so that it always belongs in that range. Furthermore, in all cases that angles are not used geometrically ( e.g. for rotations), we In 3 dimensions, the computation of rotation matrices is more involved than the 2D case. As explained in section 2.1, input trajectories are described by the states In the following equations, we remove time indices to reduce clutter.