Quaternion EKF with Expanded State Vector

Expanded the state vector to include gyro bias to mitigate prediction drift