Covariance values too large for robot_localization
Nick-Sullivan opened this issue · comments
When fusing the odometry from the kobuki (mobile_base/odom) with GPS into a robot_localization EKF, the output becomes filled with NaN's.
If I republish the odom with smaller covariances for the unused variables, everything works fine.
Default odom:
header:
seq: 888
stamp:
secs: 1518127185
nsecs: 997307393
frame_id: "odom"
child_frame_id: "base_link"
pose:
pose:
position:
x: 0.0
y: 0.0
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
covariance: [0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.7976931348623157e+308, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.7976931348623157e+308, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.7976931348623157e+308, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05]
twist:
twist:
linear:
x: 0.0
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.0
covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
This causes the output of the EKF to look like this:
header:
seq: 152
stamp:
secs: 1455208206
nsecs: 544255733
frame_id: "map"
child_frame_id: "base_link"
pose:
pose:
position:
x: nan
y: nan
z: nan
orientation:
x: nan
y: nan
z: nan
w: nan
covariance: [nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan]
twist:
twist:
linear:
x: nan
y: nan
z: nan
angular:
x: nan
y: nan
z: nan
covariance: [nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan]
If I republish the odom with smaller covariances for the unused variables (1e308 to 1e6):
header:
seq: 114
stamp:
secs: 1518127292
nsecs: 450093413
frame_id: "odom"
child_frame_id: "base_link"
pose:
pose:
position:
x: 0.0
y: 0.0
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
covariance: [0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05]
twist:
twist:
linear:
x: 0.0
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.0
covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
Then the fusion works fine.
This is on Kinetic, and my EKF fusion looks like this:
<!-- Create EKF localisation. -->
<node pkg="robot_localization" type="ekf_localization_node" name="global_ekf_localization" output="screen">
<!-- Set frame links -->
<param name="map_frame" value="map"/>
<param name="odom_frame" value="odom"/>
<param name="base_link_frame" value="base_link"/>
<param name="world_frame" value="map"/>
<!-- Set modes -->
<param name="frequency" value="10"/>
<param name="two_d_mode" value="true"/>
<!-- Set input types -->
<param name="odom0" value="odometry/map_to_baselink"/>
<param name="odom1" value="odometry/odom_to_baselink"/> <!-- kobuki odom-->
<!-- Configure inputs -->
<rosparam param="odom0_config">[ true, true, false,
false, false, true,
false, false, false,
false, false, false,
false, false, false]</rosparam>
<param name="odom0_differential" value="false"/>
<rosparam param="odom1_config">[ true, true, false,
false, false, true,
false, false, false,
false, false, false,
false, false, false]</rosparam>
<param name="odom1_differential" value="false"/>
<!-- Remap output. -->
<remap from="odometry/filtered" to="odometry/global_filtered"/>
</node>
It seems the high values stem from lines 152 -154 in odometry.ccp:
odom->pose.covariance[14] = DBL_MAX; // set a non-zero covariance on unused
odom->pose.covariance[21] = DBL_MAX; // dimensions (z, pitch and roll); this
odom->pose.covariance[28] = DBL_MAX; // is a requirement of robot_pose_ekf
DBL_MAX = 1.79769e+308
I've a suspicion these high values also cause rviz to crash when trying to visualize odom
data.
Maybe some double - float casting going wrong in places. @Nick-Sullivan send us a PR for something that works and that you think is reasonable.