Using more robust filtering parameters, will be heavy on robot