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
1. Motivation
A node that fuses ten sensor sources via hand-tuned scalar complementary filters:
$$z_\text{fused} = w \cdot z_\text{sensor} + (1-w) \cdot z_\text{prior}$$
The weights $w$ are constants set at launch time. They do not adapt when sensor quality degrades (e.g. FAST-LIO scan-matching fails at altitude), and they carry no covariance — uncertainty is not propagated across sensor boundaries.
The Error-State Extended Kalman Filter (ESKF) replaces these fixed weights with a principled estimator that:
- Computes the Kalman gain $\mathbf{K}$ dynamically from current state uncertainty $\mathbf{P}$ and measurement noise $\mathbf{R}$
- Propagates uncertainty through the full state vector so every measurement update informs every correlated state
- Augments the navigation state with MARID-specific physical parameters ($k_d$, $m_{h_2}$) that participate in the dynamics model
- Replaces the RLS drag estimator and SFC mass integrator with a unified estimator
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 additive update $\hat{\mathbf{q}} \leftarrow \hat{\mathbf{q}} + \delta\mathbf{q}$ violates this constraint. The ESKF splits the state into:
- Nominal state $\hat{\mathbf{x}}$: propagated with physics, uses full quaternion
- Error state $\delta\mathbf{x}$: estimated by the EKF, uses a minimal 3-component rotation vector $\delta\boldsymbol{\theta}$
Corrections are applied multiplicatively to $\hat{\mathbf{q}}$ via quaternion product, 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 |
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 (not estimated)
| Symbol | Description | Source |
|---|---|---|
| $T$ | Thrust force (N) | /model/marid/.../cmd_thrust |
| $\boldsymbol{\omega}$ | Angular velocity (body frame, rad/s) | IMU gyroscope |
| $\mathbf{a}_{imu}$ | Specific force (body frame, m/s²) | IMU accelerometer |
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$$
$$\sigma \triangleq T - k_d \rho\, v_{bx} |v_{bx}|, \quad m \triangleq m_\text{empty} + \hat{m}_{h_2}$$
where $\hat{\mathbf{e}}_1 = [1,0,0]^\top$, $\hat{\mathbf{e}}_2 = [0,1,0]^\top$, $\hat{\mathbf{e}}_3 = [0,0,1]^\top$ are body-frame unit vectors, and $\rho$ is air density.
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, but unmodeled aero forces (lift, sideslip, gusts) add directly 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$ | Unmodeled aero forces (lift, sideslip) | $0.1$–$1.0$ 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}$$
6.2 FAST-LIO — Position (3×14)
$$h(\hat{\mathbf{x}}) = \hat{\mathbf{p}}$$
$$\mathbf{H}_\text{fl} = \begin{bmatrix} \mathbf{I}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0} & \mathbf{0} \end{bmatrix}$$
$\mathbf{R}_\text{fl}$: taken from FAST-LIO's published pose covariance, or fixed at $\approx 0.05\,\text{m}^2$.
6.3 Barometer — Altitude (1×14)
$$h(\hat{\mathbf{x}}) = \hat{p}_z$$
$$\mathbf{H}_\text{baro} = \begin{bmatrix} 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \end{bmatrix}$$
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} = \mathbf{H}_\text{baro}$$
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} = \begin{bmatrix} \mathbf{0}_{1\times3} & \mathbf{c}_1^\top & (\hat{\mathbf{e}}_1 \times \mathbf{v}_b)^\top & \mathbf{0}_{1\times3} & 0 & 0 \end{bmatrix}$$
With wind correction ($\mathbf{w}$ = wind vector in world frame):
$$h(\hat{\mathbf{x}}) = \mathbf{c}_1^\top(\hat{\mathbf{v}} - \mathbf{w})$$
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} = \begin{bmatrix} \mathbf{0}_{1\times3} & \mathbf{c}_1^\top & (\hat{\mathbf{e}}_1 \times \mathbf{v}_b)^\top & \mathbf{0}_{1\times3} & 0 & 0 \\ \mathbf{0}_{1\times3} & \mathbf{c}_2^\top & (\hat{\mathbf{e}}_2 \times \mathbf{v}_b)^\top & \mathbf{0}_{1\times3} & 0 & 0 \end{bmatrix}$$
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} = \begin{bmatrix} \mathbf{0}_{1\times3} & \mathbf{c}_2^\top & (\hat{\mathbf{e}}_2 \times \mathbf{v}_b)^\top & \mathbf{0}_{1\times3} & 0 & 0 \\ \mathbf{0}_{1\times3} & \mathbf{c}_3^\top & (\hat{\mathbf{e}}_3 \times \mathbf{v}_b)^\top & \mathbf{0}_{1\times3} & 0 & 0 \end{bmatrix}$$
Active only when $\hat{p}_z \geq z_\text{min,fwd}$.
6.8 Wheel Odometry — [px, py] + vfwd (3×14, ground-gated)
Position component (2×14):
$$\mathbf{H}_\text{wh,p} = \begin{bmatrix} 1 & 0 & 0 & \mathbf{0}_{1\times3} & \mathbf{0}_{1\times3} & \mathbf{0}_{1\times3} & 0 & 0 \\ 0 & 1 & 0 & \mathbf{0}_{1\times3} & \mathbf{0}_{1\times3} & \mathbf{0}_{1\times3} & 0 & 0 \end{bmatrix}$$
Forward speed component (1×14) — same form as airspeed:
$$\mathbf{H}_\text{wh,v} = \begin{bmatrix} \mathbf{0}_{1\times3} & \mathbf{c}_1^\top & (\hat{\mathbf{e}}_1 \times \mathbf{v}_b)^\top & \mathbf{0}_{1\times3} & 0 & 0 \end{bmatrix}$$
Both active only when _on_ground() is true.
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} = \begin{bmatrix} \mathbf{0}_{1\times3} & \mathbf{0}_{1\times3} & \mathbf{0}_{1\times3} & \mathbf{0}_{1\times3} & 0 & 1 \end{bmatrix}$$
$\mathbf{R}_\text{tank}$ should be large to reflect ideal gas law model error.
6.10 IMU Accelerometer — Specific Force (3×14)
The predicted specific force in body frame is:
$$h(\hat{\mathbf{x}}) = \mathbf{C}^\top(\dot{\mathbf{v}} - \mathbf{g}) + \hat{\mathbf{b}}_a = \frac{\sigma}{m}\,\hat{\mathbf{e}}_1 + \hat{\mathbf{b}}_a$$
This is the most informative measurement — it simultaneously observes $\delta\mathbf{b}_a$, $\delta k_d$, and $\delta m_{h_2}$:
$$\frac{\partial h}{\partial \delta\mathbf{v}} = -\frac{2 k_d \rho |v_{bx}|}{m}\,\hat{\mathbf{e}}_1 \mathbf{c}_1^\top \qquad (3\times3)$$
$$\frac{\partial h}{\partial \delta\boldsymbol{\theta}} = -\frac{2 k_d \rho |v_{bx}|}{m}\,\hat{\mathbf{e}}_1\,(\hat{\mathbf{e}}_1 \times \mathbf{v}_b)^\top \qquad (3\times3)$$
$$\frac{\partial h}{\partial \delta\mathbf{b}_a} = \mathbf{I}_3 \qquad \leftarrow\text{ direct bias observation}$$
$$\frac{\partial h}{\partial \delta k_d} = -\frac{\rho\, v_{bx}|v_{bx}|}{m}\,\hat{\mathbf{e}}_1 \qquad (3\times1)$$
$$\frac{\partial h}{\partial \delta m_{h_2}} = -\frac{\sigma}{m^2}\,\hat{\mathbf{e}}_1 \qquad (3\times1)$$
Full Jacobian (3×14):
$$\mathbf{H}_\text{imu} = \left[\,\mathbf{0}_3 \;\Big|\; \frac{\partial h}{\partial \delta\mathbf{v}} \;\Big|\; \frac{\partial h}{\partial \delta\boldsymbol{\theta}} \;\Big|\; \mathbf{I}_3 \;\Big|\; \frac{\partial h}{\partial \delta k_d} \;\Big|\; \frac{\partial h}{\partial \delta m_{h_2}}\,\right]$$
6.11 Madgwick Orientation — Quaternion (3×14)
The innovation is computed in rotation space, not by subtraction:
$$\delta\mathbf{q} = \hat{\mathbf{q}}^{-1} \otimes \mathbf{q}_\text{madgwick}$$
$$\boldsymbol{\nu} = 2\,\delta\mathbf{q}_\text{vec} \qquad \text{(vector part of }\delta\mathbf{q}\text{)}$$
$$\mathbf{H}_\text{ori} = \begin{bmatrix} \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{I}_3 & \mathbf{0}_3 & \mathbf{0} & \mathbf{0} \end{bmatrix}$$
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 measurement updates (skip when stale) |
| Altitude gating | Gate OF, sonar, fwd-camera updates |
_on_ground() |
Gate wheel odometry updates |
| 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}$ |
Comments
Post a Comment