In order to save time and sanity, you should not measure the pitch ->yaw and yaw-> pitch. It makes things too complicated and so far is just not significant. In the past we do not use these for the matrix work.
i.e. there should just be a 3x3 pitch matrix and a 3x3 yaw matrix. Once the loops are working we could investigate these things, but its really a very fine tweak at the end. There are quite a few other, more significant effects to handle before then.
To make things faster, I think we can just make a LOCKIN which has 3 inputs: it would have one oscillator, but 6 mixers. Should be simple to make.