诊断 PX4 + FAST-LIO 室内 SLAM 无人机的“马桶效应”：理解自主飞行前的位置、偏航、磁力计和 EKF 行为

Diagnosing Toilet-Bowl Behavior in PX4 + FAST-LIO Indoor SLAM Drone

Introduction

A toilet-bowl effect is one of the more dangerous behaviors in an indoor position-controlled drone. Instead of holding one position, the aircraft begins making a circular or spiral movement around the desired hold point.

In a SLAM drone, this can be confusing because the SLAM position itself may look excellent. FAST-LIO may report only millimeter-level movement while the aircraft still circles during POSCTL or Offboard flight.

The important lesson is:
Good position estimation alone is not enough. The position estimate and the heading reference must also remain geometrically consistent.

The System Chain

Livox MID-360S │ ├── LiDAR └── IMU ↓ FAST-LIO ↓ /Odometry ↓ FAST-LIO → MAVROS bridge ↓ /mavros/odometry/out ↓ PX4 EKF ↓ /mavros/local_position/odom ↓ PX4 Position Control ↓ Drone

At the same time, PX4 has its own heading estimate:
Magnetometer → Gyroscope → EKF heading fusion → PX4 yaw

If the horizontal position is correct but PX4 yaw slowly rotates, the position controller may apply corrections in a direction that is slightly rotated from the correct one.

Test 0 — First verify FAST-LIO position stability

Before diagnosing PX4, establish that SLAM itself is not moving significantly while the drone is physically stationary.

ros2 topic echo /Odometry --field pose.pose.position

x: 0.0488 y: -0.0007 z: -0.0271
x: 0.0482 y: 0.0010 z: -0.0266
x: 0.0496 y: 0.0001 z: -0.0287

The absolute values do not need to be zero. What matters is the variation.
A few millimeters of noise is normal.
Suspicious: 0.048 → 0.060 → 0.085 → 0.130 → 0.220 while stationary.

First question: Δp ≈ 0? If FAST-LIO itself is stable, continue downstream.

Test 1 — FAST-LIO vs PX4 Position and Yaw

This is the most useful general toilet-bowl diagnostic. It simultaneously shows ARM/DISARM state, flight mode, FAST-LIO X/Y, PX4 X/Y, FAST-LIO yaw, PX4 yaw, and the offset between them.

The purpose is not to make FAST-LIO yaw and PX4 yaw numerically equal. They can have different zero headings. The important quantity is Δψ = ψ_PX4 − ψ_FAST. If that offset remains approximately constant, the two coordinate frames maintain a stable relationship.

source /opt/ros/humble/setup.bash
source ~/slam_ws/install/setup.bash
source ~/super_ws/install/setup.bash

python3 - <<'PY'
import rclpy
import math
import time
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
from nav_msgs.msg import Odometry
from mavros_msgs.msg import State

def yaw_deg(q):
 return math.degrees(math.atan2(
 2.0 * (q.w * q.z + q.x * q.y),
 1.0 - 2.0 * (q.y * q.y + q.z * q.z)
 ))

def wrap(a):
 return (a + 180.0) % 360.0 - 180.0

class Check(Node):
 def __init__(self):
 super().__init__('toilet_bowl_check')
 qos = QoSProfile(
 reliability=ReliabilityPolicy.BEST_EFFORT,
 history=HistoryPolicy.KEEP_LAST,
 depth=10
 )
 self.fast = None
 self.px4 = None
 self.armed = None
 self.mode = "UNKNOWN"
 self.prev_armed = None
 self.prev_mode = None
 self.start_time = time.monotonic()

 self.create_subscription(Odometry, '/Odometry', self.fast_cb, qos)
 self.create_subscription(Odometry, '/mavros/local_position/odom', self.px4_cb, qos)
 self.create_subscription(State, '/mavros/state', self.state_cb, qos)
 self.create_timer(0.5, self.show)

 def fast_cb(self, m):
 p = m.pose.pose.position
 q = m.pose.pose.orientation
 self.fast = (p.x, p.y, p.z, yaw_deg(q))

 def px4_cb(self, m):
 p = m.pose.pose.position
 q = m.pose.pose.orientation
 self.px4 = (p.x, p.y, p.z, yaw_deg(q))

 def state_cb(self, m):
 self.armed = m.armed
 self.mode = m.mode

 def show(self):
 if self.fast is None or self.px4 is None or self.armed is None:
 return
 fx, fy, fz, fyaw = self.fast
 px, py, pz, pyaw = self.px4
 armed_str = "ARMED" if self.armed else "DISARMED"
 print(f"[{time.monotonic() - self.start_time:6.1f}s] {armed_str} mode={self.mode}")
 print(f" FAST: x={fx:8.4f} y={fy:8.4f} z={fz:8.4f} yaw={fyaw:8.2f}")
 print(f" PX4: x={px:8.4f} y={py:8.4f} z={pz:8.4f} yaw={pyaw:8.2f}")
 print(f" OFFSET: {wrap(pyaw - fyaw):8.2f}")
 print("-" * 70)

rclpy.init()
node = Check()
rclpy.spin(node)
PY

Healthy pattern:
FAST yaw ─────────────────
PX4 yaw ─────────────────
OFFSET approximately constant

Suspicious pattern:
FAST yaw ─────────────────
PX4 yaw ───────────╲
╲
╲
OFFSET continuously changes

Test 2 — Is the Magnetometer Causing the Yaw Movement?

After discovering PX4 yaw movement correlated with ARM, the next question is: Is the raw magnetic field changing when the motors/current become active?

source /opt/ros/humble/setup.bash
source ~/slam_ws/install/setup.bash
source ~/super_ws/install/setup.bash

python3 - <<'PY'
import rclpy
import math
import time
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
from nav_msgs.msg import Odometry
from mavros_msgs.msg import State
from sensor_msgs.msg import MagneticField

def yaw_deg(q):
 return math.degrees(math.atan2(
 2.0 * (q.w*q.z + q.x*q.y),
 1.0 - 2.0 * (q.y*q.y + q.z*q.z)
 ))

def mag_magnitude(m):
 return math.sqrt(m.x*m.x + m.y*m.y + m.z*m.z)

class Check(Node):
 def __init__(self):
 super().__init__('mag_yaw_check')
 qos = QoSProfile(
 reliability=ReliabilityPolicy.BEST_EFFORT,
 history=HistoryPolicy.KEEP_LAST,
 depth=10
 )
 self.fast_yaw = None
 self.px4_yaw = None
 self.mag = None
 self.armed = None
 self.mode = "UNKNOWN"
 self.start_time = time.monotonic()

 self.create_subscription(Odometry, '/Odometry', self.fast_cb, qos)
 self.create_subscription(Odometry, '/mavros/local_position/odom', self.px4_cb, qos)
 self.create_subscription(MagneticField, '/mavros/imu/mag', self.mag_cb, qos)
 self.create_subscription(State, '/mavros/state', self.state_cb, qos)
 self.create_timer(0.5, self.show)

 def fast_cb(self, m):
 q = m.pose.pose.orientation
 self.fast_yaw = yaw_deg(q)

 def px4_cb(self, m):
 q = m.pose.pose.orientation
 self.px4_yaw = yaw_deg(q)

 def mag_cb(self, m):
 self.mag = m.magnetic_field

 def state_cb(self, m):
 self.armed = m.armed
 self.mode = m.mode

 def show(self):
 if self.fast_yaw is None or self.px4_yaw is None or self.mag is None or self.armed is None:
 return
 armed_str = "ARMED" if self.armed else "DISARMED"
 m = self.mag
 print(f"[{time.monotonic() - self.start_time:6.1f}s] {armed_str} mode={self.mode}")
 print(f" FAST yaw: {self.fast_yaw:8.2f} PX4 yaw: {self.px4_yaw:8.2f}")
 print(f" MAG: Bx={m.x:8.2f} By={m.y:8.2f} Bz={m.z:8.2f} |B|={mag_magnitude(m):8.2f}")
 print("-" * 70)

rclpy.init()
node = Check()
rclpy.spin(node)
PY

This test answers a different question:
Test 1: "Are FAST-LIO and PX4 yaw remaining geometrically aligned?"
Test 2: "If PX4 yaw changes, does the physical magnetometer measurement change at the same time?"

Test 3 — What Is PX4's EKF Doing Internally?

The third diagnostic asks: What estimator mode is PX4 reporting before and after ARM?

source /opt/ros/humble/setup.bash
source ~/slam_ws/install/setup.bash
source ~/super_ws/install/setup.bash

python3 - <<'PY'
import rclpy
import math
import time
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
from nav_msgs.msg import Odometry
from mavros_msgs.msg import State, EstimatorStatus

def yaw_deg(q):
 return math.degrees(math.atan2(
 2.0 * (q.w*q.z + q.x*q.y),
 1.0 - 2.0 * (q.y*q.y + q.z*q.z)
 ))

class Check(Node):
 def __init__(self):
 super().__init__('ekf_arm_check')
 qos = QoSProfile(
 reliability=ReliabilityPolicy.BEST_EFFORT,
 history=HistoryPolicy.KEEP_LAST,
 depth=10
 )
 self.armed = None
 self.mode = "UNKNOWN"
 self.px4_yaw = None
 self.est = None
 self.start = time.monotonic()

 self.create_subscription(State, '/mavros/state', self.state_cb, qos)
 self.create_subscription(Odometry, '/mavros/local_position/odom', self.odom_cb, qos)
 self.create_subscription(EstimatorStatus, '/mavros/estimator_status', self.est_cb, qos)
 self.create_timer(0.5, self.show)

 def state_cb(self, m):
 self.armed = m.armed
 self.mode = m.mode

 def odom_cb(self, m):
 q = m.pose.pose.orientation
 self.px4_yaw = yaw_deg(q)

 def est_cb(self, m):
 self.est = m

 def show(self):
 if self.armed is None or self.px4_yaw is None or self.est is None:
 return
 armed_str = "ARMED" if self.armed else "DISARMED"
 e = self.est
 print(f"[{time.monotonic() - self.start:6.1f}s] {armed_str} mode={self.mode}")
 print(f" PX4 yaw: {self.px4_yaw:8.2f}")
 print(f" EKF flags: CONST_POS={e.const_pos} HREL={e.hrel} PRED_HREL={e.pred_hrel} ATT={e.att} GPS_GLITCH={e.gps_glitch} ACC_ERR={e.acc_err}")
 print("-" * 70)

rclpy.init()
node = Check()
rclpy.spin(node)
PY

Key fields:
CONST_POS — estimator reports constant-position mode
HREL — relative horizontal position estimate valid
PRED_HREL — predicted relative horizontal position valid
ATT — attitude estimate valid
GPS_GLITCH — GPS glitch flag
ACC_ERR — accelerometer error flag

A healthy active horizontal-aiding state often shows: HREL=1, PRED_HREL=1, ATT=1, GPS_GLITCH=0, ACC_ERR=0

Test 4 — Verify the External-Vision Pipeline Is Actually Active

echo "=== FAST-LIO ==="
timeout 5 ros2 topic hz /Odometry

echo "=== PX4 EV INPUT ==="
timeout 5 ros2 topic hz /mavros/odometry/out

echo "=== PX4 LOCAL ==="
timeout 5 ros2 topic hz /mavros/local_position/odom

/Odometry: ~10 Hz
/mavros/odometry/out: ~10 Hz
/mavros/local_position/odom: ~30 Hz

FAST-LIO	EV input	PX4 local	Interpretation
10 Hz	10 Hz	~30 Hz	normal chain
0 Hz	0 Hz	may exist	FAST-LIO/LiDAR issue
10 Hz	0 Hz	exists	bridge/MAVROS issue
10 Hz	10 Hz	missing	PX4/MAVROS estimator issue
all healthy	all healthy	drifting	fusion/frame/sensor issue

Test 5 — Physical Yaw Rotation Test

Once yaw is stable while stationary, physically rotate the drone with props removed. For example, ~45° clockwise. Then observe PX4 yaw.

BEFORE: FAST = +20°, PX4 = -10°, OFFSET = -30°
Rotate 45°
AFTER: FAST ≈ -25°, PX4 ≈ -55°, OFFSET ≈ -30°
通过 Excellent! Both frames observed the same physical rotation.

This is particularly important for the SUPER bridge because it computes an alignment between the FAST-LIO world and PX4 local frame.

Recommended Diagnostic Order

Stage	Diagnostic	Purpose
1	FAST-LIO /Odometry	prove SLAM position is stable
2	Topic-rate chain	prove SLAM data reaches PX4
3	Test 1	compare FAST/PX4 XY and yaw
4	Test 3	examine PX4 EKF status around ARM
5	Test 2	investigate magnetic-field correlation if yaw moves
6	45° rotation	verify yaw sign, response and frame consistency
7	POSCTL flight	first physical position-hold validation
8	simple Offboard	validate automated movement
9	SUPER	autonomous planning after lower layers pass

The Mathematical Idea Behind the Entire Test

Ultimately, we are checking whether two estimated coordinate systems remain related by approximately one fixed rotation.

If ψ_F is FAST-LIO yaw and ψ_P is PX4 yaw, define:
Δψ = wrap(ψ_P − ψ_F)

We do not require ψ_P = ψ_F. We require approximately Δψ ≈ constant.

If Δψ changes without physical movement, the coordinate relationship itself changed. For autonomous navigation, that matters because a vector generated in the FAST-LIO world must be correctly rotated into PX4's local frame.

Final Principle

The three tests represent three layers of investigation:

TEST 1: FAST-LIO vs PX4 — "Are the two navigation frames agreeing?"
TEST 2: Magnetometer + yaw — "Is the magnetic measurement connected to the yaw problem?"
TEST 3: PX4 EKF status — "What is the flight estimator doing internally?"

Together they form a very good toilet-bowl diagnostic toolkit for a PX4 + FAST-LIO indoor drone. Rather than treating localization, estimation, control, networking and planning as one black box, we observe the state at each interface and mathematically test whether the relationship between layers remains consistent.

平台：Jetson · ROS2 Humble · Livox Mid360 · FAST-LIO · PX4 · MAVROS
(c) 卓博泰科技 · Toilet-Bowl 诊断指南

Diagnosing Toilet-Bowl Behavior in a PX4 + FAST-LIO Indoor SLAM Drone

Related Drobotics guides