Fix/sim test dp reference convergence

[jorgenfj]

The waypoint navigation test has been flaky every since the reference filter convergence criteria was changed from reference convergence to actual pose convergence.

My hypothesis is that the workflow runner environment is not capable of handling high pitch values causes the test to never complete.

[codecov]

Codecov Report

✅ All modified and coverable lines are covered by tests.
✅ Project coverage is 33.93%. Comparing base (8bd1fb4) to head (498f9a9).
⚠️ Report is 1 commits behind head on main.

Additional details and impacted files

@@            Coverage Diff             @@
##             main     #672      +/-   ##
==========================================
+ Coverage   33.79%   33.93%   +0.13%     
==========================================
  Files          44       44              
  Lines        3003     3003              
  Branches      780      780              
==========================================
+ Hits         1015     1019       +4     
+ Misses       1805     1801       -4     
  Partials      183      183              

Flag	Coverage Δ
unittests	33.93% <ø> (+0.13%)	⬆️

Flags with carried forward coverage won't be shown. Click here to find out more.
see 2 files with indirect coverage changes

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

[github-actions]

🎉 This PR is included in version 2.7.0 🎉

The release is available on GitHub release

Your semantic-release bot 📦🚀