Fixed the plane vector equation to a simpler one (only dependent on the normal)

Removed the calculation of the inverse matrix since the rotation matrix is orthogonal, therefore inverted == transposed.
Much simpler and mathematically robust.
Implemented a least squares fit of the bed equation for auto bed leveling.

The code for the LSQ solver (qr_solve) is copyrighted by John Burkardt and released under LGPL here:
http://people.sc.fsu.edu/~%20jburkardt/c_src/qr_solve/qr_solve.html
(see qr_solve.cpp for further copyright information)