Just for confirmation, is your T265 facing down, with the USB port to the right of the vehicle (with respect to moving forward direction)? If it is, then the first set of matrices should be correct.
I usually tilt the copter’s nose up like below, to have a consistent yaw:
When the quad icon appears on the Mission Planner map, does it face to the north or to some random direction? Could you provide more details on how is the path incorrect?