Revision history [back]

Your IMU data is provided in the imu_link frame and your GPS data is in the gps_link frame. Do you have transforms defined from those frames to the robot's base_link frame?

Also, your only reference for Z or its velocity is your odometry data, and you're fusing absolute Z, rather than Z velocity. So that means

1. Tier 2 EKF (map frame EKF) gets an odometry measurement. frame_id on the pose data in that measurement is odom
2. EKF attempts to transform from odom->map, because all pose data needs to be transformed into its world_frame. However, that instance of the EKF itself is generating that transform. I'm not totally convinced this is your issue, but it is an issue.

What I would recommend is that your turn off your GPS data as an input to the T2 EKF. See if its state estimate still explodes and report back. Exploding state estimates/covariance usually means some dimension is not being measured.

Your IMU data is provided in the imu_link frame and your GPS data is in the gps_link frame. Do you have transforms defined from those frames to the robot's base_link frame?

Also, your only reference for Z or its velocity is your odometry data, and you're fusing absolute Z, rather than Z velocity. So that means

1. Tier 2 EKF (map frame EKF) gets an odometry measurement. frame_id on the pose data in that measurement is odom
2. EKF attempts to transform from odom->map, because all pose data needs to be transformed into its world_frame. However, that instance of the EKF itself is generating that transform. I'm not totally convinced this is your issue, but it is an issue.

What I would recommend is that your turn off your GPS data as an input to the T2 EKF. See if its state estimate still explodes and report back. Exploding state estimates/covariance usually means some dimension is not being measured.