robot_localization nan output when using ODOM x,y in differential mode
Hello Everyone!
As I am trying to solve my previous question i have encountered another problem when trying to run robot_localization.
Brief background:
base on my experiments in which data to fuse from ODOM and IMU on robot_localization i did modify the testekflocalizationnodebag2.test with this configuration:
<rosparam param="odom0_config">[true, true, false,
false, false, false,
false, false, false,
false, false, false,
false, false, false]</rosparam>
<rosparam param="imu0_config">[false, false, false,
false, false, true,
false, false, false,
false, false, false,
false, false, false]</rosparam>
<param name="odom0_queue_size" value="10"/>
<param name="imu0_queue_size" value="10"/>
<param name="imu0_remove_gravitational_acceleration" value="false"/>
<param name="odom0_differential" value="true"/>
<param name="imu0_differential" value="false"/>
output i got is something like this:
Problem: Making the same pattern as the test2.bag, My configuration is as follows
ODOM Source: iRobotcreate2 using autonomy_create driver publishing odom->base_link
at 50Hz, frameid = odom
childframeid = `baselink`
IMU Source: LPMS USBAL2 using lpms_imu from larics publishing /imu_data
topic at 50Hz, frameid = `baselink`
<node name="robot_localization" pkg="robot_localization" type="ekf_localization_node" clear_params="true" output="screen" >
<param name="frequency" value="50"/>
<param name="sensor_timeout" value="0.1"/>
<param name="odom0" value="/odom"/>
<param name="imu0" value="/imu_data"/>
<param name="odom_frame" value="odom"/>
<param name="base_link_frame" value="base_link"/>
<param name="world_frame" value="odom"/>
<param name="two_d_mod" value="true"/>
<param name="publish_tf" value="false"/>
<rosparam param="odom0_config">[true, true, false,
false, false, false,
false, false, false,
false, false, false,
false, false, false]</rosparam>
<rosparam param="imu0_config">[false, false, false,
false, false, true,
false, false, false,
false, false, false,
false, false, false]</rosparam>
<param name="odom0_queue_size" value="10"/>
<param name="imu0_queue_size" value="10"/>
<param name="imu0_remove_gravitational_acceleration" value="false"/>
<param name="odom0_differential" value="true"/>
<param name="imu0_differential" value="false"/>
<rosparam param="process_noise_covariance">[0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0.4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.05, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.002, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.002, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.004, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01]</rosparam>
<rosparam param="initial_estimate_covariance">[1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9]</rosparam>
</node>
upon running robot_localization, my /odometry/filtered output is all "nan"
header:
seq: 5824
stamp:
secs: 1507613653
nsecs: 460683823
frame_id: odom
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]
what do you think this may cause the error? I uploaded my bagfile here.
Asked by fj138696 on 2017-10-10 00:51:57 UTC
Comments
I'm currrently battling the same NaN issue, with a different configuration. Can you update your ticket to include a sample IMU and odom message? I'm starting to think my covariences on them are bad and causing the instability. What are yours like?
Asked by stevemacenski on 2017-10-12 13:16:30 UTC
Hi, my bagfile is upload here.
Asked by fj138696 on 2017-10-12 21:00:36 UTC
Please post sample IMU and odometry messages with the question.
Asked by Tom Moore on 2018-01-11 05:37:21 UTC
I have no trouble running using your bag file and the launch file you posted. What version are you using? Also, you misspelled
two_d_mode
in your launch file.Asked by Tom Moore on 2018-01-11 06:17:26 UTC