Use imu to estimate rotation

[val-ba]

Summary

Right now it is rotated around the base link, so it does not work completely yet

Proposed changes

Related issues

Checklist

• [ ] Run colcon build
• [ ] Write documentation
• [ ] Test on your machine
• [ ] Test on the robot
• [ ] Create issues for future work
• [ ] Triage this PR and label it