( edit: I added the best working approach in my augmented reality framework and now also take the gyroscope into account which makes it much more stable again: Droid
I couldn't test the code yet (but I will, looks really interesting). One thing that caught my attention is that you don't seem to filter the sensor data in any way.
Sensor readings are very noisy by nature, specially the magnetic sensor. I'd suggest you implement some low pass filtering.
See my previous answer for further reading.