robot_localization with GPS + IMU

Question

I'm trying to setup a localization system for my aerial vehicle, however I never used robot_localization or the TF package, so everything seems a little overwhelming, setup wise. I have a couple of questions:

1. Is it possible to use robot_localization with only GPS and IMU? Would the output be any good?

2. I have a bottom camera that is used from ground target detection. Should I use it for some kind of visual odometry? I'm afraid that this will put the processor under too much strain (odroid X2).

Can I get some tips on how to setup all of this? I find myself a little lost.

EDIT1: Just for some context, my system is an aerial vehicle supported by a balloon that uses two rotors in order to adjust its position:

(1-baloon; 5-rotors; 4-bottom camera (webcam); 3-main "box" with GPS, IMU, odroid X2, etc)

I had already changed the IMU output to be as specified in REP-103. Initially I was using "udm_odometry_node" from gps_common, but switched to the "navsat_transforme_node". For the altitude I'm using the GPS, since the IMU I'm using doesn't have altitude information.

The bottom camera (simple webcam) is meant for ground target detection/tracking, however I could use it for some kind of visual odometry if the odroid is able to handle it.

I think I was able to setup everything correctly since I'm able to "echo" the output on odometry/filtered. However it seems like it has a lot of noise. I still have to do some real tests, since I only tested this with my laptop and the sensors were stationary. (Note: Are the velocities in Twist set as m/s?)

However I got an error when trying to set the process_noise_covariance (type 7 if I recall correctly). Since I was using the covariance from the template launch file, I just deleted it in order to check if everything was working (if I was able to get an output).

EDIT2: (For some reason I'm unable to comment your answer. Whenever I write a comment it disappears when I refresh the page)

One of the problems might be connected to the fact that all covariances on the IMU message are set to 0, since I wasn't able to find information about those parameters for the mpu9150. Besides that, the IMU might need some new calibration.

Sample of the IMU msg:

---
header: 
  seq: 14983
  stamp: 
    secs: 1429104187
    nsecs: 410562312
  frame_id: imu
  orientation: 
   x: 0.00824941202457
   y: 0.0174910227596
   z: 0.0350623021512
   w: 0.997508466348
 orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
 angular_velocity: 
   x: -0.0017719756579
   y: 0.00648836186156
   z: 0.000426339596743
 angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
 linear_acceleration: 
   x: -0.210282728076
   y: 0.174357444048
   z: 9.848320961
 linear_acceleration_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
 ---

I'm going to do some outdoor's testing, to understand the performance of the gps+imu.

EDIT3:

After an outdoor test I found that something must be wrong with my setup. This is the plot from the odometry/gps. The path was a rectangle of around 100m for 40m. However, as you can see, although the output is close to that, the angles between parts of the path are not 90degrees.

And the plot from odometry/filtered is even worse. From what I can tell I think the IMU msg is being created as specified in REP-103. However the output is this:

FYI my launch file is:

<launch>
<node pkg="tf" type="static_transform_publisher" name="imu_tf" args="0 0 0 0 0 0 1 /base_link /imu 20"/>
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true">
   <param name="frequency" value="30"/>  
  <param name="sensor_timeout" value="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="odom"/>
 <param name="imu0" value="/imu/data"/> 


  <rosparam param="odom0_config">[true,  true,  true, 
                              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"/>
       <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.047124"/>
  <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>

EDIT4: I will post a 2 bags with the test of the rectangular path of around 100m for 40m. The launch file used was the one posted above, but with 'two_d_mode' set to false: bagfiles

---

Originally posted by nvoltex on ROS Answers with karma: 131 on 2015-04-14

Post score: 1

---

Original comments

Comment by Tom Moore on 2015-05-01:
Thanks, I'll check out the bags when I get a chance.

Comment by Tom Moore on 2015-05-05:
Question: as this is a UAV, does the robot often fly in a direction other than the direction it's facing? In other words, is its course over ground different from its heading/yaw? Even though the direction of travel in your GPS data is west-to-east, the IMU reads about -0.99 radians.

Comment by Tom Moore on 2015-05-05:
Also, can you add a cheap barometer to this thing? You should really have an altitude estimate of some kind. I have a launch file that worked for me (with the latest source), and I will post it when you answer these questions.

Comment by nvoltex on 2015-05-06:
The bagfile is from a test I did with only the GPS and IMU. That is, it was not the UAV that performed the path. I performed the path with the laptop, GPS and IMU while mantaining the imu in a position similar to the one it would be in the system. Notice that this IMU has no magnometer information.

Comment by nvoltex on 2015-05-06:
The GPS altitude information is really bad, so I will have to add a barometer to the system. By the end of this month I might have a new IMU with barometer, which should be enough, right? Another question, is there a way to get a better estimate of linear velocity? with the GPS alone it's too noisy

Comment by Tom Moore on 2015-05-06:
If you don't have a world-referenced yaw/heading estimate, navsat_transform_node will not perform correctly. You need a magnetometer. You could always try pointing a camera downward and using optical flow + altitude to determine linear velocity.

Comment by nvoltex on 2015-05-07:
The IMU I'm using has a magnetometer, however I decided not to use it since it seemed to be sensible to electrical noise (from other hardware around the IMU). The use of the downward camera and optical flow was my first idea, however I'm not sure if it will be too demanding for the odroid.

Comment by Tom Moore on 2015-05-07:
Can you calibrate the IMU for hard and soft iron distortion? Also, if you downsampled the image data enough, you could probably get by with optical flow on an Odroid. I'm not sure how powerful the X2 is (I have a U3).

Comment by nvoltex on 2015-05-07:
I think I should be able to calibrate the IMU for hard and soft iron distortion. I will try that ;) Thanks Btw, which launch file did you use?

Comment by Tom Moore on 2015-05-10:
I put a simple one together myself. As your bag files lacked an absolute heading reference, it was really no better than the ones you had.

Answer 1

It is possible, certainly. The quality of the output will be heavily dependent on the input, however.

Have you read through the wiki? I realize it's a lot to take in, but if you had some feedback as to which parts could use some clarification, I'd be happy to update them.

One thing to be aware of with your IMU is that the state estimation nodes in robot_localization assume that the IMU data is in the ENU frame, and IMUs commonly report data in NED. What you really want is for the signs of your rotation angles to increase in the correct direction.

Just an FYI: if you integrate GPS data, your position estimate will be subject to discrete jumps. The accelerometers will smooth that out a little bit, but they won't eliminate it.

Do you have an altimeter, or are you going to use the GPS altitude?

Finally, if you can find a lightweight ARM-friendly visual odometry node, then yes, having more data to fuse is always better. I haven't had much luck on that front, however, as many of the packages are optimized for x86/x86-64 CPUs.

EDIT 1 (in response to EDIT 1 above): Which version of the software are you using? I'll look into the template covariance issue. It used to be that all the values of the covariance matrix had to have decimal points, but in the latest version (not yet released), it can take values without decimal points.

When you say "it seems like it has a lot of noise," can you quantify the error/noise you're seeing? If you can, it would be very helpful if you could post (1) a sample message for each input to ekf_localization_node, and (2) your launch file for ekf_localization_node.

EDIT 2 (in reponse to edits 2 and 3 above):

First, your rectangular plot looks like it has 90-degree angles in it to me. Make sure you do axis equal when you plot things in MATLAB. Remember: the /odometry/gps data will not align perfectly with your X and Y axes when you plot it. It's going to be based on whatever yaw value the IMU had when it received the GPS data. In other words, the first MATLAB plot looks solid to me.

Second, I'm a bit confused as to your setup. What is your odom topic (the odom0 input source)? I don't see you feeding the /odometry/gps topic back into the ekf_localization_node, which is how it's intended to be used.

Also, can you post either a bag file of your run, or perhaps a printout of the /odometry/filtered topic?

---

Originally posted by Tom Moore with karma: 13689 on 2015-04-14

This answer was ACCEPTED on the original site

Post score: 2

---

Original comments

Comment by nvoltex on 2015-04-21:
That might actually be the problem! I don't have an odometry measure and I should be feeding the /odometry/gps back to the ekf_localization_node in odom0. For some reason I didn't save the changes and didn't noticed it was like this until you pointed it out. Just to make sure is the config correct?

Comment by Tom Moore on 2015-04-21:
You have two_d_mode set to true, but you are trying to fuze Z position from odom0, as well as roll and pitch (and their velocities) and Z acceleration from your IMU. If you want to use 3D data, set two_d_mode to false. Also, I would just advise that you double-check your IMU data's signs.

Comment by nvoltex on 2015-04-29:
Thanks for all the help Tom. I have been doing some more tests and one doubt that I have is connected to the "imuN_remove_gravitational_acceleration". When should it be 'true'? If I want the acceleration due to gravity to be removed by filter or if that acceleration is already removed?

Comment by nvoltex on 2015-04-29:
I already found the answer after some small tests. We need to set the parameter to true if we want the acceleration to be removed. On a different note, do you want me to send you a bagfile of the run? Do you have any advice in order to reduce the noisy linear velocity I obtain?

Comment by Tom Moore on 2015-04-29:
Right, true means it will remove acceleration due to gravity.

Be careful using linear acceleration, though: when you only have a GPS and IMU, if your IMU has biases in the acceleration data, it's going to start to run away. Sure, post a bag, and I'll look at it when I can.

Comment by nvoltex on 2015-04-30:
Posted the bag as an edit to the main question. Another thing I noticed is that i'm only feeding the gps to the navsat_transform_node, however from the tutorials it seems like the node requires 3 inputs (gps, imu, odometry).

Comment by Tom Moore on 2015-05-01:
navsat_transform_node must have all of those inputs. However, if you are getting output on your /odometry/gps topic, then it's working. The IMU topic defaults to /imu/data, and the odometry topic defaults to /odometry/filtered, so perhaps you just happen to have those same topic names?

Comment by nvoltex on 2015-05-07:
That must be it.