Model simulation with estimated psi

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