FTHTrading / drone · Production-grade Guidance, Navigation & Control research system
Engineered at senior aerospace level. Tested under dispersion. Authority-aware by design.
drone is a closed-loop 6-DoF UAV simulation and GNC research environment built to
aerospace engineering standards. It provides rigorous state estimation (EKF · UKF · PF ·
15-state Error-State KF), verified trajectory generation (minimum-snap, Dubins), and
authority-aware control (LQR · PID · MPC) with explicit saturation accounting.
This is not a hobby project. Every module is typed, documented, tested, and benchmarked.
┌──────────────────────────────────────────────────────────────────────┐
│ CLOSED-LOOP SIMULATION │
│ ┌─────────────┐ p_ref,v_ref,a_ff ┌────────────────────────────┐ │
│ │ Min-Snap │ ─────────────────▶ │ Outer Loop (LQR) │ │
│ │ Trajectory │ │ pos/vel error + ref gov. │ │
│ └─────────────┘ └─────────────┬──────────────┘ │
│ │ a_des (m/s²) │
│ ┌─────────────▼──────────────┐ │
│ ┌─────────────┐ φ_des, θ_des │ Feasibility Envelope │ │
│ │ Inner Loop │ ◀───────────────── │ h_max = g·tan(θ_max) │ │
│ │ (PID att.) │ └────────────────────────────┘ │
│ └──────┬──────┘ │
│ │ [Fx Fy Fz Mx My Mz] │
│ ┌──────▼──────────────────────────────────────────────────────┐ │
│ │ 6-DoF Quaternion Dynamics (RK4, 1 kHz) │ │
│ │ ṗ=v v̇=R·F/m+g q̇=½Ξ(q)ω ω̇=I⁻¹(M−ω×Iω) │ │
│ └──────┬──────────────────────────────────────────────────────┘ │
│ │ true state │
│ ┌──────▼──────┐ f_meas,ω_meas ┌─────────────────────────────┐ │
│ │ IMU Model │ ──────────────▶ │ Strapdown INS + 15-state │ │
│ │ Allan noise │ │ Error-State Kalman Filter │ │
│ └─────────────┘ └──────────────┬──────────────┘ │
│ ┌─────────────┐ p_gps, v_gps │ p̂, v̂ │
│ │ GPS Model │ ─────────────────────────────┘ │
│ └─────────────┘ │
└──────────────────────────────────────────────────────────────────────┘
Plant (hover linearisation):
A = [[0 I], B = [[0], State: x = [Δp(3), Δv(3)]
[0 0]] [I]]
CARE: AᵀP + PA − PBR⁻¹BᵀP + Q = 0
Gain: K = R⁻¹BᵀP
Tuning: Q = diag[5,5,8, 3,3,4] R = 0.5·I₃
Fundamental constraint — horizontal acceleration is bounded by tilt authority:
a_horiz_max = g · tan(θ_max) # e.g. 40° → 8.2 m/s²
The feasibility envelope projector scales the horizontal demand vector (preserving direction) rather than clipping axes independently. This prevents commanded heading from rotating under saturation.
theta_max(t) = 5° + 35° * min(1.0, t / 5.0) # 5° → 40° over first 5 sGives the attitude loop 83 rise-time cycles to reach full authority, preventing the initial-condition flip caused by attitude bandwidth mismatch.
kp=14.0 ki=0.3 kd=1.0 τ_deriv=0.02 s output sat=±120 Nm
Derivative on measurement (no derivative kick on setpoint step)
Back-calculation anti-windup
src/gnc/
├── dynamics/
│ ├── rigid_body.py Quaternion 6-DoF RK4, VehicleState, euler↔quat
│ └── environment.py ISA atmosphere, WGS-84 J2 gravity, Dryden turbulence
├── estimation/
│ ├── ekf.py Extended Kalman Filter (Van Loan, Joseph form, NIS)
│ ├── ukf.py Unscented KF (scaled UT, 2n+1 sigma points)
│ └── particle_filter.py Bootstrap SIR (log-sum-exp, systematic resample)
├── control/
│ ├── pid.py PID (back-calculation anti-windup, deriv-on-measurement)
│ ├── lqr.py LQR (CARE), integral LQR, finite-horizon sweep
│ └── mpc.py Linear MPC (condensed QP, SLSQP, box constraints)
├── guidance/
│ ├── trajectory.py 7th-order min-snap polynomial, Dubins path
│ └── proportional_nav.py TPN, Augmented PN, Zero-Effort-Miss (orders 2 & 3)
├── navigation/
│ └── ins.py Strapdown INS, 15-state Error-State KF (GPS/INS)
├── sensors/
│ ├── imu.py Allan variance IMU (ARW, VRW, Gauss-Markov bias, SF)
│ └── gps.py GPS (PDOP-scaled), barometric altimeter
└── simulation/
└── simulator.py Closed-loop MC simulator, Logger, per-run factories
| Metric | Target | Measured (nominal) |
|---|---|---|
| 3-D RMS tracking error | < 15 m | 9.67 m |
| X-axis RMS | < 10 m | 2.45 m |
| Navigation RMS | < 2 m | 0.75 m |
| Attitude rise time | < 150 ms | 58 ms (roll) |
| Attitude overshoot | < 10 % | 2.1 % |
| Attitude settling | < 1.5 s | 420 ms |
| MC success rate (±20% mass) | > 60 % | ~65 % |
git clone https://github.com/FTHTrading/drone.git
cd drone
pip install -e ".[dev]"
pytest # 32 tests, ~2 s
python scripts/run_simulation.py # UAV mission → simulation_results.png
python scripts/run_monte_carlo.py # 20-run MC → monte_carlo_results.png
python scripts/run_bandwidth_test.py# Bandwidth → bandwidth_test.png
python scripts/run_intercept.py # PN/ZEM → intercept_engagement.png
python scripts/run_ci_checks.py # CI regression checkmake test # pytest
make sim # nominal simulation
make mc # Monte Carlo
make bandwidth # bandwidth test
make ci # regression checktests/
├── test_control.py PID step response, LQR CARE, integral LQR (8)
├── test_dynamics.py Quaternion round-trip, free-fall, ISA layers (9)
├── test_estimation.py EKF/UKF convergence, NIS consistency (7)
├── test_trajectory.py Min-snap BCs, PN/APN/ZEM guidance (8)
└── golden/
└── baseline_metrics.json CI regression thresholds
32 / 32 tests pass in < 2 s.
python scripts/run_ci_checks.pyRuns 5-case Monte Carlo + bandwidth sweep against tests/golden/baseline_metrics.json.
Exits non-zero if any threshold is breached (used by GitHub Actions).
- No global mutable state — all simulation state in typed dataclasses
- No magic numbers — every physical constant named and documented
- Full type annotations on every public function
- Independent RNGs for sensor noise and turbulence (no cross-correlation)
- Reproducible seeds — deterministic output for any given seed
- Offline-capable — all plots write to disk
MIT — see LICENSE.