Ask Your Question
1

Pose estimation with GNSS (RTK) + heading + IMU is drifting

asked 2021-02-18 04:12:50 -0500

TooVee gravatar image

updated 2021-02-19 12:31:59 -0500

I'm trying to use robot_localization to get a pose estimation for my Moving Base RTK Application. As Sensors I only use a GPS module which outputs position and world referenced heading information as well as a BNO055 IMU. I fuse GPS position and heading into navsat_transform and let it produce odometry messages. Position, Odometry and accelleration is fused into two ekf nodes to produce map->odom and odom->base_link tf. So far my tf tree looks promising, but when I watch my data via mapviz my estimated position drifts drastically and random over time even when I don't move.

My ekf param file for map->odom:

frequency: 5
two_d_mode: true
print_diagnostics: true
publish_tf: true

map_frame: map
odom_frame: odom
base_link_frame: headingkit/base_link
world_frame: map

odom0: odometry/gps
odom0_config: [true,  true,  false,
                false, false, false,
                false, false, false,
                false, false, false,
                false, false, false]

imu0: ubxreader/navheading
imu0_config: [false, false, false,
                false,  false, true,
                false, false, false,
                false,  false, false,
                false,  false, false]
imu0_nodelay: false
imu0_differential: false
imu0_relative: true
imu0_queue_size: 5

imu1: imureader/imu
imu1_config: [false, false, false,
                false, false, false,
                false, false, false,
                false, false, true,
                true, true,false]
imu1_nodelay: false
imu1_differential: false
imu1_relative: false
imu1_queue_size: 5

My ekf param file for odom->base_link:

frequency: 5
two_d_mode: true
print_diagnostics: true
publish_tf: true

map_frame: map
odom_frame: odom
base_link_frame: headingkit/base_link
world_frame: odom

imu0: ubxreader/navheading
imu0_config: [false, false, false,
                false,  false, true,
                false, false, false,
                false,  false, false,
                false,  false, false]
imu0_nodelay: false
imu0_differential: false
imu0_relative: true
imu0_queue_size: 5

imu1: imureader/imu
imu1_config: [false, false, false,
                false, false, false,
                false, false, false,
                false, false, true,
                true, true,false]
imu1_nodelay: false
imu1_differential: false
imu1_relative: false
imu1_queue_size: false

I double checked the coordinate frame of my IMU and it turns the same way as my GPS heading does. What did I do wrong?

EDIT1:

When removing /imureader/imu from both ekf I noticed 3 things: 1) While testing wihtout editing my files any further, the odometry & position estimate drift sothwards. (even with no motion) 2) When commenting out "magnetic declination radians" @ navsat node I get a reasonable result for my position estimation. BUT: Every Position seems to be displaced (by ~5-10° with respect to north probably). 3) My estimated position always starts at my map origin. And visualized odometry (mapviz) always point to the same heading (0).

edit retag flag offensive close merge delete

Comments

Try removing the imureader/imu entirely from both nodes (if its not too hard) and observe if you get the same behavior please.

JackB gravatar image JackB  ( 2021-02-19 08:13:25 -0500 )edit

I tried it out and edited my question.

TooVee gravatar image TooVee  ( 2021-02-19 12:32:37 -0500 )edit

1 Answer

Sort by » oldest newest most voted
2

answered 2021-03-25 04:33:19 -0500

Tom Moore gravatar image

Please also include a sample sensor message from each sensor input, and maybe a better description (or image) of what the problem is with the filter output.

I see a couple things wrong here.

  • In your odom->base_link EKF instance, you are just fusing IMU data. That's going to be unbelievably noisy. If you watch your odom-frame state estimate, I would think it's probably drifting off endlessly. That won't really matter for your map-frame state estimate, but it makes me question why you are bothering with two EKF instances.
  • You have a similar problem in your map->odom EKF. You have no velocity reference (e.g., no wheel odometry or visual odometry), so you are just feeding the filter poses and accelerations. The GPS positions will keep dragging the EKF back on track, but the acceleration biases are still going to get integrated into velocity and try to drag the state estimate away. I generally don't recommend straight IMU + GPS fusion. The package just doesn't handle it well, frankly.

I think you're going to need a velocity reference for this to work well.

edit flag offensive delete link more

Comments

Thank you very much for your reply. The reason why I used two instances of EKF Nodes is, because the package description tells me to fuse continuous data into my odom->base_link TF. I though the right procedure would be to kind of split it, so that GPS data is only fused into map->odom. I guess I have to find an alternative solution.

TooVee gravatar image TooVee  ( 2021-03-30 11:13:12 -0500 )edit

We are having the same issue with a similar setup. Is this an example of where velocity from wheel encoders will greatly improve the estimate? (What other kind of velocity estimates exist?)

saulzar gravatar image saulzar  ( 2021-06-21 00:42:24 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2021-02-18 04:12:50 -0500

Seen: 249 times

Last updated: Mar 25