rf_tracker_ekf.py

Lives on: the Fusion Pi 5 — takes MAVLink telemetry from the Cube Orange (over USB) and KrakenSDR direction-of-arrival data from both drones, and fuses them into a single GPS estimate of the RF emitter's location. Source lives in this repo under fusion/.

What it does

Each drone carries a GPS and a KrakenSDR array. Both relay their GPS position/heading and a direction-of-arrival (DoA) bearing to the target over a long-range radio link to a shared Cube Orange flight controller, which connects by USB to the Pi 5 running this script. The script demultiplexes MAVLink traffic by system ID, runs an Extended Kalman Filter (EKF) to fuse the two drones' bearings into one target position/velocity estimate, and can optionally command the second ("slave") drone to a better position for triangulation.

class SystemConfig:
    SERIAL_PORT: str = '/dev/ttyACM0'
    BAUD_RATE: int = 115200
    SYSID_MASTER_DRONE: int = 1
    SYSID_SLAVE_DRONE: int = 2
    ENABLE_AUTONOMOUS_REPOSITIONING: bool = True
    OPTIMIZATION_TARGET_UNCERTAINTY_M: float = 10.0
    OPTIMAL_BASELINE_M: float = 500.0

Walkthrough

Data flow:

[UAV 1 (Master): GPS + KrakenSDR] --(radio)--┐
                                              v
[UAV 2 (Slave):  GPS + KrakenSDR] --(radio)--> [Cube Orange] --(USB)--> [Pi 5, this script]

MAVLinkManager reads everything coming over the USB serial link and sorts incoming messages by get_srcSystem() into per-drone state (SYSID_MASTER_DRONE / SYSID_SLAVE_DRONE), so the two drones' telemetry never gets mixed up.

The EKF (TargetEKF class):

def predict(self, t: float):
    ...
    F = np.eye(self.state_dim)
    if self.cfg.TARGET_IS_MOBILE: F[0,2], F[1,3] = dt, dt
    ...
    self.x = F @ self.x
    self.P = F @ self.P @ F.T + Q

The filter tracks four numbers — north position, east position, north velocity, east velocity (N, E, Vn, Ve) — in a flat local frame centered on wherever the first GPS fix came in (the "datum"). predict() advances that estimate forward in time assuming constant velocity; update_bearing() corrects it every time a new DoA reading comes in from either drone:

def update_bearing(self, n, e, bearing_rad, conf, rssi):
    ...
    h_pred = math.atan2(delta_e, delta_n)
    innov = Geodesy.normalize_angle_rad(bearing_rad - h_pred)
    var_r = self._compute_r(conf, rssi)

Each measurement is only a bearing (an angle), not a position, so the measurement model is nonlinear — that's why this needs to be an EKF rather than a plain linear Kalman filter. _compute_r() weighs each bearing by how much the filter should trust it: a low-confidence or weak-signal (RSSI) reading pulls the estimate less than a strong one.

Cooperative repositioning (CooperativePositioning class):

Two bearings taken from nearly the same angle don't triangulate well — the best geometry has the two drones roughly 90° apart as seen from the target. When ENABLE_AUTONOMOUS_REPOSITIONING is on and the EKF's position uncertainty is worse than OPTIMIZATION_TARGET_UNCERTAINTY_M, this class works out a better waypoint for the slave drone and sends it a MAV_CMD_DO_REPOSITION command.

Output:

The fused target estimate goes out two ways: a MAVLink STATUSTEXT message (so it shows up directly in Mission Planner) and a JSON packet broadcast over UDP for any other ground station tooling.

Simulation mode:

Running with --sim swaps in SimulationEngine, which fakes two moving drones and a moving target so the EKF and repositioning logic can be exercised without any hardware — useful for bench-testing while the Cube/KrakenSDR link is still being verified.