diff --git a/inc/hardware.h b/inc/hardware.h
index aaeec7c..c48ee1f 100644
--- a/inc/hardware.h
+++ b/inc/hardware.h
@@ -17,7 +17,7 @@ extern const struct device *imu;
 
 int CheckHardware();
 int InitHardware();
-int ReadIMU(std::array<double, 3> &accel, std::array<double, 3> &gyro);
+int ReadIMU(std::array<double, 3> &accel, std::array<double, 3> &gyro, std::array<double, 3> &magn);
 
 };  // namespace hardware
 };  // namespace RbfpidBalbot
diff --git a/src/app_main.cpp b/src/app_main.cpp
index 7980390..329c711 100644
--- a/src/app_main.cpp
+++ b/src/app_main.cpp
@@ -28,15 +28,12 @@ void AppMain(void) {
   LOG_INF("application started");
   LOG_INF("RBF-PID Balbot");
 
-  const int dt_ms = 10;
-  std::array<double, 3> d_accel, d_gyro, euler;
-  std::array<float, 3> f_accel, f_gyro;
-  posture::MahonyAHRS mahony(dt_ms);
-
+  std::array<double, 3> d_accel, d_gyro, d_magn, euler;
+  std::array<float, 3> f_accel, f_gyro, f_magn;
   for (;;) {
     k_sleep(K_MSEC(dt_ms));
 
-    if (int ret = hardware::ReadIMU(d_accel, d_gyro) < 0) {
+    if (int ret = hardware::ReadIMU(d_accel, d_gyro, d_magn) < 0) {
       LOG_ERR("fail to read IMU, ret=%d", ret);
       continue;
     }
@@ -44,9 +41,10 @@ void AppMain(void) {
     for (int i = 0; i < 3; i++) {
       f_accel[i] = (float)d_accel[i];
       f_gyro[i] = (float)d_accel[i];
+      f_magn[i] = (float)d_magn[i];
     }
 
-    mahony.Update(f_gyro[0], f_gyro[1], f_gyro[2], f_accel[0], f_accel[1], f_accel[2]);
+    mahony.Update(f_gyro[0], f_gyro[1], f_gyro[2], f_accel[0], f_accel[1], f_accel[2], f_magn[0], f_magn[1], f_magn[2]);
     euler = mahony.GetEuler();
     LOG_INF("r %f p %f y %f", euler[0], euler[1], euler[2]);
   }
diff --git a/src/hardware.cpp b/src/hardware.cpp
index bfe6857..b0c6b01 100644
--- a/src/hardware.cpp
+++ b/src/hardware.cpp
@@ -37,8 +37,8 @@ int hardware::InitHardware() {
   return 0;
 }
 
-int hardware::ReadIMU(std::array<double, 3> &accel, std::array<double, 3> &gyro) {
-  struct sensor_value tmp_a[3], tmp_g[3];
+int hardware::ReadIMU(std::array<double, 3> &accel, std::array<double, 3> &gyro, std::array<double, 3> &magn) {
+  struct sensor_value tmp_a[3], tmp_g[3], tmp_m[3];
   int rc = sensor_sample_fetch(hardware::imu);
 
   if (rc == 0) {
@@ -47,10 +47,14 @@ int hardware::ReadIMU(std::array<double, 3> &accel, std::array<double, 3> &gyro)
   if (rc == 0) {
     rc = sensor_channel_get(hardware::imu, SENSOR_CHAN_GYRO_XYZ, tmp_g);
   }
+  if (rc == 0) {
+    rc = sensor_channel_get(hardware::imu, SENSOR_CHAN_MAGN_XYZ, tmp_m);
+  }
   if (rc == 0) {
     for (int i = 0; i < 3; i++) {
       accel[i] = sensor_value_to_double(&tmp_a[i]);
       gyro[i] = sensor_value_to_double(&tmp_g[i]);
+      magn[i] = sensor_value_to_double(&tmp_m[i]);
     }
   } else {
     LOG_ERR("sample fetch/get failed: %d\n", rc);