hector_quadrotor_demo is unstable when using EKF estimation for control instead of ground truth
Hello All!
I have hector_quadrotor_demo installed along with ROS indigo and Gazebo 2.2.6. Instead of relying on ground truth for control, I'm trying to use the state estimator provided in the package. To do that, I changed the following configuration from "true" to "false" in "spawn_quadrotor.launch".
<arg name="use_ground_truth_for_control" default="false" />
Now, I have verified that /state topic is used for control instead of /ground_truth/state. Here comes the problem:
- the quadrotor is unstable almost immediately after it receives any cmd_vel command to move. That is, once the motors are engaged, the quadrotor deviates from its correct path and ends up flipped over.
With lots of diagnostics, here are the most important observations that I get. Note that these observations are valid even when the quadrotor is still on the ground (all motors are off, no control command yet).
- The estimated state diverges from the real state even when the quadrotor is still on the ground. Estimated height - for instance - diverges and goes to infinity (that is when the estimated state becomes invalid and resets). For example, here is a comparison between ground truth and estimated height and pose_w.
The estimated Yaw angle is always out of range. Here is one of the always-appearing warnings.
[ WARN] [1470585623.108378020, 3.038000000]: Update for yaw with error [ -1.8419 ], |error| = 93.3946 sigma exceeds max_error!
Estimated state becomes invalid and the filter is reset every few seconds.
- Sensor outputs are all OK, nothing faulty.
Any idea what is happening here? Is the pose estimation node provided in the tutorial supposed to work normally or am I missing something?
