search

buckbaskin / formak

https://buckbaskin.com/formak/generated/html/index.html
MIT License
3 stars 0 forks source link

Explicit ROS Message Support [Enhancement] #29

Open buckbaskin opened 7 months ago

buckbaskin commented 7 months ago

Feature Request

One liner: Provide support for using ROS message types as inputs to the ManagedFilter class

FormaK aims to combine symbolic modeling for fast, efficient system modelling with code generation to create performant code that is easy to use.

The Five Key Elements the library provides to achieve this user experience are:

This design focuses on the 2nd and 5th elements for Python and C++ support for different use cases. Many robotic systems run in the ROS environment, so supporting ROS messages directly would allow for easier interoperability with existing systems. There may also be performance benefits (e.g. reduced memory usage) to directly supporting the message types instead of having each user re-implement an interoperability layer between ROS messages and the C++ (or Python) structures used by the FormaK runtime.

Design Decisions

References

oliver-batchelor commented 7 months ago

How would you recommend to handle different rates and possibly lack of exact synchronisation for the models which formak produces?

I'm envisioning using it to replace things like robot localisation. I presume having a motion model specific to your vehicle will be much better than using one very general kalman filter?

buckbaskin commented 7 months ago

How would you recommend to handle different rates and possibly lack of exact synchronisation for the models which formak produces?

Are you referring to synchronizing inputs, or the outputs of the estimator?

For the input side, I'm working on a design for a more automated system for performing synchronization as #24 to improve the "FormaK Runtime".

In the current version of the library, there is manual work required to perform synchronization. The outline of the algorithm I'd recommend is:

  1. Maintain a clock reference for publishing time
  2. Queue inputs as they arrive (e.g. as ROS messages from subscribers), sorted by time
  3. At the desired output rate, take the front of the message queue that are at or before the desired reference time
  4. Pass that list/vector as input to the FormaK ManagedFilter .tick method (managed.tick(reference_time, list_of_messages)) and the ManagedFilter will process the messages according to their time stamp using the process model to shift in time and align with the stamped time
  5. The output from the tick function will be the estimate of the filter for all of the messages up to the reference time

Example usage of the ManagedFilter class

Note: Right now, there's a conversion step from the ROS message to a C++ structure that I'm skipping over. This design #29 would update the library to generalize the current logic to interoperate with ROS types and reduce the effort required to perform these conversions.

I'm envisioning using it to replace things like robot localisation. I presume having a motion model specific to your vehicle will be much better than using one very general kalman filter?

In general, a more specific model will provide more "information" by constraining the estimator. For example, integrating IMU motion could be used in many vehicles/applications, but the expected motion of say a rocket launch vs a person walking would be different, so you could likely get a better estimate for each use case by using a more specialized model.

The caveat that I'd offer is that sometimes adding many extra parameters for a more complicated model can increase its sensitivity to noise, so there can be diminishing returns to making a more complicated model of the system.

One of the areas that I'd like to work on but haven't come to a solid design for is automating the model selection process to allow for choosing between, say, a general model or a specific model or even simplifying a complicated model based on data from the system.

oliver-batchelor commented 7 months ago

Hi Buck,

Seems you've thought about it quite a bit, seems this is exactly the problem the ManagedFilter tries to solve. Regarding synchronization I was thinking about the inputs, though synchronizing with the outputs is also important!

Definitely look forward to playing around sometime, probably in this context!

On Sun, Feb 25, 2024 at 6:30 PM Buck Baskin @.***> wrote:

How would you recommend to handle different rates and possibly lack of exact synchronisation for the models which formak produces?

Are you referring to synchronizing inputs, or the outputs of the estimator?

For the input side, I'm working on a design for a more automated system for performing synchronization as #24 https://github.com/buckbaskin/formak/issues/24 to improve the "FormaK Runtime".

In the current version https://github.com/buckbaskin/formak/commit/f7b5267ae81494b4327d66f3152f915d0fa4c5c9 of the library, there is manual work required to perform synchronization. The outline of the algorithm I'd recommend is:

  1. Maintain a clock reference for publishing time
  2. Queue inputs as they arrive (e.g. as ROS messages from subscribers), sorted by time
  3. At the desired output rate, take the front of the message queue that are at or before the desired reference time
  4. Pass that list/vector as input to the FormaK ManagedFilter .tick method (managed.tick(reference_time, list_of_messages)) and the ManagedFilter will process the messages according to their time stamp using the process model to shift in time and align with the stamped time
  5. The output from the tick function will be the estimate of the filter for all of the messages up to the reference time

Example usage of the ManagedFilter class https://github.com/buckbaskin/formak/blob/f7b5267ae81494b4327d66f3152f915d0fa4c5c9/py/test/unit/runtime/ManagedFilter_no_calibration_test.py#L218

Note: Right now, there's a conversion step from the ROS message to a C++ structure that I'm skipping over. This design #29 https://github.com/buckbaskin/formak/issues/29 would update the library to generalize the current logic to interoperate with ROS types and reduce the effort required to perform these conversions.

I'm envisioning using it to replace things like robot localisation. I presume having a motion model specific to your vehicle will be much better than using one very general kalman filter?

In general, a more specific model will provide more "information" by constraining the estimator. For example, integrating IMU motion could be used in many vehicles/applications, but the expected motion of say a rocket launch vs a person walking https://www.mdpi.com/1424-8220/22/9/3289 would be different, so you could likely get a better estimate for each use case by using a more specialized model.

The caveat that I'd offer is that sometimes adding many extra parameters for a more complicated model can increase its sensitivity to noise, so there can be diminishing returns to making a more complicated model of the system.

One of the areas that I'd like to work on but haven't come to a solid design for is automating the model selection process to allow for choosing between, say, a general model or a specific model or even simplifying a complicated model based on data from the system.

— Reply to this email directly, view it on GitHub https://github.com/buckbaskin/formak/issues/29#issuecomment-1962819732, or unsubscribe https://github.com/notifications/unsubscribe-auth/AAITRZKG3YGRJDNC5SP27R3YVLD6VAVCNFSM6AAAAABDSOEIT2VHI2DSMVQWIX3LMV43OSLTON2WKQ3PNVWWK3TUHMYTSNRSHAYTSNZTGI . You are receiving this because you commented.Message ID: @.***>