@brtaylor - @Don Kelly

Looks like its working. Getting a nice steady Yaw most of the time when not moving. I added a option to zero the gyro when I start seeing yaw drifting. Use it once or twice after starting the Filter and yaw stays steady. If moving there doesn't seem to be problem with yaw.