fix(gyro_odometer): use all data

[kminoda]

Signed-off-by: kminoda koji.minoda@tier4.jp

Description

I would like to change the implementation of gyro_odometer.

Current gyro_odometer had an issue when the IMU frequency is higher than the velocity frequency. For example when IMU freq = 200 Hz and velocity freq = 50 Hz, the current gyro_odometer will concatenate both data and output twist in 200 Hz. Since the velocity frequency is not that high, it upsamples velocity observation by simply publishing one observed data four times.

This may be a problem because, assuming that the covariance of IMU and velocity are properly estimated, EKF may be overly confident on the velocity observation.

To be accurate on probability theory, I would like to change the implementation to the following:

• publish twist for every dt [sec] (e.g. dt=0.02[sec] or 50 Hz. This is also a parameter)
• if either velocity or IMU is not observed in the last dt [sec], do not publish twist
• if two or more data is observed for either velocity or IMU in the last dt [sec], publish a mean value of all the observed data (and lower the covariance based on central limit theorem)

Related links

Tests performed

Normal scenario test

Run Autoware with tutorial data and confirmed that the localization works fine and the covariance estimation is also reasonable

Edge-case test

• Run Autoware without velocity (and also IMU) input and confirmed that the warning is output in log
• Run Autoware, wait for a few seconds and kill velocity (and also IMU) topic, and confirmed that the error is output in log

Notes for reviewers

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

Base: 11.09% // Head: 11.08% // Decreases project coverage by -0.01% ⚠️

Coverage data is based on head (6ae751f) compared to base (f75c14b).
Patch coverage: 0.00% of modified lines in pull request are covered.

❗ Current head 6ae751f differs from pull request most recent head 4d97a37. Consider uploading reports for the commit 4d97a37 to get more accurate results

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #2293      +/-   ##
==========================================
- Coverage   11.09%   11.08%   -0.02%     
==========================================
  Files        1208     1208              
  Lines       86572    86675     +103     
  Branches    20837    20837              
==========================================
  Hits         9606     9606              
- Misses      66807    66910     +103     
  Partials    10159    10159              

Flag	Coverage Δ		*Carryforward flag
differential	0.00% <0.00%> (?)
total	11.07% <0.00%> (ø)		Carriedforward from 74d38f4

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

Impacted Files	Coverage Δ
...alization/gyro_odometer/src/gyro_odometer_core.cpp	0.00% <0.00%> (ø)

Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here.

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

[kminoda]

Let me make this PR a draft for now.
I encountered a possible degradation. The twist rate drops to 23-25 Hz even though output_rate = 50, IMU freq is 28-30Hz, wheel odometry freq is 28-30Hz.