Integrated velocity motion model

[fbattocchia]

Proposed changes

This branch adds integrated velocity motion model to the Beluga

Type of change

• [ ] 🐛 Bugfix (change which fixes an issue)
• [x] 🚀 Feature (change which adds functionality)
• [ ] 📚 Documentation (change which fixes or extends documentation)

💥 Breaking change! Explain why a non-backwards compatible change is necessary or remove this line entirely if not applicable.

Checklist

Put an x in the boxes that apply. This is simply a reminder of what we will require before merging your code.

• [x] Lint and unit tests (if any) pass locally with my changes
• [x] I have added tests that prove my fix is effective or that my feature works
• [x] I have added necessary documentation (if appropriate)
• [x] All commits have been signed for DCO

Additional comments

Anything worth mentioning to the reviewers.

[fbattocchia]

@glpuga @hidmic I have developed an initial implementation of the Integrated Velocity Motion Model based on the "Probabilistic Robotics" textbook. Below, I outline the design decisions and seek guidance on next steps: Current implementation features:

• Noise parameters: I used 4 parameters (α1-α4) maintaining compatibility with the existing odometric model, instead of the 6 parameters (α1-α6) specified in the textbook. For the additional orientation noise calculation (γ̂), I reused parameters α3 and α4.
• Extended interface: I preserved the reception of current and previous poses, adding corresponding timestamps (current time and previous time) to calculate the time interval internally.
• Development status: The implementation is in its initial phase, without unit tests or AMCL integration.

Questions for review:

• Noise parameters: Do you consider it appropriate to maintain 4 parameters for compatibility, or would it be preferable to implement the complete 6 parameters (α5, α6) from the theoretical model?
• Model selection: What would be the recommended mechanism for selecting between the existing odometric model and the new velocity model when integrating with amcl.hpp?
• Interface validation: Is the proposed extended interface with separate timestamps consistent with what is desired for the project?

I look forward to your feedback on these aspects before proceeding with testing and full system integration into AMCL.

[hidmic]

Do you consider it appropriate to maintain 4 parameters for compatibility, or would it be preferable to implement the complete 6 parameters (α5, α6) from the theoretical model?

I'm a bit of a broken record, but there's no fundamental reason why we should maintain some form of backwards compatibility for new features. In this particular case, if the new model has 6 noise parameters, 6 noise parameters it is.

What would be the recommended mechanism for selecting between the existing odometric model and the new velocity model when integrating with amcl.hpp?

For simplicity's sake, the same as for other models. But that doesn't match the expectations of existing models taking a pair of poses. In this specific case, you can probably get away with changing that pair of poses to a pair of timestamped poses everywhere. We are due for something like:

template <typename T, typename ClockT = std::chrono::steady_clock>
struct TimeStamped {
    T value;
    std::chrono::time_point<ClockT> timestamp;
};

in the core. FWIW tf2 does something similar (though in a less modern fashion).

Is the proposed extended interface with separate timestamps consistent with what is desired for the project?

See above.

[fbattocchia]

@hidmic Thanks for the suggestions, I'm attentive to any changes that may be needed.

[glpuga]

Not to lose track of our goals, this is the ticket for this task: https://github.com/ekumenlabs/monaco_f1tenth/issues/58

[glpuga]

Also, reminder that we aim of the task is to create a prototype to measure and decide if we continue this, not a polished version. We don't want to spend time polishing code we may decide to throw away after measuring.