Quaternion SUKF Orientation Refinement

• Quaternion-focused SUKF orientation refinement is a filtering technique that integrates quaternion manifolds and sigma-point methods to nonlinearly enhance 3D rigid body orientation estimation.
• It employs specialized boxplus/boxminus operators and tangent-space sampling to maintain minimal-error representation and numerical stability in visual-inertial navigation.
• Hybrid architectures combining ESKF and SUKF achieve up to 57% RMSE improvement while reducing runtime by 48%, ensuring robust and efficient performance.

Quaternion-focused SUKF orientation refinement refers to a class of filtering techniques that employ quaternion representation of attitude in the Scaled Unscented Kalman Filter (SUKF) structure to nonlinearly refine orientation estimates of a 3D rigid body, particularly within visual-inertial navigation or odometry frameworks. Central to these approaches are (i) the direct treatment of attitude on the unit 3-sphere manifold ($\mathbb{S}^3$ or $\text{SO}(3)$), (ii) special geometric operators for quaternion increments, and (iii) sigma-point methods adapted for proper propagation and update of covariance and state. Modern hybrid architectures further embed quaternion-focused SUKF steps within overarching error-state or full-manifold frameworks to balance computational efficiency and estimation accuracy (Ghanizadegan et al., 2024, Asil et al., 19 Dec 2025, Solà, 2017, Hanson, 2018).

1. State Space and Quaternion Manifold Operators

Quaternion-focused SUKF orientation refinement operates on composite state spaces such as $$\mathbb{S}^{3} \times \mathbb{R}^{3} \times \mathbb{R}^{3}$$ or $$\text{SO}(3) \times \mathbb{R}^{3p} \times \mathbb{R}^{3v} \times \mathbb{R}^{3b_a} \times \mathbb{R}^{3b_g}$$, depending on the fusion scenario. The orientation is encoded by a unit quaternion $q \in \mathbb{S}^3$ ($\|q\| = 1$), with quaternion multiplication $\otimes$ and specialized boxplus/boxminus operators for minimal-error representations:

• Augmentation: $$q \oplus \delta r := \text{Exp}_n(\tfrac{1}{2}\delta r)\otimes q$$, $\delta r \in \mathbb{R}^{3}$
• Local error: $$q_1 \ominus q_2 := 2\cdot \text{Log}_n(q_1\otimes q_2^{-1}) \in \mathbb{R}^3$$ Other state components (position, velocity, biases) remain in standard vector spaces. These operators ensure minimal error parametrization and maintain manifold consistency throughout sigma-point motions and correction cycles (Ghanizadegan et al., 2024, Asil et al., 19 Dec 2025, Solà, 2017).

2. Kinematics and Sigma-Point Construction

Orientation propagation follows nonlinear quaternion kinematics:

$\text{SO}(3)$0

with $\text{SO}(3)$1 and discrete integration over timestep $\text{SO}(3)$2 via the 4×4 quaternion-update matrix

$\text{SO}(3)$3

where

$\text{SO}(3)$4

is typically computed in closed form or via truncated Taylor expansion.

To respect the underlying manifold in UKF or SUKF construction, sigma-points are generated in the minimal tangent space—e.g., sampling $\text{SO}(3)$5 in $\text{SO}(3)$6 and lifting to quaternions via the exponential map $\text{SO}(3)$7 for each point:

• For hybrid error-state ESKF/SUKF: generate $\text{SO}(3)$8 sigma-points $\text{SO}(3)$9 from the $$\mathbb{S}^{3} \times \mathbb{R}^{3} \times \mathbb{R}^{3}$$0 covariance $$\mathbb{S}^{3} \times \mathbb{R}^{3} \times \mathbb{R}^{3}$$1; each yields $$\mathbb{S}^{3} \times \mathbb{R}^{3} \times \mathbb{R}^{3}$$2 (Asil et al., 19 Dec 2025).
• For full-manifold QNUKF: $$\mathbb{S}^{3} \times \mathbb{R}^{3} \times \mathbb{R}^{3}$$3 sigma-points with $$\mathbb{S}^{3} \times \mathbb{R}^{3} \times \mathbb{R}^{3}$$4 for the augmented state (Ghanizadegan et al., 2024).

Weights are set according to the Julier–Uhlmann prescription:

$$\mathbb{S}^{3} \times \mathbb{R}^{3} \times \mathbb{R}^{3}$$5

with $$\mathbb{S}^{3} \times \mathbb{R}^{3} \times \mathbb{R}^{3}$$6, $$\mathbb{S}^{3} \times \mathbb{R}^{3} \times \mathbb{R}^{3}$$7 for Gaussianity. Proper quaternion normalization occurs after each update to mitigate numerical drift (Ghanizadegan et al., 2024, Asil et al., 19 Dec 2025, Solà, 2017).

3. Prediction and Update Mechanisms

Each sigma-point is propagated through nonlinear IMU-driven kinematics, then retracted to the predicted nominal via the log map:

$$\mathbb{S}^{3} \times \mathbb{R}^{3} \times \mathbb{R}^{3}$$8

$$\mathbb{S}^{3} \times \mathbb{R}^{3} \times \mathbb{R}^{3}$$9

The predicted mean on the manifold is obtained by quaternion-weighted averaging via the principal eigenvector of $$\text{SO}(3) \times \mathbb{R}^{3p} \times \mathbb{R}^{3v} \times \mathbb{R}^{3b_a} \times \mathbb{R}^{3b_g}$$0 rather than naive component-wise means. Update steps correct the orientation using the minimal (tangent-space) innovation, applying state correction via:

$$\text{SO}(3) \times \mathbb{R}^{3p} \times \mathbb{R}^{3v} \times \mathbb{R}^{3b_a} \times \mathbb{R}^{3b_g}$$1

where $$\text{SO}(3) \times \mathbb{R}^{3p} \times \mathbb{R}^{3v} \times \mathbb{R}^{3b_a} \times \mathbb{R}^{3b_g}$$2 is the innovation in $$\text{SO}(3) \times \mathbb{R}^{3p} \times \mathbb{R}^{3v} \times \mathbb{R}^{3b_a} \times \mathbb{R}^{3b_g}$$3, typically obtained from the minimal geodesic between predicted and measured visual or IMU-derived quaternion. Covariances are symmetrized post-update for numerical stability (Ghanizadegan et al., 2024, Asil et al., 19 Dec 2025).

The measurement update leverages all stereo/visual correspondences as a single stacked measurement vector $$\text{SO}(3) \times \mathbb{R}^{3p} \times \mathbb{R}^{3v} \times \mathbb{R}^{3b_a} \times \mathbb{R}^{3b_g}$$4, leading to over-constrained attitude corrections and greater robustness to outliers. The associated linearized observation model relates observed features to the quaternion-controlled pose via $$\text{SO}(3) \times \mathbb{R}^{3p} \times \mathbb{R}^{3v} \times \mathbb{R}^{3b_a} \times \mathbb{R}^{3b_g}$$5 (Ghanizadegan et al., 2024).

4. Orientation Frame Alignment Refinement

Refinement of attitude updates within the SUKF or in a post-processing step can leverage quaternion frame alignment methods, specifically, the minimization of summed squared geodesic (or chordal) distances between estimated and reference frames:

$$\text{SO}(3) \times \mathbb{R}^{3p} \times \mathbb{R}^{3v} \times \mathbb{R}^{3b_a} \times \mathbb{R}^{3b_g}$$6

A tractable surrogate employs the chordal metric, reducing to a maximization over the principal eigenvector of aggregate tensors:

$$\text{SO}(3) \times \mathbb{R}^{3p} \times \mathbb{R}^{3v} \times \mathbb{R}^{3b_a} \times \mathbb{R}^{3b_g}$$7

Optimal $$\text{SO}(3) \times \mathbb{R}^{3p} \times \mathbb{R}^{3v} \times \mathbb{R}^{3b_a} \times \mathbb{R}^{3b_g}$$8 is the eigenvector for the largest eigenvalue of $$\text{SO}(3) \times \mathbb{R}^{3p} \times \mathbb{R}^{3v} \times \mathbb{R}^{3b_a} \times \mathbb{R}^{3b_g}$$9, which can be obtained analytically via quartic (Cardano) solutions or directly via numerical symmetric eigensolvers (Hanson, 2018). This procedure is entirely consistent with rotation averaging, and its integration into SUKF updating provides bias-free orientation means (Ghanizadegan et al., 2024, Hanson, 2018).

5. Hybrid and Computationally Efficient Architectures

Recent developments combine error-state ESKF propagation with selective SUKF orientation refinement. The error-state is propagated for all states using ESKF (Jacobian-based updates), while SUKF is applied to the $q \in \mathbb{S}^3$0 orientation covariance block only, utilizing sigma-point prediction and correction in the $q \in \mathbb{S}^3$1 tangent space. The refined orientation block is re-injected into the full state covariance, preserving the efficiency of ESKF's $q \in \mathbb{S}^3$2 complexity while matching the accuracy of SUKF for attitude. This yields approximately 48% reduction in runtime compared to full SUKF while preserving orientation error improvements (57% RMSE reduction on benchmarks) (Asil et al., 19 Dec 2025). Quaternion normalization and block covariance replacement are enforced after every quaternion update, guaranteeing unit norm constraints and manifold coherence (Asil et al., 19 Dec 2025, Solà, 2017).

Approach	Principal Orientation Update	Computational Complexity	RMSE Improvement
Standard ESKF	Jacobian Linearization	$q \in \mathbb{S}^3$3	Baseline
Full SUKF	Unscented Transform (All)	$q \in \mathbb{S}^3$4	+57% orientation
Hybrid ESKF+SUKF (Qf)	UKF on SO(3), ESKF rest	$q \in \mathbb{S}^3$5	≃ Full SUKF, ~48% faster

6. Experimental Benchmarking and Tuning Strategies

On the EuRoC MAV and V1_02_medium datasets, quaternion-focused SUKF methods achieve steady-state orientation error $q \in \mathbb{S}^3$6 rad (2.8°) within seconds and combined RMSE $q \in \mathbb{S}^3$7 (vs EKF $q \in \mathbb{S}^3$8). In hybrid architectures, rotation RMSE is improved by 57% and position error by 49% in challenging conditions, achieving SUKF-level orientation accuracy at half the runtime. Noise covariance for visual measurements is empirically tuned (e.g., $q \in \mathbb{S}^3$9) based on stereo-triangulation residuals; the use of all feature correspondences further mitigates sensitivity to outliers. Symmetrization of the estimated covariance $\|q\| = 1$0 is standard to control numerical drift (Ghanizadegan et al., 2024, Asil et al., 19 Dec 2025).

A plausible implication is that the quaternion-focused SUKF refines orientation even under ambiguous or partially degraded visual conditions, provided sufficiently many feature correspondences pass robustness checks.

7. Connections, Special Considerations, and Theoretical Context

Quaternion-focused SUKF orientation refinement captures rotation geometry exactly via exponential and logarithm maps, avoids first-order linearization bias inherent in EKF approaches, and brings theoretical rigor to mean estimation via eigen-analysis of quaternion aggregations. The method is robust to the double-cover ($\|q\| = 1$1), and practical implementations ensure sign-unwrapping for minimal deviations (Solà, 2017, Hanson, 2018).

The approach is most advantageous in high-dynamic, GPS-denied, and visual-inertial navigation scenarios, notably for unmanned aerial vehicles (UAVs), where attitude coupling and nonlinearity dominate estimation uncertainty (Ghanizadegan et al., 2024, Asil et al., 19 Dec 2025). Sophisticated quaternion averaging is critical in over-constrained or outlier-prone settings; failing to use principal eigenvector averaging can introduce estimator bias.

Hybridization and tangent-space sampling play a central role in enabling efficient, scalable deployment of these techniques without sacrificing statistical optimality for orientation blocks (Asil et al., 19 Dec 2025). The general methodology is broadly extensible to any application requiring high-precision attitude refinement under nonlinear process and measurement models.

References:

• (Ghanizadegan et al., 2024, Asil et al., 19 Dec 2025, Solà, 2017, Hanson, 2018)