Correct XYZ after obtaining plan_bed_level_matrix

The current position needs to be updated by using the inverse
`plan_bed_level_matrix` on X, Y, and Z. All moves in the planner are
transformed by the `plan_bed_level_matrix` from this point forward.