predicting with Q
... nope
using only yaw+roll as an observation due to gimbal lock
added gyro back to plot