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

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

TooVee
11 ●1 ●1 ●2

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

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 ( 2021-02-19 08:13:25 -0600 )edit

I tried it out and edited my question.

TooVee ( 2021-02-19 12:32:37 -0600 )edit

add a comment

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

Tom Moore

flag of United Kingdom of Great Britain and Northern Ireland

13689 ●74 ●240 ●232 http://github.com/ayrton04

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.

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 ( 2021-03-30 11:13:12 -0600 )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 ( 2021-06-21 00:42:24 -0600 )edit

add a comment

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

Comments

1 Answer

Comments

Question Tools

Stats

Related questions

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

Comments

1 Answer

Comments

Question Tools

Stats

Related questions

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