robot_localization delayed yaw response
Our robot has wheel odometry and an IMU (gyro plus accelerometers) and we are migrating from robot_pose_ekf to robot_localization. We expect to add additional IMUs as well as gps once this base configuration is working. I am sending test data to robot_localization as follows:
vx = 0.1
imu.angular_velocity = 0.05 for the first 30 seconds, then -0.1 for 30 sec and then 0.2
acclerometers are all set to 0
All the other inputs are excluded using "false" entries in the sensor selection matrix. We do not specify an absolute yaw value because we are not directly measuring one. robot_localization diganostics notices this and outputs a warning that Yaw is not being sent and that can result in unbounded errors.
The output from robot_localization agrees with the input for x linear twist (0.1) and the z angular twist initially agrees with imu.angular_velocity, but when imu.angular_velocity changes (at 30 seconds), the z angular twist output from robot_localization persists as the original value. This remains the case at 60 seconds when the input changes again, the output is still around the original input value. When first setting up robot_localization, I did not change the covariance matrix and, in that case, the z angular twist output would stay near 0 for the first 5 seconds or so and then jump to equal the imu.angular_velocity. When I modified the covariance matrix, increasing the value for vYaw, then the output jumps much sooner to the input value. However, when the input value changes, the output does not.
I would attach the robot_localization.launch file and a bag file, but ros.answers does not support those file types for upload, only image files.
two_d_mode is set true. differential and relative modes are set false. Here are the frame settings and sensor selection matrices:
<!-- Defaults to "map" if unspecified -->
<param name="map_frame" value="map"/>
<!-- Defaults to "odom" if unspecified -->
<param name="odom_frame" value="odometry/filtered"/>
<!-- Defaults to "base_link" if unspecified -->
<param name="base_link_frame" value="base_footprint"/>
<!-- Defaults to the value of "odom_frame" if unspecified -->
<param name="world_frame" value="odometry/filtered"/>
<!-- we are going to only use vx because that is all the wheel encoders actually report -->
<rosparam param="odom0_config">[false, false, false,
false, false, false,
true, false, false,
false, false, false,
false, false, false]</rosparam>
<!-- we are going to only use vyaw, ax, ay, az because the robot does not roll significantly in the other axes (and two_d_mode is set to true) -->
<rosparam param="imu0_config">[false, false, false,
false, false, false,
false, false, false,
false, false, true,
true, true, true]</rosparam>
Can you use Google Drive or DropBox to post a bag file? It would also be useful to post a sample IMU message. Thanks!
Here is a bag file and an example IMU message from the original issue, as well as the robot_localization.launch file: robot_localization_link
I found that if I set the vz covariance to a high value the output tracks better (see source file in dropbox). However, it does not make sense to continuously be setting that and also, I thought that one advantage of the UKF was that it tracks the covariance based on the statistics of the data.
The measurement covariances will always be critical. Usually, when convergence is slow, it means your initial estimate covariance for that variable is too low, or the measurement covariance for that variable is too high. I don't have DropBox access right now, so I'll check it out later today.
The original issue was that I was not setting an initial angular vel covariance (so they were all 0), as I thought UKF would build an estimate quickly. Setting a large initial covariance helped, but there is still more lag in the ang vel output than I would like. Is there a way to tune that?
replaced the bag file, the robot_localization.launch file and the source code file in the dropbox folder with updated ones that show the updated results where the robot_localization output of z angular velocity changes appropriately, but fairly slowly.