Ok well I think I somewhat understand now. However I might be tempted to dump the matrices altogether and set up three pairs of vectors for the gyroscopes and then calculate everything with cross-products and dot-products.  I'm not really familial with the hardware though.  For instance as far as I know the only way a gyro won't move is if you are rotating around it's axis. So it seems to me that at least two of the gyros will always be moving if you are changing orientation, but I guess maybe the computer combines the 3 gyros input and gives you the solution.

In any case I'm still betting you could do it a much easier way.

Bottom line, I have narrowed down the fault to this piece of code:

double yr = (rate[0] * sb + rate[1] * cb) * (1.0 / cp);

This is supposed to be the horizon relative yaw rate, made by combining body pitch rate (rate[0]) and body yaw rate (rate[1]). In order to get the proper value, I need to establish what is the relationship rate[1] has with pitch and heading, since those 2 combined are what cause it to go off the expected value.