search

cra-ros-pkg / robot_localization

robot_localization is a package of nonlinear state estimation nodes. The package was developed by Charles River Analytics, Inc. Please ask questions on answers.ros.org.
http://www.cra.com
Other
1.4k stars 896 forks source link

Covariance initialization issue when covariance is not specified (rolling) #809

Open daisukes opened 1 year ago

daisukes commented 1 year ago

I happened to find this issue with my latest humble image for my project. https://github.com/ros/rosdistro/issues/37126

My EKF node could not output odometry well with 3.5.0 (rolling), but it works well with 3.3.1 (humble). I checked the differences between them and found the issue in this commit. https://github.com/cra-ros-pkg/robot_localization/commit/a8b92e965ce7934782a6f9abd86440bbfc68f4be#diff-376c7ff2a0637ccaaa018b18e572c3775d84bde2c782eb4defc27f1359b72a8d

The filter is initialized with default covariance values here. https://github.com/cra-ros-pkg/robot_localization/blob/284c2eccf029e680cd05688a400b582b2c9594bc/src/filter_base.cpp#L91-L125

So if the node does not have process_noise_covariance or initial_estimate_covariance, covariance should not be overwritten by zeros. https://github.com/cra-ros-pkg/robot_localization/blob/a8b92e965ce7934782a6f9abd86440bbfc68f4be/src/ros_filter.cpp#L1819-L1825

dsato80 commented 1 year ago

Hello @ayrton04

This is related to your change https://github.com/cra-ros-pkg/robot_localization/commit/a8b92e965ce7934782a6f9abd86440bbfc68f4be . Could you check my fix #810? Thank you.

SteveMacenski commented 1 year ago

@ayrton04 merged it so he should take a look over it, but I will as well today