position_estimator_inav, multirotor_pos_control: bugs fixed