Position Solvers & Safety Gates

The Trilateration Problem

Ranging is only the first half of the equation. To fly, the Android app must solve for $(x, y, z)$ given the distances $(d_1, d_2, d_3, \dots, d_n)$ to $n$ Access Points at known coordinates.

Non-Linear Least Squares (NLLS)

Since RTT measurements are noisy, a standard geometric intersection will fail. You must use an NLLS solver to find the position that minimizes the error across all anchors.

• Algorithm: The Levenberg-Marquardt algorithm is the industry standard for this.
• Performance: For flight control, this should be implemented in C++ via the Android NDK. Java/Kotlin garbage collection spikes can add fatal latency to the solver loop.

---

The Safety Gate (Validity Checking)

Sending corrupt or jumpy data to ArduPilot is more dangerous than sending no data at all. The Extended Kalman Filter (EKF) can handle a lost signal, but it can be "distracted" by a jumpy one into a crash.

Implementing a Threshold

Your app must implement a "Validity Gate" before sending any MAVLink packets:

1. Standard Deviation: Calculate the $\sigma$ of the latest burst of RTT measurements.
2. Threshold: If $\sigma > 2.0$ meters, STOP sending GPS_INPUT messages immediately.
3. Hysteresis: Do not resume sending data until $\sigma$ has remained below 1.0m for at least 500ms.

The Drone's Reaction

When the app stops sending packets, ArduPilot will detect a "GPS Timeout."

• If you have an Optical Flow sensor: The drone will automatically switch to FlowHold, maintaining a steady hover.
• If not: The drone will switch to AltHold, and the pilot must take manual control.
• The Benefit: This is infinitely safer than the drone "teleporting" 5 meters and crashing into a stadium wall because of a multipath glitch.