Hybrid Qf-ES-EKF/UKF: Efficient VIO Fusion

• The paper introduces a hybrid sensor fusion filter that integrates ESKF for full state propagation with a selective SUKF for quaternion-based orientation refinement.
• This method achieves high orientation accuracy while reducing computational cost by over 10× compared to a full SUKF, making it ideal for real-time UAV applications.
• Adaptive visual measurement covariance further enhances robustness by dynamically tuning noise models based on real-time image quality metrics, significantly improving rotation and position estimates.

A Hybrid Quaternion-focused Error-State EKF/UKF (Qf-ES-EKF/UKF) is a sequential sensor fusion technique that combines the computational efficiency of the Error-State Extended Kalman Filter (ESKF) for global state propagation with the high-fidelity nonlinear estimation of a Scaled Unscented Kalman Filter (SUKF) applied selectively to a rotation-critical subspace, specifically the orientation quaternion. The architecture was introduced for robust, adaptive visual-inertial odometry (VIO) in Unmanned Aerial Vehicles (UAVs), with mechanisms for dynamic sensor reliability assessment and measurement noise adaptation to enhance pose estimation under challenging conditions (Asil et al., 19 Dec 2025).

1. State and Error-State Representations

The Qf-ES-EKF/UKF framework utilizes a 16-dimensional nominal state vector:

• Quaternion $q \in \mathrm{SO}(3)$ (4 × 1, body-to-world orientation)
• Velocity $v \in \mathbb{R}^3$ (world frame)
• Position $p \in \mathbb{R}^3$ (world frame)
• Accelerometer bias $b_a \in \mathbb{R}^3$
• Gyroscope bias $b_g \in \mathbb{R}^3$

In vector form:

$$x = [q^\top, v^\top, p^\top, b_a^\top, b_g^\top]^\top, \qquad \dim(x) = 16$$

The associated 15-dimensional error-state vector is:

• Attitude error $\theta \in \mathbb{R}^3$ (so(3) vector)
• Velocity error $\delta v \in \mathbb{R}^3$
• Position error $\delta p \in \mathbb{R}^3$
• Accelerometer bias error $\delta b_a \in \mathbb{R}^3$
• Gyroscope bias error $\delta b_g \in \mathbb{R}^3$

With

$$\delta x = [ \theta^\top, \delta v^\top, \delta p^\top, \delta b_a^\top, \delta b_g^\top ]^\top, \qquad \dim(\delta x) = 15$$

State correction and composition employ the “ ⊕ ” operator for quaternion error injection:

• $$q_\text{true} = q_\text{nom} \otimes \exp_q(\theta)$$
• $$exp_q(\theta) = [ \cos(\|\theta\|/2); (\theta/\|\theta\|) \sin(\|\theta\|/2) ]$$

2. Error-State EKF Propagation

The core propagation leverages ESKF for the full state. The continuous-time nominal dynamics are:

• $\dot{q} = \frac{1}{2} \Omega(\omega_m - b_g) q$
• $\dot{v} = R(q) (a_m - b_a) + g$
• $\dot{p} = v$
• $$\dot{b}_a = -\frac{1}{\tau_a}(b_a - b_{a0}) + w_{b_a}$$
• $$\dot{b}_g = -\frac{1}{\tau_g}(b_g - b_{g0}) + w_{b_g}$$

Here, $\Omega(\omega)$ is the quaternion multiplication matrix, and $R(q)$ is the rotation matrix from $q$.

The 15-dimensional error-state is propagated with a linear time-varying system:

$$\delta \dot{x} = A(t) \delta x + G(t) n,\quad n \sim \mathcal{N}(0,Q_c)$$

Discretization uses the Van-Loan method on a $30 \times 30$ block matrix to compute the state-transition matrix $F$ and discrete process noise $Q_d$.

The ESKF propagation step at each IMU sample is:

• $$\hat{x}_{k|k-1} = \text{propagate\_nominal}(\hat{x}_{k-1|k-1}, u_k, \Delta t)$$
• $P_{k|k-1} = F P_{k-1|k-1} F^\top + Q_d$

3. Quaternion-Focused SUKF Orientation Refinement

After the global ESKF propagation, a targeted SUKF refinement step is applied solely to the $3 \times 3$ orientation-error covariance block $P_{\theta\theta}$. The process is as follows:

• Generate $2n_h + 1$ sigma points in the orientation subspace ($n_h = 3$) using scaled unscented transform parameters $(\alpha, \beta, \kappa)$.
• Compute Cholesky decomposition $S = \operatorname{chol}(P_{\theta\theta})$, define $\Theta^0 = 0$, and construct $\Theta^i$.
• Map each sigma point to quaternion perturbations via $exp_q(\Theta^i)$ and apply to the nominal quaternion.
• Each perturbed quaternion is propagated through IMU integration for orientation only.
• Retract propagated quaternions to the nominal orientation using the log map, yielding error vectors $\Theta^i_\text{prop}$.
• Refine $P_{\theta\theta}$ using the sigma-point statistics:

$$P_{\theta\theta}^{SU} = \sum_{i=0}^{2n_h} w_c^i [\Theta^i_\text{prop} - \bar{\Theta}][\Theta^i_\text{prop} - \bar{\Theta}]^\top$$

• Replace original $P_{\theta\theta}$ in the error covariance with $P_{\theta\theta}^{SU}$, leaving other submatrices unchanged.

4. Sequential Hybrid Propagation and Update

The complete Qf-ES-EKF/UKF filter loop includes:

1. ESKF predict for full nominal state and error covariance.
2. Extract $P_{\theta\theta}$ and refine with SUKF in the quaternion subspace.
3. When a visual or zero-velocity measurement becomes available, update the full error state with the standard EKF measurement update using measurement Jacobian $H$ and noise $R$.
4. Inject estimated error $\delta \hat{x}$ into the nominal state and reset the error state.

This sequence achieves computational savings by limiting the costly SUKF step to a three-dimensional subspace while maintaining full ESKF update for all state variables.

5. Adaptive Visual Measurement Covariance

Measurement noise adaptation is governed by a dynamic sensor confidence score derived from real-time image quality metrics, including:

• Inverted (normalized) entropy, normalized blur, pose-chi$^2$ error, culled keyframes ratio for position covariance ($\sigma_p^2$)
• Derivative metrics (intensity, blur, chi$^2$, keyframes) for velocity covariance ($\sigma_v^2$)

A combined utility measure ($U_p$ or $U_v$) is calculated as the maximum among individual metrics (static or dynamic), transformed through a CASEF activation function:

$$\text{CASEF}(x; s) = \frac{\exp(s \cdot \operatorname{clip}(x, 0, 1)) - 1}{\exp(s) - 1}$$

Covariance values are selected using a thresholding and interpolation scheme based on confidence scores, ensuring higher noise is assigned to outlier or low-confidence measurements.

The visual measurement covariance in the EKF update is then:

$$R_\text{vis} = \operatorname{diag}(\sigma_p^2 \cdot I_3,\, \sigma_v^2 \cdot I_3)$$

6. Computational Complexity

The computational complexity is as follows:

Method	Complexity	Steps Included
Full ESKF	$O(n^2)$	Full state prediction
Full SUKF (16D)	$O(n^3)$	Sigma point expansion
Qf-ES-EKF/UKF (hybrid, 3D UKF)	$O(n^2 + n_h^3)$	ESKF + 3D SUKF

With $n=15$, $n_h=3$, the hybrid yields $\sim$252 operations vs. $\sim$3375 for full SUKF—a more than $10\times$ reduction in state-propagation step. This enables real-time VIO on UAVs where high-fidelity orientation estimation is critical, without the prohibitive cost of applying a full SUKF to the complete state.

7. Experimental Results and Performance

Evaluation was performed on the EuRoC MAV (MH01–MH05) dataset:

• Rotation accuracy (quaternion RMSE, degrees, avg. over all sequences):
  • ESKF: 1.42°
  • SUKF: 0.85° $(\downarrow 40\%)$
  • Qf-ES-EKF/UKF (no adaptive covariance): 0.74° $(\downarrow 48\%)$
  • Adaptive Qf-ES-EKF/UKF: 0.61° $(\downarrow 57\%)$
• Position accuracy (ATE, meters; MH04 + MH05):
  • ESKF: 0.56 m
  • SUKF: 0.56 m
  • Qf-ES-EKF/UKF: 0.53 m
  • Adaptive Qf-ES-EKF/UKF: 0.26 m $(\downarrow 53\%)$
• Timing (filter only, CPU, s):
  • ESKF: 2.44 s (RTF$\approx$0.92)
  • SUKF: 147.2 s (RTF$\approx$0.092)
  • Qf-ES-EKF/UKF: 76.9 s (RTF$\approx$0.175)

The hybrid approach achieves SUKF-level orientation accuracy at approximately 48% lower computational cost compared to a full SUKF, and only %%%%69$2n_h + 1$70%%%% slower than plain ESKF. This attests to its suitability for real-time UAV VIO, especially in scenarios that are rotation-critical and subject to variable sensor reliability (Asil et al., 19 Dec 2025).