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.