ekf_localization with gps and imu

Question

Hi everyone: I'm working with robot localization package be position estimated of a boat, my sistem consist of:

Harware: -Imu MicroStrain 3DM-GX2 (I am only interested yaw) - GPS Conceptronic Bluetooth (I am only interested position 2D (X,Y))

Nodes: -Microstrain_3dmgx2_imu (driver imu) -nmea_serial_driver (driver GPS) -ekf (kalman filter) -navsat_transform (with UTM transform odom->utm) -tf_transform (broadcast link base_link->imu)

When I launch and conect all I have:

After changed micros_strain node i got that work successful, i connected only imu and ekf and work well.

My problem is when I integrate all system such as I explain above, the topic /odometry/filtered that is publishing for the filter not work well, because without reason some time the position starting to increase until hundreds of meters, i revised the position that i get from GPS and it work well. I think that the filter not work well. Any idea?

I let in dropbox a rosbag with the system working. https://www.dropbox.com/s/xqc7m47q28dciir/miercoles.bag?dl=0

Thanks!!

EDIT 1: For now I'm using the laptop as a robot, the imu will be mounted as shown in the picture. In the laptop serial cable I have placed facing the screen, which is the direction of my movement. The fork used is https://github.com/cra-ros-pkg/microstrain_3dmgx2_imu . Navsat information is correct, the problem is the position in the case of filtered odometry, as sometimes hundreds of meters unexplained increases.

And i ploted odometry/filtered and odometry/gps and the result is this:

---

Originally posted by Porti77 on ROS Answers with karma: 183 on 2015-03-27

Post score: 2

---

Original comments

Comment by Tom Moore on 2015-03-27:
Please post a picture of your IMU mounted on your robot. I want to make sure it's oriented correctly.

Comment by murdock on 2016-04-13:
How did you plot your data? What did you use?

Comment by Porti77 on 2016-04-18:
You can stack the position so rostopic echo -p /odometry/gps > odom_gps.csv and then plot a graphic with LibreOffice calc, other option is create a specific plugin for rviz, i did this plug-in that plot a odometry msg like a path msg. If you want this plug-in i can send you a link.

Comment by Nico.Zhou on 2016-04-21:
Can you send me the link for the plug-in. Thanks

Comment by Porti77 on 2016-04-21:
https://github.com/porti77/rviz_plugin_odometry_to_path I hope I've helped

Comment by Nico.Zhou on 2016-04-21:
Yeah,it helped me, thanks so much

Comment by asimay_y on 2016-06-14:
hi, @Porti77, odometry msg path plot, as I know the rviz can do it already, just set the buffer up to 5000, it can plot the path. is this what you want? but gps path which rviz cannot plot.

Answer 1

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:

1. Downloaded your miercoles.bag file

2. 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'"
   

3. Downloaded the latest source for robot_localization, and used the indigo-devel branch

4. Created this launch file:

<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>

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.

---

Originally posted by Tom Moore with karma: 13689 on 2015-03-27

This answer was ACCEPTED on the original site

Post score: 5

---

Original comments

Comment by Envo on 2016-06-27:
Sir, may I ask about how could I create the lauch file on my own? Any example or tutorial about it?

Comment by Porti77 on 2016-06-28:
You can see a tutorial about launch files here http://wiki.ros.org/roslaunch/XML, if you are using the robot localization node you can use the launch file that are above or the example attached in the package. Regards