Changed Gains a bit to tune the pitch and roll with R matrix being used