0
$\begingroup$

Rosanswers logo

Hi,

I have a GPS and IMU system for a land based robot that I want to fuse using robot_localization. I've configured navsat_transform as mentioned here.

I had a couple of questions:

  1. Direction of motion does not match with the display (Rviz). How do I configure the system to get the correct orientation for odometry/gps and odometry/filtered?
  2. What could be the cause for the difference in angle between gps and filtered output ?
  3. In case of gps jumps, I see that filtered output jumps very far away (from 100,100 to 6000,7000) , odometry/gps still seems to be somewhat on track. Which output should I be using for motion control?

Rviz Output : http://imgur.com/a/0UMBf

Yellow - odometry/gps Red - odometry/filtered Data is captured while moving in a counter clockwise circle.

Bag file: https://drive.google.com/file/d/0BxVo6y1UQOweOWtZLXpfZVFxOVU/view?usp=sharing

IMU Setup:

I've tried to convert our IMU into the specs of REP-103 as follows:

  1. Acceleration and angular velocity are reported in body frame - X is forward, Y is left and Z is upwards. Counter clockwise movement is positive angular velocity on yaw.
  2. Orientation 0 towards magnetic north, so added 90 offset in navsat_transform and ekf_node.
  3. I assume not correcting for magnetic declination will give a fixed offset in the readings.

Launch File:

<launch>

    <node pkg="tf2_ros" type="static_transform_publisher" name="bl_imu" args="0 0 0 0 0 0 1 base_link imu" />
    <node pkg="tf2_ros" type="static_transform_publisher" name="bl_gnss" args="0 0 0 0 0 0 1 base_link gnss" />

    <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" clear_params="true">
      <param name="frequency" value="50"/>
      <param name="delay" value="1.0"/>
      <param name="magnetic_declination_radians" value="0"/>

      <param name="zero_altitude" value="true"/>
      <param name="broadcast_utm_transform" value="true"/>
      <param name="publish_filtered_gps" value="true"/>
      <param name="use_odometry_yaw" value="true"/>
      <!-- NED to END conversion for IMU -->
      <param name="yaw_offset" value="1.570796327"/>

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

      <remap from="imu/data" to="/imu"/>
      <remap from="gps/fix" to="/fix"/>

    </node>

    <arg name="output_final_position" default="true" />
    <arg name="output_location" default="localizatio_log.txt" />

    <param name="/use_sim_time" value="true" />
    <node pkg="rosbag" type="play" name="rosbagplay" args="--clock updated_imu.bag -d 5" required="true"/>

    <node name="ekf_localization" pkg="robot_localization" type="ekf_localization_node" clear_params="true">

      <param name="frequency" value="50"/>
      <param name="sensor_timeout" value="0.1"/>
      <param name="two_d_mode" value="true"/>
      

      <param name="map_frame" value="gnss"/>
      <param name="odom_frame" value="odom"/>
      <param name="base_link_frame" value="base_link"/>
      <param name="world_frame" value="gnss"/>

      <param name="odom0" value="/odometry/gps"/>

      <!-- # values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types -->
      <rosparam param="odom0_config">[true, true, false,
                                      false, false, false,
                                      false, false, false,
                                      false, false, false,
                                      false, false, false]</rosparam>
      <param name="yaw_offset" value="1.570796327"/>
                            

      <param name="transform_time_offset" value="0.0"/>
      <param name="transform_timeout" value="0.0"/>

      <param name="imu0" value="/imu"/>
      
      <rosparam param="imu0_config">[false, false, false,
                                     true,  true,  true,
                                     false, false, false,
                                     false, false, false,
                                     true,  true,  true]</rosparam>

      <param name="odom0_differential" value="false"/>
      <param name="imu0_differential" value="false"/>

      <param name="odom0_relative" value="false"/>
      <param name="imu0_relative" value="false"/>

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

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

      <param name="imu0_queue_size" value="10"/>

      <param name="debug"           value="true"/>
      <param name="debug_out_file"  value="debug_ekf_localization.txt"/>

      <rosparam param="process_noise_covariance">[0.001, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0.001, 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.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.04, 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.02, 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.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>



</launch>

Originally posted by geewhiz on ROS Answers with karma: 1 on 2017-07-05

Post score: 0


Original comments

Comment by Sid05 on 2022-05-09:
Hi.

You mention this in your navsat transform parameter,

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

is that the reason you are not considering to fuse raw odometry in the EKF ?

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

First, turn debug mode off. It will drastically affect performance and fill your hard drive up in a hurry.

  1. odometry/gps has no orientation. It only has X, Y, and, if configured, Z. You're not measuring yaw from odometry_gps in your EKF config, so it won't affect your output.
  2. As above.
  3. I'd need more information for this. While I realize you posted a bag, reviewing bag files eats a lot of time, so I would request that you add sample input messages for each sensor message that you have. Ordinarily, that behavior is a result of not measuring enough state variables and having your covariance explode in something that isn't being measured, but your configuration doesn't suggest that it should be happening here.

BTW, you have a yaw_offset param inside both the navsat_transform_node and EKF config, and the EKF doesn't have such a parameter.

The IMU-and-GPS-alone use case isn't one that I've had a lot of success with, owing to biases in acceleration data, but YMMV.


Originally posted by Tom Moore with karma: 13689 on 2017-09-26

This answer was ACCEPTED on the original site

Post score: 0

$\endgroup$

Your Answer

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