Magnetometer and gyro fusion

I am aware of multiwii, but I have Already written the software for it, and fusing the Z-gyro to a magnetometer would just be a small addition so was hoping to write it myself rather than use a ready made solution.