fix(ekf_localizer): skip update of z/roll/pitch when mahalanobis gate enabled

[kminoda]

Description

Current ekf localizer updates z/roll/pitch regardless of the mahalanobis gate which is performed on x & y. This induces an error when ekf receives odd (x,y,z). In this case, it skips x&y update based on this observation but unintentionally updates z.

Related links

x

Tests performed

Integration test using ros_testing

I have added two new test scripts in python.

Test with rosbag

Given some rosbags, I confirmed that

• z error is within several cm
• roll/pitch is within 0.1 deg
  
  

Notes for reviewers

x

Interface changes

None

Effects on system behavior

• z/roll/pitch will be correctly ignored when receiving odd x & y values.
• z/roll/pitch estimation will slightly change due to the time-delay smoothing

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

• I've confirmed the contribution guidelines.
• The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

• The PR follows the pull request guidelines.
• The PR has been properly tested.
• The PR has been reviewed by the code owners.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

• There are no open discussions or they are tracked via tickets.
• The PR is ready for merge.

After all checkboxes are checked, anyone who has write access can merge the PR.

[codecov]

Codecov Report

Patch coverage has no change and project coverage change: +0.02 🎉

Comparison is base (35bc65a) 14.05% compared to head (2018931) 14.08%.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #3915      +/-   ##
==========================================
+ Coverage   14.05%   14.08%   +0.02%     
==========================================
  Files        1456     1449       -7     
  Lines      102521   102311     -210     
  Branches    27295    27294       -1     
==========================================
+ Hits        14411    14412       +1     
+ Misses      72323    72115     -208     
+ Partials    15787    15784       -3     

Flag	Coverage Δ		*Carryforward flag
differential	22.91% <0.00%> (?)
total	14.08% <ø> (+0.03%)	⬆️	Carriedforward from 742a57a

*This pull request uses carry forward flags. Click here to find out more.

Impacted Files	Coverage Δ
..._localizer/include/ekf_localizer/ekf_localizer.hpp	0.00% <ø> (ø)
localization/ekf_localizer/src/ekf_localizer.cpp	0.00% <0.00%> (ø)

... and 11 files with indirect coverage changes

☔ View full report in Codecov by Sentry.
📢 Do you have feedback about the report comment? Let us know in this issue.

[YamatoAndo]

LGTM