Real-time GPS-denied INS for UAVs using a Pixhawk Cube Orange and a Raspberry Pi 4.
We basically ported a giant MATLAB headache into Python, slapped it on a drone, and it actually flies.
NavCore-Pixhawk is the result of asking, "What if the GPS dies and the drone panics?" It implements a tightly-coupled Inertial Navigation System (INS) that fuses IMU, barometer, and magnetometer data. It uses a 16-state Error-State Quaternion EKF (ESKF), which is just a fancy way of saying "a math wizard that guesses where the drone is so it doesn't crash."
We take these guesses and feed them back into ArduPilot's EKF3 as a fake GPS signal via VISION_POSITION_ESTIMATE. ArduPilot is happy, the drone flies, and we get to look like geniuses.
We pushed the codebase beyond a simple Kalman Filter by adding advanced perception and safety features:
- ML Predictive Safety: An unsupervised
IsolationForestruns in the background. If it detects anomalous vibration or filter variance, it flags an imminent failure before the drone diverges. - 3D Lidar & Radar Fusion: Tailored for the Livox Mid-360 and TI mmWave radar. It handles voxel downsampling and Doppler velocity averaging.
- Asynchronous Execution: Heavy math like point cloud downsampling and ML inference is offloaded to a
ThreadPoolExecutorso it never blocks the 100Hz real-time loop. - Smart Return to Home (RTH): If a fault occurs, the companion computer commands the drone back to launch. It uses Lidar to slow down for obstacles and respects altitude (no blind climbing into ceilings).
| Parameter | Value |
|---|---|
| Estimator | 16-state Error-State Quaternion EKF (ESKF) |
| EKF Rate | 50 Hz / 100 Hz (configurable) |
| Sensors Fused | IMU (ICM-42688) + Baro (MS5611) + Mag (RM3100) + Livox Lidar + TI Radar |
| Position RMSE | 0.4 -- 0.8 m (simulation only -- real-flight validation requires RTK/AprilTag ground truth) |
| Protocol | MAVLink 2.0 via pymavlink |
| GPS Injection | VISION_POSITION_ESTIMATE into ArduPilot EKF3 |
| Logging | CSV at 50 Hz + structured JSONL + optional UDP telemetry to GCS |
| Safety | Hard velocity/tilt/position-jump limits, automatic vision injection disable on fault |
| Platform | Hexacopter UAV |
Important
Live Flight Test Validation
Live flight test of the hexacopter with NavCore-Pixhawk INS active, validating real-time state estimation performance under actual flight conditions. GPS was enabled during this test solely as a safety fallback and was not used as a navigation input to the INS pipeline.
If the video does not render inline, download flight.mp4 directly from the repository.
Five-waypoint autonomous mission configured in Mission Planner GCS over satellite imagery. Waypoints are set at 100 m AGL with computed distances and azimuth bearings between each point. The total mission covers approximately 0.8 km with loiter radius and altitude verification enabled. Connected via COM3 at 115200 baud.
Mission Planner compass priority and onboard magnetometer calibration interface. Six compass sensors detected: primary UAVCAN compass (DevID 97539), SPI-based LSM303D and AK8963, and three additional UAVCAN sensors. The onboard MagCal panel provides per-magnetometer calibration progress bars (Mag 1/2/3) with fitness validation. Proper compass calibration is critical for accurate heading estimation in GPS-denied navigation.
Carbon-fibre hexacopter frame with Pixhawk Cube Orange flight controller (orange housing, centre-mounted), external GPS/compass module on mast, ESCs with XT60 connectors, and telemetry radio. The companion computer (Raspberry Pi 4) connects via TELEM2 UART for real-time INS data streaming.
NavCore-Pixhawk/
├── src/
│ ├── core/
│ │ ├── m.py <- Main entry point & MAVLink param server
│ │ ├── eskf.py <- 16-state Error-State Quaternion EKF
│ │ └── dr.py <- Fallback dead-reckoning
│ ├── fusion/
│ │ ├── lr.py <- Livox Lidar & TI Radar fusion
│ │ └── opt_flow.py <- Optical flow velocity estimator
│ ├── safety/
│ │ ├── mlp.py <- Machine Learning failure predictor
│ │ ├── fault.py <- Fault manager & state machine
│ │ └── safety.py <- Hard limits enforcer
│ ├── interfaces/
│ │ └── mavlink.py <- MAVLink 2.0 interface
│ ├── logger/ <- CSV and JSONL loggers
│ └── utils/ <- PIDs, noise params, time sync
├── tests/ <- PyTest suite & benchmarks
├── config/ <- Tunable noise parameters
├── scripts/ <- RPi4 setup scripts
└── README.md
Top-level entry point that orchestrates the entire INS pipeline. Initialises all subsystems (MAVLink bridge, EKF, dead-reckoning, adaptive PID, optical flow), manages the real-time main loop, and handles graceful shutdown via SIGINT/SIGTERM. The main loop dispatches incoming MAVLink messages (RAW_IMU, SCALED_PRESSURE, SCALED_IMU2/3, ATTITUDE, GPS_RAW_INT, OPTICAL_FLOW_RAD) to their respective handlers. Periodic tasks include CSV logging at 50 Hz, console output at 10 Hz, and system statistics every 5 seconds. Monitors Raspberry Pi CPU temperature and sends MAVLink STATUSTEXT warnings when it exceeds 80 C. Supports UART, USB, and TCP (SITL) connections via command-line arguments.
Production state estimation engine implementing a 16-state Error-State Kalman Filter with quaternion attitude representation. State vector: x = [px, py, pz, vx, vy, vz, qw, qx, qy, qz, ba_x, ba_y, ba_z, bg_x, bg_y, bg_z] covering position (NED), velocity (NED), attitude quaternion (gimbal-lock-free), accelerometer bias, and gyroscope bias. The error state uses a 15-dimensional vector with rotation error parameterised as a 3-vector. The predict step performs IMU mechanisation with bias compensation using the rotation matrix derived from the quaternion. Measurement updates for barometric altitude and magnetometer yaw use Joseph-form covariance updates (P = (I-KH)P(I-KH)^T + KRK^T) with innovation gating and 3-tier magnetometer rejection. Covariance hardening enforces symmetry and bounds eigenvalues every step. The legacy Euler-angle EKF has been permanently removed.
Low-level MAVLink 2.0 communication layer for the Pixhawk Cube Orange. Handles connection management (UART/USB/TCP with auto-reconnect), heartbeat handshake, and data stream rate configuration using both legacy REQUEST_DATA_STREAM and modern SET_MESSAGE_INTERVAL commands. Sensor parsers convert raw MAVLink messages to engineering units: RAW_IMU uses ICM-42688 scale factors (2048 LSB/g for accel, 16.384 LSB/deg/s for gyro), barometric altitude via ISA formula from MS5611 pressure, and magnetometer yaw from RM3100 horizontal field components with validity checking. Includes command helpers for arm/disarm, flight mode setting, statustext broadcasting, and VISION_POSITION_ESTIMATE injection.
Sensor noise model for Pixhawk Cube Orange hardware. Stores standard deviations for accelerometer (0.05 m/s^2), gyroscope (0.005 rad/s), barometer (0.30 m), and magnetometer (0.02 rad) white noise, plus bias instability parameters with 300-second correlation time. Values derived from ICM-42688-P, MS5611, and RM3100 datasheets. Loads override values from config/noise_params.yaml at runtime, falling back to hardcoded defaults if the file is absent or malformed.
The main entry point. Orchestrates the 100Hz loop, handles MAVLink communication, and serves as a parameter server for Mission Planner. It also manages the background thread pool for heavy math.
The core navigation filter. Fuses IMU, Baro, Mag, and now Lidar/Radar data using a 16-state error-state formulation.
Processes Livox Mid-360 point clouds and TI mmWave radar targets. Handles voxel downsampling and Doppler velocity averaging.
Uses an unsupervised IsolationForest to predict imminent hardware or sensor failures based on state variance.
Comprehensive pytest suite covering: EKF initial state verification, covariance positive-definiteness, uncertainty growth under prediction-only operation, gravity cancellation for stationary hover, barometric altitude correction, magnetometer yaw convergence, covariance reduction after measurement updates, state reset, and angle wrapping. Dead-reckoning tests verify stationary drift bounds and forward motion response. Noise parameter tests validate positive defaults and summary string generation.
Standalone performance benchmark requiring no hardware. Generates a 30-second synthetic UAV trajectory (vertical climb, figure-8, banked turn, descent) with noisy IMU measurements, then runs both EKF and dead-reckoning in parallel. Reports wall time, effective processing rate, position RMSE for both estimators, drift improvement percentage, and per-axis error breakdown. Validates that the Python EKF achieves sub-metre accuracy and processes faster than real-time on Raspberry Pi 4 hardware.
Runtime-configurable sensor noise parameters for the Pixhawk Cube Orange sensor suite. Values correspond to ICM-42688-P accelerometer and gyroscope, MS5611 barometer, and RM3100 magnetometer. Tuning guidelines: decrease baro.std to reduce vertical drift, increase mag.std to reduce yaw oscillation, decrease imu.accel_std to reduce overall position drift.
Automated Raspberry Pi 4 provisioning script. Installs system packages, creates a Python virtual environment with pymavlink/numpy/pyyaml/pytest, configures UART by disabling Bluetooth and enabling ttyAMA0, installs a systemd service for auto-start on boot with automatic restart on failure, and runs the software benchmark to validate the installation. Post-setup, the INS starts automatically after each reboot.
The system follows a three-layer pipeline: sensor acquisition, state estimation, and output distribution.
graph TD
%% Configuration & Time Sync
Config["config_loader.py<br>(YAML Schema Validation)"]
TimeSync["time_sync.py<br>(Hardware Timestamps)"]
%% Sensor Layer (Hardware)
subgraph "Sensor Acquisition Layer (Hardware: Pixhawk Cube Orange)"
PX4["ICM-42688-P IMU<br>MS5611 Barometer<br>RM3100 Magnetometer<br>Optical Flow (Optional)"]
MAV["MAVLink 2.0 Interface<br>UART @ 921600 baud"]
PX4 -- "RAW_IMU (100Hz)<br>SCALED_PRESSURE (10Hz)<br>SCALED_IMU3 (50Hz)" --> MAV
end
%% State Estimation (Raspberry Pi 4)
subgraph "State Estimation Layer (Compute: Raspberry Pi 4)"
Bridge["mavlink_bridge.py<br>(Message Parser & Dispatch)"]
subgraph "Primary Navigation Core"
ESKF["eskf_core.py<br>16-State Error-State Quaternion EKF"]
Predict["Predict Step (100Hz)<br>x̄ = f(x, u)<br>P = FPFᵀ + QΔt"]
Update["Update Step (10-50Hz)<br>y = z - h(x̄)<br>P = (I - KH)P(I - KH)ᵀ + KRKᵀ"]
Harden["Numerical Hardening<br>Eigenvalue Bounds & Symmetry"]
ESKF -.-> Predict
Predict --> Update
Update --> Harden
end
subgraph "Redundancy & Safety Handlers"
DR["dead_reckon.py<br>(Strapdown Fallback)"]
Flow["optical_flow_ins.py<br>(Velocity Cross-check)"]
Fault["fault_manager.py<br>(Sensor Dropout & Escalation)"]
Safety["safety_monitor.py<br>(Velocity/Tilt Limits)"]
end
Config -. "Noise Params" .-> ESKF
TimeSync -. "Sync" .-> Bridge
MAV -- "Binary Stream" --> Bridge
Bridge -- "Accel/Gyro/Baro/Mag" --> ESKF
Bridge -- "Raw IMU" --> DR
Bridge -- "Flow/Gyro" --> Flow
Harden -- "State: [p, v, q, b_a, b_g]" --> Safety
Flow -. "Flow Vel" .-> Fault
DR -. "DR Pose" .-> Fault
Harden -. "Innovations / Covariance" .-> Fault
Fault -- "Trigger Mode Switch" --> Safety
end
%% Output Distribution
subgraph "Output & Control Layer"
Log["ins_logger.py / structured_logger.py<br>CSV (50Hz) + JSONL"]
Vis["vision_position_injector.py<br>VISION_POSITION_ESTIMATE (30Hz)"]
Ctrl["adaptive_pid.py<br>Gain-Scheduled Control"]
ArduPilot["ArduPilot EKF3<br>(Flight Controller)"]
Safety -- "Validated State" --> Log
Safety -- "Validated State" --> Vis
Safety -- "Validated State" --> Ctrl
Vis -- "Fused Global Pose" --> ArduPilot
Ctrl -- "Thrust/Attitude Commands" --> ArduPilot
end
Sensor Layer -- The Pixhawk Cube Orange streams raw IMU data at 100 Hz, barometric pressure at 10 Hz, and magnetometer readings at 50 Hz over a MAVLink 2.0 UART link at 921600 baud.
Estimation Layer -- mavlink_bridge.py on the Raspberry Pi 4 receives and parses RAW_IMU, SCALED_PRESSURE, and SCALED_IMU3 messages. The parsed sensor data is passed to eskf_core.py, which implements a 16-state Error-State Quaternion EKF:
- State Vector:
x = [px, py, pz, vx, vy, vz, qw, qx, qy, qz, ba_x, ba_y, ba_z, bg_x, bg_y, bg_z] - Predict: IMU mechanisation with quaternion rotation and bias compensation
- Propagate: Covariance via
P = F * P * F^T + Q * dtwith Joseph-form measurement updates - Harden: Symmetry enforcement, eigenvalue bounding, condition number monitoring
- Safety Gate: Hard velocity/tilt/position-jump limits via
safety_monitor.py
Output Layer -- Estimated states are distributed to: ins_logger (CSV at 50 Hz), vision_position_injector (30 Hz to ArduPilot EKF3), adaptive_pid (gain-scheduled altitude control), and console output for real-time monitoring.
Pixhawk Cube Orange Raspberry Pi 4
---------------------------------------------------------------------------
TELEM2 TX (3.3 V logic) -------- GPIO 15 / Pin 10 (RXD)
TELEM2 RX -------- GPIO 14 / Pin 8 (TXD)
TELEM2 GND -------- Pin 6 (GND)
WARNING: Do NOT connect 5V. Cube TELEM2 uses 3.3 V logic levels.
Baud rate: 921600
| Parameter | Value | Description |
|---|---|---|
SERIAL2_BAUD |
921 |
921600 baud |
SERIAL2_PROTOCOL |
2 |
MAVLink 2 |
EK3_SRC1_POSXY |
6 |
Position from ExternalNav (INS output) |
EK3_SRC1_POSZ |
6 |
Altitude from ExternalNav |
EK3_SRC1_YAW |
6 |
Yaw from ExternalNav |
VISO_TYPE |
1 |
Enable vision odometry input |
git clone https://github.com/ARYA-mgc/NavCore-Pixhawk.git
cd NavCore-Pixhawk
chmod +x scripts/setup_rpi4.sh
./scripts/setup_rpi4.sh
sudo rebootsource ~/ins-venv/bin/activate
cd src
python main_ins_navigation.py --connection /dev/ttyAMA0 --baud 921600 --hz 100python main_ins_navigation.py --connection /dev/ttyACM0 --baud 115200 --hz 100python main_ins_navigation.py --connection tcp:127.0.0.1:5760 --hz 100python tests/benchmark_sitl.pyExpected output:
==================================================
Benchmark @ 100 Hz (dt=10 ms, 3000 steps)
==================================================
Wall time : 412.3 ms
Effective rate : 7280 Hz
EKF Pos RMSE : 0.421 m
DR Pos RMSE : 0.489 m
Drift improvement : 13.9 %
Per-axis RMSE X=0.231 Y=0.284 Z=0.187 m
==================================================
pytest tests/test_ins.py -vEdit config/noise_params.yaml to adjust filter behaviour:
imu:
accel_std: 0.05 # Lower value = trust IMU more = tighter position
gyro_std: 0.005
baro:
std: 0.30 # Lower value = trust barometer more = less Z drift
mag:
std: 0.02 # Higher value = trust magnetometer less = less yaw oscillationTuning guidelines (consistent with MATLAB version):
- Vertical position drift: decrease
baro.std - Yaw oscillation: increase
mag.std - Overall position drift: decrease
imu.accel_std
The EKF core and navigation algorithms were first prototyped and validated in a high-fidelity MATLAB/Simulink simulation environment before being ported to Python for embedded deployment on the Raspberry Pi 4. The simulation uses a 15-state Euler-angle EKF; the production system has since migrated to a 16-state quaternion ESKF.
For full simulation documentation, results, and visual analysis, see the MATLAB Simulation README.
- Groves, P.D. (2013). Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems.
- Farrell, J.A. (2008). Aided Navigation: GPS with High Rate Sensors.
- Kalman, R.E. (1960). A New Approach to Linear Filtering and Prediction Problems.
This section documents current constraints honestly. Yes, we know about them. No, we aren't fixing them today.
Accuracy Validation: The 0.4-0.8 m RMSE figure is validated in simulation only. In real life? Well, it hasn't hit a tree yet, but real-flight accuracy still needs RTK GPS ground truth to be mathematically proven.
State Representation: We use a 16-state Error-State Quaternion EKF. The legacy Euler-angle EKF was permanently deleted because gimbal lock is for losers. There is no fallback.
Sensor Limitations: Pure IMU + barometer + magnetometer fusion will drift over time without external correction. The magnetometer is treated as unreliable by default — 3-tier rejection (norm check, EMI cooldown, multi-sample re-enable) mitigates interference but cannot eliminate it. Long-duration GPS-denied flights require visual-inertial correction.
Optical Flow: Flow-based velocity estimation requires a valid rangefinder (distance > 0.05m) for height scaling. Without range data, flow fusion is completely disabled. High angular rate (> 1.5 rad/s) samples are also rejected to prevent motion smearing. Low-texture surfaces produce unreliable flow.
Real-Time Constraints: Python with GIL cannot guarantee hard real-time scheduling. Loop jitter is monitored via loop_monitor.py with histogram tracking and OS-level scheduling hints, but timing is not bounded. For flight-critical production deployments, the C++ port (cpp_port/) with SCHED_FIFO priority is recommended.
Bias Tuning: Gauss-Markov bias correlation time (tau) significantly affects convergence. Use allan_variance.py with stationary sensor logs to extract proper noise parameters. The default tau=300s works for typical flights but should be tuned per-airframe.
- Never arm the drone from code unless the area is clear of personnel and obstacles.
- Test with propellers removed before live flights.
- The
vision_position_injectoris disabled by default. It auto-enables only after the ESKF reports HEALTHY status and auto-disables on FAULT. - Innovation gating rejects anomalous sensor readings, but is not a substitute for pilot override capability.
- Always have an RC transmitter ready for manual override.
| Priority | Item | Status |
|---|---|---|
| High | Error-State Quaternion EKF (ESKF) | Done (eskf_core.py) |
| High | Legacy Euler EKF removal | Done (permanently deleted) |
| High | Joseph-form covariance updates | Done |
| High | Innovation gating (Mahalanobis) | Done |
| High | 3-tier magnetometer rejection | Done (multi-sample re-enable) |
| High | Sensor initialization from accel+mag | Done |
| High | Hard safety enforcement (velocity/tilt/position-jump) | Done (safety_monitor.py) |
| High | Hardware timestamp synchronization | Done (time_sync.py) |
| High | Strict config validation (fail-fast) | Done (config_loader.py) |
| High | Failure scenario testing | Done (sensor dropout, noise spike, covariance) |
| Medium | Monte Carlo validation (100+ runs) | Done (monte_carlo_sim.py) |
| Medium | Ground truth evaluation tool (APE/RPE) | Done (ground_truth_eval.py) |
| Medium | ESKF vs EKF3 divergence analysis | Done (ekf_comparison.py) |
| Medium | Allan Variance auto-tuning | Done (allan_variance.py) |
| Medium | Offline log replay | Done (log_replay.py) |
| Medium | Structured JSONL logging | Done (structured_logger.py) |
| Medium | Loop timing and jitter monitoring | Done (loop_monitor.py) |
| Medium | Fault manager state machine | Done (fault_manager.py) |
| Medium | C++17 + Eigen3 port scaffold | Done (cpp_port/) |
| Medium | Visual-Inertial Odometry (VIO) | Done (vio_pipeline.py) |
| Medium | ROS2 interface (/odom, /imu topics) | Done (ros2_interface.py) |
| Medium | UWB range fusion | Done (uwb_fusion.py) |
| Medium | ArduPilot EKF3 blending (Covariance Intersection) | Done (ekf3_blender.py) |
| Medium | SLAM pose fusion (Umeyama alignment) | Done (slam_interface.py) |
| Medium | Generic external measurement update | Done (eskf_core.py: update_external) |
| Future | Real-flight RTK ground truth validation | Pending hardware test |
ARYA MGC
MATLAB simulation prototype: ins-system-for-drone (simulation only -- NavCore-Pixhawk is the hardware + real-time implementation)
MIT License -- see LICENSE for details.