Partial Invariant EKF (PIEKF)

• PIEKF is a specialized filter that embeds a subset of state variables on a Lie group to improve localization in scenarios with limited sensor data.
• It fuses partial measurements—such as roll-pitch cues and wheel odometry—to achieve faster convergence and enhanced estimation accuracy under practical constraints.
• By assigning infinite covariance to unobservable directions, PIEKF maintains filter consistency and robust performance in both marine and ground vehicle applications.

The Partial Invariant Extended Kalman Filter (PIEKF) is a specialized variant of the Invariant Extended Kalman Filter (InEKF) that integrates only a subset of the system state—most typically exploiting partial orientation, rotation, and velocity information—within a matrix Lie group error structure. PIEKF is designed to address state estimation scenarios where full orientation or measurement accessibility is unavailable or where structural system constraints warrant a reduced invariance. Key use cases include autonomous surface vessels (ASVs) with horizon-based roll-pitch cues and ground vehicles combining visual-inertial odometry with nonholonomic wheel constraints. PIEKF methods have demonstrated improved estimation accuracy, fast convergence, and superior filter consistency compared to classical or full-group EKF approaches when only partial measurement information or constraints are available (Benham et al., 12 Jun 2025, Hua et al., 2023).

1. Lie Group Formulation and State Definition

PIEKF adopts a tailored group structure, embedding only the invariant sub-blocks of the state, dependent on the sensing modalities and the planar or semi-planar constraints inherent to the platform.

For ASV applications, the filter state is defined on the matrix Lie group $SE_2(3)$: $$SE_2(3)=\left\{\, X=\begin{pmatrix}R & v & p\ 0 & 1 & 0\ 0&0&1\end{pmatrix}\ |\ R\in SO(3),\,v,p\in\mathbb{R}^3\,\right\}$$ where:

• $R$: orientation (body frame to world, $SO(3)$)
• $v$: 3D velocity in body coordinates
• $p$: 3D position in world coordinates

A "seafaring-plane" constraint enforces that roll ($\phi$) and pitch ($\theta$) remain small (near level), with motion predominantly horizontal and yaw ambiguous from the main exteroceptive cue (the visual horizon).

For ground vehicle visual–inertial–wheel odometry (VIWO), PIEKF typically embeds only the rotation and velocity states in a composite group $$SE_2(3)′ = \{(R,v) \mid R\in SO(3), v\in\mathbb{R}^3\}$$, using right-invariant error representations for the uncertainty on IMU and camera states (Hua et al., 2023).

2. Error Propagation and Invariance Properties

The invariant error is defined on the group, either left-invariant (e.g., $\eta^l = X^{-1}\hat{X}$) or right-invariant, as appropriate for the system dynamics and update structure: $\xi^l = \log(\eta^l)^\vee\in\mathbb{R}^9$ Linearization of the error dynamics yields equations of the form: $\dot \xi^l = F(\hat X)\xi^l + G\,w$ where $F$ is the Jacobian of the group error dynamics, and $G$ modulates process noise. The covariance $P$ propagates via the continuous Riccati equation: $\dot P = F\,P + P\,F^T + G\,Q\,G^T$ In practice, both continuous- and discrete-time formulations are handled using standard exponential mapping, matrix exponentiation, and first-order Taylor approximations as dictated by IMU sampling frequencies (Benham et al., 12 Jun 2025, Hua et al., 2023).

By embedding only a subset of the state (e.g., rotation and velocity) in the group structure and representing the remainder (e.g., position, biases) in Euclidean blocks, PIEKF achieves a reduced error coupling and better maintenance of system observability properties.

3. Partial Measurement Fusion and Infinite Covariance Strategy

A key innovation is the principled fusion of partial orientation or rotation–velocity measurements within the invariant error framework, incorporating only those state elements for which strong geometric constraints are directly available from sensors:

• For ASVs: roll and pitch from monocular camera horizon segmentation, disregarding unobservable yaw.
• For ground vehicles: wheel odometry delivers direct forward velocity and planar yaw increments but minimal information about roll or pitch.

The measurement model for roll–pitch uses a projection operator $P(R)$ that retains only the observable subspace: $z = P(h(X))\exp(-w_m^\wedge)$ where $P(R) = R_y(\theta)R_x(\phi)$ and $h(X) = R$. The measurement noise covariance $M$ is constructed as $\mathrm{diag}(\sigma_\phi^2, \sigma_\theta^2, L)$ with $L\rightarrow\infty$ ensuring no spurious yaw correction. The update leverages the Woodbury identity to analytically discard the unobservable yaw direction, maintaining filter consistency and well-conditioned innovation covariance.

For wheel odometry, PIEKF directly fuses pre-integrated yaw increments and forward velocity by constructing algebraic residuals, sidestepping 3D pose pre-integration and associated consistency pitfalls (Hua et al., 2023).

4. Augmentation with Motion Constraints and Outlier Detection

Planar or semi-planar motion constraints are introduced to further regularize state estimation in systems constrained to move in a plane. For example, ground robots with known kinematics enforce: $$z_{rot} = \Lambda \cdot {}^\pi_G R \cdot {}^G_{O_k}R\,e_3 = 0_2$$

$$z_{tran} = e_3^T \cdot {}^\pi_G R \cdot {}^G p_{O_k} = 0$$

where $\Lambda$ selects roll–pitch components and $e_3^T$ enforces zero vertical displacement. Enforcement of such constraints helps maintain roll, pitch, and height errors near zero.

PIEKF also supports adaptive outlier detection strategies, including leveraging velocity estimates for foreground–background segmentation in visual feature tracking (Hua et al., 2023).

5. Observability, Stability, and Theoretical Guarantees

PIEKF inherits the fundamental property of IEKF: autonomous error dynamics on the Lie algebra under linearization, independent of the estimated trajectory. This ensures predictable convergence under Gaussian noise models. By explicitly assigning infinite covariance to unobservable components (e.g., yaw not present in horizon measurements), PIEKF maintains statistical consistency and avoids false confidence. Observability is assured provided that the union of partial orientation (e.g., roll–pitch), yaw (e.g., from GPS heading), and position updates (e.g., from GPS or wheel odometry) collectively span the full pose on $SE_2(3)$. Empirical results indicate PIEKF exhibits faster and more reliable convergence under large initial attitude errors than multiplicative EKF alternatives and eliminates divergence cases observed in non-invariant formulations (Benham et al., 12 Jun 2025).

6. Empirical Performance and Application Domains

Simulation and empirical studies substantiate the advantages of PIEKF relative to both classical multiplicative and full-group IEKF approaches:

• For ASVs (simulated 100 m open-ocean trajectory): position $L_2$ error $\approx 1.7$ m (PIEKF) vs. $\approx 2.5$ m (full-orientation InEKF), orientation RMS error $\approx 0.8^\circ$ (roll/pitch) and $1.2^\circ$ (yaw). No observed divergence and tighter error distribution in both position and orientation (Benham et al., 12 Jun 2025).
• For visual–inertial–wheel odometry: 50-run Monte Carlo yields position RMSE $0.648$ m and orientation RMSE $0.283^\circ$. Real-world urban tests (3–11 km, 100 Hz IMU, wheel, 10 Hz camera) yield lowest RMSE among competing VIWO, VIO, MSCKF-VIWO, and IEKF-VIWO approaches. Yaw errors remain within $3\sigma$ over long ranges; roll, pitch, and height errors are strongly controlled on planar ground (Hua et al., 2023).

PIEKF delivers computational efficiency ($\sim5.9$ ms per 100 Hz update on commodity hardware), enabling real-time deployment for marine and mobile robotics applications with significant partial-measurement structure.

7. Comparative Features and Practical Implications

Distinctive features of PIEKF relative to full InEKF and traditional EKF variants include:

• Restriction of the group embedding to the invariantly-influenced state block, preventing covariance inconsistency from partial or ambiguous measurements.
• Analytical replacement of unobservable measurement components with infinite variances, ensuring correct filter confidence allocation.
• Direct fusion of partial cues (roll/pitch, forward velocity, yaw increment) without full 3D measurement preintegration, avoiding computational and modeling complexity.
• Rigorous observability and convergence properties maintained under realistic motion constraints and actuator/sensor uncertainties.

These traits make PIEKF particularly suitable for navigation of vehicles with partial or degenerate state observability, such as surface vessels observing a receding horizon or wheeled robots on planar terrain, where fusion of diverse sensor cues and kinematic constraints is paramount for robust localization (Benham et al., 12 Jun 2025, Hua et al., 2023).