Similar to how this was being done for SLAM. It's significantly less jittery and still has a good response. Smarter filters might benefit from raw measurements so the functionality can be enabled again with a checkbox.
All at least visually inspected, some revised from the auto-fixit