0
$\begingroup$

Rosanswers logo

Since this post I have added a magnetometer to the system in order to get a world-referenced yaw/heading estimate as well as a barometer for better altitude estimations.

First problem [solved] However, I'm still doing some tests on ground, so what I have been doing lately is to feed a fixed altitude (1.2m) as a new odometry msg (nav_msgs/Odometry), instead of using the altitude calculated from the barometer. After correcting a small problem with the msg stamp the altitude started to be used by the filter. Since then I have added a velocity to the same odometry msg and it seems to be working. I'm going to edit the post to reflect this changes.

Right now the launch file used to test the "fake" odometry msg 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="20"/>  
  <param name="sensor_timeout" value="5"/>  
  <param name="two_d_mode" value="false"/>

  <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="/imu/altitude"/>
  <param name="imu0"  value="/imu/data"/> 


  <rosparam param="odom0_config">[false, false, true,
                                  false, false, false, 
                                  true , 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="odom0_relative" value="false"/>
  <param name="imu0_relative"  value="false"/>

  <param name="print_diagnostics" value="true"/>

  <!-- ======== 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="zero_altitude" value="false"/>

  <param name="broadcast_utm_transform" value="false"/>

  <param name="publish_filtered_gps" value="false"/>

  <param name="yaw_offset"     value="1.5707963"/>
</node>
</launch>

Second problem [acceleration data is too noisy]: Even though in this launch file I'm using the acceleration data from the IMU, I came to the conclusion that it is too noisy and I'm better off not adding it to the filter.

image description

So I have been running some tests without using the accelerations from the IMU, however this should mean that the estimation of the linear velocity is basically relying on the GPS alone, right?

First doubt [pose quaternion is obtained from gyro + accel + mag]: On the other hand I don't know if I should be using roll, pitch, yaw and respective angular velocities, since the roll, pitch and yaw are obtained from the angular velocities and acceleration using the Madgwick filter.

Second doubt [which data to use in the filter]: Another doubt I have is about the use of roll, pitch and yaw. Since the direction of movement of the vehicle is governed by it's orientation (yaw) and it's supposed to have a somewhat planar movement, should I still add the roll and pitch to the filter?

In fact, if I only add the yaw I get a warning on the diagnostics topic, about not using roll/pitch and having two_d_mode set to false.

This is the kind of output I'm getting right now when performing a rectangular path (GPS + IMU): image description

EDIT1:

Here is a sample of the IMU topict:

header: 
  seq: 95
  stamp: 
    secs: 1436025714
    nsecs: 71387649
  frame_id: imu
orientation: 
  x: 0.00455226427862
  y: -0.0462875419141
  z: -0.604611424228
  w: 0.79467713799
orientation_covariance: [0.0006250000000000001, 0.0, 0.0, 0.0, 0.0006250000000000001, 0.0, 0.0, 0.0, 0.0006250000000000001]
angular_velocity: 
  x: 0.0883323171029
  y: 0.0558238933806
  z: 0.631249659415 
angular_velocity_covariance: [5.26445e-06, 0.0, 0.0, 0.0, 0.00152884, 0.0, 0.0, 0.0, 0.00289536]
linear_acceleration: 
  x: 0.26405090332
  y: 0.641865234375
  z: 10.1003961182
linear_acceleration_covariance: [0.00840479, 0.0, 0.0, 0.0, 0.00127959, 0.0, 0.0, 0.0, 0.218029]

The odometry/gps output:

header: 
  seq: 17
  stamp: 
    secs: 1434470351
    nsecs: 400000095
  frame_id: odom
child_frame_id: '' 
pose: 
  pose: 
    position: 
      x: -0.000307746231556
      y: 0.0145109614241
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
  covariance: [11.713630260079219, 0.06028916405309637, 0.0, 0.0, 0.0, 0.0, 0.06028916405309642, 9.061369739920782, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 34.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
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]

Right now the second odometry topic is a "blank" odometry msg with z set to a fixed value (1.20 in this case):

header: 
  seq: 461
  stamp: 
    secs: 1436214275
    nsecs: 734612605
  frame_id: odom
child_frame_id: base_link
pose: 
  pose: 
    position: 
      x: 0.0
      y: 0.0
      z: 1.2
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.0
      w: 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]
twist: 
  twist: 
    linear: 
      x: 1.1
      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]

In this test I use GPS (x,y), fake_odometry (z) and IMU (yaw, yaw velocity).

Bagfile of the with GPS + IMU + fake odometry z

However, for some reason I get a different result from before (as seen below). It seems like adding the odometry msg with the fixed z is changing the orientation somehow.. image description


Originally posted by nvoltex on ROS Answers with karma: 131 on 2015-07-03

Post score: 1


Original comments

Comment by l0g1x on 2015-07-05:
can you edit your question and display the covariance matricies for the input sources?

Comment by nvoltex on 2015-07-06:
I added a sample of the IMU topic. The topic with the altitude calculated from the barometer topic still doesn't have an associated covariance matrix, since I'm still trying to get it to work.

Comment by l0g1x on 2015-07-06:
what about the other inputs?

Comment by nvoltex on 2015-07-06:
I ended up forgeting to add the remaining ones since one of them is created from the nav_sat_transform_node and the other is a rather simple one with covariances yet to be defined. I edited the question in order to add them. Thanks in advance!

Comment by Tom Moore on 2015-07-06:
For problem 1, at first glance I don't see anything wrong with your configuration or input messages. What do you see when you look at the diagnostics topic? Have you tried only fusing /imu/altitude as odom0? Do that, turn on debug mode, run for a few seconds, and then look through the output.

Comment by Tom Moore on 2015-07-06:
Feel free to post a bag file as well. I can usually troubleshoot faster that way.

Comment by nvoltex on 2015-07-07:
I updated the question with some new info and corrected some things. I still have the doubts from before, but since I corrected the odometry msg I'm getting a strange behavior from the filter (explained in the question). Thanks for the help!

Comment by Tom Moore on 2015-07-08:
Does your IMU have a magnetometer?

Comment by Tom Moore on 2015-07-08:
Did you change the yaw_offset and magnetic_declination_radians parameters for navsat_transform_node since your last question? That could account for the change in position.

Comment by Tom Moore on 2015-07-08:
Never mind. See my answer.

Comment by nvoltex on 2015-07-08:
The IMU does have a magnetometer and I think I have set both yaw_offset and magnetic_declination_radians properly. (I haven't changed them since my last question)

Comment by Evgeny Zakutin on 2020-11-27:
@nvoltex, if it is possible, could you please share your final working launch/config files for gps+imu+barometer setup?

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

Ah, I figured our your issue from edit 1. navsat_transform_node needs ukf_localization_node to produce one message before it can compute the odom->utm transform. When you don't include your /imu/altitude topic, the first message that ukf_localization_node receives (and therefore the first filtered odometry message it produces) is an IMU message with orientation data in it. This causes the odom->utm transform to be computed with the heading that's in that first IMU message. However, when you do include the /imu/altitude topic, the first message ukf_localization_node receives is an altitude message, and so its first output has a different heading, because it hasn't received any heading data yet.

Two things:

  1. This shouldn't have any effect on any higher-level behaviors or any node that uses the output of ukf_localization_node. In fact, if you turn on publish_filtered_gps, you should find that the GPS data it outputs (/gps/filtered) is the same, even if the X-Y odom frame positions are different.
  2. This will be solved when I add this feature. If the filter were to wait until it had received at least one IMU message, then it would have given you the exact same output.

EDIT: actually, one more thing: you should also be able to fix this (I say fix, but really nothing is broken) by setting the delay parameter in navsat_transform_node. If the value is high enough, then you will give ukf_localization_node time to receive at least one IMU message with orientation data in it.

EDIT 2 (to actually respond to your original questions): First, yes, if you don't fuse linear acceleration, the filter will be using only the GPS data to determine its pose. Interestingly, the filter will "create" velocities when you fuse pose only data (due to math within the EKF itself, not from anything I'm doing), though the covariances on those velocities in the filter output will be relatively high. You can try fusing the acceleration data and see what happens. It may just smooth the signal, or the integration of acceleration may start to generate velocity that causes the state estimate to run away.

Second, if your IMU measures roll and pitch and two_d_mode is false, then yes, fuse roll and pitch. To clarify, ekf_localization_node is not doing the same thing as imu_filter_madgwick. imu_filter_madgwick takes raw accelerometer, gyro, and (optionally) magnetometer data and produces an estimate of the orientation. ekf_localization_node does not use raw magnetometer data, and the sensor_msgs/Imu message doesn't contain raw mag data anyway. We use the IMU message's linear acceleration data to compute the vehicle's linear acceleration, linear velocity (integration over time), and position (double integration over time), but we do not use it to compute roll and pitch. Likewise, we use the IMU message's angular velocity data to compute the vehicle's angular velocity and orientation (integration over time).


Originally posted by Tom Moore with karma: 13689 on 2015-07-08

This answer was ACCEPTED on the original site

Post score: 1


Original comments

Comment by nvoltex on 2015-07-09:
Oh ok, thanks! Could you give me some insight on the doubts I posted in the question? Another thing, the GPS I'm using is a garmin 18x, which besides the longitude, latitude and altitude also outputs a "speed" parameter. Is it a bad idea to use this as the velocity on the new odometry msg?

Comment by nvoltex on 2015-07-09:
One of the problems I see is that I don't have the heading associated with that velocity and I'm feeding it as the linear.x. However if the wind conditions make the vehicle move sideways, that won't be compensated by the filter.

Comment by Tom Moore on 2015-07-09:
ekf_localization_node does not assume that your robot is moving in the direction of its yaw/heading. If you're moving sideways, then you'll just have Y velocity. If the GPS starts reporting sideways motion, then the filter should too. You can only do so much with a GPS and IMU.

Comment by nvoltex on 2015-07-10:
What i meant when I asked if I should fuse the roll and pitch is that, due to the vehicle being sustained in the air thanks to a ballon, changes on roll and pitch will be small and due to noise from the rotors (or wind). That being said, those changes won't have much effect on the overhall movement

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.