Infinity Explorers | SNS312

Navigating the Edge of Space
Through Sensor Fusion

This project represents a comprehensive Hardware-in-the-Loop (HIL) implementation for tracking vertical rocket trajectories. We bridge the gap between high-frequency IMU drift and low-frequency GPS noise using advanced discrete state estimation.

1. The Embedded Architecture

Our hardware layer focuses on deterministic timing and high-throughput data packetization to ensure that mathematical integration errors are kept to a minimum.

01

Asynchronous Polling Strategy

To maintain a constant 100Hz integration rate for the INS while waiting for 1Hz GPS updates, we utilize a non-blocking hardware timer logic.

if (millis() - lastIMU >= 10) {
  readMPU6050();
  packetize();
}
  • ● I2C Fast Mode: 400kHz clock for zero-latency IMU reads.
  • ● Baud Rate: 500,000 bps serial link to prevent buffer bloat.
02

Binary Packetization (C++)

Using ASCII strings for high-speed telemetry is inefficient. We use a packed C++ structure to ensure atomic, thread-safe data transmission.

struct __attribute__((packed)) {
  uint16_t sync = 0xBBAA;
  uint32_t t_ms;
  float acc_z, gps_alt;
  uint8_t fix_flag;
} Packet;

Total Packet Size: 27 Bytes | Transmission Time: 0.54ms

2. The Python Processing Pipeline

To handle high-speed data without UI lag, we implemented a Producer-Consumer architecture using multithreading and thread-safe queues.

Serial Manager

Thread 1: The Producer. Continuously polls the COM port, aligns bytes to the sync word, and pushes data classes to the queue.

Data Processor

Thread 2: The Consumer. Executes kinematics and the Kalman Filter. It handles the delta-t calculations and CSV logging.

Visualizer UI

Main Thread: Uses PyQtGraph for GPU-accelerated plotting, receiving filtered data via Qt Signals.

3. Mathematical Logic: 1D Fusion

The core of our system is the reconciliation of two sensors with opposing error profiles.

Trapezoidal Integration (INS)

Standard Euler integration builds rectangles. We use trapezoids to approximate the area under the acceleration curve, providing better precision during high-acceleration boost phases.

v(k) = v(k-1) + 0.5 * (a(k) + a(k-1)) * dt
p(k) = p(k-1) + 0.5 * (v(k) + v(k-1)) * dt

The Trap: Error grows by t²! Constant bias leads to parabolic divergence.

Discrete Kalman State

We track three states to ensure we can correct for internal sensor bias automatically.

p
Pos
v
Vel
b
Bias

The Covariance Update (Joseph Form)

Standard Kalman updates can become numerically unstable on microcontrollers. We use the Joseph Form to ensure the error covariance matrix (P) remains positive semi-definite.

Pk = (I - K*H) * Pk-pre * (I - K*H)' + K*R*K'

4. Interactive Fusion Lab

Tune the Kalman parameters to see how the filter handles simulated rocket telemetry.

Smooth (Low) Responsive (High)
Trust GPS Dampen GPS
*Low Q makes the filter rely more on its internal model (smooth but slow). High R ignores the jumpy GPS data more aggressively.
Telemetry Comparison: Altitude (m)
Raw GPS Raw INS Kalman Filter