add correct vz to position estimator, better PD height controller