Multi-threaded telemetry processing, 6-DOF EKF, critically-damped
setpoint filtering, and VISCA zoom control on Windows
This paper describes the architecture and key design decisions of the Python ground station software for the MicroRobo Systems GPS beacon tracking system. The application receives binary telemetry from the airborne tracker over XBee 802.15.4, validates CRC-16 integrity, and fuses GPS position and velocity measurements in a 6-DOF Extended Kalman Filter running at 100 Hz. A GimbalControllerThread at 200 Hz applies EKF velocity-based lead compensation, filters the angular setpoint through a critically-damped second-order filter (ζ = 1.0, default ωn = 6.0 rad/s), and writes angle commands to the Teensy 4.1 gimbal controller over USB Serial. VISCA RS-485 controls zoom on a BirdDog MAKI Ultra 20× camera as a function of slant range. All inter-thread communication uses bounded lock-free deques defined in a single module with no upward calls between threads. Binary flight logs are written in MRSLOG03 format with backward-compatible reading of MRSLOG01 and MRSLOG02 files. The critically-damped filter rise time (~650 ms at default tuning) dominates the end-to-end tracking latency; lead compensation with a configurable prediction horizon partially recovers this lag.
The ground station software performs four distinct functions simultaneously: receiving and validating airborne telemetry, estimating target state via sensor fusion, commanding the gimbal at 200 Hz, and presenting a real-time operator interface with 3D visualization, satellite map overlay, and status displays. These functions have incompatible timing requirements — telemetry arrives asynchronously, the EKF runs on a fixed schedule, the gimbal demands minimum-latency command delivery, and the UI refreshes at 60 Hz. The architecture resolves this through a strict threading model in which each function runs in an independent thread communicating exclusively through typed, bounded deques.
The platform is Python 3.11 on Windows 10/11 x64. Python was chosen for the rapid development of the signal processing and visualization layers. The CPython GIL prevents true thread parallelism for CPU-bound work, but the ground station's dominant operations — serial I/O, EKF matrix computation via numpy, and UI rendering — release the GIL sufficiently for the threading model to work correctly in practice. The EKF and gimbal controller threads both meet their timing requirements on a modern Windows workstation.
collections.deque objects defined in ipc_channels.py. Orange arrows indicate the primary control path; navy arrows indicate secondary paths. The ReplayEngine injects into ekf_state_queue directly, bypassing the EKF — the downstream GimbalControllerThread is unaware of the data source.All shared mutable state — every deque, every channel, every dataclass definition — lives in ipc_channels.py. No thread imports from another thread's module. No thread calls a function defined in another thread's module. This constraint is enforced by convention rather than language mechanism, but it makes the data flow auditable from a single file: every channel's producer, consumer, and capacity is documented in one place, and any new data dependency requires an explicit addition to that file.
| Channel | maxlen | Rationale |
|---|---|---|
| raw_telemetry_queue | 100 | Airborne at ≤112 Hz; EKF drains before each predict step; 100 entries = ~1 s headroom at full rate |
| ekf_state_queue | 10 | EKF at 100 Hz; GimbalController reads [-1] only; 10 entries = 100 ms ring is ample |
| render_state_queue | 4 | 200 Hz producer, 60 Hz consumer; [-1] always gives most recent command; 4 entries prevents stale data accumulation |
| log_queue | 500 | Logger may lag one flush cycle (~1 s at 50 Hz drain); 500 entries = 5 s headroom before data loss |
| optical_correction_queue | 4 | Camera tracker at ≤30 Hz; GimbalController reads [-1] only |
| timing_report_queue | 8 | One report per thread per second; very low rate |
SerialReceiver implements a byte-level state machine that scans the incoming byte stream for the two-byte sync pattern (0xAA 0x55), accumulates the subsequent 32 bytes of payload, and validates the trailing CRC-16/CCITT-FALSE. Frames that fail CRC are counted and discarded; the state machine resets to scanning for the next sync pattern. This design tolerates partial frames and arbitrary byte-stream resets without requiring explicit resynchronisation logic.
On successful validation the binary payload is unpacked using Python's struct module into a TelemetryFrame dataclass. Unit conversions are applied at this point — latitude and longitude from 1e-7 degree integers to decimal degrees, velocities from cm/s integers to m/s floats, quaternion components from scaled int16 to unit float. The rx_time field is set to time.perf_counter() immediately after CRC validation, providing a high-resolution receive timestamp for EKF latency compensation.
A LegacyProtocolAdapter thread bridges the older ASCII line-based telemetry format to the same raw_telemetry_queue. This allows the ground station to operate with airborne hardware that has not yet been updated to the binary protocol. The adapter is a migration path — it will be removed once all deployed airborne units run the binary firmware.
The ground station EKF maintains a 6-element state vector: NED position (3) and NED velocity (3). Unlike the airborne EKF, no accelerometer bias states are estimated — the ground station has no accelerometer input. The process model is a constant-velocity plant: position integrates from velocity, and velocity propagates unchanged between measurement updates. This is appropriate for UAS targets at tracking ranges where acceleration changes are small relative to the GPS position uncertainty.
Process noise Q is computed using the Van Loan continuous-discrete noise model rather than a fixed diagonal matrix. The Van Loan formulation ensures that Q scales correctly with dt when the filter runs at irregular intervals — a common occurrence when the OS scheduler introduces jitter into the 100 Hz EKF tick. A fixed Q would over-trust the process model during long gaps and under-trust it during short gaps, leading to systematic position errors during link interruptions.
GPS position and velocity measurements are fused separately. Position measurement noise is set adaptively from the telemetry frame's HDOP encoding: fixes with high HDOP receive proportionally inflated measurement noise, reducing their weight without hard rejection. The covariance update uses the Joseph form rather than the standard P = (I − KH)P form, preserving positive-definiteness under the numerical conditions that arise from high-rate, low-dimensional updates.
Three gates are applied in series before any GPS measurement update is accepted:
| Gate | Criterion | Rationale |
|---|---|---|
| Mahalanobis distance | χ² < 7.815 (3 DOF, 3σ) | Rejects fixes inconsistent with current covariance; principal outlier filter |
| Acceleration plausibility | Implied ΔV / Δt < 5g | Catches GPS multipath spikes that pass the Mahalanobis gate by riding within a large covariance |
| Range gate | Haversine distance from GS < 20 km | Catches GPS receiver faults reporting positions far from the operational area |
The airborne firmware does not transmit the NEO-M9N NAV-PVT iTOW timestamp. The EKF fuses each received GPS fix using the wall-clock time at which the serial byte arrived (rx_time). Under low-latency link conditions this introduces a timing error of 5–10 ms. Under lossy link conditions the error can reach one full GPS epoch (100 ms), causing the EKF to fuse a stale position as if it were current. The Mahalanobis gate partially mitigates this by rejecting fixes that are inconsistent with the predicted position, but does not eliminate the systematic timing error. Adding iTOW to the airborne telemetry frame is the highest-priority cross-component fix.
The end-to-end tracking latency from GPS fix generation to gimbal motion is dominated by the critically-damped filter rise time (~650 ms at default tuning). Without compensation the gimbal would point at where the target was 650 ms ago — at a target ground speed of 20 m/s this corresponds to a 13 m position lag, equivalent to approximately 7.4° of angular error at 100 m range.
Lead compensation projects the EKF position forward by latency_s seconds using the EKF velocity estimate:
pos_lead = pos_ned + vel_ned × latency_s
The lead position is converted to azimuth and elevation angles, which become the CDF setpoint. The recommended tuning range for latency_s is 0.4–0.7 s, covering the pre-filter pipeline (~73–138 ms average-to-worst-case) plus the dominant CDF contribution. The EKF velocity estimate introduces its own latency through the measurement update process — under low GPS rate conditions the velocity estimate may itself be 100–200 ms old, slightly reducing the effective lead. For constant-velocity targets the compensation is accurate; for accelerating targets it introduces an error proportional to target acceleration multiplied by the prediction horizon.
Near-stationary targets (ground speed below approximately 1 m/s) receive a tapered lead to prevent the compensation from amplifying EKF velocity noise into unnecessary gimbal motion. The taper is applied as a linear ramp from 0 to full lead between 0 and 2 m/s ground speed.
The CDF is a second-order linear filter with damping ratio ζ = 1.0. Critical damping eliminates overshoot — the filter reaches its setpoint without crossing it. This is the correct choice for a gimbal command filter: an underdamped response causes the camera to swing past the target on step commands, which is unacceptable for optical tracking.
The filter is discretised using the semi-implicit Euler method at 200 Hz. This method is first-order accurate in time but unconditionally stable for any positive ωn and any step size — a useful property because the 200 Hz tick is not perfectly uniform under Windows scheduling. The filter state comprises the current output position and velocity; both are integrated each tick regardless of whether a new setpoint has arrived, ensuring smooth output even during telemetry dropouts.
| ωn (rad/s) | Rise time (0→90%) | Latency contribution | Appropriate for |
|---|---|---|---|
| 3.0 | ~1.3 s | Very high | Slow, smooth targets; high GPS noise |
| 6.0 (default) | ~650 ms | Dominant | General tracking; lead comp. required |
| 10.0 | ~390 ms | Moderate | Fast targets; GPS noise more visible |
| 20.0 | ~195 ms | Low | Near-passthrough; requires clean GPS |
The ωn spinbox in the UI is currently capped at 10 rad/s. This cap should be raised to 25 rad/s to allow the full tuning range without code changes. The latency_s spinbox is currently capped at 300 ms, below the recommended 400–700 ms range — this cap should be raised to 1000 ms.
The BirdDog MAKI Ultra 20× camera accepts zoom commands via VISCA RS-485 at 9600 baud. Zoom position is commanded as an absolute value in the range [0x0000, 0x4000], where 0x0000 is full wide and 0x4000 is full tele. The ground station maps slant range to zoom position using a configurable smooth-step anchor table — a piecewise cubic interpolation between user-defined (range, zoom) pairs that produces a smooth, monotonically-decreasing zoom curve as the target approaches.
Zoom position is computed at 200 Hz in GimbalControllerThread from the current EKF range estimate. The VISCA I/O thread transmits only when the commanded position changes by more than a threshold and no more than once every 500 ms (2 Hz). This rate limit is imposed by the RS-485 bus capacity and the camera's VISCA command processing rate — sending zoom commands at 200 Hz saturates the bus and causes the camera to ignore or queue commands unpredictably.
The VISCA protocol requires an Address Set command followed by an IF Clear command before the camera will respond to zoom commands. These are sent during the connect sequence when the operator clicks the VISCA connect button in the UI. Skipping initialisation causes the camera to ignore subsequent zoom commands without any error indication. The ground station UI disables the zoom enable checkbox until initialisation completes successfully.
DataLoggerThread writes binary flight logs in MRSLOG03 format. Each file begins with a 28-byte header containing the magic bytes and ground station geodetic coordinates, followed by fixed-length 89-byte records written for every EKF output at ~100 Hz. Records are batched in memory and written to disk in groups of 100 to reduce write syscall frequency; the batch is flushed every 20 ms regardless of batch size to prevent data loss on unexpected shutdown.
The MRSLOG03 format extends the earlier MRSLOG01 (72-byte records, no GS coordinates in header) and MRSLOG02 (72-byte records, GS coordinates added) formats by expanding the record to 89 bytes to include quaternion attitude fields (has_quat flag plus quat_w/x/y/z). All three formats are read transparently by ReplayEngine and convert_json_log.py based on the 8-byte magic field at the start of the file.
| Format | Header | Record | GS in header | Quaternion |
|---|---|---|---|---|
| MRSLOG01 | 8 bytes | 72 bytes | No | No |
| MRSLOG02 | 28 bytes | 72 bytes | Yes | No |
| MRSLOG03 | 28 bytes | 89 bytes | Yes | Yes |
CSVLoggerThread writes a parallel CSV log at 200 Hz containing 13 columns: timestamp, target azimuth and elevation, slant range, commanded azimuth and elevation, zoom target fraction, actual shaft angles from the gimbal feedback telemetry, quadrature voltages Vq for both axes, and tracking errors (command minus actual). The gimbal feedback columns are populated from the GimbalSerial.latest_feedback property, which is updated by the binary feedback parser on every gimbal tick. If no feedback is available the columns are written as 0.0 with no error indication — this is a known limitation for post-flight analysis.
ReplayEngine reads MRSLOG binary files and injects AircraftState records into ekf_state_queue at a configurable playback rate (0.1× to 10×). By injecting at the same queue that the live EKF publishes to, the downstream GimbalControllerThread and UI are unaware of whether they are consuming live or replayed data. Gimbal commands are computed during replay but not transmitted to hardware. This allows complete offline analysis of flight data, including re-tuning the CDF parameters against recorded data before a subsequent flight.
LoopTimingMonitor provides rolling-window interval statistics for each fixed-rate thread. It tracks mean interval, 1-σ jitter, and cumulative missed ticks (intervals exceeding 1.5× the target period). Statistics are recomputed once per second and published to timing_report_queue, from which the UI status bar assembles a single-line diagnostic string:
EKF 100.1Hz ±0.31ms miss=0 | Gimbal 200.0Hz ±0.28ms miss=0
Missed tick counts above zero indicate OS scheduling interference. On a Windows workstation with no competing real-time loads, both threads typically report zero missed ticks during a flight. Missed ticks during USB enumeration or disk I/O bursts are normal and do not affect tracking quality unless they exceed 5–10 consecutive ticks.
The application is tested and supported on Windows 10/11 x64. Serial port naming (COMx vs /dev/ttyXXX), timer resolution calls, and the Windows-specific timeBeginPeriod(1) sleep granularity adjustment are all Windows-specific. The core EKF and gimbal control logic is platform-agnostic, but a Linux or macOS port requires addressing these platform dependencies.
The NED coordinate conversion in geometry.py uses a flat-earth model valid to 0.01% error at ranges up to 10 km from the ground station. At 10 km range the position error introduced by the flat-earth approximation is approximately 1 m — comparable to the GPS position accuracy. Beyond 10 km, an ellipsoidal geodetic model is required.
The latency_s UI spinbox is capped at 300 ms, below the recommended 400–700 ms tuning range. The ωn spinbox is capped at 10 rad/s. Both limits are set via QDoubleSpinBox.setRange() calls with no backend enforcement — raising the UI caps requires no algorithmic changes.
Map tiles are fetched centred on the ground station position at zoom level 18. The 5×5 tile mosaic covers approximately 1.5 km across — targets beyond roughly 750 m from the GS position will not appear on the satellite map overlay, though they continue to be tracked correctly by the EKF and gimbal. Reducing MAP_ZOOM or increasing MAP_GRID_TILES in main.py extends coverage at the cost of reduced resolution or increased tile fetch time.
DetectionBasedCameraTracker implements an OpenCV-based optical tracking loop that publishes angular corrections to optical_correction_queue. GimbalControllerThread adds these corrections to the EKF setpoint before the CDF. The module is under active development and disabled by default. Current limitations include sensitivity to target appearance changes at long zoom and the absence of a handoff protocol between GPS-primary and optical-primary tracking modes.
CRC-16 validation confirms frame integrity but does not authenticate the source. A malicious transmitter producing valid-CRC frames at the correct frequency and on the correct XBee channel would be accepted by the EKF without indication. For operational security in contested environments, a lightweight message authentication code (HMAC-SHA256 truncated to 4 bytes) over the payload is a practical addition that would fit within the existing frame size budget if the iTOW field is added simultaneously.
The ground station software demonstrates that Python 3.11 with numpy is sufficient for 100 Hz EKF execution and 200 Hz gimbal command generation on a Windows workstation, provided the threading model isolates each function's timing requirements correctly. The bounded deque IPC architecture eliminates shared-state races without locks on the hot path, and the single-file IPC module keeps the inter-thread data flow auditable as the codebase grows.
The two most impactful improvements to tracking accuracy are both cross-component: adding the GPS iTOW timestamp to the airborne telemetry frame (eliminating stale measurement fusion) and raising the latency_s spinbox cap to enable full lead compensation tuning. Both changes require coordinated updates across the airborne firmware and ground station parser but involve no fundamental algorithmic redesign.
Source code is available at github.com/lcumbridge/mrs-ground-station. Commercial licensing inquiries should be directed to microrobosys.com.