Bug in ekf using orientation data?

[tscho1l]

Hi,
in file ekf.cpp, line 165 to 187 [https://github.com/cra-ros-pkg/robot_localization/blob/indigo-devel/src/ekf.cpp#L165-L187] the points (2) and (3) should be switched.
While the filter state gets updated with the cropped orientation values from step (3), the mahalanobis check is done with the uncropped orientation data which leads to a jump of these values in the innovationSubset if any orientation exceeds the value +/- PI. If the jump is leading to a bigger value than the mahalanobis threshold allows, the filter update isn't executed.

[ayrton04]

Nice catch! Any chance you'd be willing to submit a PR?

[tscho1l]

Yes, I could do that.

[ayrton04]

Merged in PR #238