I found some nice work done here Arduino Sketch to automatically calculate MPU6050 offsets - MPU-6050 6-axis accelerometer/gyroscope (InvenSense) - I2Cdevlib Forums to calibrate the MPU.
I modified his code a bit to write the calibrated offset values to EEPROM during calibration and then read it back and apply it during the setup phase of my sketch. The stability on yaw looks really good, still need to test repeated 360 degrees loops to makes sure the values come back to zero after a few loops.
Thanks,
Conrad