Improve robust_angle_estimator and change var to max