Magnetometer calibration algorithm