simplify the calculation of the ground plane and make it robust agains...

simplify the calculation of the ground plane and make it robust agains parameter changes mid-movement