Add low pass filter to pose prediction

[mhuzai]

Even though our VIO setup is accurate over longer time frames (as confirmed by trajectory plots), it suffers from jitter over shorter durations. We should add a low pass filter to the output of pose prediction to remove the jitter. In terms of the actual choice of filter, the 1 Euro filter seems very appropriate as it has an adaptive cut-off to balance jitter and lag.

[ltstein]

Along with filtering, I think it would be good to add a cutoff to pose prediction if our slow pose fails to update within a certain timeframe to avoid the pose drifting as the IMU errors accumulate. Do you think that would be something to add at the same time?

[mhuzai]

Yeah, that would be good a feature to have, but let's keep it separate from this feature. The cutoff would require some manual tuning, and that would be dependent on a number of things (type of integrator, type of predictor, etc.). So better to experiment with that separately.

[shg8]

Filter added in project-filter branch

[mhuzai]

Please open a PR and link this issue to it. It'll automatically get closed when the PR gets merged in. Also, take a look at CONTRIBUTING.md for instructions on naming branches, opening PRs, etc.