Adjust equations for feedback, test on rough terrain