Estimation And Planning For Dynamic Robot Behaviors