Addition of velocity scale factor estimation to EKF

[rsasaki0109]

What does this implement?

I have added velocity scale factor estimation to extended_kalman_filter.py.
This is for correcting the vehicle speed measured with scale factor errors due to factors such as wheel wear.

Additional information

I have adjusted the noise for velocity scale factor estimation, as it was not well-estimated with the default parameters.
It is assumed that the GNSS is RTK FIXed.

CheckList

• Did you add an unittest for your new example or defect fix?
• Did you add documents for your new example?
• All CIs are green? (You can check it after submitting)

[AtsushiSakai]

Hi, thank you for great PR!!
Is it possible to add a new script for it, not modify the current file?

[rsasaki0109]

Thank you for your suggestion.
I've now added the new script ekf_with_velocity_correction.py to the Localization/extended_kalman_filter directory, which introduces the velocity correction feature for EKF.
Does this look good to you?

[AtsushiSakai]

Thank you. Is it possible to update this doc to explain your code too?

[rsasaki0109]

OK, I will do that.

[rsasaki0109]

@AtsushiSakai
I have added the document. Is this the correct way to add it?

[AtsushiSakai]

I'm sorry for my late reply. @rsasaki0109
For document, I think it is better to modify this file https://github.com/AtsushiSakai/PythonRobotics/blob/master/docs/modules/localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization_main.rst
instead of adding new docs.

You can do grouping the "Filter design", "Motion model", "Observation model" for only position estimation and your position and velocity estimation.

[AtsushiSakai]

Only remaining comment is for doc structure.

[AtsushiSakai]

@rsasaki0109 Hi. Is it possible to finish this PR? Only remaining task is just organizing docs. If it is difficult for you, I can do it. Let me know. Thanks for good PR.

[rsasaki0109]

@AtsushiSakai
Sorry, I didn't notice your comment. I'll work on it. Probably this weekend.

(Note: Since I couldn't do it last weekend, I will do it in my free time...).

[rsasaki0109]

@AtsushiSakai
Is your comment meant to mean this?

ex)

• Motion Model

  • only position estimation
  • position and velocity estimation

• Observation model

  • only position estimation
  • position and velocity estimation

[AtsushiSakai]

Is your comment meant to mean this?

Yes. I think other two “filter design” and “Extended Kalman Filter” are common.
@rsasaki0109

[AtsushiSakai]

sorry, is it possible to merge the latest master code to build doc in CI? @rsasaki0109

[AtsushiSakai]

LGTM. Thank you!! @rsasaki0109

[rsasaki0109]

I'm sorry, I've been a bit busy this month and my response was delayed...
Thank you for your assistance!