Merge "Revert back to lsq2 velocity strategy"