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.

Marlin/Marlin_main.cpp

         //corrected_position.debug("position after");
         current_position[X_AXIS] = corrected_position.x;
         current_position[Y_AXIS] = corrected_position.y;
-        current_position[Z_AXIS] = zprobe_zoffset; // was: corrected_position.z
+        current_position[Z_AXIS] = corrected_position.z;
 
         sync_plan_position();
       }
       vector_3 corrected_position = plan_get_position();
       current_position[X_AXIS] = corrected_position.x;
       current_position[Y_AXIS] = corrected_position.y;
-      current_position[Z_AXIS] = zprobe_zoffset; // was: corrected_position.z
+      current_position[Z_AXIS] = corrected_position.z;
 
       sync_plan_position();
     }