Added underestimation factor - need to move to new EKF for stability