Updated rotation matrix, problem was focal length. Need to get correct yaw from new matrix