Filtering on magnetometer (HMC5883L)

Hi dear all!
Recently I'm trying to build a head tracking module using Arduino. The sensors I'm using is MPU6050 and HMC5883L.
The pitch angle is measured by accelerator and gyro while a compensation filter is employed and works very well.
For the yaw angle it is a bit more complex. First, the magnetometer has a tilt problem (which means the sensor must be placed on a surface parallel to ground for correct measurement). So I combine the accelerator data with the compass to conquer it. Second, use the compensation filter with the gyro again for gathering a steady data.
Now the problem is, however, the magnetometer is so unstable which makes the final result shaking randomly. The shaking range is around 1°-2°. I've tried to decrease the sensitivity of HMC5883L but didn't make much differences. So I'm wondering if there is a kind of filter can be applied to this without causing to much delays?
Sorry for mistakes I may made in my English and appreciate for any of your advice, thank you!
Just in case of you may be interested, here is the material which helps me a lot:
for pitch angle:http://www.instructables.com/id/Angle-measurement-using-gyro-accelerometer-and-Ar/?ALLSTEPS
for yaw angle:Create a Digital Compass with the Raspberry Pi - Part 2

I've found that taking an average of 3 readings from the HMC5883L sensor reduces noise significantly- dont know if your code has time for that. I didnt measure the time but it didnt seem to take that long.

You should take advantage of years of other people's work and use IMU (AHRS) software. This open source package is state of the art and will work with your sensors.