diff --git a/software/firmware/src/peripherals/include/imu.h b/software/firmware/src/peripherals/include/imu.h
index 43ffffde..aaf12c54 100644
--- a/software/firmware/src/peripherals/include/imu.h
+++ b/software/firmware/src/peripherals/include/imu.h
@@ -274,6 +274,7 @@ void imu_read_fw_version(uint8_t *msb, uint8_t *lsb);
 void imu_read_calibration_status(bno55_calib_status_t *status);
 void imu_read_calibration_offsets(bno055_calib_offsets_t *offsets);
 void imu_read_axis_remap(bno055_axis_remap_t *remap);
+bool imu_set_axis_remap(bno055_axis_remap_t remap);
 bool imu_read_in_motion(void);
 
 // Math utilities
diff --git a/software/firmware/src/peripherals/src/imu.c b/software/firmware/src/peripherals/src/imu.c
index 4eb2241d..08e1e7be 100644
--- a/software/firmware/src/peripherals/src/imu.c
+++ b/software/firmware/src/peripherals/src/imu.c
@@ -348,9 +348,31 @@ void imu_read_axis_remap(bno055_axis_remap_t *remap)
    remap->z_remap_val = (reg_axis_map_config >> 4) & 0x03;
 
    uint8_t reg_axis_map_sign = i2c_read8(BNO055_AXIS_MAP_SIGN_ADDR);
-   remap->x_remap_sign = reg_axis_map_sign & 0x03;
-   remap->y_remap_sign = (reg_axis_map_sign >> 2) & 0x03;
-   remap->z_remap_sign = (reg_axis_map_sign >> 4) & 0x03;
+   remap->x_remap_sign = reg_axis_map_sign & 0x04;
+   remap->y_remap_sign = reg_axis_map_sign & 0x02;
+   remap->z_remap_sign = reg_axis_map_sign & 0x01;
+}
+
+bool imu_set_axis_remap(bno055_axis_remap_t remap)
+{
+   uint8_t reg_axis_map_config = remap.x_remap_val | (remap.y_remap_val << 2) | (remap.z_remap_val << 4);
+   uint8_t reg_axis_map_sign = (remap.x_remap_sign << 2) | (remap.y_remap_sign << 1) | (remap.z_remap_sign);
+
+   bno055_opmode_t saved_mode = get_mode();
+   set_mode(OPERATION_MODE_CONFIG);
+   i2c_write8(BNO055_AXIS_MAP_CONFIG_ADDR, reg_axis_map_config);
+   i2c_write8(BNO055_AXIS_MAP_SIGN_ADDR, reg_axis_map_sign);
+   set_mode(saved_mode);
+
+   //test whether the set is successful
+   bno055_axis_remap_t remap_verification = {0};
+   imu_read_axis_remap(&remap_verification);
+   if ((remap_verification.x_remap_val == remap.x_remap_val) && (remap_verification.y_remap_val == remap.y_remap_val) && (remap_verification.z_remap_val == remap.z_remap_val) &&
+      (remap_verification.x_remap_sign == remap.x_remap_sign) && (remap_verification.y_remap_sign == remap.y_remap_sign) && (remap_verification.z_remap_sign == remap.z_remap_sign))
+   {
+      return 1;
+   }
+   return 0;
 }
 
 bool imu_read_in_motion(void)
diff --git a/software/firmware/tests/peripherals/test_imu.c b/software/firmware/tests/peripherals/test_imu.c
index 84e8fa25..fea0f999 100644
--- a/software/firmware/tests/peripherals/test_imu.c
+++ b/software/firmware/tests/peripherals/test_imu.c
@@ -21,7 +21,7 @@ int main(void)
    uint8_t rev_msb, rev_lsb;
    bno55_calib_status_t status = {0};
    bno055_calib_offsets_t offsets = {0};
-   bno055_axis_remap_t remap = {0};
+   bno055_axis_remap_t remap = {.x_remap_val = 1, .y_remap_val = 0, .z_remap_val = 2};
    bno055_quaternion_t quaternion = {0};
    bno055_euler_t euler = {0};
    bno055_acc_t acc = {0};
@@ -31,8 +31,9 @@ int main(void)
    imu_read_fw_version(&rev_msb, &rev_lsb);
    print("BNO055 firmware version:%u.%u\n",rev_msb, rev_lsb);
 
-   imu_read_axis_remap(&remap);
-   print("BNO055 X mapping:%u, Y mapping:%u, Z mapping:%u, X sign:%u, Y sign:%u, Z sign:%u\n", remap.x_remap_val, remap.y_remap_val, remap.z_remap_val, remap.x_remap_sign, remap.y_remap_sign, remap.z_remap_sign);
+   //imu_read_axis_remap(&remap);
+   if (imu_set_axis_remap(remap)){print("remap success!\n");}
+   //print("BNO055 X mapping:%u, Y mapping:%u, Z mapping:%u, X sign:%u, Y sign:%u, Z sign:%u\n", remap.x_remap_val, remap.y_remap_val, remap.z_remap_val, remap.x_remap_sign, remap.y_remap_sign, remap.z_remap_sign);
 
    while (true)
    {
@@ -41,9 +42,13 @@ int main(void)
       print("Calibration status: sys %u, gyro %u, accel %u, mag %u\n",status.sys, status.gyro, status.accel, status.mag);
       //imu_read_calibration_offsets(&offsets);
       //print("Calibration offsets: %d, %d, %d \n", offsets.gyro_offset_x, offsets.gyro_offset_y, offsets.gyro_offset_z);
-      am_hal_delay_us(40000);
+      am_hal_delay_us(100000);
       //imu_read_accel_data(&acc);
       //print("Accel X = %d, Y = %d, Z = %d\n", (int32_t)acc.x, (int32_t)acc.y, (int32_t)acc.z);
+      memset(&acc, 0, sizeof(acc));
+      memset(&quaternion, 0, sizeof(quaternion));
+      memset(&euler, 0, sizeof(euler));
+
       imu_read_linear_accel_data(&acc);
       imu_read_quaternion_data(&quaternion);
       quaternion_to_euler(quaternion, &euler);