V3D predictedRPY = qt.ToEulerRPY();
float predictedRoll = predictedRPY.x;
float predictedPitch = predictedRPY.y;
ekfState(6) = (float)predictedRPY.z; // yaw