Now you can set all the parameters, with gravity of your choice. Drive u = * theta + * omega + * phi + * psi Gravity = Initial theta =