Robot localization: GPS causing discrete jumps in map frame

asked 2017-07-17 17:38:54 -0500

I am using the robot localization node to fuse IMU, encoder, and GPS data. When I view my robot in RVIZ in the map frame it jumps all around to wherever the GPS is saying that it is. In other words it seems that it is not utilizing the encoder data. Anyone had this problem before? Seems like I'm missing something simple. Thanks in advance.

Here is a bunch of info on my system:

Launch file: Note that I am not using the GPS transform node because I am using an RTK GPS system that can provide my location relative to a base station.

 <!-- Odom Node (IMU + Encoders)-->
    <node pkg="robot_localization" type="ekf_localization_node" clear_params="true" name="ekf_odom_node" >
    <param name="frequency" value="30"/>
    <param name="sensor_timeout" value="0.1"/>
    <param name="two_d_mode" value="true"/>
    <remap from="odometry/filtered" to="odom/ekf/enc_imu"/>
    <remap from="set_pose" to="initialpose"/>
    <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="transform_time_offset" value="0.0" />
    <param name="imu0" value="/imu/data"/>
    <param name="imu0_differential" value="false" />
    <param name="imu0_relative" value="false" />
    <param name="imu0_queue_size" value="10" />
    <param name="imu0_remove_gravitational_acceleration" value="true" />
    <rosparam param="imu0_config">[false,  false,  false,
                                   true,   true,   true,
                                   false,  false,  false,
                                   true,   true,   false,
                                   true,   true,   true]</rosparam>
    <param name="odom0" value="/odom"/>
    <param name="odom0_differential" value="false" />
    <param name="odom0_relative" value="false" />
    <param name="odom0_queue_size" value="5" />
    <rosparam param="odom0_config">[false,  false,  false,
                                    false,  false,  false,
                                    true,   true,   false,
                                    false,  false,  false,
                                    false,  false,  false]</rosparam>
    <param name="print_diagnostics" value="true" />
    <param name="debug" value="false" />
    <param name="degub_out_file" value="debug_odom_ekf.txt" />
    <rosparam param="process_noise_covariance">[0.05, 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.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.005,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 ...
(more)
edit retag flag offensive close merge delete

Comments

Might not be the cause of your problem, but your covariances for odom and imu are either 0 or 1. I recommend to set valid covariances before continuing.

Humpelstilzchen gravatar imageHumpelstilzchen ( 2017-07-18 02:11:12 -0500 )edit

Thank you for including sample messages. I would back this up and start with just the odom-frame EKF, and fuse only the wheel odom. Then add the IMU. When you are happy with the odom-frame performance, then move to the map-frame EKF. Do you have a video of what you're seeing?

Tom Moore gravatar imageTom Moore ( 2017-09-28 05:55:19 -0500 )edit

Also, this isn't critical, but you have two_d_mode on for both EKFs, but appear to be fusing a lot of 3D variables, like roll, pitch, and Z acceleration. Those are going to get ignored.

Tom Moore gravatar imageTom Moore ( 2017-09-28 05:55:55 -0500 )edit