diff --git a/IMU.cpp b/IMU.cpp
index e2622b6..9516cee 100644
--- a/IMU.cpp
+++ b/IMU.cpp
@@ -315,14 +315,29 @@ void imuInit()
     if (qmi8658_.begin() == 0)
 	      Serial.println("qmi8658_init fail");
 
-    if (magnetometer_.initialize())
-        Serial.println("AK09918_init fail") ;
+    // Verify AK09918 is present on I2C before trying to configure it.
+    // WIA2 register (0x01) must return 0x09 on a real AK09918.
+    {
+        const uint16_t devId = magnetometer_.getDeviceID();
+        const uint8_t wia2 = (uint8_t)(devId & 0xFF);
+        if (wia2 != 0x09) {
+            Serial.printf("AK09918_init fail: WIA2=0x%02X (expected 0x09). "
+                          "Check I2C wiring on SDA/SCL (GPIO32/33), "
+                          "address 0x0C, and power supply.\n", wia2);
+        } else {
+            Serial.println("AK09918 found on I2C.");
+        }
+    }
+
+    // Initialize to power-down first so the subsequent switchMode goes through
+    // a defined state (unlike AK09918_NORMAL which skips I2C entirely).
+    magnetometer_.initialize(AK09918_POWER_DOWN);
+    delay(1);
     magnetometer_.switchMode(AK09918_CONTINUOUS_100HZ);
     err = magnetometer_.isDataReady();
     int retry_times = 0;
     while (err != AK09918_ERR_OK) {
-        Serial.println(err);
-        Serial.println("Waiting Sensor");
+        Serial.printf("AK09918 not ready (err=%d), retry %d/10...\n", err, retry_times + 1);
         delay(100);
         magnetometer_.reset();
         delay(100);
@@ -330,9 +345,13 @@ void imuInit()
         err = magnetometer_.isDataReady();
         retry_times ++;
         if (retry_times > 10) {
-          break;
+            Serial.println("AK09918 init timed out. Magnetometer will be unavailable.");
+            break;
         }
     }
+    if (err == AK09918_ERR_OK) {
+        Serial.println("AK09918 ready.");
+    }
     // Serial.println("Start figure-8 calibration after 1 seconds.");
     // delay(1000);
     // calibrate(10000, &offset_x, &offset_y, &offset_z);
@@ -365,10 +384,15 @@ void imuDataGet(EulerAngles *pstAngles,
   }
   if (magErr == AK09918_ERR_OK || magErr == AK09918_ERR_OVERFLOW) {
     updateMagCalibrationSession(x, y, z);
+    // Only feed the low-pass filter when we have real data.
+    lowPassMagneticSample((float)x, (float)y, (float)z, &correctedMagX, &correctedMagY, &correctedMagZ);
+  } else {
+    // Read failed: reuse last corrected values (filter state stays unchanged).
+    correctedMagX = filteredMagX - offset_x;
+    correctedMagY = filteredMagY - offset_y;
+    correctedMagZ = filteredMagZ - offset_z;
   }
 
-  lowPassMagneticSample((float)x, (float)y, (float)z, &correctedMagX, &correctedMagY, &correctedMagZ);
-
   pstMagnRawData->s16X = (int16_t)lroundf(correctedMagX);
   pstMagnRawData->s16Y = (int16_t)lroundf(correctedMagY);
   pstMagnRawData->s16Z = (int16_t)lroundf(correctedMagZ);