First draft of velocity kalman filter. Need to redo test to make sure it works.