[TASK] Add Dynamic Model for Constant Pose Filtering

[jorgenfj]

Description of task

Write a dynamic model class that does 6DOF filtering with respect to pose = [x, y, z, theta_x, theta_y, theta_z]

Suggested Workflow

No response

Specifications

No response

Contacts

No response

Code Quality

• Every function in header files are documented (inputs/returns/exceptions)
• The project has automated tests that cover MOST of the functions and branches in functions (pytest/gtest)
• The code is documented on the wiki (provide link)