The site is read-only. Please transition to use Robotics Stack Exchange
| ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
 | 1 | initial version |
I looked at the data from your other post, and this is what navsat_transform_node is outputting:
You can get this plot by doing:
rostopic echo -p /odometry/gps > odom_gps.txt
...and then plotting the output with MATLAB.
This says to me that navsat_transform_node is behaving correctly, but keep reading.
Without doing a deeper analysis, my first guess is that your IMU may not be mounted correctly. If, for example, you have it turned around and facing the wrong direction, then navsat_transform_node will be outputting incorrect positions if you don't correct for the offset, and ekf_localization_node will think your robot is facing the wrong way, so when the EKF makes a prediction, it will move your robot the wrong way.
Please do this: do not remove any information from your question. Instead, at the bottom, add an "EDIT" section, and post a photo, if possible, of the IMU on your robot, and please make it clear where the front of your robot is. Also, although I believe I am aware of it, let me know if you're using this fork of the GX2 node, or the default one, as it will affect my answer.
| | 2 | No.2 Revision |
I looked at the data from your other post, and this is what navsat_transform_node is outputting:
You can get this plot by doing:
rostopic echo -p /odometry/gps > odom_gps.txt
...and then plotting the output with MATLAB.
This says to me that navsat_transform_node is behaving correctly, but keep reading.
Without doing a deeper analysis, my first guess is that your IMU may not be mounted correctly. If, for example, you have it turned around and facing the wrong direction, then navsat_transform_node will be outputting incorrect positions if you don't correct for the offset, and ekf_localization_node will think your robot is facing the wrong way, so when the EKF makes a prediction, it will move your robot the wrong way.
Please do this: do not remove any information from your question. Instead, at the bottom, add an "EDIT" section, and post a photo, if possible, of the IMU on your robot, and please make it clear where the front of your robot is. Also, although I believe I am aware of it, let me know if you're using this fork of the GX2 node, or the default one, as it will affect my answer.
EDIT 1:
Here's what I did:
Ran this command (to get rid of the topics I didn't want):
rosbag filter miercoles.bag gps_test.bag "topic != '/diagnostics' and topic != '/odometry/filtered' and topic != '/rosout' and topic != '/rosout_agg' and topic != '/tf' and topic != '/odometry/gps'"
robot_localization, and used the indigo-devel branch<launch>
<param name="/use_sim_time" value="true" />
<node pkg="tf" type="static_transform_publisher" name="bl_imu" args="0 0 0 0 0 0 1 /base_link /imu 20" />
<node pkg="rosbag" type="play" name="player" output="screen" args="--clock /home/tmoore/Desktop/gps_test.bag -d 3"/>
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true">
<param name="frequency" value="30"/>
<param name="sensor_timeout" value="0.1"/>
<param name="two_d_mode" value="true"/>
<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="odom"/>
<param name="odom0" value="odometry/gps"/>
<param name="imu0" value="/imu/data"/>
<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,
true, true, true,
false, false, false,
true, true, true,
true, true, true]</rosparam>
<param name="odom0_differential" value="false"/>
<param name="imu0_differential" value="false"/>
<param name="imu0_remove_gravitational_acceleration" value="true"/>
<param name="print_diagnostics" value="false"/>
<!-- ======== ADVANCED PARAMETERS ======== -->
<param name="odom0_queue_size" value="2"/>
<param name="imu0_queue_size" value="10"/>
<param name="debug" value="false"/>
<param name="debug_out_file" value="/home/tmoore/Desktop/debug_ekf_localization.txt"/>
<rosparam param="process_noise_covariance">[0.05, 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.05, 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.06, 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.03, 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.03, 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.06, 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.025, 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.025, 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.04, 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.01, 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.01, 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.02, 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.01, 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.01, 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.015]</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>
<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="true" output="screen">
<remap from="/gps/fix" to="/fix"/>
<param name="magnetic_declination_radians" value="0"/>
<param name="yaw_offset" value="0"/>
<param name="zero_altitude" value="false"/>
<param name="broadcast_utm_transform" value="false"/>
<param name="publish_filtered_gps" value="false"/>
</node>
</launch>
This is the output I get in rviz:
This is the same data in MATLAB:
One important note: even though the filter fused the position just fine, the IMU headings were not correct. I can't tell if they're offset, have the wrong sign, or both. Throughout the run, the vehicle was traveling in a direction that did not match the IMU's heading. In any case, you need to really analyze the IMU data. It may be that your IMU doesn't work well the my driver fork, or that there's something else going on. I'd have to know more to make a determination. So...
What you need to do: as an experiment, if you have time, please try moving in a perfectly straight line for a long distance, e.g., 50 meters. Then turn left 90 degrees, move straight for 50 meters, turn left 90 degrees again, move another 50 meters, and then turn left 90 degrees once more, and move a final 50 meters, back to your start point. You should have completed a square. Please post that bag file.
Also, I didn't add it, but you should really have your magnetic declination set.
| | 3 | No.3 Revision |
I looked at the data from your other post, and this is what navsat_transform_node is outputting:
You can get this plot by doing:
rostopic echo -p /odometry/gps > odom_gps.txt
...and then plotting the output with MATLAB.
This says to me that navsat_transform_node is behaving correctly, but keep reading.
Without doing a deeper analysis, my first guess is that your IMU may not be mounted correctly. If, for example, you have it turned around and facing the wrong direction, then navsat_transform_node will be outputting incorrect positions if you don't correct for the offset, and ekf_localization_node will think your robot is facing the wrong way, so when the EKF makes a prediction, it will move your robot the wrong way.
Please do this: do not remove any information from your question. Instead, at the bottom, add an "EDIT" section, and post a photo, if possible, of the IMU on your robot, and please make it clear where the front of your robot is. Also, although I believe I am aware of it, let me know if you're using this fork of the GX2 node, or the default one, as it will affect my answer.
EDIT 1:
Here's what I did:
Ran this command (to get rid of the topics I didn't want):
rosbag filter miercoles.bag gps_test.bag "topic != '/diagnostics' and topic != '/odometry/filtered' and topic != '/rosout' and topic != '/rosout_agg' and topic != '/tf' and topic != '/odometry/gps'"
robot_localization, and used the indigo-devel branch<launch>
<param name="/use_sim_time" value="true" />
<node pkg="tf" type="static_transform_publisher" name="bl_imu" args="0 0 0 0 0 0 1 /base_link /imu 20" />
<node pkg="rosbag" type="play" name="player" output="screen" args="--clock /home/tmoore/Desktop/gps_test.bag -d 3"/>
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true">
<param name="frequency" value="30"/>
<param name="sensor_timeout" value="0.1"/>
<param name="two_d_mode" value="true"/>
<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="odom"/>
<param name="odom0" value="odometry/gps"/>
<param name="imu0" value="/imu/data"/>
<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,
true, true, true,
false, false, false,
true, true, true,
true, true, true]</rosparam>
<param name="odom0_differential" value="false"/>
<param name="imu0_differential" value="false"/>
<param name="imu0_remove_gravitational_acceleration" value="true"/>
<param name="print_diagnostics" value="false"/>
<!-- ======== ADVANCED PARAMETERS ======== -->
<param name="odom0_queue_size" value="2"/>
<param name="imu0_queue_size" value="10"/>
<param name="debug" value="false"/>
<param name="debug_out_file" value="/home/tmoore/Desktop/debug_ekf_localization.txt"/>
<rosparam param="process_noise_covariance">[0.05, 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.05, 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.06, 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.03, 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.03, 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.06, 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.025, 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.025, 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.04, 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.01, 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.01, 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.02, 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.01, 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.01, 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.015]</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>
<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="true" output="screen">
<remap from="/gps/fix" to="/fix"/>
<param name="magnetic_declination_radians" value="0"/>
<param name="yaw_offset" value="0"/>
<param name="zero_altitude" value="false"/>
<param name="broadcast_utm_transform" value="false"/>
<param name="publish_filtered_gps" value="false"/>
</node>
</launch>
This is the output I get in rviz:
This is the same data in MATLAB:
One important note: even though the filter fused the position just fine, the IMU headings were not correct. I can't tell if they're offset, have the wrong sign, or both. Throughout the run, the vehicle was traveling in a direction that did not match the IMU's heading. In any case, you need to really analyze the IMU data. It may be that your IMU doesn't work well the my driver fork, or that there's something else going on. I'd have to know more to make a determination. So...
What you need to do: as an experiment, if you have time, please try moving in a perfectly straight line for a long distance, e.g., 50 meters. Then turn left 90 degrees, move straight for 50 meters, turn left 90 degrees again, move another 50 meters, and then turn left 90 degrees once more, and move a final 50 meters, back to your start point. You should have completed a square. Please post that bag file.
Also, I didn't add it, but you should really have your magnetic declination set.
EDIT 2:
One more thing: I noticed in your launch files in the other post that you have the frequency parameter set to 30 Hz. This is fine, but if you play the miercoles.bag file, and do rostopic hz /odometry/filtered, you'll notice it's reporting 60 Hz. This means that (a) you had two instances of ekf_localization_node running, (b), you ran ekf_localization_node while you were recording the data, and then ran it again while playing back the data and recorded all the data again, or (c) you updated the frequency to 60 Hz. I'm guessing it's (a) or (b), but you should look into that. Also, when you record a bag file, you should either record only the raw data topics (here, just the IMU and GPS data), or record everything and run rosbag filter like I did so you can get rid of the topics that you don't want.
| | 4 | No.4 Revision |
I looked at the data from your other post, and this is what navsat_transform_node is outputting:
You can get this plot by doing:
rostopic echo -p /odometry/gps > odom_gps.txt
...and then plotting the output with MATLAB.
This says to me that navsat_transform_node is behaving correctly, but keep reading.
Without doing a deeper analysis, my first guess is that your IMU may not be mounted correctly. If, for example, you have it turned around and facing the wrong direction, then navsat_transform_node will be outputting incorrect positions if you don't correct for the offset, and ekf_localization_node will think your robot is facing the wrong way, so when the EKF makes a prediction, it will move your robot the wrong way.
Please do this: do not remove any information from your question. Instead, at the bottom, add an "EDIT" section, and post a photo, if possible, of the IMU on your robot, and please make it clear where the front of your robot is. Also, although I believe I am aware of it, let me know if you're using this fork of the GX2 node, or the default one, as it will affect my answer.
EDIT 1:
Here's what I did:
Ran this command (to get rid of the topics I didn't want):
rosbag filter miercoles.bag gps_test.bag "topic != '/diagnostics' and topic != '/odometry/filtered' and topic != '/rosout' and topic != '/rosout_agg' and topic != '/tf' and topic != '/odometry/gps'"
robot_localization, and used the indigo-devel branch<launch>
<param name="/use_sim_time" value="true" />
<node pkg="tf" type="static_transform_publisher" name="bl_imu" args="0 0 0 0 0 0 1 /base_link /imu 20" />
<node pkg="rosbag" type="play" name="player" output="screen" args="--clock /home/tmoore/Desktop/gps_test.bag -d 3"/>
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true">
<param name="frequency" value="30"/>
<param name="sensor_timeout" value="0.1"/>
<param name="two_d_mode" value="true"/>
<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="odom"/>
<param name="odom0" value="odometry/gps"/>
<param name="imu0" value="/imu/data"/>
<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,
true, true, true,
false, false, false,
true, true, true,
true, true, true]</rosparam>
<param name="odom0_differential" value="false"/>
<param name="imu0_differential" value="false"/>
<param name="imu0_remove_gravitational_acceleration" value="true"/>
<param name="print_diagnostics" value="false"/>
<!-- ======== ADVANCED PARAMETERS ======== -->
<param name="odom0_queue_size" value="2"/>
<param name="imu0_queue_size" value="10"/>
<param name="debug" value="false"/>
<param name="debug_out_file" value="/home/tmoore/Desktop/debug_ekf_localization.txt"/>
<rosparam param="process_noise_covariance">[0.05, 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.05, 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.06, 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.03, 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.03, 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.06, 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.025, 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.025, 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.04, 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.01, 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.01, 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.02, 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.01, 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.01, 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.015]</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>
<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="true" output="screen">
<remap from="/gps/fix" to="/fix"/>
<param name="magnetic_declination_radians" value="0"/>
<param name="yaw_offset" value="0"/>
<param name="zero_altitude" value="false"/>
<param name="broadcast_utm_transform" value="false"/>
<param name="publish_filtered_gps" value="false"/>
</node>
</launch>
This is the output I get in rviz:
This is the same data in MATLAB:
One important note: even though the filter fused the position just fine, the IMU headings were not correct. I can't tell if they're offset, have the wrong sign, or both. Throughout the run, the vehicle was traveling in a direction that did not match the IMU's heading. In any case, you need to really analyze the IMU data. It may be that your IMU doesn't work well the my driver fork, or that there's something else going on. I'd have to know more to make a determination. So...
What you need to do: as an experiment, if you have time, please try moving in a perfectly straight line for a long distance, e.g., 50 meters. Then turn left 90 degrees, move straight for 50 meters, turn left 90 degrees again, move another 50 meters, and then turn left 90 degrees once more, and move a final 50 meters, back to your start point. You should have completed a square. Please post that bag file.
Also, I didn't add it, but you should really have your magnetic declination set.
EDIT 2: 2:
One more thing: I noticed in your launch files in the other post that you have the frequency parameter set to 30 Hz. This is fine, but if you play the miercoles.bag file, and do rostopic hz /odometry/filtered, you'll notice it's reporting 60 Hz. This means that (a) you had two instances of ekf_localization_node running, (b), you ran ekf_localization_node while you were recording the data, and then ran it again while playing back the data and recorded all the data again, or (c) you updated the frequency to 60 Hz. I'm guessing it's (a) or (b), but you should look into that. Also, when you record a bag file, you should either record only the raw data topics (here, just the IMU and GPS data), or record everything and run rosbag filter like I did so you can get rid of the topics that you don't want.
ROS Answers is licensed under Creative Commons Attribution 3.0 Content on this site is licensed under a Creative Commons Attribution Share Alike 3.0 license.