SE(2)-Constrained SE(3) Wheel Pre-Integration

• The paper presents a novel framework that integrates wheel odometry measurements, constrained in SE(2), into full SE(3) pre-integration to ensure accurate state propagation with nonholonomic constraints.
• It employs Lie group methods and natural equation analysis to derive robust integration schemes that exactly satisfy the geometric submanifolds of constrained motion.
• The methodology enhances sensor fusion and uncertainty propagation by tightly coupling wheel and inertial data within an extended SE₂(3) pose formulation, improving estimation accuracy.

SE(2)-Constrained SE(3) Wheel Pre-Integration refers to the principled integration of wheel (or wheel-like) odometric measurements—whose noise, geometry, and physical restrictions primarily live in the planar Lie group SE(2)—within the full three-dimensional rigid body group SE(3), with attention to the preservation of nonholonomic constraints, the exact satisfaction of geometric submanifolds, and computational compatibility with modern estimation and sensor-fusion pipelines. This topic interconnects the analytics of nonholonomic rolling, Lie group integration, uncertainty propagation on manifolds, variational time-stepping, and the precise exploitation of subgroup structure within the SE(3) configuration space.

1. Geometric Foundations: SE(2) Constraints within SE(3)

Rigid body motion in three dimensions is most naturally described by the Lie group SE(3), which couples rotations (SO(3)) and translations (ℝ³) into screw motions. However, many practical mechanical systems—such as ground vehicles or rolling discs—are subject to geometric and physical constraints that restrict the admissible velocities to a specific subgroup of SE(3). For wheeled systems, these constraints often define a planar SE(2) subgroup (translations in a plane and rotation about a fixed vertical axis).

The significance of SE(3) as the "proper" configuration space is twofold:

• Its exponential map, when applied to a twist within an SE(2)-like subalgebra of se(3), generates a rigid body motion that lies exactly in the constrained subgroup (Mueller et al., 18 Jun 2024).
• Time-integration schemes that update the configuration along the SE(2) subgroup via the SE(3) exponential mapping inherently and exactly respect the nonholonomic constraints imposed by rolling or joint mechanisms, without need for explicit stabilization or projection (Mueller et al., 18 Jun 2024).

This geometric embedding facilitates accurate state propagation and constraint satisfaction in both model-based simulation and real-time estimation contexts.

2. Analytical Integration: The Natural Equation Approach

The integrability of nonholonomic rolling constraints fundamentally hinges on the "natural equation" parameterization of a trajectory, in which the curvature $k(s)$ of the path is given as an explicit function of arc length $s$ (Mityushov, 2011). For a wheel or disc rolling on a plane,

$$\begin{align*} dx_p &= R\cos\psi\, d\varphi, \ dy_p &= R\sin\psi\, d\varphi, \end{align*}$$

where $R$ is wheel radius, $\varphi$ is the rotation angle, and $\psi$ is the precession (heading). With arc length $s = R\varphi$ and curvature $k(s) = d\psi/ds$, the coordinates of the contact point are recovered via nested integrals: $$x_p(s) = \int_0^s \cos\left(\int_0^\tau k(\sigma)\, d\sigma\right) d\tau,\qquad y_p(s) = \int_0^s \sin\left(\int_0^\tau k(\sigma)\, d\sigma\right) d\tau.$$ Wheel encoder or planar odometry yields discrete increments in $s$ and $\psi$. By substituting these into the natural equation, the nonholonomic (no-slip) constraints are satisfied by construction, and the full pose can then be reconstructed in SE(3) by "lifting" the planar motion, using extrinsic calibration to account for offsets between the integrated frame and the vehicle body frame.

This methodology ensures both mathematical tractability (integration via closed-form or numerically robust quadrature) and precise constraint realization (Mityushov, 2011).

3. Lie Group Methods and Manifold-Based Pre-Integration

Pre-integration of wheel and inertial measurements in SE(2)-constrained SE(3) problems leverages the structure of Lie groups for both propagation and uncertainty modeling (Gallo, 2022). On SE(3), discrete integration must employ the exponential map to guarantee the updated pose remains on the manifold: $g_{i+1} = g_i \exp(\Delta\xi),$ where $\Delta\xi$ is the integrated (possibly constrained) twist in se(3), with planar twists for SE(2) constraints (i.e., nonzero yaw and planar translation, zero roll/pitch and out-of-plane components).

This framework extends to the propagation of uncertainty and the implementation of gradient-based optimization and Kalman filtering. The core steps are:

• Use of plus ($\oplus$) and minus ($\ominus$) operators for perturbations in Lie algebra, mapping to and from SE(3).
• For filtering/optimization, Jacobians are defined on the manifold, not in Euclidean parameter space, respecting the underlying geometry.
• By constraining twists to the SE(2) subalgebra, the problem specification and solution remain consistent with real-world vehicle kinematics (Gallo, 2022).

In modern estimation stacks, this allows the pre-integration of wheel odometry increments, possibly joined by inertial measurements, by restricting the increments to a subspace while maintaining global consistency in SE(3).

4. Uncertainty Propagation and SE₂(3) Extended Poses

For the joint modeling of position, orientation, and velocity (the so-called "extended pose"), preintegration on the matrix Lie group SE₂(3) provides a coupled, log-linear representation of error evolution (Barrau et al., 2020, Brossard et al., 2020).

In preintegration,

• The extended pose $T$ is defined as a 5×5 matrix: $T = [R\ |\ V\ |\ X; 0\ \ 1]$, encoding orientation, velocity, and position (Brossard et al., 2020).
• Right-perturbed concentrated Gaussian noise $\xi \sim \mathcal{N}(0,Q)$ is mapped via the exponential to SE₂(3): $T = \bar T\exp(\xi)$.
• The log-linear property of SE₂(3) allows exact linear error propagation in exponential coordinates: $$\delta_{k+1} = \mathrm{Ad}\Big((\Delta Y_k)^{-1}F\Big)\delta_k,$$ where $F$ is a mapping arising from time discretization and $\Delta Y_k$ the measurement increment (Barrau et al., 2020).

This approach is particularly powerful in SE(2)-constrained SE(3) wheel pre-integration since uncertainties—both process and measurement noise—are captured as "banana-shaped" clouds, accurately reflecting the true spread after noncommutative integration. The framework is extensible to capture rotating Earth effects (Coriolis and centrifugal forces) by additional augmentation of the velocity equations and their representation in the extended pose (Barrau et al., 2020, Brossard et al., 2020).

5. Numerical Integration and Variational Methods with Constraints

To numerically integrate the equations of motion (including nonholonomic constraints), variational integrators are employed to guarantee compatibility with both the dynamical symmetries and the imposed geometric constraints (Maciel et al., 2022).

For nonconservative, nonholonomic systems (e.g., a rolling wheel with dissipation), the Herglotz variational principle introduces an action-dependent Lagrangian $L(q, \dot q, z)$ with a supplementary variable $z$, yielding

$\dot z = L(q, \dot q, z).$

Discretization yields a (contact) integrator that preserves dissipation structure: $z_{j+1} - z_j = h L_d(q_j, q_{j+1}, z_j, z_{j+1}),$ subject to discrete nonholonomic constraints $A_d(q_j, q_{j+1}) = 0$ compatible with SE(2) restrictions on translational and rotational motion (Maciel et al., 2022).

Key advantages compared to standard (e.g., Runge–Kutta) methods:

• The nonholonomic constraints are strictly satisfied at each integration step if the discrete configuration updates are restricted to the appropriate SE(3) subgroup (e.g., SE(2)).
• Dissipation is built-in, and the integrator preserves geometric structure longer-term than conventional, force-augmented schemes.

This guarantees that in simulation and estimation, both rolling constraints and geometric integrity are preserved.

6. Practical Estimation, Sensor Fusion, and Robustness

Modern ground vehicle localization stacks tightly fuse SE(2)-constrained odometric increments with high-frequency inertial or LiDAR measurements. In such pipelines, the planar constraints are exploited as follows (Chen et al., 2023):

• Pose estimates are parameterized as

$$R_j = \exp([0~0~\phi_j]^T), \qquad P_j = [d_j^T~0]^T$$

ensuring all candidate configurations are planar.

• Out-of-plane perturbations (vertical translations $\eta_z$ and roll/pitch rotations $\eta_{\theta_{xy}}$) are explicitly incorporated into the measurement noise model:

$$R_j \leftarrow \exp([\eta_{\theta_{xy}}^T~0]^T ) R_j, \qquad P_j \leftarrow P_j + [0~0~\eta_z]^T$$

with their effect propagated via the Jacobian into the pose residual covariance.

• The optimization then minimizes LiDAR and inertial/IMU residuals under this augmented noise, resulting in improved accuracy and robustness, particularly in real-world (non-ideal, bumpy) conditions.

Extensive experimental evidence demonstrates significant reductions in root mean square errors for both translation and rotation—even when real vehicles violate strict SE(2) assumptions—thanks to the proper integration of these perturbations (Chen et al., 2023). The approach is computationally efficient, and the constrained structure leads to better convergence and runtime characteristics.

7. Theoretical and Computational Implications

The theoretical justification for SE(2)-constrained SE(3) wheel pre-integration is grounded in the invariance of subgroup structure under the SE(3) exponential map and the preservation of geometric constraints by group-consistent integration (Mueller et al., 18 Jun 2024):

• If the instantaneous velocity (twist) lies in the Lie subalgebra of a subgroup (e.g., SE(2)), the integrated update remains in that subgroup.
• This ensures that the resulting trajectory exactly satisfies nonholonomic constraints (rolling, no-slip) if those constraints define an SE(3) subgroup (as in ideal wheel/ground contact or lower-pair kinematic joints).
• In the decoupled SO(3) × ℝ³ formulation, constraint drift may occur as translational and rotational updates are independent, which is mitigated by the proper use of SE(3) integrators exploiting subgroup structure (Mueller et al., 18 Jun 2024).

Thus, in both estimation and simulation, choosing the SE(3) configuration space with explicit attention to SE(2) constraints ensures structural correctness, a property not only of theoretical but also practical importance for high-fidelity estimation and control in robotics and multibody systems.

---

This synthesis demonstrates that SE(2)-constrained SE(3) wheel pre-integration is best understood as a confluence of natural equation analytics, Lie group integration on constrained subgroups, robust uncertainty propagation via extended matrix Lie group structures, and the use of geometric variational integrators—all aimed at exact constraint satisfaction and efficient integration within full-state estimation and sensor-fusion pipelines for constrained mechanical systems [(Mityushov, 2011); (Maciel et al., 2022); (Gallo, 2022); (Barrau et al., 2020); (Brossard et al., 2020); (Chen et al., 2023); (Mueller et al., 18 Jun 2024)].