# Source Registry — C5: State estimator / sensor fusion

> Mode A Phase 2 — engine Step 2 (Source Tiering & Exhaustive Web Investigation). Sources for C5 (state estimator / sensor fusion) candidates.
>
> Index: [`00_summary.md`](00_summary.md). Sibling categories: [SQ6](SQ6_external_positioning.md), [SQ1](SQ1_existing_systems.md), [SQ2](SQ2_canonical_pipeline.md), [C1](C1_vio.md), [C2](C2_vpr.md), [C3](C3_matchers.md), [C4](C4_pose_estimation.md). Backing fact cards: [`../02_fact_cards/C5_state_estimator.md`](../02_fact_cards/C5_state_estimator.md). Component fit matrix row: [`../06_component_fit_matrix/C5_state_estimator.md`](../06_component_fit_matrix/C5_state_estimator.md).

---

## Source #88 — Solà 2017 "Quaternion kinematics for the error-state Kalman filter" (canonical aerial/quaternion ESKF tutorial)

**Title**: "Quaternion kinematics for the error-state Kalman filter"
**Author**: Joan Solà
**Venue**: arXiv preprint cs.RO 1711.02508 (HAL hal-01122406; Semantic Scholar 12412090e46d1b21eecc59d1326edb8e47e9640e)
**Submitted**: 2017-11-03 (revision v5 hosted on HAL); originally drafted earlier and continually revised since 2014
**URL**: <https://arxiv.org/abs/1711.02508> (canonical) + <https://hal.science/hal-01122406v5> (HAL mirror)
**Tier**: L1 (canonical authoritative tutorial; 592 citations per Semantic Scholar; the de-facto industry reference for ESKF + quaternion algebra in robotics + aerospace + UAV applications since 2017; open-access public-domain academic preprint)
**Length**: 73 sections including 9 main parts (§1 quaternion definition + §2 rotations + §3 conventions + §4 perturbations/derivatives/integrals + §5 error-state kinematics for IMU-driven systems + §6 fusing IMU with complementary sensory data + §7 ESKF using global angular errors + §8 high-order integration variants + §9 references + §10 appendix)
**Date Accessed**: 2026-05-08

**Why it matters for C5**:
- §5.1 lists the THREE structural advantages of ESKF over standard EKF that drive its dominance for UAV applications: (i) minimal orientation error-state (no over-parametrization, no covariance singularity), (ii) error-state always near origin (linearization always valid), (iii) error-state always small (Jacobians fast and often constant).
- §5.4 provides discrete-time error-state Jacobians directly usable for project's IMU integration.
- §6 (sub-divided into §6.1 measurement update + §6.2 injection + §6.3 covariance reset) is the canonical recipe for fusing IMU with complementary sensors (project's case = C1 VIO + C4 satellite anchors + FC IMU).
- §6 explicitly states (line 2013 of the paper text): "At the arrival of other kind of information than IMU, such as GPS or vision, we proceed to correct the ESKF. ... These vision + IMU setups are very interesting for use in **GPS-denied environments**, and can be implemented on mobile devices ... but also on **UAVs and other small, agile platforms**." — a direct project-relevant endorsement from the canonical tutorial.
- §1675-1677 of the paper text frames the project's exact problem statement: "Integrating IMU readings leads to dead-reckoning positioning systems, which drift with time. Avoiding drift is a matter of fusing this information with absolute position readings such as GPS or vision."
- §6.3 explicitly notes that the canonical reset Jacobian G can be approximated as `G = I_18` in most implementations, "but the expression here provided should produce more precise results, which might be of interest for reducing long-term error drift in odometry systems" — relevant for project's 8-hour fixed-wing flights where long-term drift is a binding concern.
- §7 provides an alternate formulation using global angular errors (vs §5's local angular errors); both are valid; project must pick one and stick with it.

---

## Source #89 — Reference open-source ESKF implementations (canonical-paper-derived)

**Repositories examined**:

| # | Repo | Language | License | Sensors fused | Project relevance |
|---|---|---|---|---|---|
| 89.a | `ludvigls/ESKF` | Python | (LICENSE not declared in front-page README — Plan-phase verification gate **D-C5-1 NEW** required if adoption) | IMU + GNSS for fixed-wing UAVs | **DIRECTLY MATCHES project hardware family (fixed-wing UAV + IMU + GNSS-replacement)** — closest documentary template; tested on simulated + real datasets per author description |
| 89.b | `cggos/imu_x_fusion` | C++/ROS | (Plan-phase verification gate **D-C5-1 NEW** required if adoption) | IMU + GNSS + 6DoF-Odom (loosely-coupled) — also IEKF, UKF (UKF/SPKF, JUKF, SVD-UKF), MAP variants | **MATCHES project pattern** — multi-source loosely-coupled fusion (IMU + GNSS-as-satellite_anchor + Odom-as-VIO) |
| 89.c | `EliaTarasov/ESKF` | C++/ROS | (Plan-phase verification gate **D-C5-1 NEW** required if adoption) | GPS + Magnetometer + Vision Pose + Optical Flow + Range Finder fused with IMU (ROS Error-State Kalman Filter based on PX4/ecl) | **CLOSE MATCH but PX4-derived** — license-clear if PX4/ecl BSD-3-Clause, but verify that the derived code is BSD-3-Clause (PX4 is dual BSD/Apache, ecl is BSD-3-Clause) |
| 89.d | `koledickarlo/ESKF-ESP32` | C++ | (LICENSE not declared in front-page README — Plan-phase verification gate **D-C5-1 NEW** required if adoption) | Accelerometer + Gyroscope + Optical Flow + Time-of-Flight (microcontroller-class, ESP32) | NOT MATCH — microcontroller-class targets (ESP32) not Jetson; useful only as small-state ESKF reference (Solà 2017 paper explicit citation) |
| 89.e | `joansola/slamtb` | MATLAB | (LICENSE not declared in front-page README) | EKF-SLAM (full visual-inertial SLAM toolbox) | Author Joan Solà's own SLAM Toolbox in MATLAB — the most authoritative reference for the canonical paper but MATLAB-only, NOT deployable on JetPack 6 |

**Interpretation**: For Fact #88, project does NOT directly reuse any of the above repositories at the source-code level (license verification gates D-C5-1 NEW + cross-domain adaptation costs). Instead, the project implements ESKF following Solà 2017 §5+§6 equations directly in Python (NumPy/SciPy) or C++17 (Eigen3), using ludvigls/ESKF (89.a) as the closest documentary reference template for fixed-wing UAV ESKF structure. The reference implementations serve as evidence that Solà 2017 ESKF is implementable + deployable on UAV-class platforms with multi-sensor fusion patterns identical to the project's pinned configuration.

**URLs accessed (full canonical README pages)**:
- <https://github.com/ludvigls/ESKF>
- <https://github.com/cggos/imu_x_fusion>
- <https://github.com/EliaTarasov/ESKF>
- <https://github.com/koledickarlo/ESKF-ESP32>
- <https://github.com/joansola/slamtb>

**Tier**: L1 (canonical project repositories; multiple independent reproductions of Solà 2017 paper across Python, C++/ROS, MATLAB, and microcontroller-class) + L2 (reference template only; project does NOT directly reuse).
**Date Accessed**: 2026-05-08

---

## Source #90 — GTSAM `ImuFactor` / `CombinedImuFactor` / `PreintegratedImuMeasurements` / `PreintegratedCombinedMeasurements` (context7 query-docs at `/borglab/gtsam` — IMU pre-integration sub-API)

**Title**: GTSAM canonical `ImuFactor` and `CombinedImuFactor` API reference + canonical Python runnable examples
**Source**: context7 query-docs at `/borglab/gtsam` version 4.3a1 with 1121 code snippets (cross-cite to Source #87 from C4 Fact #54 — same library, different sub-API surface; queried 2026-05-08 for IMU + state-estimation extension to C5)
**Returned canonical Python notebooks**:
- `gtsam/navigation/doc/ImuFactor.ipynb` — basic `ImuFactor(X(0), V(0), X(1), V(1), B(0), pim)` 5-key factor + canonical `PreintegrationParams.MakeSharedU(9.81)` setup + `PreintegratedImuMeasurements(params, bias_hat)` + `pim.integrateMeasurement(acc_meas, gyro_meas, dt)` + `pim.predict(initial_state, current_best_bias)` + `imu_factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias_i)`
- `gtsam/navigation/doc/CombinedImuFactor.ipynb` — modern `CombinedImuFactor(X(0), V(0), X(1), V(1), B(0), B(1), pim)` 6-key factor with bias evolution per random walk via `PreintegrationCombinedParams.MakeSharedU(9.81)` + `params.setBiasAccCovariance(np.eye(3) * bias_acc_rw_sigma**2)` + `params.setBiasOmegaCovariance(np.eye(3) * bias_gyro_rw_sigma**2)` + `params.setBiasAccOmegaInit(initial_bias_cov)` + `PreintegratedCombinedMeasurements(params, bias_hat)`
- `gtsam/navigation/doc/PreintegratedImuMeasurements.ipynb` — full PIM workflow: `pim.integrateMeasurement(acc, gyro, dt)` × N → `pim.deltaTij()` / `pim.deltaRij().matrix()` / `pim.deltaPij()` / `pim.deltaVij()` / `pim.biasHat()` / `pim.preintMeasCov()` 9×9 covariance + `pim.predict(initial_state, current_best_bias)` for IMU-only state extrapolation
- `gtsam/navigation/doc/GPSFactor.ipynb` — `GPSFactor(pose_key, gps_measurement_enu, gps_noise_model)` for 3-DoF GPS prior + `GPSFactorArmCalib(pose_key, lever_arm_key, gps_measurement_enu, gps_noise_model)` for GPS with unknown lever-arm calibration

**Tier**: L1 (canonical context7-indexed library documentation at version 4.3a1; cross-validated against canonical Doxygen portal `borglab.github.io/gtsam/`).
**URL**: context7 indexing of <https://github.com/borglab/gtsam/tree/develop/gtsam/navigation/doc/> (canonical Borg Lab navigation documentation; access via context7 server at queried-date 2026-05-08)
**Cross-cite**: Source #86 (canonical `borglab/gtsam` GitHub repo + LICENSE.BSD direct WebFetch — BSD-3-Clause throughout per C4 Fact #54), Source #87 (canonical GTSAM Python examples via context7 query-docs at version 4.3a1 — `CameraResectioning.ipynb` + `Pose2SLAMExample.ipynb` + `PlanarSLAMExample.ipynb` per C4 Fact #54)

**Date Accessed**: 2026-05-08 (~13:00 UTC, immediately after C4 Fact #54 closure — same daily-active GTSAM master branch state)

---

## Source #91 — GTSAM `ISAM2` / `IncrementalFixedLagSmoother` / `Marginals` with iSAM2 results (context7 query-docs at `/borglab/gtsam` — incremental smoothing sub-API)

**Title**: GTSAM canonical `ISAM2` and `IncrementalFixedLagSmoother` incremental smoothing API + `Marginals` posterior recovery for iSAM2 results
**Source**: context7 query-docs at `/borglab/gtsam` version 4.3a1 with 1121 code snippets (queried 2026-05-08 for incremental smoothing sub-API)
**Returned canonical Python notebooks**:
- `gtsam/inference/doc/ISAM.ipynb` — `GaussianISAM(initial_bayes_tree)` constructor + `isam.update(new_factors)` incremental graph modification + `isam.print()` introspection (legacy linear `GaussianISAM`; modern nonlinear `ISAM2` follows the same API pattern with additional `ISAM2Params(relinearizeThreshold, relinearizeSkip, factorization, evaluateNonlinearError, cacheLinearizedFactors, ...)` configuration)
- `python/gtsam/examples/PlanarSLAMExample.ipynb` — `Marginals(graph, result).marginalCovariance(key)` 6×6 posterior covariance recovery (works with both batch `LevenbergMarquardtOptimizer` results and `ISAM2.calculateEstimate()` results)
- `python/gtsam/examples/Pose2SLAMExample.ipynb` — same canonical `PriorFactorPose2(1, Pose2(0, 0, 0), PRIOR_NOISE)` initial-pose anchor pattern; reusable for Pose3 (`PriorFactorPose3(X(0), Pose3(...), prior_noise)`) for project's 3D state estimation
- `gtsam/slam/doc/lago.ipynb` — `lago.initialize(graph)` linear-and-iterative-pose-graph initialization (good for cold-start pose initialization from FC GPS-extrapolated pose at boot per AC-NEW-1)
- `gtsam/slam/doc/InitializePose3.ipynb` — `InitializePose3.initialize(graph)` chordal-relaxation 3D initialization (modern alternative for Pose3 cold-start)
- `gtsam/inference/doc/FactorGraph.ipynb` — `NonlinearFactorGraph()` + `BetweenFactorPose2(X(0), X(1), Pose2(1, 0, 0), odometry_noise)` + `PriorFactorPose2(X(0), Pose2(0, 0, 0), prior_noise)` core factor-graph patterns (project applies Pose3 variants: `BetweenFactorPose3` + `PriorFactorPose3` + `GenericProjectionFactorCal3DS2`)

**Note on `IncrementalFixedLagSmoother`**: context7 query-docs at /borglab/gtsam returned ISAM (legacy GaussianISAM) examples but did NOT return a top-3 `IncrementalFixedLagSmoother` snippet on the queried search. The IncrementalFixedLagSmoother class is documented in the canonical GTSAM source tree at `gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h` (not in the `develop` branch's stable area; in the `gtsam_unstable` namespace, requiring user to opt-in to unstable APIs). Project must verify at Plan-phase Jetson MVE that IncrementalFixedLagSmoother is the correct sliding-window primitive vs writing custom marginalization on top of `ISAM2.marginalizeLeaves(keys_to_marginalize)`.

**Tier**: L1 (canonical context7-indexed library documentation at version 4.3a1) + L2 (IncrementalFixedLagSmoother — gtsam_unstable namespace, verification at Plan phase required).
**URL**: context7 indexing of <https://github.com/borglab/gtsam/tree/develop/gtsam/inference/doc/> + <https://github.com/borglab/gtsam/tree/develop/python/gtsam/examples/> (canonical Borg Lab inference + examples documentation; access via context7 server at queried-date 2026-05-08)
**Cross-cite**: Source #86 + Source #87 + Source #90 (all GTSAM library; same daily-active master branch state)

**Date Accessed**: 2026-05-08

---
