Now you can set all the parameters, with gravity of your choice. Motor drive u = * theta + * omega + * phi + * psi Acceleration psidot = * u - * psi. Gravity = Sensor error = radian. Initial theta =