MARID ESKF
MARID Error-State Extended Kalman Filter
GPS-Denied Sensor Fusion — Design Document
Table of Contents
- Motivation
- State Vectors
- Process Model
- Process Jacobian F
- Process Noise Q
- Measurement Models H
- Update Step Mechanics
- Full ESKF Cycle
- Implementation Notes
- Future Work: Learning-Augmented ESKF
1. Motivation
Reliable state estimation in GPS-denied environments is a core challenge for autonomous aerial vehicles. Without satellite positioning, a UAV must infer its full navigation state — position, velocity, and attitude — exclusively from onboard sensors, each carrying its own noise characteristics, failure modes, and operational envelope.
MARID is a hydrogen-powered UAV with a single aft thruster, rotating lifting wings, and four independently-actuated control surfaces. Its operating profile compounds the estimation challenge: hydrogen usage continuously depletes onboard mass, altering the thrust-to-weight ratio and aerodynamic trim throughout flight; the rotating wings couple angle-of-attack changes to both lift and drag in ways that standard rigid-body models do not capture; and the target deployment environments — below the tree canopy, inside structures, and in areas with compromised RF infrastructure — exclude GPS entirely.
A common approach to multi-sensor fusion is the scalar complementary filter, which blends each measurement with a prior estimate using a fixed weight:
$$z_\text{fused} = w \cdot z_\text{sensor} + (1-w) \cdot z_\text{prior}$$
The weights $w$ are tuned offline and remain constant at runtime. This has two fundamental limitations. First, it carries no uncertainty: when a sensor degrades — LiDAR scan-matching fails at altitude, optical flow saturates in low-texture environments, or sonar returns become ambiguous — the filter continues to weight it identically, with no mechanism to reflect reduced confidence. Second, it is decoupled: each sensor corrects only its directly associated state, and correlations between states are discarded.
The Error-State Extended Kalman Filter (ESKF) addresses both limitations through principled uncertainty propagation:
- The Kalman gain $\mathbf{K}$ is computed dynamically from the ratio of state uncertainty $\mathbf{P}$ to measurement noise $\mathbf{R}$ — a degraded sensor automatically receives less weight
- The full $14 \times 14$ covariance matrix propagates uncertainty across all correlated states simultaneously
- The navigation state is augmented with vehicle-specific physical parameters — aerodynamic drag coefficient $k_d$ and hydrogen mass $m_{h_2}$ — so the dynamics model and state estimator are unified
- Mahalanobis gating provides statistically principled outlier rejection before every measurement update
Why ESKF over standard EKF?
Quaternion orientation $\mathbf{q} \in \mathbb{R}^4$ is subject to the unit-norm constraint $\|\mathbf{q}\| = 1$. A standard EKF applies additive corrections $\hat{\mathbf{q}} \leftarrow \hat{\mathbf{q}} + \delta\mathbf{q}$, which violates this constraint and requires renormalization that breaks the filter's covariance assumptions. The ESKF avoids this by splitting the state into two layers:
- Nominal state $\hat{\mathbf{x}}$: propagated with full nonlinear physics; carries the unit quaternion
- Error state $\delta\mathbf{x}$: the small perturbation estimated by the EKF; uses a minimal 3-component rotation vector $\delta\boldsymbol{\theta}$ (one degree of freedom fewer than the quaternion)
Corrections are injected multiplicatively into $\hat{\mathbf{q}}$ via quaternion product, preserving the unit-norm constraint exactly, then the error state is reset to zero.
2. State Vectors
2.1 Nominal State (15 elements)
$$\hat{\mathbf{x}} = \begin{bmatrix} \mathbf{p} \\ \mathbf{v} \\ \mathbf{q} \\ \mathbf{b}_a \\ k_d \\ m_{h_2} \end{bmatrix} \in \mathbb{R}^{15}$$
| Symbol | Description | Dim |
|---|---|---|
| $\mathbf{p} = [p_x, p_y, p_z]^\top$ | Position in odom frame (ENU) | 3 |
| $\mathbf{v} = [v_x, v_y, v_z]^\top$ | Velocity in odom frame | 3 |
| $\mathbf{q} = [q_w, q_x, q_y, q_z]^\top$ | Unit quaternion, body→world | 4 |
| $\mathbf{b}_a = [b_{ax}, b_{ay}, b_{az}]^\top$ | Accelerometer bias (body frame) | 3 |
| $k_d$ | Aerodynamic drag coefficient | 1 |
| $m_{h_2}$ | Hydrogen mass remaining (kg) | 1 |
Note on $m_{h_2}$: Included as a nominal state because H₂ usage rate continuously changes total mass, which enters both the thrust and lift models through $1/m$. However, if tank pressure sensor accuracy proves sufficient in testing, $m_{h_2}$ may be demoted to a direct measurement input (fed into u or used as a known scalar) rather than an estimated state, reducing the error state to $\mathbb{R}^{13}$.
2.2 Error State (14 elements)
$$\delta\mathbf{x} = \begin{bmatrix} \delta\mathbf{p} \\ \delta\mathbf{v} \\ \delta\boldsymbol{\theta} \\ \delta\mathbf{b}_a \\ \delta k_d \\ \delta m_{h_2} \end{bmatrix} \in \mathbb{R}^{14}$$
$$\delta\mathbf{x} = [\,\underbrace{\delta p_x,\,\delta p_y,\,\delta p_z}_{3},\;\underbrace{\delta v_x,\,\delta v_y,\,\delta v_z}_{3},\;\underbrace{\delta\theta_x,\,\delta\theta_y,\,\delta\theta_z}_{3},\;\underbrace{\delta b_{ax},\,\delta b_{ay},\,\delta b_{az}}_{3},\;\underbrace{\delta k_d}_{1},\;\underbrace{\delta m_{h_2}}_{1}\,]^\top$$
$\delta\boldsymbol{\theta} \in \mathbb{R}^3$ is the axis-angle rotation error. The true quaternion is recovered as:
$$\mathbf{q}_\text{true} = \hat{\mathbf{q}} \otimes \begin{bmatrix} 1 \\ \delta\boldsymbol{\theta}/2 \end{bmatrix} \quad \text{(first-order approximation)}$$
The covariance matrix $\mathbf{P} \in \mathbb{R}^{14 \times 14}$ corresponds to $\delta\mathbf{x}$.
Note on dimensions: The nominal state has 15 elements; the error state has 14. The difference of 1 comes from replacing the 4-component quaternion $\mathbf{q}$ with the 3-component rotation error $\delta\boldsymbol{\theta}$ (the unit-norm constraint removes one degree of freedom).
2.3 Process Inputs $\mathbf{u}$ (not estimated)
$$\mathbf{u} = [T,\;\omega_1,\;\omega_2,\;\omega_3,\;\delta_{lw},\;\delta_{rw},\;\delta_{tl},\;\delta_{tr}]^\top \in \mathbb{R}^8$$
| Symbol | Description | Source |
|---|---|---|
| $T$ | Thrust force (N) | /model/marid/.../cmd_thrust |
| $\boldsymbol{\omega}$ | Angular velocity (body frame, rad/s) | IMU gyroscope |
| $\delta_{lw}$ | Left main wing deflection (rad) | /joint_states: left_wing_joint |
| $\delta_{rw}$ | Right main wing deflection (rad) | /joint_states: right_wing_joint |
| $\delta_{tl}$ | Left V-tail deflection (rad) | /joint_states: tail_left_joint |
| $\delta_{tr}$ | Right V-tail deflection (rad) | /joint_states: tail_right_joint |
MARID has four control surfaces across two pairs with distinct aerodynamic roles:
| Pair | Joints | $C_{L,ctrl}$ | Symmetric effect | Differential effect |
|---|---|---|---|---|
| Main wings | left/right_wing_joint | 2.2 each | Pitch + lift | Roll |
| V-tail | tail_left/right_joint | 0.4 each | Pitch | Yaw |
All four individual angles are passed in $\mathbf{u}$. Inside the prediction step the symmetric averages are computed as $\bar{\delta}_{wing}=(\delta_{lw}+\delta_{rw})/2$ and $\bar{\delta}_{tail}=(\delta_{tl}+\delta_{tr})/2$ for use in $C_L$ and $\alpha_{eff}$. Drag uses the sum of absolute values of individual angles (see §2.5), which correctly accounts for drag even when surfaces deflect in opposite directions (differential mode: $\delta_{tl}=-\delta_{tr} \Rightarrow \bar{\delta}_{tail}=0$ but drag is nonzero). The differential deflections drive roll and yaw moments, tracked through the gyroscope → Madgwick → ESKF attitude path.
Note on $\mathbf{a}_{imu}$: The IMU accelerometer specific force is received at each tick but does not enter the prediction step. It enters the filter as a measurement update (§6.10). In strict notation it belongs with $\mathbf{z}$, not $\mathbf{u}$.
2.4 Notation (recomputed each step)
$$\mathbf{C} \triangleq \mathbf{C}(\hat{\mathbf{q}}), \quad \mathbf{c}_i \triangleq \mathbf{C}\hat{\mathbf{e}}_i, \quad \mathbf{v}_b \triangleq \mathbf{C}^\top \hat{\mathbf{v}}, \quad v_{bx} \triangleq \hat{\mathbf{e}}_1^\top \mathbf{v}_b$$
Aerodynamic model (computed each prediction step):
$$\bar{\delta}_{wing} \triangleq \tfrac{1}{2}(\delta_{lw}+\delta_{rw}), \qquad \bar{\delta}_{tail} \triangleq \tfrac{1}{2}(\delta_{tl}+\delta_{tr})$$
$$\alpha_{eff} \triangleq \theta_{pitch} + \bar{\delta}_{wing}$$
$$C_L \triangleq C_{L0} + C_{L\alpha}\,\alpha_{eff} + 2\,C_{L,ctrl}^{tail}\,\bar{\delta}_{tail}$$
$$C_{D,nom} \triangleq C_{D0} + k_{ind}\,C_L^2 + C_{D,ctrl}^{wing}\bigl(|\delta_{lw}|+|\delta_{rw}|\bigr) + C_{D,ctrl}^{tail}\bigl(|\delta_{tl}|+|\delta_{tr}|\bigr)$$
$$k_{d,nom} \triangleq \tfrac{1}{2}\rho S\,C_{D,nom}, \quad k_{d,total} \triangleq k_{d,nom} + \delta k_d, \quad F_{lift} \triangleq \tfrac{1}{2}\rho S\,C_L\,v_{bx}^2$$
$$\sigma \triangleq T - k_{d,total}\,\rho\, v_{bx} |v_{bx}|, \quad m \triangleq m_\text{empty} + \hat{m}_{h_2}$$
Constants from marid_gazebo.xacro: $C_{L0}=0.15188$, $C_{L\alpha}=5.015$, $C_{L,ctrl}^{tail}=0.4$, $C_{D0}=0.04$, $C_{D,ctrl}^{wing}=0.06$, $C_{D,ctrl}^{tail}=0.02$, $AR=6.5$, $e=0.97$, $S=2.0\,\text{m}^2$.
Notation guide: $\mathbf{C}$ is the Direction Cosine Matrix (rotation matrix, body→world). $\mathbf{c}_i$ is its $i$-th column. $\mathbf{R}$ (capital, elsewhere) denotes the measurement noise covariance — a completely different object. $\boldsymbol{\nu}$ denotes the innovation (residual).
3. Process Model
The process model is dynamics-driven: thrust and drag physics propagate velocity; gyroscope kinematics propagate orientation. The IMU accelerometer is used only in the measurement update (Section 6.10).
Design note: physics-driven vs. IMU-driven prediction.
Most ESKFs drive the velocity prediction with the raw accelerometer ($\dot{\mathbf{v}} = \mathbf{C}\mathbf{a}_{imu} + \mathbf{g}$). Here the thrust–drag physics model is used instead.
- Advantage: accelerometer bias $\mathbf{b}_a$ is not in the prediction path, so propagation is not corrupted by bias before it is estimated. There is no risk of double-counting the accelerometer (once in propagation, once as a measurement).
- Risk: propagation quality depends on accurate thrust command $T$, drag coefficient $k_d$, and hydrogen mass $m_{h_2}$. The filter compensates via the augmented state. Lift and structured drag are explicitly modeled via wing joint angles from/joint_states; remaining unmodeled forces (sideslip, gusts) still contribute to process noise $q_v$.
3.1 Continuous-Time Dynamics $f(\hat{\mathbf{x}}, \mathbf{u})$
$$\dot{\mathbf{p}} = \mathbf{v}$$
$$\dot{\mathbf{v}} = \frac{\sigma}{m}\,\mathbf{c}_1 + \mathbf{g}, \quad \mathbf{g} = [0,\, 0,\, -g]^\top \text{ (ENU)}$$
$$\dot{\mathbf{q}} = \frac{1}{2}\,\hat{\mathbf{q}} \otimes \begin{bmatrix} 0 \\ \boldsymbol{\omega} \end{bmatrix}$$
$$\dot{\mathbf{b}}_a = \mathbf{0} \quad \text{(random walk)}$$
$$\dot{k}_d = 0 \quad \text{(random walk)}$$
$$\dot{m}_{h_2} = -\text{SFC} \cdot T$$
3.2 Discrete Nominal State Propagation
Applied at each IMU tick (timestep $\Delta t$):
$$\mathbf{p} \leftarrow \mathbf{p} + \mathbf{v}\,\Delta t$$
$$\mathbf{v} \leftarrow \mathbf{v} + \left(\frac{\sigma}{m}\,\mathbf{c}_1 + \mathbf{g}\right)\Delta t$$
$$\hat{\mathbf{q}} \leftarrow \hat{\mathbf{q}} \otimes \begin{bmatrix} \cos\!\left(\frac{\|\boldsymbol{\omega}\|\Delta t}{2}\right) \\[4pt] \sin\!\left(\frac{\|\boldsymbol{\omega}\|\Delta t}{2}\right)\dfrac{\boldsymbol{\omega}}{\|\boldsymbol{\omega}\|} \end{bmatrix}$$
$$m_{h_2} \leftarrow m_{h_2} - \text{SFC}\cdot T\cdot\Delta t$$
$\mathbf{b}_a$ and $k_d$ remain unchanged during prediction; they are corrected only via measurement updates.
4. Process Jacobian F
The error-state Jacobian $\mathbf{F} \in \mathbb{R}^{14 \times 14}$ is evaluated at the current nominal state.
Perturbation convention. Throughout this document $\delta\boldsymbol{\theta}$ is a body-frame rotation vector (right perturbation):
$$\mathbf{C}_\text{true} = \hat{\mathbf{C}}\,\mathrm{Exp}(\delta\boldsymbol{\theta}), \qquad \mathbf{q}_\text{true} = \hat{\mathbf{q}} \otimes \begin{bmatrix}1\\\delta\boldsymbol{\theta}/2\end{bmatrix}$$
This is consistent with the inject step (Section 7.3), with $\mathbf{F}_{\theta\theta}=-[\boldsymbol{\omega}]_\times$, and with all $\mathbf{H}$ Jacobians. The consequence for $\mathbf{F}_{v\theta}$ is shown in Section 4.3.
4.1 Block Structure (14×14)
Columns and rows follow the same error-state grouping $[\delta\mathbf{p}(3)\;|\;\delta\mathbf{v}(3)\;|\;\delta\boldsymbol{\theta}(3)\;|\;\delta\mathbf{b}_a(3)\;|\;\delta k_d(1)\;|\;\delta m_{h_2}(1)]$:
$$\mathbf{F} = \begin{bmatrix} \mathbf{0}_3 & \mathbf{I}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0} & \mathbf{0} \\[4pt] \mathbf{0}_3 & \mathbf{F}_{vv} & \mathbf{F}_{v\theta} & \mathbf{0}_3 & \mathbf{f}_{vk} & \mathbf{f}_{vm} \\[4pt] \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{F}_{\theta\theta} & \mathbf{0}_3 & \mathbf{0} & \mathbf{0} \\[4pt] \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0} & \mathbf{0} \\[4pt] \mathbf{0}^\top & \mathbf{0}^\top & \mathbf{0}^\top & \mathbf{0}^\top & 0 & 0 \\[4pt] \mathbf{0}^\top & \mathbf{0}^\top & \mathbf{0}^\top & \mathbf{0}^\top & 0 & 0 \end{bmatrix}$$
where block sizes are determined by group widths:
- $\mathbf{0}_3$, $\mathbf{I}_3$, $\mathbf{F}_{vv}$, $\mathbf{F}_{v\theta}$, $\mathbf{F}_{\theta\theta}$: 3×3
- $\mathbf{0}$ (in 3-row, 1-col cell): 3×1 column vector
- $\mathbf{0}^\top$ (in 1-row, 3-col cell): 1×3 row vector
- $\mathbf{f}_{vk}$, $\mathbf{f}_{vm}$: 3×1 column vectors
4.2 Fully Expanded F (14×14)
Define shorthand scalars (evaluated at current nominal state):
$$\alpha = \frac{2\,k_d\,\rho\,|v_{bx}|}{m}, \qquad \beta = \frac{\rho\,v_{bx}|v_{bx}|}{m}, \qquad \gamma = \frac{\sigma}{m^2}$$
$$\mathbf{c}_1 = [c_x,\,c_y,\,c_z]^\top = \mathbf{C}\hat{\mathbf{e}}_1, \quad [d_x,\,d_y,\,d_z]^\top = \mathbf{c}_1\times\hat{\mathbf{v}}, \quad \boldsymbol{\omega} = [\omega_1,\,\omega_2,\,\omega_3]^\top$$
$$\mathbf{F} = \left[\begin{array}{ccc|ccc|ccc|ccc|c|c} 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ \hline 0 & 0 & 0 & -\alpha c_x^2 & -\alpha c_x c_y & -\alpha c_x c_z & -\alpha c_x d_x & \frac{\sigma c_z}{m}-\alpha c_x d_y & -\frac{\sigma c_y}{m}-\alpha c_x d_z & 0 & 0 & 0 & -\beta c_x & -\gamma c_x \\[4pt] 0 & 0 & 0 & -\alpha c_x c_y & -\alpha c_y^2 & -\alpha c_y c_z & -\frac{\sigma c_z}{m}-\alpha c_y d_x & -\alpha c_y d_y & \frac{\sigma c_x}{m}-\alpha c_y d_z & 0 & 0 & 0 & -\beta c_y & -\gamma c_y \\[4pt] 0 & 0 & 0 & -\alpha c_x c_z & -\alpha c_y c_z & -\alpha c_z^2 & \frac{\sigma c_y}{m}-\alpha c_z d_x & -\frac{\sigma c_x}{m}-\alpha c_z d_y & -\alpha c_z d_z & 0 & 0 & 0 & -\beta c_z & -\gamma c_z \\ \hline 0 & 0 & 0 & 0 & 0 & 0 & 0 & \omega_3 & -\omega_2 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & -\omega_3 & 0 & \omega_1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & \omega_2 & -\omega_1 & 0 & 0 & 0 & 0 & 0 & 0 \\ \hline 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ \hline 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ \end{array}\right]$$
Row labels (top to bottom): $\dot{\delta p}_x,\,\dot{\delta p}_y,\,\dot{\delta p}_z,\;\dot{\delta v}_x,\,\dot{\delta v}_y,\,\dot{\delta v}_z,\;\dot{\delta\theta}_x,\,\dot{\delta\theta}_y,\,\dot{\delta\theta}_z,\;\dot{\delta b}_{ax},\,\dot{\delta b}_{ay},\,\dot{\delta b}_{az},\;\dot{\delta k}_d,\;\dot{\delta m}_{h_2}$
Note on columns 7–9 ($\delta\boldsymbol{\theta}$): The $\mathbf{F}_{v\theta}$ entries shown above are the left-perturbation (world-frame $\delta\boldsymbol{\theta}$) form, useful for visual inspection and valid when $\mathbf{C}\approx\mathbf{I}$. For general attitude angles, the exact right-perturbation formula $\mathbf{F}_{v\theta} = [\cdots]\,\mathbf{C}$ given in Section 4.3 must be used in the implementation.
4.3 Non-Trivial Blocks
$\mathbf{F}_{vv}$ — drag–velocity coupling (3×3):
Drag damps forward motion; the Jacobian is rank-1 along the world-frame forward direction:
$$\mathbf{F}_{vv} = -\frac{2 k_d \rho |v_{bx}|}{m}\,\mathbf{c}_1 \mathbf{c}_1^\top$$
$\mathbf{F}_{v\theta}$ — orientation coupling to velocity prediction (3×3):
A body-frame rotation perturbation $\delta\boldsymbol{\theta}$ changes both the thrust/drag direction $\mathbf{c}_1$ and the projected forward speed $v_{bx}$. With right perturbation ($\mathbf{C}_\text{true} = \hat{\mathbf{C}}\,\mathrm{Exp}(\delta\boldsymbol{\theta})$):
$$\frac{\partial \mathbf{c}_1}{\partial \delta\boldsymbol{\theta}} = -[\mathbf{c}_1]_\times\,\mathbf{C}, \qquad \frac{\partial v_{bx}}{\partial \delta\boldsymbol{\theta}} = (\hat{\mathbf{e}}_1 \times \mathbf{v}_b)^\top = (\mathbf{c}_1 \times \hat{\mathbf{v}})^\top \mathbf{C}$$
Combining both contributions:
$$\boxed{\mathbf{F}_{v\theta} = \Bigl(-\frac{\sigma}{m}\,[\mathbf{c}_1]_\times - \frac{2 k_d \rho |v_{bx}|}{m}\,\mathbf{c}_1\,(\mathbf{c}_1 \times \hat{\mathbf{v}})^\top\Bigr)\,\mathbf{C}}$$
The trailing $\mathbf{C}$ converts the body-frame $\delta\boldsymbol{\theta}$ into the world-frame direction that perturbs the thrust axis and projected speed. It reduces to the bracket alone only when $\mathbf{C} \approx \mathbf{I}$ (near-level flight).
where $[\mathbf{a}]_\times$ denotes the skew-symmetric matrix of vector $\mathbf{a}$:
$$[\mathbf{a}]_\times = \begin{bmatrix} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \end{bmatrix}$$
$\mathbf{F}_{\theta\theta}$ — Euler kinematic nonlinearity (3×3):
With quaternion/ESKF, this simplifies dramatically compared to Euler angles:
$$\mathbf{F}_{\theta\theta} = -[\boldsymbol{\omega}]_\times$$
This is just the skew-symmetric matrix of the current gyro reading — no angle-dependent nonlinearity.
$\mathbf{f}_{vk}$ — drag coefficient coupling (3×1):
$$\mathbf{f}_{vk} = -\frac{\rho\, v_{bx} |v_{bx}|}{m}\,\mathbf{c}_1$$
Makes $k_d$ observable through velocity prediction residuals.
$\mathbf{f}_{vm}$ — H₂ mass coupling (3×1):
$$\mathbf{f}_{vm} = -\frac{\sigma}{m^2}\,\mathbf{c}_1$$
As fuel burns and $m$ decreases, the same thrust produces higher acceleration; the filter tracks this.
4.4 Discrete Covariance Prediction
$$\boldsymbol{\Phi} = \mathbf{I}_{14} + \mathbf{F}\,\Delta t$$
$$\mathbf{P}^- = \boldsymbol{\Phi}\,\mathbf{P}\,\boldsymbol{\Phi}^\top + \mathbf{Q}_d$$
5. Process Noise Q
$$\mathbf{Q}_d = \mathrm{diag}\!\left( \underbrace{0,0,0}_{\delta\mathbf{p}},\; \underbrace{q_v, q_v, q_v}_{\delta\mathbf{v}},\; \underbrace{q_\theta, q_\theta, q_\theta}_{\delta\boldsymbol{\theta}},\; \underbrace{q_{ba}, q_{ba}, q_{ba}}_{\delta\mathbf{b}_a},\; \underbrace{q_{kd}}_{\delta k_d},\; \underbrace{q_{m}}_{\delta m_{h_2}} \right) \cdot \Delta t$$
| Parameter | Physical meaning | Typical initial value |
|---|---|---|
| $q_p = 0$ | Position has no direct noise | — |
| $q_v$ | Residual unmodeled aero forces (sideslip, gusts; lift and structured drag now modeled) | $0.01$–$0.1$ m²/s³ |
| $q_\theta$ | Gyro noise spectral density (from datasheet) | $\sigma_\omega^2$ rad²/s |
| $q_{ba}$ | Accelerometer bias random walk | $10^{-5}$–$10^{-4}$ m²/s⁵ |
| $q_{kd}$ | Drag coefficient variation (speed, altitude) | $10^{-4}$ |
| $q_m$ | SFC model uncertainty | $(\text{SFC}\cdot\sigma_T)^2$ |
6. Measurement Models H
For each sensor, define the predicted measurement $h(\hat{\mathbf{x}})$ and the Jacobian $\mathbf{H} = \partial h / \partial \delta\mathbf{x}$ evaluated at the nominal state. The innovation is:
$$\boldsymbol{\nu} = \mathbf{z} - h(\hat{\mathbf{x}})$$
Columns of every $\mathbf{H}$ follow the error-state ordering:
$$[\,\underbrace{\delta\mathbf{p}}_{3}\;|\;\underbrace{\delta\mathbf{v}}_{3}\;|\;\underbrace{\delta\boldsymbol{\theta}}_{3}\;|\;\underbrace{\delta\mathbf{b}_a}_{3}\;|\;\underbrace{\delta k_d}_{1}\;|\;\underbrace{\delta m_{h_2}}_{1}\,]$$
6.1 General Pattern — Body-Frame Velocity Measurement
Any sensor measuring the body-axis $i$ velocity component follows the same structure.
Predicted measurement:
$$h_i(\hat{\mathbf{x}}) = \mathbf{c}_i^\top\,\mathbf{v}, \quad \mathbf{c}_i = \mathbf{C}\hat{\mathbf{e}}_i$$
Jacobians:
$$\frac{\partial h_i}{\partial \delta\mathbf{v}} = \mathbf{c}_i^\top \qquad (1\times 3)$$
$$\frac{\partial h_i}{\partial \delta\boldsymbol{\theta}} = (\hat{\mathbf{e}}_i \times \mathbf{v}_b)^\top \qquad (1\times 3)$$
The cross products evaluate to:
$$\hat{\mathbf{e}}_1 \times \mathbf{v}_b = \begin{bmatrix}0 \\ -v_{bz} \\ v_{by}\end{bmatrix}, \quad \hat{\mathbf{e}}_2 \times \mathbf{v}_b = \begin{bmatrix}v_{bz} \\ 0 \\ -v_{bx}\end{bmatrix}, \quad \hat{\mathbf{e}}_3 \times \mathbf{v}_b = \begin{bmatrix}-v_{by} \\ v_{bx} \\ 0\end{bmatrix}$$
All expanded matrices share the same 14-column layout (vertical bars separate groups):
$$\underbrace{\delta p_x,\;\delta p_y,\;\delta p_z}_{3}\;\Big|\;\underbrace{\delta v_x,\;\delta v_y,\;\delta v_z}_{3}\;\Big|\;\underbrace{\delta\theta_x,\;\delta\theta_y,\;\delta\theta_z}_{3}\;\Big|\;\underbrace{\delta b_{ax},\;\delta b_{ay},\;\delta b_{az}}_{3}\;\Big|\;\underbrace{\delta k_d}_{1}\;\Big|\;\underbrace{\delta m_{h_2}}_{1}$$
Shorthands: $\alpha=2k_{d,tot}\rho|v_{bx}|/m$, $\beta=\rho v_{bx}|v_{bx}|/m$, $\gamma=\sigma/m^2$; $\mathbf{c}_1=[c_x,c_y,c_z]^\top$, $\mathbf{c}_2=[c_{2x},c_{2y},c_{2z}]^\top$, $\mathbf{c}_3=[e_x,e_y,e_z]^\top$.
6.2 FAST-LIO — Position (3×14)
$$h(\hat{\mathbf{x}}) = \hat{\mathbf{p}}$$
$$\mathbf{H}_\text{fl} = \left[\begin{array}{ccc|ccc|ccc|ccc|c|c} 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \end{array}\right]$$
$\mathbf{R}_\text{fl}$: taken from FAST-LIO's published pose covariance, or fixed at $\approx 0.05\,\text{m}^2$.
Skip this update when $\hat{p}_z > z_\text{max,fl} = 10\,\text{m}$. LiDAR scan-matching degrades severely at altitude — point cloud density thins, ground returns disappear, and loop-closure fails — producing position jumps that corrupt the filter. Barometer + airspeed + optical flow cover the altitude regime where FAST-LIO is unreliable.
6.3 Barometer — Altitude (1×14)
$$h(\hat{\mathbf{x}}) = \hat{p}_z$$
$$\mathbf{H}_\text{baro} = \left[\begin{array}{ccc|ccc|ccc|ccc|c|c} 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \end{array}\right]$$
6.4 Sonar — AGL Altitude (1×14)
$$h(\hat{\mathbf{x}}) = \hat{p}_z \quad \text{(assumes flat ground at } z = 0\text{)}$$
$$\mathbf{H}_\text{sonar} = \left[\begin{array}{ccc|ccc|ccc|ccc|c|c} 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \end{array}\right]$$
Skip this update when $\hat{p}_z > z_\text{max,sonar}$ (altitude gating, handled externally).
6.5 Airspeed — Scalar Forward Speed (1×14)
$$h(\hat{\mathbf{x}}) = \mathbf{c}_1^\top\,\hat{\mathbf{v}} = v_{bx}$$
$$\mathbf{H}_\text{as} = \left[\begin{array}{ccc|ccc|ccc|ccc|c|c} 0 & 0 & 0 & c_x & c_y & c_z & 0 & -v_{bz} & v_{by} & 0 & 0 & 0 & 0 & 0 \end{array}\right]$$
With wind correction ($\mathbf{w}$ = wind vector in world frame): $h = \mathbf{c}_1^\top(\hat{\mathbf{v}} - \mathbf{w})$; $\mathbf{H}$ unchanged.
6.6 Optical Flow — Body [vx, vy] (2×14)
$$h(\hat{\mathbf{x}}) = \begin{bmatrix} \mathbf{c}_1^\top\,\hat{\mathbf{v}} \\ \mathbf{c}_2^\top\,\hat{\mathbf{v}} \end{bmatrix}$$
$$\mathbf{H}_\text{of} = \left[\begin{array}{ccc|ccc|ccc|ccc|c|c} 0 & 0 & 0 & c_x & c_y & c_z & 0 & -v_{bz} & v_{by} & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & c_{2x} & c_{2y} & c_{2z} & v_{bz} & 0 & -v_{bx} & 0 & 0 & 0 & 0 & 0 \end{array}\right]$$
Active only when $z_\text{min,of} \leq \hat{p}_z \leq z_\text{max,of}$.
6.7 Forward Camera — Body [vy, vz] (2×14)
$$h(\hat{\mathbf{x}}) = \begin{bmatrix} \mathbf{c}_2^\top\,\hat{\mathbf{v}} \\ \mathbf{c}_3^\top\,\hat{\mathbf{v}} \end{bmatrix}$$
$$\mathbf{H}_\text{fwd} = \left[\begin{array}{ccc|ccc|ccc|ccc|c|c} 0 & 0 & 0 & c_{2x} & c_{2y} & c_{2z} & v_{bz} & 0 & -v_{bx} & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & e_x & e_y & e_z & -v_{by} & v_{bx} & 0 & 0 & 0 & 0 & 0 & 0 \end{array}\right]$$
Active only when $\hat{p}_z \geq z_\text{min,fwd}$.
6.8 Wheel Odometry — [px, py] + vfwd (3×14, ground-gated)
$$\mathbf{H}_\text{wh} = \left[\begin{array}{ccc|ccc|ccc|ccc|c|c} 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & c_x & c_y & c_z & 0 & -v_{bz} & v_{by} & 0 & 0 & 0 & 0 & 0 \end{array}\right]$$
Rows 1–2 observe $p_x, p_y$ directly. Row 3 observes $v_{bx}$ identically to airspeed. Both active only when _on_ground() is true (with hysteresis).
6.9 Tank Pressure/Temperature — H₂ Mass (1×14)
$$z_\text{tank} = \frac{p_\text{tank}\, V}{R_{H_2}\, T_\text{tank}}, \quad h(\hat{\mathbf{x}}) = \hat{m}_{h_2}$$
$$\mathbf{H}_\text{tank} = \left[\begin{array}{ccc|ccc|ccc|ccc|c|c} 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 \end{array}\right]$$
$\mathbf{R}_\text{tank}$ should be large to reflect ideal gas law model error.
6.10 IMU Accelerometer — Specific Force (3×14)
$$h(\hat{\mathbf{x}}) = \frac{\sigma}{m}\,\hat{\mathbf{e}}_1 + \hat{\mathbf{b}}_a$$
Most informative measurement — observes $\delta\mathbf{b}_a$, $\delta k_d$, and $\delta m_{h_2}$ simultaneously.
$$\mathbf{H}_\text{imu} = \left[\begin{array}{ccc|ccc|ccc|ccc|c|c} 0 & 0 & 0 & -\alpha c_x & -\alpha c_y & -\alpha c_z & 0 & \alpha v_{bz} & -\alpha v_{by} & 1 & 0 & 0 & -\beta & -\gamma \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 \end{array}\right]$$
Row 1 ($a_x$): drag couples $\delta\mathbf{v}$ and $\delta\boldsymbol{\theta}$; directly observes $\delta b_{ax}$, $\delta k_d$, $\delta m_{h_2}$. Rows 2–3 ($a_y, a_z$): observe only $\delta b_{ay}, \delta b_{az}$ — drag model is 1-D along $\hat{\mathbf{e}}_1$.
6.11 Madgwick Orientation — Quaternion (3×14)
$$\delta\mathbf{q} = \hat{\mathbf{q}}^{-1} \otimes \mathbf{q}_\text{madgwick}, \qquad \boldsymbol{\nu} = 2\,\delta\mathbf{q}_\text{vec}$$
$$\mathbf{H}_\text{ori} = \left[\begin{array}{ccc|ccc|ccc|ccc|c|c} 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 \end{array}\right]$$
Directly corrects $\delta\boldsymbol{\theta}$ with identity — the simplest possible orientation measurement.
6.12 Sensor Summary
| Sensor | Dim | Updates |
|---|---|---|
| FAST-LIO | 3 | $\delta\mathbf{p}$ |
| Barometer | 1 | $\delta p_z$ |
| Sonar | 1 | $\delta p_z$ |
| Airspeed | 1 | $\delta\mathbf{v}$, $\delta\boldsymbol{\theta}$ |
| Optical flow | 2 | $\delta\mathbf{v}$, $\delta\boldsymbol{\theta}$ |
| Forward camera | 2 | $\delta\mathbf{v}$, $\delta\boldsymbol{\theta}$ |
| Wheel odometry | 3 | $\delta\mathbf{p}$, $\delta\mathbf{v}$, $\delta\boldsymbol{\theta}$ |
| Tank P/T | 1 | $\delta m_{h_2}$ |
| IMU accel | 3 | $\delta\mathbf{v}$, $\delta\boldsymbol{\theta}$, $\delta\mathbf{b}_a$, $\delta k_d$, $\delta m_{h_2}$ |
| Madgwick | 3 | $\delta\boldsymbol{\theta}$ |
7. Update Step Mechanics
7.1 Phase 1 — Predict
At each process model tick (50 Hz):
- Propagate nominal state $\hat{\mathbf{x}}$ using Section 3.2
- Evaluate $\mathbf{F}$ at current $\hat{\mathbf{x}}$ (Section 4)
- Form $\boldsymbol{\Phi} = \mathbf{I}_{14} + \mathbf{F}\,\Delta t$
- Propagate covariance:
$$\mathbf{P}^- = \boldsymbol{\Phi}\,\mathbf{P}\,\boldsymbol{\Phi}^\top + \mathbf{Q}_d$$
7.2 Phase 2 — Update
Whenever a measurement $\mathbf{z}$ arrives:
Outlier rejection (Mahalanobis test):
$$\boldsymbol{\nu} = \mathbf{z} - h(\hat{\mathbf{x}}), \quad \mathbf{S} = \mathbf{H}\mathbf{P}^-\mathbf{H}^\top + \mathbf{R}$$
$$d^2 = \boldsymbol{\nu}^\top\,\mathbf{S}^{-1}\,\boldsymbol{\nu}$$
Reject if $d^2 > \chi^2_{m,\,0.95}$ ($m$ = measurement dimension).
Kalman gain:
$$\mathbf{K} = \mathbf{P}^-\mathbf{H}^\top\mathbf{S}^{-1} \qquad (14 \times m)$$
Error state correction:
$$\delta\hat{\mathbf{x}} = \mathbf{K}\,\boldsymbol{\nu}$$
Covariance update (Joseph form — numerically stable):
$$\mathbf{P}^+ = (\mathbf{I} - \mathbf{K}\mathbf{H})\,\mathbf{P}^-\,(\mathbf{I} - \mathbf{K}\mathbf{H})^\top + \mathbf{K}\,\mathbf{R}\,\mathbf{K}^\top$$
Note: The simple form $\mathbf{P}^+ = (\mathbf{I} - \mathbf{K}\mathbf{H})\mathbf{P}^-$ can lose positive definiteness under floating-point errors. Always use the Joseph form.
7.3 Phase 3 — Inject
Apply the error state correction to the nominal state:
$$\hat{\mathbf{p}} \leftarrow \hat{\mathbf{p}} + \delta\mathbf{p}$$
$$\hat{\mathbf{v}} \leftarrow \hat{\mathbf{v}} + \delta\mathbf{v}$$
$$\hat{\mathbf{q}} \leftarrow \hat{\mathbf{q}} \otimes \begin{bmatrix} 1 \\ \delta\boldsymbol{\theta}/2 \end{bmatrix}, \qquad \hat{\mathbf{q}} \leftarrow \frac{\hat{\mathbf{q}}}{\|\hat{\mathbf{q}}\|}$$
$$\hat{\mathbf{b}}_a \leftarrow \hat{\mathbf{b}}_a + \delta\mathbf{b}_a$$
$$\hat{k}_d \leftarrow \max(0,\; \hat{k}_d + \delta k_d)$$
$$\hat{m}_{h_2} \leftarrow \mathrm{clamp}(\hat{m}_{h_2} + \delta m_{h_2},\; 0,\; m_{h_2,\text{init}})$$
7.4 Phase 4 — Reset
Zero the error state and propagate the covariance through the reset Jacobian:
$$\delta\mathbf{x} \leftarrow \mathbf{0}$$
$$\mathbf{G} = \mathrm{diag}\!\left(\mathbf{I}_3,\; \mathbf{I}_3,\; \mathbf{I}_3 - [\delta\boldsymbol{\theta}/2]_\times,\; \mathbf{I}_3,\; 1,\; 1\right) \approx \mathbf{I}_{14}$$
$$\mathbf{P} \leftarrow \mathbf{G}\,\mathbf{P}^+\,\mathbf{G}^\top \approx \mathbf{P}^+$$
For small $\delta\boldsymbol{\theta}$ (well-tuned filter), $\mathbf{G} \approx \mathbf{I}_{14}$ and the reset covariance step is often skipped in practice.
8. Full ESKF Cycle
┌──────────────────────────────────────────────────────┐
│ PREDICT (50 Hz) │
│ x̂ ← f(x̂, u) [nominal state] │
│ P⁻ ← Φ P Φᵀ + Q_d [covariance] │
└───────────────────────┬──────────────────────────────┘
│ measurement arrives
▼
┌──────────────────────────────────────────────────────┐
│ UPDATE │
│ ν = z − h(x̂) │
│ S = H P⁻ Hᵀ + R │
│ reject if νᵀS⁻¹ν > χ²_m,0.95 │
│ K = P⁻ Hᵀ S⁻¹ │
│ δx = K ν │
│ P⁺ = (I−KH) P⁻ (I−KH)ᵀ + K R Kᵀ [Joseph form] │
└───────────────────────┬──────────────────────────────┘
▼
┌──────────────────────────────────────────────────────┐
│ INJECT │
│ p̂ ← p̂ + δp │
│ v̂ ← v̂ + δv │
│ q̂ ← q̂ ⊗ [1, δθ/2]ᵀ, then normalize │
│ b̂a ← b̂a + δba │
│ k̂d ← max(0, k̂d + δkd) │
│ m̂h2← clamp(m̂h2 + δmh2, 0, m_init) │
└───────────────────────┬──────────────────────────────┘
▼
┌──────────────────────────────────────────────────────┐
│ RESET │
│ δx ← 0 │
│ P ← G P⁺ Gᵀ (≈ P⁺ for small δθ) │
└──────────────────────────────────────────────────────┘
Multiple sensors in one timestep: Apply updates sequentially (one $\mathbf{H}$ at a time), ordered by descending reliability:
$$\text{FAST-LIO} \to \text{Madgwick} \to \text{airspeed} \to \text{optical flow} \to \text{forward camera}$$
Each update uses the $\mathbf{P}$ improved by the previous one.
9. Implementation Notes
9.1 Initialization
p̂ = [0, 0, 0]ᵀ
v̂ = [0, 0, 0]ᵀ
q̂ = [1, 0, 0, 0]ᵀ (identity)
b̂a = [0, 0, 0]ᵀ
k̂d = k_d,init (from parameter)
m̂h2= m_h2,init (from parameter)
P = diag([1, 1, 1, ← p: large init uncertainty
1, 1, 1, ← v
0.1, 0.1, 0.1, ← θ: moderate
0.01, 0.01, 0.01,← ba: small
1.0, ← kd: large (unknown)
1.0]) ← mh2: large
9.2 Quaternion Utilities
def quat_mult(q, p):
"""Hamilton product q ⊗ p."""
qw,qx,qy,qz = q
pw,px,py,pz = p
return np.array([
qw*pw - qx*px - qy*py - qz*pz,
qw*px + qx*pw + qy*pz - qz*py,
qw*py - qx*pz + qy*pw + qz*px,
qw*pz + qx*py - qy*px + qz*pw])
def quat_to_C(q):
"""Direction cosine matrix from unit quaternion (body→world)."""
qw,qx,qy,qz = q
return np.array([
[1-2*(qy**2+qz**2), 2*(qx*qy-qw*qz), 2*(qx*qz+qw*qy)],
[2*(qx*qy+qw*qz), 1-2*(qx**2+qz**2), 2*(qy*qz-qw*qx)],
[2*(qx*qz-qw*qy), 2*(qy*qz+qw*qx), 1-2*(qx**2+qy**2)]])
def skew(v):
"""3×3 skew-symmetric matrix of vector v."""
return np.array([[0,-v[2],v[1]],[v[2],0,-v[0]],[-v[1],v[0],0]])
9.3 Numerical Safeguards
- Joseph form for $\mathbf{P}^+$: always, no exceptions
- Symmetrize $\mathbf{P}$ after each update:
P = 0.5*(P + P.T) - Normalize $\hat{\mathbf{q}}$ after each inject step
- Clamp $k_d > 0$ and $m_{h_2} \in [0, m_\text{init}]$ after inject
- Mahalanobis gating before every update: reject outliers
- Minimum eigenvalue check: if $\lambda_\text{min}(\mathbf{P}) < 0$, force $\mathbf{P} \leftarrow \mathbf{P} + \epsilon\mathbf{I}$
9.4 Integration into marid_odom_pub.py
The ESKF replaces only the scalar blending lines. The existing infrastructure is preserved:
| Existing | Role in ESKF |
|---|---|
| Sensor staleness checks | Gate all measurement updates (skip when stale) |
| Altitude gating | Gate FAST-LIO ($>$10 m), sonar ($>z_\text{max}$), OF (band), fwd-camera ($<z_\text{min}$) |
_on_ground() |
Gate wheel odometry updates (with 0.5 s hysteresis) |
| RLS drag estimator | Removed — replaced by $k_d$ in ESKF state |
| SFC integration | Removed — replaced by $m_{h_2}$ process model |
Static baro_weight, of_weight, etc. |
Removed — replaced by Kalman gain $\mathbf{K}$ |
9.5 Altitude Architecture — z is Sensor-Owned, Not ESKF-Published
Design decision: the published altitude comes from the complementary filter (
z_fused_), not from the ESKF's $\hat{p}_z$.
Although the ESKF maintains $\hat{p}_z$ and $\hat{v}_z$ as nominal state components and receives sonar (§6.4) and barometer (§6.3) updates to keep internal covariance consistent, it does not drive the published altitude. The complementary filter priority is:
$$z_\text{pub} = \begin{cases} w_\text{sonar}\,z_\text{sonar} + (1-w_\text{sonar})\,z_\text{imu} & \text{sonar valid (AGL} \leq z_\text{max,sonar}\text{)} \\[4pt] (1-w_\text{baro})\,z_\text{imu} + w_\text{baro}\,z_\text{baro} & \text{baro ready} \\[4pt] z_\text{imu} & \text{otherwise} \end{cases}$$Why not publish $\hat{p}_z$?
- Lift-model inaccuracies cause z drift. The aerodynamic lift model produces non-zero $\dot{v}_z$ even in level flight (AoA estimation error, wing-angle uncertainty). If $\hat{v}_z$ is physics-integrated those errors accumulate into altitude drift. Sonar and barometer hold absolute altitude references that physics integration cannot match.
- z is fully observable by dedicated sensors. Sonar is accurate to centimetres below 5 m AGL; barometer resolves changes of ~0.1 m above that. Neither has the integration drift that affects IMU-based $v_z$. Keeping z sensor-owned avoids the sawtooth altitude artefact seen when lift-model forcing fights a barometric hold.
Consequence for the F matrix. The $\hat{v}_z$ row (row 5, 0-indexed) in the physics-mode $\mathbf{F}$ matrix is intentionally zeroed: $v_z$ is not integrated from the force model. Its covariance is driven only by process noise $q_v$ and measurement updates from sonar and baro. This is reflected in predict_physics where self.v[2] is not updated from a_world[2].
Why the ESKF z state is retained. Two reasons: (a) altitude-gated decisions (FAST-LIO above 10 m, OF in its band, forward camera above $z_\text{min}$) reference $\hat{p}_z$ for gating logic; (b) the full covariance $\mathbf{P}$ must remain $14\times14$ — zeroing the z rows/columns would break the position–velocity coupling for $\hat{p}_x, \hat{p}_y$ through the identity block $\mathbf{F}_{pv} = \mathbf{I}_3$.
10. Future Work: Learning-Augmented ESKF for Full GPS-Denied State Estimation
Motivation
The ESKF described in this document provides principled sensor fusion but cannot recover absolute position truth in GPS-denied environments — position error accumulates through drift in all available sensors. The planned extension addresses this by augmenting the ESKF with a frozen, offline-trained ML model that provides a full state estimate as a virtual sensor.
Architecture
The ML model is trained offline and supervised using Gazebo simulation ground truth (position, velocity, attitude, joint angles, forces) across thousands of diverse fixed-wing flight regimes — takeoff roll, cruise, climbing, descending, banked turns, aggressive maneuvers, wind gusts, ground effect on landing, and varying hydrogen burn states. Once trained, the model is frozen and deployed alongside the ESKF with no online learning.
At runtime, the ML model receives the same sensor inputs available to the ESKF and outputs:
- Full state estimate: position (3), velocity (3), attitude (3)
- Per-output learned covariance — representing prediction confidence
This output enters the ESKF as a virtual measurement update (new rows in $\mathbf{H}$), weighted by the Kalman gain against the physics-based prediction. Confident ML estimates receive high gain; uncertain ones are naturally down-weighted.
Why This Combination Achieves High-Trust GPS-Denied Estimation
| Layer | Role |
|---|---|
| ESKF (physics) | Short-term accuracy, noise filtering, covariance propagation |
| ML model (learned) | Long-term drift correction, nonlinear aero effects, systematic bias the physics model misses |
| Together | Full state estimate with propagated uncertainty — no GPS required |
Publication Plan
This architecture is the subject of a planned research paper targeting GPS-denied full-state estimation for hydrogen-powered fixed-wing UAVs. The contribution covers: (1) the physics-based ESKF design, (2) the Gazebo-supervised training pipeline, (3) the virtual-sensor fusion architecture, and (4) validation across simulation flight regimes with a path toward hardware deployment.
Comments
Post a Comment