final commit, contains updated code with verified velocity values and bag file