Improved update rate for velocity estimate, not yet where we want it to be