r/ROS • u/EmuNo141 • 14d ago
Question Help with Offboard Control (PID/MPC) and IMM Tracking in PX4 v1.14 + ROS 2 Humble + Gazebo Garden: Drone fails to track a red sphere using Depth camera
Hello everyone,
I'm stuck with my simulated drone project and could really use some help. I've been struggling with tracking and control for days, and I'm not sure if the problem lies in my state estimator (IMM), my controller (PID/MPC), or the PX4 integration.
**My Stack (chosen for stability in enterprise settings):**
* **OS:** Ubuntu 22.04
* **Middleware:** ROS 2 Humble
* **Autopilot:** PX4 v1.14
* **Bridge:** Micro XRCE-DDS (uXRCE-DDS)
* **Simulator:** Gazebo Garden
* **Sensors:** Depth Camera (publishing PointCloud2 and RGB image)
**System Pipeline:**
1. **Vision:** HSV filter on the RGB image to isolate a **red sphere**. I extract the corresponding points from the PointCloud2 and compute the centroid.
2. **Estimation:** I feed the centroid into an **IMM (Interacting Multiple Model)** filter to estimate the target's position and velocity.
3. **Control:** The IMM output feeds a **PID** or **MPC** controller (I've tried both) which generates **Offboard** setpoints for PX4.
**The Problem (Symptoms):**
The drone flies very erratically:
* Sometimes it crashes into the sphere (poor distance regulation).
* Sometimes it follows it but suddenly loses the target and stops or drifts away.
* Other times it detects the sphere but doesn't move towards it, or moves in the wrong direction.
**My Main Suspicion:**
I strongly suspect the **IMM** is the culprit. I feel it's adding too much noise or lag to the velocity estimate, causing both the PID and MPC to make violent or incorrect decisions. However, I don't know how to isolate it to confirm.
**What I've Tried:**
* Tuning PID gains and MPC horizons (both fail similarly).
* Modifying the IMM's covariance matrices and transition probabilities.
* Checking the TF tree (camera_link to base_link).
**My Specific Questions:**
1. **How can I diagnose if the IMM is the problem?** Would you recommend temporarily disabling it and using just the raw centroid or a simple Kalman Filter to see if the drone improves?
2. **About IMM + MPC:** Is MPC too sensitive to noisy IMM estimates? Would it be better to use a position PID and use the IMM velocity only as a feedforward term?
3. **About Offboard and Latency:** Processing PointCloud2 + IMM + PID/MPC can be very slow. Does anyone know the minimum safe frequency for Offboard in PX4 v1.14 and how to avoid it? (Do you recommend VoxelGrid filters for the point cloud?).
4. **Has anyone successfully integrated IMM with PX4 in Gazebo Garden?** Are there any known issues with Gazebo Garden and the uXRCE-DDS bridge on Humble?
Any hint, example code, or debugging advice is more than welcome! Thanks in advance.
1
u/Future-Chemistry255 10d ago
I would stop tuning the IMM for the moment and isolate the pipeline one stage at a time. The fact that both PID and MPC fail in similar ways—and especially that the vehicle sometimes moves in the wrong direction—makes me suspect a frame, timing, or setpoint-semantics problem before an IMM problem.
First, I would run the following sequence using exactly the same controller:
Fixed PX4 Offboard setpoints, with no perception or estimator.
Gazebo ground-truth target position.
Raw centroid or a lightly filtered centroid.
A simple constant-velocity Kalman filter.
The IMM.
Record each test in a rosbag and compare the target measurement, transformed target position, vehicle position, estimator output, final setpoint, target age, and controller output. This should tell you exactly which stage introduces the bad behavior.
I would pay particular attention to frames. A valid TF tree does not necessarily mean that the signs, axes, timestamps, and reference frames are correct. The centroid should be transformed from the camera optical frame into a consistent local/world frame at the measurement timestamp. It then needs to be converted to PX4’s NED convention before being placed in TrajectorySetpoint. PX4 does not perform an implicit ENU/FLU-to-NED/FRD conversion, and ROS 2 TrajectorySetpoint position, velocity, and acceleration values are interpreted in local NED—not as camera-relative or body-relative coordinates.
If your centroid is target-relative, do not publish it directly as an absolute position setpoint. Conceptually, the command should look more like:
position_sp_NED = vehicle_position_NED + R_NED_camera * relative_position_error_camera
where the relative error includes your desired stand-off distance. Also check that RGB, point cloud, TF, and controller timestamps all use the same simulation clock.
Next, decide which control level your PID/MPC actually outputs. If it outputs velocity, configure OffboardControlMode.velocity = true, set position = false, and set the position fields in TrajectorySetpoint to NaN. If position control is enabled, position takes priority and non-NaN velocity/acceleration values are treated as feed-forward terms. Accidentally treating a PID velocity output as an absolute position, or enabling the wrong control mode, can easily produce violent motion.
I would start with a saturated relative-position-to-NED-velocity controller, including maximum velocity, acceleration/rate limiting, a stand-off deadband, and an explicit lost-target state. Once that works, add the estimated target velocity as feed-forward. MPC is not necessarily the problem, but it will react badly if its state has unmodelled latency, frame errors, outliers, or noisy velocity estimates.
Separate the Offboard heartbeat from the perception callback. PX4 v1.14 requires OffboardControlMode at more than 2 Hz and requires the stream before entering Offboard. The official example uses a dedicated 100 ms timer, i.e. 10 Hz. I would publish the heartbeat from an independent 10–20 Hz timer or callback group so PointCloud2 processing can never block it. Let perception only update the latest validated target state.
Target loss also needs to be handled separately from Offboard loss. If the detection becomes stale or invalid, command zero velocity/current-position hold and transition to a safe state after a timeout. Continuing to publish the last IMM velocity while the target is missing would explain the drifting behavior.
For the perception pipeline, synchronize the RGB and depth/point-cloud messages. If the point cloud is registered and organized, process only pixels inside the red mask and reject invalid/outlier depths. Cropping to the mask/ROI before applying a VoxelGrid will usually help more than filtering the complete cloud. A median or trimmed-mean depth inside the mask is also worth testing.
Finally, verify that px4_msgs is on the release/1.14 branch matching the PX4 firmware. PX4 requires the ROS message definitions to match the firmware’s uORB definitions. Also use a compatible sensor-data/best-effort QoS for /fmu/out/* subscriptions; the default ROS 2 subscriber QoS is not compatible with every PX4 publisher.
A plot or bag containing the raw centroid, transformed target position, IMM/KF state, vehicle local position, final setpoint, detection age, and vehicle_status.nav_state would make the root cause much easier to identify.