---
title: 'Hybrid Qf-ES-EKF/UKF: Efficient VIO Fusion'
url: https://www.emergentmind.com/topics/hybrid-quaternion-focused-error-state-ekf-ukf-qf-es-ekf-ukf
type: topic
---

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

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 [2512.17505].

## 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 $\sim$30$\times$ 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 [2512.17505].

Source: https://www.emergentmind.com/topics/hybrid-quaternion-focused-error-state-ekf-ukf-qf-es-ekf-ukf