ZED2 + GPS Kalman Filter

bw1903 · April 28, 2024, 10:41pm

Hello,

I’m currently working on an EKF with robot_localization that fuses GPS,IMU, and the ZED2 visual odometry. The issue is that the covariance in /zed2/zed_node/odom is very low (order of ~2e-7) while the GPS is around 2m. This means the GPS measurements aren’t taken in due to the unrealistically low error of the zed2, and this leads to the map->odom frame drifting over time. I’m wondering if this covariance is expected behavior

Myzhar · April 29, 2024, 8:20am

Hi @bw1903
Welcome to the Stereolabs community.

If the covariance of the GPS is around 2m, then the behavior you described is correct. 2m is a huge error and the Kalman Filter correctly does not trust this information.
You should improve GPS readings.

bw1903 · April 29, 2024, 4:32pm

Overtime though (after traveling 50m or so) the ZED2 drifts and has an error much greater than the GPS but the covariance matrix of the ZED2 doesn’t appear to reflect this. The GPS does not drift, although 2m is relatively large, it is still the better reading.

Myzhar · April 29, 2024, 5:07pm

If you think you can trust the GPS more than the ZED Positional Tracking pose, then simply rescale the GPS covariance by a fixed rescale factor.

For example:

github.com

stereolabs/zed-ros2-wrapper/blob/master/zed_components/src/zed_camera/src/zed_camera_component.cpp#L10137-L10143


      
          std::array<double, 9> position_covariance;
          position_covariance[0] = gnssData.latitude_std * mGnssHcovMul;    // X
          position_covariance[1 * 3 + 1] =
            gnssData.longitude_std * mGnssHcovMul;      // Y
          position_covariance[2 * 3 + 2] =
            gnssData.altitude_std * mGnssVcovMul;      // Z
          gnssData.position_covariance = position_covariance;