diff --git a/main/FlightController/controllerTick.h b/main/FlightController/controllerTick.h
index a7b767e..41f571e 100644
--- a/main/FlightController/controllerTick.h
+++ b/main/FlightController/controllerTick.h
@@ -4,7 +4,7 @@
  * Created Date: 2024-03-29 22:57:12
  * Author: Guoyi
  * -----
- * Last Modified: 2024-04-28 15:22:56
+ * Last Modified: 2024-04-29 23:03:26
  * Modified By: Guoyi
  * -----
  * Copyright (c) 2024 Guoyi Inc.
@@ -27,7 +27,7 @@
 
 uint32_t tickCount = 0;
 
-void controllerTick(int dt)
+void controllerTick(int dt, bool shouldDriveMotors)
 {
     /* 从全局变量读取用户指令 */
 
@@ -72,15 +72,21 @@ void controllerTick(int dt)
     }
 #endif
 #ifndef PRINT_PWM_MODE
-    // if (tickCount * dt >= 3000)
-    // {
-    //     stopAllMotors();
-    //     esp_restart();
-    // }
-    setMotorPWMPercentage(0, PWM1);
-    setMotorPWMPercentage(1, PWM2);
-    setMotorPWMPercentage(2, PWM3);
-    setMotorPWMPercentage(3, PWM4);
+    if (shouldDriveMotors)
+    {
+        if (tickCount % 2) // 每隔两个tick，更新一次电机PWM
+        {
+            setMotorPWMPercentage(0, PWM1);
+            setMotorPWMPercentage(1, PWM2);
+            setMotorPWMPercentage(2, PWM3);
+            setMotorPWMPercentage(3, PWM4);
+        }
+    }
+    else
+    {
+        stopAllMotors();
+    }
+
 #endif
     tickCount++;
 }
diff --git a/main/mpu6050/algorithm/kalmanFilter.h b/main/mpu6050/algorithm/kalmanFilter.h
new file mode 100644
index 0000000..d887c96
--- /dev/null
+++ b/main/mpu6050/algorithm/kalmanFilter.h
@@ -0,0 +1,63 @@
+/*
+ * File: \kalmanFilter.h
+ * Project: algorithm
+ * Created Date: 2024-04-29 20:41:02
+ * Author: Guoyi
+ * -----
+ * Last Modified: 2024-04-29 20:57:21
+ * Modified By: Guoyi
+ * -----
+ * Copyright (c) 2024 Guoyi Inc.
+ * 
+ * ------------------------------------
+ */
+
+float kalman_filter(float angle_m, float gyro_m, float *angle_f, float *angle_dot_f)
+{
+    //------------------------------
+    static float angle, angle_dot;
+    const float Q_angle = 0.000001, Q_gyro = 0.0001, R_angle = 0.5, dt = 0.002;
+    static float P[2][2] = {
+        {1, 0},
+        {0, 1}};
+    static float Pdot[4] = {0, 0, 0, 0};
+    const uint8 C_0 = 1;
+    static float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
+    //------------------------------
+    angle += (gyro_m - q_bias) * dt;
+
+    Pdot[0] = Q_angle - P[0][1] - P[1][0];
+    Pdot[1] = -P[1][1];
+    Pdot[2] = -P[1][1];
+    Pdot[3] = Q_gyro;
+
+    P[0][0] += Pdot[0] * dt;
+    P[0][1] += Pdot[1] * dt;
+    P[1][0] += Pdot[2] * dt;
+    P[1][1] += Pdot[3] * dt;
+
+    angle_err = angle_m - angle;
+
+    PCt_0 = C_0 * P[0][0];
+    PCt_1 = C_0 * P[1][0];
+
+    E = R_angle + C_0 * PCt_0;
+
+    K_0 = PCt_0 / E;
+    K_1 = PCt_1 / E;
+
+    t_0 = PCt_0;
+    t_1 = C_0 * P[0][1];
+
+    P[0][0] -= K_0 * t_0;
+    P[0][1] -= K_0 * t_1;
+    P[1][0] -= K_1 * t_0;
+    P[1][1] -= K_1 * t_1;
+
+    angle += K_0 * angle_err;
+    q_bias += K_1 * angle_err;
+    angle_dot = gyro_m - q_bias;
+
+    *angle_f = angle;
+    *angle_dot_f = angle_dot;
+}
\ No newline at end of file
diff --git a/main/mpu6050/algorithm/slidingFilter.h b/main/mpu6050/algorithm/slidingFilter.h
new file mode 100644
index 0000000..3fc6298
--- /dev/null
+++ b/main/mpu6050/algorithm/slidingFilter.h
@@ -0,0 +1,60 @@
+/*
+ * File: \slidingFilter.h
+ * Project: algorithm
+ * Created Date: 2024-04-29 21:10:08
+ * Author: Guoyi
+ * -----
+ * Last Modified: 2024-04-29 22:54:36
+ * Modified By: Guoyi
+ * -----
+ * Copyright (c) 2024 Guoyi Inc.
+ *
+ * ------------------------------------
+ */
+
+#ifndef SLIDING_FILTER_H
+#define SLIDING_FILTER_H
+
+#include <stdio.h>
+#include "utils/F3D.h"
+
+#define SLIDING_FILTER_QUEUE_LEN 10
+
+// 只对加速度进行滑动滤波
+static float SlidingFilterQueue[3][SLIDING_FILTER_QUEUE_LEN]; // gcc默认初始化全0，不用手动置为零
+static int insertStart = 0;
+static float sumX = 0;
+static float sumY = 0;
+static float sumZ = 0;
+
+F3D performSlidingFilter(F3D OriginalData)
+{
+    float ax = OriginalData.x;
+    float ay = OriginalData.y;
+    float az = OriginalData.z;
+    
+    // 更新sum
+    sumX += ax / SLIDING_FILTER_QUEUE_LEN - SlidingFilterQueue[0][insertStart];
+    sumY += ay / SLIDING_FILTER_QUEUE_LEN - SlidingFilterQueue[1][insertStart];
+    sumZ += az / SLIDING_FILTER_QUEUE_LEN - SlidingFilterQueue[2][insertStart];
+
+    // 更新队列
+    SlidingFilterQueue[0][insertStart] = ax / SLIDING_FILTER_QUEUE_LEN;
+    SlidingFilterQueue[1][insertStart] = ay / SLIDING_FILTER_QUEUE_LEN;
+    SlidingFilterQueue[2][insertStart] = az / SLIDING_FILTER_QUEUE_LEN;
+    insertStart++;
+    if (insertStart >= SLIDING_FILTER_QUEUE_LEN)
+    {
+        insertStart = 0;
+    }
+
+    // 包装值，并返回F3D
+    F3D ret = {
+        .x = sumX,
+        .y = sumY,
+        .z = sumZ,
+    };
+    return ret;
+}
+
+#endif
diff --git a/main/mpu6050/motionData.h b/main/mpu6050/motionData.h
index 7e98f72..e7929a8 100644
--- a/main/mpu6050/motionData.h
+++ b/main/mpu6050/motionData.h
@@ -4,7 +4,7 @@
  * Created Date: 2024-03-07 22:51:03
  * Author: Guoyi
  * -----
- * Last Modified: 2024-04-29 18:50:44
+ * Last Modified: 2024-04-29 23:53:44
  * Modified By: Guoyi
  * -----
  * Copyright (c) 2024 Guoyi Inc.
@@ -21,9 +21,16 @@
 #include "freertos/FreeRTOS.h"
 #include "freertos/task.h"
 #include "LED/StatusLED.h"
+#include "./algorithm/slidingFilter.h"
+
+#define MIN(X,Y) ((X)<(Y)?(X):(Y))
 
 #define ACCEL_RANGE 2
 #define GYRO_RANGE 250
+/* 校准相关配置 */
+// 加速度计包含了太多的噪声。
+// 陀螺仪一般不需要滤波，原始值就很稳定，就是陀螺仪刚开始可能会有零偏，需要我们手动减去这个值。
+#define CALIBRATION_COUNT 30.0f
 /* 欧拉角解算相关的配置 */
 #define Kp 100.0f    // 比例增益支配率收敛到加速度计/磁强计
 #define Ki 0.002f    // 积分增益支配率的陀螺仪偏见的衔接
@@ -72,15 +79,21 @@ void MotionData_Calibrate()
 
     // 开始校准
     enableStatusLED();
+    float gyroX_SUM = 0;
+    float gyroY_SUM = 0;
     float gyroZ_SUM = 0;
-    for (int i = 0; i < 30; i++)
+    for (int i = 0; i < CALIBRATION_COUNT; i++)
     {
         vTaskDelay(30 / portTICK_PERIOD_MS);
         // F3D accel = getAccelData();
         F3D gyro = getGyroData();
+        gyroX_SUM += gyro.x;
+        gyroY_SUM += gyro.y;
         gyroZ_SUM += gyro.z;
     }
-    CalibrationOffset.gz = gyroZ_SUM / 10.0f;
+    CalibrationOffset.gx = gyroX_SUM / CALIBRATION_COUNT;
+    CalibrationOffset.gy = gyroY_SUM / CALIBRATION_COUNT;
+    CalibrationOffset.gz = gyroZ_SUM / CALIBRATION_COUNT;
     disableStatusLED();
 }
 
@@ -89,11 +102,12 @@ void MotionData_Calibrate()
  */
 F3D getAccelData()
 {
-    F3D ret = {
-        .x = MPU6050_Get_16bit_Data(ACCEL_XOUT_H) / (double)(65536 / 2 / ACCEL_RANGE),
-        .y = MPU6050_Get_16bit_Data(ACCEL_YOUT_H) / (double)(65536 / 2 / ACCEL_RANGE),
-        .z = MPU6050_Get_16bit_Data(ACCEL_ZOUT_H) / (double)(65536 / 2 / ACCEL_RANGE),
+    F3D originalVal = {
+        .x = MPU6050_Get_16bit_Data(ACCEL_XOUT_H) / (double)(65536 / 2 / ACCEL_RANGE) - CalibrationOffset.ax,
+        .y = MPU6050_Get_16bit_Data(ACCEL_YOUT_H) / (double)(65536 / 2 / ACCEL_RANGE) - CalibrationOffset.ay,
+        .z = MPU6050_Get_16bit_Data(ACCEL_ZOUT_H) / (double)(65536 / 2 / ACCEL_RANGE) - CalibrationOffset.az,
     };
+    F3D ret = performSlidingFilter(originalVal);
     return ret;
 }
 
@@ -108,10 +122,11 @@ float getAccelMagnitude()
  */
 F3D getGyroData()
 {
+    // 限制角速度感知范围
     F3D ret = {
-        .x = MPU6050_Get_16bit_Data(GYRO_XOUT_H) / (double)(65536 / 2 / GYRO_RANGE),
-        .y = MPU6050_Get_16bit_Data(GYRO_YOUT_H) / (double)(65536 / 2 / GYRO_RANGE),
-        .z = MPU6050_Get_16bit_Data(GYRO_ZOUT_H) / (double)(65536 / 2 / GYRO_RANGE) - CalibrationOffset.gz,
+        .x = MIN(MPU6050_Get_16bit_Data(GYRO_XOUT_H) / (double)(65536 / 2 / GYRO_RANGE) - CalibrationOffset.gx, 120),
+        .y = MIN(MPU6050_Get_16bit_Data(GYRO_YOUT_H) / (double)(65536 / 2 / GYRO_RANGE) - CalibrationOffset.gy, 120),
+        .z = MIN(MPU6050_Get_16bit_Data(GYRO_ZOUT_H) / (double)(65536 / 2 / GYRO_RANGE) - CalibrationOffset.gz, 120),
     };
     return ret;
 }
@@ -167,7 +182,7 @@ F3D calcEulerAngle(F3D accel, F3D gyro)
     float Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 57.3; // rollv
     // float Yaw = atan2(2 * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * 57.3;
     float Yaw = atan2(2 * (q1 * q2 + q0 * q3), 1 - 2 * (q2 * q2 + q3 * q3)) * 57.3;
-    
+
     F3D ret;
     ret.x = Pitch;
     ret.y = Roll;
diff --git a/main/mpu6050/mpu6050.h b/main/mpu6050/mpu6050.h
index 9236ac8..92d9470 100644
--- a/main/mpu6050/mpu6050.h
+++ b/main/mpu6050/mpu6050.h
@@ -4,7 +4,7 @@
  * Created Date: 2024-03-07 19:55:56
  * Author: Guoyi
  * -----
- * Last Modified: 2024-04-27 23:12:23
+ * Last Modified: 2024-04-29 23:48:41
  * Modified By: Guoyi
  * -----
  * Copyright (c) 2024 Guoyi Inc.
diff --git a/main/tasks/controllerTickLoop.h b/main/tasks/controllerTickLoop.h
index b85ce46..721b903 100644
--- a/main/tasks/controllerTickLoop.h
+++ b/main/tasks/controllerTickLoop.h
@@ -4,7 +4,7 @@
  * Created Date: 2024-03-11 13:27:21
  * Author: Guoyi
  * -----
- * Last Modified: 2024-04-28 14:58:33
+ * Last Modified: 2024-04-29 23:01:23
  * Modified By: Guoyi
  * -----
  * Copyright (c) 2024 Guoyi Inc.
@@ -22,6 +22,8 @@
 
 #include "mpu6050/motionData.h"
 
+#define CONTROLLER_TICK_DURATION 5
+
 void controllerTickLoop(void *argument)
 {
     vTaskDelay(5000 / portTICK_PERIOD_MS); // 等待5000秒之后，开始初始化mpu6050，并进行校准
@@ -29,15 +31,15 @@ void controllerTickLoop(void *argument)
     vTaskDelay(200 / portTICK_PERIOD_MS);
     while (1)
     {
-        vTaskDelay(10 / portTICK_PERIOD_MS);
+        vTaskDelay(CONTROLLER_TICK_DURATION / portTICK_PERIOD_MS);
         switch (flightState)
         {
-        case 0:
-            stopAllMotors();
+        case 0: // 电机停车，其他流程正常执行
+            controllerTick(CONTROLLER_TICK_DURATION, false);
             break;
 
-        case 1:
-            controllerTick(10);
+        case 1: // 系统所有逻辑正常运行
+            controllerTick(CONTROLLER_TICK_DURATION, true);
             break;
 
         default: