diff --git a/User/component/QuaternionEKF.c b/User/component/QuaternionEKF.c
new file mode 100644
index 0000000..eb962e2
--- /dev/null
+++ b/User/component/QuaternionEKF.c
@@ -0,0 +1,527 @@
+/*
+  四元数扩展卡尔曼滤波器（QEKF）
+  基于陀螺仪零偏估计和卡方检验的姿态更新算法
+
+  一阶低通滤波器传递函数:
+      1
+   ———————
+   as + 1
+*/
+
+#include "QuaternionEKF.h"
+
+#include <string.h>
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* 全局QEKF实例 */
+QEKF_INS_t QEKF_INS;
+
+/* 状态转移矩阵F初始值 */
+static const float QEKF_F_Init[36] = {
+    1, 0, 0, 0, 0, 0,
+    0, 1, 0, 0, 0, 0,
+    0, 0, 1, 0, 0, 0,
+    0, 0, 0, 1, 0, 0,
+    0, 0, 0, 0, 1, 0,
+    0, 0, 0, 0, 0, 1
+};
+
+/* 协方差矩阵P初始值 */
+static float QEKF_P_Init[36] = {
+    1, 0.1, 0.1, 0.1, 0.1, 0.1,
+    0.1, 1, 0.1, 0.1, 0.1, 0.1,
+    0.1, 0.1, 1, 0.1, 0.1, 0.1,
+    0.1, 0.1, 0.1, 1, 0.1, 0.1,
+    0.1, 0.1, 0.1, 0.1, 1, 0.1,
+    0.1, 0.1, 0.1, 0.1, 0.1, 1
+};
+
+/* 用于调试的矩阵副本 */
+static float QEKF_K[18];
+static float QEKF_H[18];
+
+/* 私有函数声明 */
+static float QEKF_InvSqrt(float x);
+static void QEKF_Observe(KF_t *kf);
+static void QEKF_F_Linearization_P_Fading(KF_t *kf);
+static void QEKF_SetH(KF_t *kf);
+static void QEKF_xhatUpdate(KF_t *kf);
+
+/**
+ * @brief 四元数EKF初始化
+ *
+ * @param process_noise1 四元数过程噪声 (推荐值: 10)
+ * @param process_noise2 陀螺仪零偏过程噪声 (推荐值: 0.001)
+ * @param measure_noise 加速度计量测噪声 (推荐值: 1000000)
+ * @param lambda 渐消因子 (推荐值: 0.9996)
+ * @param lpf 低通滤波系数 (推荐值: 0)
+ */
+void QEKF_Init(float process_noise1, float process_noise2, float measure_noise,
+               float lambda, float lpf) {
+  QEKF_INS.initialized = 1;
+  QEKF_INS.Q1 = process_noise1;
+  QEKF_INS.Q2 = process_noise2;
+  QEKF_INS.R = measure_noise;
+  QEKF_INS.chi_square_threshold = 1e-8f;
+  QEKF_INS.converge_flag = 0;
+  QEKF_INS.error_count = 0;
+  QEKF_INS.update_count = 0;
+
+  if (lambda > 1) {
+    lambda = 1;
+  }
+  QEKF_INS.lambda = lambda;
+  QEKF_INS.acc_lpf_coef = lpf;
+
+  /* 初始化卡尔曼滤波器（状态维度6，控制维度0，量测维度3） */
+  KF_Init(&QEKF_INS.qekf, 6, 0, 3);
+  KF_MatInit(&QEKF_INS.chi_square, 1, 1, QEKF_INS.chi_square_data);
+
+  /* 四元数初始化为单位四元数 */
+  QEKF_INS.qekf.xhat_data[0] = 1;
+  QEKF_INS.qekf.xhat_data[1] = 0;
+  QEKF_INS.qekf.xhat_data[2] = 0;
+  QEKF_INS.qekf.xhat_data[3] = 0;
+
+  /* 自定义函数初始化，用于扩展或增加KF的基础功能 */
+  QEKF_INS.qekf.User_Func0_f = QEKF_Observe;
+  QEKF_INS.qekf.User_Func1_f = QEKF_F_Linearization_P_Fading;
+  QEKF_INS.qekf.User_Func2_f = QEKF_SetH;
+  QEKF_INS.qekf.User_Func3_f = QEKF_xhatUpdate;
+
+  /* 设定标志位，用自定义函数替换KF标准步骤中的计算增益和后验估计 */
+  QEKF_INS.qekf.skip_eq3 = TRUE;
+  QEKF_INS.qekf.skip_eq4 = TRUE;
+
+  memcpy(QEKF_INS.qekf.F_data, QEKF_F_Init, sizeof(QEKF_F_Init));
+  memcpy(QEKF_INS.qekf.P_data, QEKF_P_Init, sizeof(QEKF_P_Init));
+}
+
+/**
+ * @brief 四元数EKF更新
+ *
+ * @param gx 陀螺仪X轴角速度 (rad/s)
+ * @param gy 陀螺仪Y轴角速度 (rad/s)
+ * @param gz 陀螺仪Z轴角速度 (rad/s)
+ * @param ax 加速度计X轴加速度 (m/s²)
+ * @param ay 加速度计Y轴加速度 (m/s²)
+ * @param az 加速度计Z轴加速度 (m/s²)
+ * @param dt 更新周期 (s)
+ */
+void QEKF_Update(float gx, float gy, float gz, float ax, float ay, float az,
+                 float dt) {
+  /* 0.5*(Ohm-Ohm^bias)*deltaT，用于更新状态转移F矩阵 */
+  float halfgxdt, halfgydt, halfgzdt;
+  float accel_inv_norm;
+
+  if (!QEKF_INS.initialized) {
+    QEKF_Init(10, 0.001f, 1000000 * 10, 0.9996f * 0 + 1, 0);
+  }
+
+  /*   F矩阵，带*号的位置需要设置
+   0      1*     2*     3*     4     5
+   6*     7      8*     9*    10    11
+  12*    13*    14     15*    16    17
+  18*    19*    20*    21     22    23
+  24     25     26     27     28    29
+  30     31     32     33     34    35
+  */
+  QEKF_INS.dt = dt;
+
+  /* 去除零偏后的陀螺仪数据 */
+  QEKF_INS.gyro[0] = gx - QEKF_INS.gyro_bias[0];
+  QEKF_INS.gyro[1] = gy - QEKF_INS.gyro_bias[1];
+  QEKF_INS.gyro[2] = gz - QEKF_INS.gyro_bias[2];
+
+  /* 设置F矩阵 */
+  halfgxdt = 0.5f * QEKF_INS.gyro[0] * dt;
+  halfgydt = 0.5f * QEKF_INS.gyro[1] * dt;
+  halfgzdt = 0.5f * QEKF_INS.gyro[2] * dt;
+
+  /* 设定状态转移矩阵F的左上角4x4子矩阵，即0.5*(Ohm-Ohm^bias)*deltaT
+     右下角有一个2x2单位阵已经初始化好了
+     注意在predict步F的右上角是4x2的零矩阵，因此每次predict都用单位阵覆盖前一轮线性化后的矩阵 */
+  memcpy(QEKF_INS.qekf.F_data, QEKF_F_Init, sizeof(QEKF_F_Init));
+
+  QEKF_INS.qekf.F_data[1] = -halfgxdt;
+  QEKF_INS.qekf.F_data[2] = -halfgydt;
+  QEKF_INS.qekf.F_data[3] = -halfgzdt;
+
+  QEKF_INS.qekf.F_data[6] = halfgxdt;
+  QEKF_INS.qekf.F_data[8] = halfgzdt;
+  QEKF_INS.qekf.F_data[9] = -halfgydt;
+
+  QEKF_INS.qekf.F_data[12] = halfgydt;
+  QEKF_INS.qekf.F_data[13] = -halfgzdt;
+  QEKF_INS.qekf.F_data[15] = halfgxdt;
+
+  QEKF_INS.qekf.F_data[18] = halfgzdt;
+  QEKF_INS.qekf.F_data[19] = halfgydt;
+  QEKF_INS.qekf.F_data[20] = -halfgxdt;
+
+  /* 加速度低通滤波，平滑数据，降低撞击和异常的影响 */
+  if (QEKF_INS.update_count == 0) {
+    /* 第一次进入，初始化低通滤波 */
+    QEKF_INS.accel[0] = ax;
+    QEKF_INS.accel[1] = ay;
+    QEKF_INS.accel[2] = az;
+  }
+  QEKF_INS.accel[0] = QEKF_INS.accel[0] * QEKF_INS.acc_lpf_coef /
+                          (QEKF_INS.dt + QEKF_INS.acc_lpf_coef) +
+                      ax * QEKF_INS.dt / (QEKF_INS.dt + QEKF_INS.acc_lpf_coef);
+  QEKF_INS.accel[1] = QEKF_INS.accel[1] * QEKF_INS.acc_lpf_coef /
+                          (QEKF_INS.dt + QEKF_INS.acc_lpf_coef) +
+                      ay * QEKF_INS.dt / (QEKF_INS.dt + QEKF_INS.acc_lpf_coef);
+  QEKF_INS.accel[2] = QEKF_INS.accel[2] * QEKF_INS.acc_lpf_coef /
+                          (QEKF_INS.dt + QEKF_INS.acc_lpf_coef) +
+                      az * QEKF_INS.dt / (QEKF_INS.dt + QEKF_INS.acc_lpf_coef);
+
+  /* 设置量测向量z，单位化重力加速度向量 */
+  accel_inv_norm =
+      QEKF_InvSqrt(QEKF_INS.accel[0] * QEKF_INS.accel[0] +
+                   QEKF_INS.accel[1] * QEKF_INS.accel[1] +
+                   QEKF_INS.accel[2] * QEKF_INS.accel[2]);
+  for (uint8_t i = 0; i < 3; i++) {
+    QEKF_INS.qekf.measured_vector[i] = QEKF_INS.accel[i] * accel_inv_norm;
+  }
+
+  /* 获取机体状态 */
+  QEKF_INS.gyro_norm =
+      1.0f / QEKF_InvSqrt(QEKF_INS.gyro[0] * QEKF_INS.gyro[0] +
+                          QEKF_INS.gyro[1] * QEKF_INS.gyro[1] +
+                          QEKF_INS.gyro[2] * QEKF_INS.gyro[2]);
+  QEKF_INS.accl_norm = 1.0f / accel_inv_norm;
+
+  /* 如果角速度小于阈值且加速度处于设定范围内，认为运动稳定，加速度可以用于修正角速度 */
+  if (QEKF_INS.gyro_norm < 0.3f && QEKF_INS.accl_norm > 9.8f - 0.5f &&
+      QEKF_INS.accl_norm < 9.8f + 0.5f) {
+    QEKF_INS.stable_flag = 1;
+  } else {
+    QEKF_INS.stable_flag = 0;
+  }
+
+  /* 设置过程噪声Q和量测噪声R矩阵 */
+  QEKF_INS.qekf.Q_data[0] = QEKF_INS.Q1 * QEKF_INS.dt;
+  QEKF_INS.qekf.Q_data[7] = QEKF_INS.Q1 * QEKF_INS.dt;
+  QEKF_INS.qekf.Q_data[14] = QEKF_INS.Q1 * QEKF_INS.dt;
+  QEKF_INS.qekf.Q_data[21] = QEKF_INS.Q1 * QEKF_INS.dt;
+  QEKF_INS.qekf.Q_data[28] = QEKF_INS.Q2 * QEKF_INS.dt;
+  QEKF_INS.qekf.Q_data[35] = QEKF_INS.Q2 * QEKF_INS.dt;
+  QEKF_INS.qekf.R_data[0] = QEKF_INS.R;
+  QEKF_INS.qekf.R_data[4] = QEKF_INS.R;
+  QEKF_INS.qekf.R_data[8] = QEKF_INS.R;
+
+  /* 调用卡尔曼滤波更新，注意几个User_Funcx_f的调用 */
+  KF_Update(&QEKF_INS.qekf);
+
+  /* 获取融合后的数据，包括四元数和xy零偏值 */
+  QEKF_INS.q[0] = QEKF_INS.qekf.filtered_value[0];
+  QEKF_INS.q[1] = QEKF_INS.qekf.filtered_value[1];
+  QEKF_INS.q[2] = QEKF_INS.qekf.filtered_value[2];
+  QEKF_INS.q[3] = QEKF_INS.qekf.filtered_value[3];
+  QEKF_INS.gyro_bias[0] = QEKF_INS.qekf.filtered_value[4];
+  QEKF_INS.gyro_bias[1] = QEKF_INS.qekf.filtered_value[5];
+  QEKF_INS.gyro_bias[2] = 0; /* 大部分时候z轴通天，无法观测yaw的漂移 */
+
+  /* 利用四元数反解欧拉角 */
+  QEKF_INS.yaw =
+      atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[3] +
+                     QEKF_INS.q[1] * QEKF_INS.q[2]),
+             2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] +
+                     QEKF_INS.q[1] * QEKF_INS.q[1]) - 1.0f) * 57.295779513f;
+  QEKF_INS.pitch =
+      atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[1] +
+                     QEKF_INS.q[2] * QEKF_INS.q[3]),
+             2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] +
+                     QEKF_INS.q[3] * QEKF_INS.q[3]) - 1.0f) * 57.295779513f;
+  QEKF_INS.roll =
+      asinf(-2.0f * (QEKF_INS.q[1] * QEKF_INS.q[3] -
+                     QEKF_INS.q[0] * QEKF_INS.q[2])) * 57.295779513f;
+
+  /* 获取yaw总角度，处理超过360度的情况（如小陀螺） */
+  if (QEKF_INS.yaw - QEKF_INS.yaw_angle_last > 180.0f) {
+    QEKF_INS.yaw_round_count--;
+  } else if (QEKF_INS.yaw - QEKF_INS.yaw_angle_last < -180.0f) {
+    QEKF_INS.yaw_round_count++;
+  }
+  QEKF_INS.yaw_total_angle = 360.0f * QEKF_INS.yaw_round_count + QEKF_INS.yaw;
+  QEKF_INS.yaw_angle_last = QEKF_INS.yaw;
+  QEKF_INS.update_count++;
+}
+
+/**
+ * @brief 更新线性化后的状态转移矩阵F右上角的4x2分块矩阵，用于协方差矩阵P的更新
+ *        并对零偏的方差进行限制，防止过度收敛并限幅防止发散
+ *
+ * @param kf 卡尔曼滤波器结构体指针
+ */
+static void QEKF_F_Linearization_P_Fading(KF_t *kf) {
+  float q0, q1, q2, q3;
+  float q_inv_norm;
+
+  q0 = kf->xhatminus_data[0];
+  q1 = kf->xhatminus_data[1];
+  q2 = kf->xhatminus_data[2];
+  q3 = kf->xhatminus_data[3];
+
+  /* 四元数归一化 */
+  q_inv_norm = QEKF_InvSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+  for (uint8_t i = 0; i < 4; i++) {
+    kf->xhatminus_data[i] *= q_inv_norm;
+  }
+
+  /*  F矩阵，带*号的位置需要设置
+   0     1     2     3     4*     5*
+   6     7     8     9    10*    11*
+  12    13    14    15    16*    17*
+  18    19    20    21    22*    23*
+  24    25    26    27    28     29
+  30    31    32    33    34     35
+  */
+  /* 设置F矩阵右上角4x2分块 */
+  kf->F_data[4] = q1 * QEKF_INS.dt / 2;
+  kf->F_data[5] = q2 * QEKF_INS.dt / 2;
+
+  kf->F_data[10] = -q0 * QEKF_INS.dt / 2;
+  kf->F_data[11] = q3 * QEKF_INS.dt / 2;
+
+  kf->F_data[16] = -q3 * QEKF_INS.dt / 2;
+  kf->F_data[17] = -q0 * QEKF_INS.dt / 2;
+
+  kf->F_data[22] = q2 * QEKF_INS.dt / 2;
+  kf->F_data[23] = -q1 * QEKF_INS.dt / 2;
+
+  /* 渐消滤波，防止零偏参数过度收敛 */
+  kf->P_data[28] /= QEKF_INS.lambda;
+  kf->P_data[35] /= QEKF_INS.lambda;
+
+  /* 限幅，防止发散 */
+  if (kf->P_data[28] > 10000) {
+    kf->P_data[28] = 10000;
+  }
+  if (kf->P_data[35] > 10000) {
+    kf->P_data[35] = 10000;
+  }
+}
+
+/**
+ * @brief 在工作点处计算观测函数h(x)的Jacobi矩阵H
+ *
+ * @param kf 卡尔曼滤波器结构体指针
+ */
+static void QEKF_SetH(KF_t *kf) {
+  float doubleq0, doubleq1, doubleq2, doubleq3;
+
+  /* H矩阵布局
+   0     1     2     3     4     5
+   6     7     8     9    10    11
+  12    13    14    15    16    17
+  最后两列是零
+  */
+  doubleq0 = 2 * kf->xhatminus_data[0];
+  doubleq1 = 2 * kf->xhatminus_data[1];
+  doubleq2 = 2 * kf->xhatminus_data[2];
+  doubleq3 = 2 * kf->xhatminus_data[3];
+
+  memset(kf->H_data, 0, sizeof(float) * kf->z_size * kf->xhat_size);
+
+  kf->H_data[0] = -doubleq2;
+  kf->H_data[1] = doubleq3;
+  kf->H_data[2] = -doubleq0;
+  kf->H_data[3] = doubleq1;
+
+  kf->H_data[6] = doubleq1;
+  kf->H_data[7] = doubleq0;
+  kf->H_data[8] = doubleq3;
+  kf->H_data[9] = doubleq2;
+
+  kf->H_data[12] = doubleq0;
+  kf->H_data[13] = -doubleq1;
+  kf->H_data[14] = -doubleq2;
+  kf->H_data[15] = doubleq3;
+}
+
+/**
+ * @brief 利用观测值和先验估计得到最优的后验估计
+ *        加入了卡方检验以判断融合加速度的条件是否满足
+ *        同时引入发散保护保证恶劣工况下的必要量测更新
+ *
+ * @param kf 卡尔曼滤波器结构体指针
+ */
+static void QEKF_xhatUpdate(KF_t *kf) {
+  float q0, q1, q2, q3;
+
+  kf->mat_status = KF_MatTrans(&kf->H, &kf->HT);
+  kf->temp_matrix.numRows = kf->H.numRows;
+  kf->temp_matrix.numCols = kf->Pminus.numCols;
+  /* temp_matrix = H·P'(k) */
+  kf->mat_status = KF_MatMult(&kf->H, &kf->Pminus, &kf->temp_matrix);
+  kf->temp_matrix1.numRows = kf->temp_matrix.numRows;
+  kf->temp_matrix1.numCols = kf->HT.numCols;
+  /* temp_matrix1 = H·P'(k)·HT */
+  kf->mat_status = KF_MatMult(&kf->temp_matrix, &kf->HT, &kf->temp_matrix1);
+  kf->S.numRows = kf->R.numRows;
+  kf->S.numCols = kf->R.numCols;
+  /* S = H·P'(k)·HT + R */
+  kf->mat_status = KF_MatAdd(&kf->temp_matrix1, &kf->R, &kf->S);
+  /* temp_matrix1 = inv(H·P'(k)·HT + R) */
+  kf->mat_status = KF_MatInv(&kf->S, &kf->temp_matrix1);
+
+  q0 = kf->xhatminus_data[0];
+  q1 = kf->xhatminus_data[1];
+  q2 = kf->xhatminus_data[2];
+  q3 = kf->xhatminus_data[3];
+
+  kf->temp_vector.numRows = kf->H.numRows;
+  kf->temp_vector.numCols = 1;
+  /* 计算预测得到的重力加速度方向（通过姿态获取） */
+  kf->temp_vector_data[0] = 2 * (q1 * q3 - q0 * q2);
+  kf->temp_vector_data[1] = 2 * (q0 * q1 + q2 * q3);
+  /* temp_vector = h(xhat'(k)) */
+  kf->temp_vector_data[2] = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;
+
+  /* 计算预测值和各个轴的方向余弦 */
+  for (uint8_t i = 0; i < 3; i++) {
+    QEKF_INS.orientation_cosine[i] = acosf(fabsf(kf->temp_vector_data[i]));
+  }
+
+  /* 利用加速度计数据修正 */
+  kf->temp_vector1.numRows = kf->z.numRows;
+  kf->temp_vector1.numCols = 1;
+  /* temp_vector1 = z(k) - h(xhat'(k)) */
+  kf->mat_status = KF_MatSub(&kf->z, &kf->temp_vector, &kf->temp_vector1);
+
+  /* 卡方检验 */
+  kf->temp_matrix.numRows = kf->temp_vector1.numRows;
+  kf->temp_matrix.numCols = 1;
+  /* temp_matrix = inv(H·P'(k)·HT + R)·(z(k) - h(xhat'(k))) */
+  kf->mat_status =
+      KF_MatMult(&kf->temp_matrix1, &kf->temp_vector1, &kf->temp_matrix);
+  kf->temp_vector.numRows = 1;
+  kf->temp_vector.numCols = kf->temp_vector1.numRows;
+  /* temp_vector = (z(k) - h(xhat'(k)))' */
+  kf->mat_status = KF_MatTrans(&kf->temp_vector1, &kf->temp_vector);
+  kf->mat_status =
+      KF_MatMult(&kf->temp_vector, &kf->temp_matrix, &QEKF_INS.chi_square);
+
+  /* rk较小，滤波器已收敛/正在收敛 */
+  if (QEKF_INS.chi_square_data[0] < 0.5f * QEKF_INS.chi_square_threshold) {
+    QEKF_INS.converge_flag = 1;
+  }
+
+  /* rk大于阈值但曾经收敛过 */
+  if (QEKF_INS.chi_square_data[0] > QEKF_INS.chi_square_threshold &&
+      QEKF_INS.converge_flag) {
+    if (QEKF_INS.stable_flag) {
+      QEKF_INS.error_count++; /* 载体静止时仍无法通过卡方检验 */
+    } else {
+      QEKF_INS.error_count = 0;
+    }
+
+    if (QEKF_INS.error_count > 50) {
+      /* 滤波器发散 */
+      QEKF_INS.converge_flag = 0;
+      kf->skip_eq5 = FALSE; /* 步骤5是协方差矩阵P更新 */
+    } else {
+      /* 残差未通过卡方检验，仅预测 */
+      /* xhat(k) = xhat'(k) */
+      /* P(k) = P'(k) */
+      memcpy(kf->xhat_data, kf->xhatminus_data,
+             sizeof(float) * kf->xhat_size);
+      memcpy(kf->P_data, kf->Pminus_data,
+             sizeof(float) * kf->xhat_size * kf->xhat_size);
+      kf->skip_eq5 = TRUE; /* 跳过P更新 */
+      return;
+    }
+  } else {
+    /* 发散或rk不大/可接受，使用自适应增益 */
+    /* 自适应缩放，rk越小则增益越大，否则更相信预测值 */
+    if (QEKF_INS.chi_square_data[0] > 0.1f * QEKF_INS.chi_square_threshold &&
+        QEKF_INS.converge_flag) {
+      QEKF_INS.adaptive_gain_scale =
+          (QEKF_INS.chi_square_threshold - QEKF_INS.chi_square_data[0]) /
+          (0.9f * QEKF_INS.chi_square_threshold);
+    } else {
+      QEKF_INS.adaptive_gain_scale = 1;
+    }
+    QEKF_INS.error_count = 0;
+    kf->skip_eq5 = FALSE;
+  }
+
+  /* 计算卡尔曼增益K */
+  kf->temp_matrix.numRows = kf->Pminus.numRows;
+  kf->temp_matrix.numCols = kf->HT.numCols;
+  /* temp_matrix = P'(k)·HT */
+  kf->mat_status = KF_MatMult(&kf->Pminus, &kf->HT, &kf->temp_matrix);
+  kf->mat_status = KF_MatMult(&kf->temp_matrix, &kf->temp_matrix1, &kf->K);
+
+  /* 应用自适应增益 */
+  for (uint8_t i = 0; i < kf->K.numRows * kf->K.numCols; i++) {
+    kf->K_data[i] *= QEKF_INS.adaptive_gain_scale;
+  }
+  for (uint8_t i = 4; i < 6; i++) {
+    for (uint8_t j = 0; j < 3; j++) {
+      kf->K_data[i * 3 + j] *=
+          QEKF_INS.orientation_cosine[i - 4] / 1.5707963f; /* 1 rad */
+    }
+  }
+
+  kf->temp_vector.numRows = kf->K.numRows;
+  kf->temp_vector.numCols = 1;
+  /* temp_vector = K(k)·(z(k) - H·xhat'(k)) */
+  kf->mat_status = KF_MatMult(&kf->K, &kf->temp_vector1, &kf->temp_vector);
+
+  /* 零偏修正限幅，一般不会有过大的漂移 */
+  if (QEKF_INS.converge_flag) {
+    for (uint8_t i = 4; i < 6; i++) {
+      if (kf->temp_vector.pData[i] > 1e-2f * QEKF_INS.dt) {
+        kf->temp_vector.pData[i] = 1e-2f * QEKF_INS.dt;
+      }
+      if (kf->temp_vector.pData[i] < -1e-2f * QEKF_INS.dt) {
+        kf->temp_vector.pData[i] = -1e-2f * QEKF_INS.dt;
+      }
+    }
+  }
+
+  /* 不修正yaw轴数据 */
+  kf->temp_vector.pData[3] = 0;
+  kf->mat_status = KF_MatAdd(&kf->xhatminus, &kf->temp_vector, &kf->xhat);
+}
+
+/**
+ * @brief EKF观测环节，复制数据用于调试
+ *
+ * @param kf 卡尔曼滤波器结构体指针
+ */
+static void QEKF_Observe(KF_t *kf) {
+  memcpy(QEKF_P_Init, kf->P_data, sizeof(QEKF_P_Init));
+  memcpy(QEKF_K, kf->K_data, sizeof(QEKF_K));
+  memcpy(QEKF_H, kf->H_data, sizeof(QEKF_H));
+}
+
+/**
+ * @brief 快速计算1/sqrt(x)
+ *
+ * @param x 输入值
+ * @return float 1/sqrt(x)的近似值
+ */
+static float QEKF_InvSqrt(float x) {
+  float halfx = 0.5f * x;
+  float y = x;
+  long i = *(long *)&y;
+  i = 0x5f375a86 - (i >> 1);
+  y = *(float *)&i;
+  y = y * (1.5f - (halfx * y * y));
+  return y;
+}
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
diff --git a/User/component/QuaternionEKF.h b/User/component/QuaternionEKF.h
new file mode 100644
index 0000000..0fdabbb
--- /dev/null
+++ b/User/component/QuaternionEKF.h
@@ -0,0 +1,111 @@
+/*
+  四元数扩展卡尔曼滤波器（QEKF）
+  基于陀螺仪零偏估计和卡方检验的姿态更新算法
+*/
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "kalman_filter.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* 布尔类型定义 */
+#ifndef TRUE
+#define TRUE 1
+#endif
+
+#ifndef FALSE
+#define FALSE 0
+#endif
+
+/* 四元数EKF惯性导航结构体 */
+typedef struct {
+  uint8_t initialized;      /* 初始化标志 */
+  KF_t qekf;                /* 卡尔曼滤波器实例 */
+  uint8_t converge_flag;    /* 收敛标志 */
+  uint8_t stable_flag;      /* 稳定标志 */
+  uint64_t error_count;     /* 错误计数 */
+  uint64_t update_count;    /* 更新计数 */
+
+  float q[4];               /* 四元数估计值 */
+  float gyro_bias[3];       /* 陀螺仪零偏估计值 */
+
+  float gyro[3];            /* 陀螺仪数据 */
+  float accel[3];           /* 加速度计数据 */
+
+  float orientation_cosine[3]; /* 方向余弦 */
+
+  float acc_lpf_coef;       /* 加速度低通滤波系数 */
+  float gyro_norm;          /* 陀螺仪模值 */
+  float accl_norm;          /* 加速度计模值 */
+  float adaptive_gain_scale; /* 自适应增益缩放 */
+
+  float roll;               /* 横滚角 */
+  float pitch;              /* 俯仰角 */
+  float yaw;                /* 偏航角 */
+
+  float yaw_total_angle;    /* 偏航总角度 */
+
+  float Q1;                 /* 四元数更新过程噪声 */
+  float Q2;                 /* 陀螺仪零偏过程噪声 */
+  float R;                  /* 加速度计量测噪声 */
+
+  float dt;                 /* 姿态更新周期 */
+  KF_Mat chi_square;        /* 卡方检验矩阵 */
+  float chi_square_data[1]; /* 卡方检验数据 */
+  float chi_square_threshold; /* 卡方检验阈值 */
+  float lambda;             /* 渐消因子 */
+
+  int16_t yaw_round_count;  /* 偏航圈数计数 */
+  float yaw_angle_last;     /* 上次偏航角 */
+} QEKF_INS_t;
+
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+extern QEKF_INS_t QEKF_INS;
+
+/**
+ * @brief 四元数EKF初始化
+ *
+ * @param process_noise1 四元数过程噪声 (推荐值: 10)
+ * @param process_noise2 陀螺仪零偏过程噪声 (推荐值: 0.001)
+ * @param measure_noise 加速度计量测噪声 (推荐值: 1000000)
+ * @param lambda 渐消因子 (推荐值: 0.9996)
+ * @param lpf 低通滤波系数 (推荐值: 0)
+ */
+void QEKF_Init(float process_noise1, float process_noise2, float measure_noise,
+               float lambda, float lpf);
+
+/**
+ * @brief 四元数EKF更新
+ *
+ * @param gx 陀螺仪X轴角速度 (rad/s)
+ * @param gy 陀螺仪Y轴角速度 (rad/s)
+ * @param gz 陀螺仪Z轴角速度 (rad/s)
+ * @param ax 加速度计X轴加速度 (m/s²)
+ * @param ay 加速度计Y轴加速度 (m/s²)
+ * @param az 加速度计Z轴加速度 (m/s²)
+ * @param dt 更新周期 (s)
+ */
+void QEKF_Update(float gx, float gy, float gz, float ax, float ay, float az,
+                 float dt);
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/User/component/kalman_filter.c b/User/component/kalman_filter.c
index 9ff4994..e5463a4 100644
--- a/User/component/kalman_filter.c
+++ b/User/component/kalman_filter.c
@@ -1,46 +1,46 @@
 /*
-  �������˲��� modified from wang hongxi
-  ֧�ֶ�̬���������ʹ��ARM CMSIS DSP�Ż���������
+  卡尔曼滤波器 modified from wang hongxi
+  支持动态量测调整，使用ARM CMSIS DSP优化矩阵运算
 
-  ��Ҫ���ԣ�
-  - ����������Ч�Ե� H��R��K ����̬����
-  - ֧�ֲ�ͬ����������Ƶ��
-  - ���� P ��������������
-  - ARM CMSIS DSP �Ż��ľ�������
-  - ����չ�ܹ���֧���û��Զ��庯����EKF/UKF/ESKF��
+  主要特性：
+  - 基于量测有效性的 H、R、K 矩阵动态调整
+  - 支持不同传感器采样频率
+  - 矩阵 P 防过度收敛机制
+  - ARM CMSIS DSP 优化的矩阵运算
+  - 可扩展架构，支持用户自定义函数（EKF/UKF/ESKF）
 
-  ʹ��˵����
-  1. �����ʼ����P��F��Q ʹ�ñ�׼��ʼ����ʽ
-     H��R ��ʹ���Զ�����ʱ��Ҫ��������ӳ��
+  使用说明：
+  1. 矩阵初始化：P、F、Q 使用标准初始化方式
+     H、R 在使用自动调整时需要配置量测映射
 
-  2. �Զ�����ģʽ (use_auto_adjustment = 1)��
-     - �ṩ measurement_map��ÿ�������Ӧ��״̬����
-     - �ṩ measurement_degree��H ����Ԫ��ֵ
-     - �ṩ mat_r_diagonal_elements��������������
+  2. 自动调整模式 (use_auto_adjustment = 1)：
+     - 提供 measurement_map：每个量测对应的状态索引
+     - 提供 measurement_degree：H 矩阵元素值
+     - 提供 mat_r_diagonal_elements：量测噪声方差
 
-  3. �̶�ģʽ (use_auto_adjustment = 0)��
-     - ���ʼ�� P ��������������ʼ�� z��H��R
+  3. 固定模式 (use_auto_adjustment = 0)：
+     - 像初始化 P 矩阵那样正常初始化 z、H、R
 
-  4. ������£�
-     - �ڴ������ص������и��� measured_vector
-     - ֵΪ 0 ��ʾ������Ч
-     - ������ÿ�� KF ���º�ᱻ����Ϊ 0
+  4. 量测更新：
+     - 在传感器回调函数中更新 measured_vector
+     - 值为 0 表示量测无效
+     - 向量在每次 KF 更新后会被重置为 0
 
-  5. ������������
-     - ���� state_min_variance ��ֹ P �����������
-     - �����˲�����Ӧ�����仯��״̬
+  5. 防过度收敛：
+     - 设置 state_min_variance 防止 P 矩阵过度收敛
+     - 帮助滤波器适应缓慢变化的状态
 
-  ʹ��ʾ�����߶ȹ���
-  ״̬���� x =
-    |   �߶�      |
-    |   �ٶ�      |
-    |   ���ٶ�    |
+  使用示例：高度估计
+  状态向量 x =
+    |   高度      |
+    |   速度      |
+    |   加速度    |
 
   KF_t Height_KF;
 
   void INS_Task_Init(void)
   {
-      // ��ʼЭ������� P 
+      // 初始协方差矩阵 P 
       static float P_Init[9] =
       {
           10, 0, 0,
@@ -48,7 +48,7 @@
           0, 0, 10,
       };
 
-      // ״̬ת�ƾ��� F�������˶�ѧģ�ͣ�
+      // 状态转移矩阵 F（根据运动学模型）
       static float F_Init[9] =
       {
           1, dt, 0.5*dt*dt,
@@ -56,7 +56,7 @@
           0, 0, 1,
       };
 
-      // ��������������� Q
+      // 过程噪声协方差矩阵 Q
       static float Q_Init[9] =
       {
           0.25*dt*dt*dt*dt, 0.5*dt*dt*dt, 0.5*dt*dt,
@@ -64,35 +64,35 @@
           0.5*dt*dt,              dt,         1,
       };
 
-      // ����״̬��С�����ֹ����������
+      // 设置状态最小方差（防止过度收敛）
       static float state_min_variance[3] = {0.03, 0.005, 0.1};
 
-      // �����Զ�����
+      // 开启自动调整
       Height_KF.use_auto_adjustment = 1;
 
-      // ����ӳ�䣺[��ѹ�߶ȶ�Ӧ״̬1, GPS�߶ȶ�Ӧ״̬1, ���ٶȼƶ�Ӧ״̬3]
+      // 量测映射：[气压高度对应状态1, GPS高度对应状态1, 加速度计对应状态3]
       static uint8_t measurement_reference[3] = {1, 1, 3};
 
-      // ����ϵ����H����Ԫ��ֵ��
+      // 量测系数（H矩阵元素值）
       static float measurement_degree[3] = {1, 1, 1};
-      // ���� measurement_reference �� measurement_degree ���� H ��������
-      // ���ڵ�ǰ����ȫ������������Ч������£�
+      // 根据 measurement_reference 与 measurement_degree 生成 H 矩阵如下
+      // （在当前周期全部量测数据有效的情况下）
       //   |1   0   0|
       //   |1   0   0|
       //   |0   0   1|
 
-      // �����������R����Խ�Ԫ�أ�
+      // 量测噪声方差（R矩阵对角元素）
       static float mat_r_diagonal_elements[3] = {30, 25, 35};
-      // ���� mat_r_diagonal_elements ���� R ��������
-      // ���ڵ�ǰ����ȫ������������Ч������£�
+      // 根据 mat_r_diagonal_elements 生成 R 矩阵如下
+      // （在当前周期全部量测数据有效的情况下）
       //   |30   0   0|
       //   | 0  25   0|
       //   | 0   0  35|
 
-      // ��ʼ���������˲�����״̬ά��3������ά��0������ά��3��
+      // 初始化卡尔曼滤波器（状态维数3，控制维数0，量测维数3）
       KF_Init(&Height_KF, 3, 0, 3);
 
-      // ���þ����ֵ
+      // 设置矩阵初值
       memcpy(Height_KF.P_data, P_Init, sizeof(P_Init));
       memcpy(Height_KF.F_data, F_Init, sizeof(F_Init));
       memcpy(Height_KF.Q_data, Q_Init, sizeof(Q_Init));
@@ -108,28 +108,28 @@
 
   void INS_Task(void const *pvParameters)
   {
-      // ѭ�����¿������˲���
+      // 循环更新卡尔曼滤波器
       KF_Update(&Height_KF);
       vTaskDelay(ts);
   }
 
-  // �������ص�����ʾ���������ݾ���ʱ���� measured_vector
+  // 传感器回调函数示例：在数据就绪时更新 measured_vector
   void Barometer_Read_Over(void)
   {
       ......
-      INS_KF.measured_vector[0] = baro_height;  // ��ѹ�Ƹ߶�
+      INS_KF.measured_vector[0] = baro_height;  // 气压计高度
   }
 
   void GPS_Read_Over(void)
   {
       ......
-      INS_KF.measured_vector[1] = GPS_height;   // GPS�߶�
+      INS_KF.measured_vector[1] = GPS_height;   // GPS高度
   }
 
   void Acc_Data_Process(void)
   {
       ......
-      INS_KF.measured_vector[2] = acc.z;        // Z����ٶ�
+      INS_KF.measured_vector[2] = acc.z;        // Z轴加速度
   }
 */
 
@@ -143,17 +143,17 @@
 
 /* USER DEFINE END */
 
-/* ˽�к������� */
+/* 私有函数声明 */
 static void KF_AdjustHKR(KF_t *kf);
 
 /**
- * @brief ��ʼ���������˲�������������ڴ�
+ * @brief 初始化卡尔曼滤波器并分配矩阵内存
  *
- * @param kf �������˲����ṹ��ָ��
- * @param xhat_size ״̬����ά��
- * @param u_size ��������ά�ȣ��޿�������ʱ��Ϊ0��
- * @param z_size ����������
- * @return int8_t 0��Ӧû�д���
+ * @param kf 卡尔曼滤波器结构体指针
+ * @param xhat_size 状态向量维度
+ * @param u_size 控制向量维度（无控制输入时设为0）
+ * @param z_size 量测向量维度
+ * @return int8_t 0对应没有错误
  */
 int8_t KF_Init(KF_t *kf, uint8_t xhat_size, uint8_t u_size, uint8_t z_size) {
   if (kf == NULL) return -1;
@@ -164,7 +164,7 @@ int8_t KF_Init(KF_t *kf, uint8_t xhat_size, uint8_t u_size, uint8_t z_size) {
 
   kf->measurement_valid_num = 0;
 
-  /* �����־���� */
+  /* 量测标志分配 */
   kf->measurement_map = (uint8_t *)user_malloc(sizeof(uint8_t) * z_size);
   memset(kf->measurement_map, 0, sizeof(uint8_t) * z_size);
 
@@ -180,7 +180,7 @@ int8_t KF_Init(KF_t *kf, uint8_t xhat_size, uint8_t u_size, uint8_t z_size) {
   kf->temp = (uint8_t *)user_malloc(sizeof(uint8_t) * z_size);
   memset(kf->temp, 0, sizeof(uint8_t) * z_size);
 
-  /* �˲����ݷ��� */
+  /* 滤波数据分配 */
   kf->filtered_value = (float *)user_malloc(sizeof(float) * xhat_size);
   memset(kf->filtered_value, 0, sizeof(float) * xhat_size);
 
@@ -190,39 +190,39 @@ int8_t KF_Init(KF_t *kf, uint8_t xhat_size, uint8_t u_size, uint8_t z_size) {
   kf->control_vector = (float *)user_malloc(sizeof(float) * u_size);
   memset(kf->control_vector, 0, sizeof(float) * u_size);
 
-  /* ״̬���� xhat x(k|k) */
+  /* 状态估计 xhat x(k|k) */
   kf->xhat_data = (float *)user_malloc(sizeof(float) * xhat_size);
   memset(kf->xhat_data, 0, sizeof(float) * xhat_size);
   KF_MatInit(&kf->xhat, kf->xhat_size, 1, kf->xhat_data);
 
-  /* ����״̬���� xhatminus x(k|k-1) */
+  /* 先验状态估计 xhatminus x(k|k-1) */
   kf->xhatminus_data = (float *)user_malloc(sizeof(float) * xhat_size);
   memset(kf->xhatminus_data, 0, sizeof(float) * xhat_size);
   KF_MatInit(&kf->xhatminus, kf->xhat_size, 1, kf->xhatminus_data);
 
-  /* �������� u */
+  /* 控制向量 u */
   if (u_size != 0) {
     kf->u_data = (float *)user_malloc(sizeof(float) * u_size);
     memset(kf->u_data, 0, sizeof(float) * u_size);
     KF_MatInit(&kf->u, kf->u_size, 1, kf->u_data);
   }
 
-  /* �������� z */
+  /* 量测向量 z */
   kf->z_data = (float *)user_malloc(sizeof(float) * z_size);
   memset(kf->z_data, 0, sizeof(float) * z_size);
   KF_MatInit(&kf->z, kf->z_size, 1, kf->z_data);
 
-  /* ������� P(k|k) */
+  /* 协方差矩阵 P(k|k) */
   kf->P_data = (float *)user_malloc(sizeof(float) * xhat_size * xhat_size);
   memset(kf->P_data, 0, sizeof(float) * xhat_size * xhat_size);
   KF_MatInit(&kf->P, kf->xhat_size, kf->xhat_size, kf->P_data);
 
-  /* ����������� P(k|k-1) */
+  /* 先验协方差矩阵 P(k|k-1) */
   kf->Pminus_data = (float *)user_malloc(sizeof(float) * xhat_size * xhat_size);
   memset(kf->Pminus_data, 0, sizeof(float) * xhat_size * xhat_size);
   KF_MatInit(&kf->Pminus, kf->xhat_size, kf->xhat_size, kf->Pminus_data);
 
-  /* ״̬ת�ƾ��� F ����ת�� FT */
+  /* 状态转移矩阵 F 及其转置 FT */
   kf->F_data = (float *)user_malloc(sizeof(float) * xhat_size * xhat_size);
   kf->FT_data = (float *)user_malloc(sizeof(float) * xhat_size * xhat_size);
   memset(kf->F_data, 0, sizeof(float) * xhat_size * xhat_size);
@@ -230,14 +230,14 @@ int8_t KF_Init(KF_t *kf, uint8_t xhat_size, uint8_t u_size, uint8_t z_size) {
   KF_MatInit(&kf->F, kf->xhat_size, kf->xhat_size, kf->F_data);
   KF_MatInit(&kf->FT, kf->xhat_size, kf->xhat_size, kf->FT_data);
 
-  /* ���ƾ��� B */
+  /* 控制矩阵 B */
   if (u_size != 0) {
     kf->B_data = (float *)user_malloc(sizeof(float) * xhat_size * u_size);
     memset(kf->B_data, 0, sizeof(float) * xhat_size * u_size);
     KF_MatInit(&kf->B, kf->xhat_size, kf->u_size, kf->B_data);
   }
 
-  /* ������� H ����ת�� HT */
+  /* 量测矩阵 H 及其转置 HT */
   kf->H_data = (float *)user_malloc(sizeof(float) * z_size * xhat_size);
   kf->HT_data = (float *)user_malloc(sizeof(float) * xhat_size * z_size);
   memset(kf->H_data, 0, sizeof(float) * z_size * xhat_size);
@@ -245,22 +245,22 @@ int8_t KF_Init(KF_t *kf, uint8_t xhat_size, uint8_t u_size, uint8_t z_size) {
   KF_MatInit(&kf->H, kf->z_size, kf->xhat_size, kf->H_data);
   KF_MatInit(&kf->HT, kf->xhat_size, kf->z_size, kf->HT_data);
 
-  /* ��������������� Q */
+  /* 过程噪声协方差矩阵 Q */
   kf->Q_data = (float *)user_malloc(sizeof(float) * xhat_size * xhat_size);
   memset(kf->Q_data, 0, sizeof(float) * xhat_size * xhat_size);
   KF_MatInit(&kf->Q, kf->xhat_size, kf->xhat_size, kf->Q_data);
 
-  /* ��������������� R */
+  /* 量测噪声协方差矩阵 R */
   kf->R_data = (float *)user_malloc(sizeof(float) * z_size * z_size);
   memset(kf->R_data, 0, sizeof(float) * z_size * z_size);
   KF_MatInit(&kf->R, kf->z_size, kf->z_size, kf->R_data);
 
-  /* ���������� K */
+  /* 卡尔曼增益 K */
   kf->K_data = (float *)user_malloc(sizeof(float) * xhat_size * z_size);
   memset(kf->K_data, 0, sizeof(float) * xhat_size * z_size);
   KF_MatInit(&kf->K, kf->xhat_size, kf->z_size, kf->K_data);
 
-  /* ��ʱ������� */
+  /* 临时矩阵分配 */
   kf->S_data = (float *)user_malloc(sizeof(float) * xhat_size * xhat_size);
   kf->temp_matrix_data =
       (float *)user_malloc(sizeof(float) * xhat_size * xhat_size);
@@ -277,14 +277,14 @@ int8_t KF_Init(KF_t *kf, uint8_t xhat_size, uint8_t u_size, uint8_t z_size) {
   KF_MatInit(&kf->temp_vector, kf->xhat_size, 1, kf->temp_vector_data);
   KF_MatInit(&kf->temp_vector1, kf->xhat_size, 1, kf->temp_vector_data1);
 
-  /* ��ʼ��������־ */
+  /* 初始化跳过标志 */
   kf->skip_eq1 = 0;
   kf->skip_eq2 = 0;
   kf->skip_eq3 = 0;
   kf->skip_eq4 = 0;
   kf->skip_eq5 = 0;
 
-  /* ��ʼ���û�����ָ�� */
+  /* 初始化用户函数指针 */
   kf->User_Func0_f = NULL;
   kf->User_Func1_f = NULL;
   kf->User_Func2_f = NULL;
@@ -297,15 +297,15 @@ int8_t KF_Init(KF_t *kf, uint8_t xhat_size, uint8_t u_size, uint8_t z_size) {
 }
 
 /**
- * @brief ��ȡ���Ⲣ�������Զ�����ʱ��������
+ * @brief 获取量测并在启用自动调整时调整矩阵
  *
- * @param kf �������˲����ṹ��ָ��
- * @return int8_t 0��Ӧû�д���
+ * @param kf 卡尔曼滤波器结构体指针
+ * @return int8_t 0对应没有错误
  */
 int8_t KF_Measure(KF_t *kf) {
   if (kf == NULL) return -1;
 
-  /* ����������Ч���Զ����� H, K, R ���� */
+  /* 根据量测有效性自动调整 H, K, R 矩阵 */
   if (kf->use_auto_adjustment != 0) {
     KF_AdjustHKR(kf);
   } else {
@@ -319,17 +319,17 @@ int8_t KF_Measure(KF_t *kf) {
 }
 
 /**
- * @brief ����1������״̬���� - xhat'(k) = F��xhat(k-1) + B��u
+ * @brief 步骤1：先验状态估计 - xhat'(k) = F·xhat(k-1) + B·u
  *
- * @param kf �������˲����ṹ��ָ��
- * @return int8_t 0��Ӧû�д���
+ * @param kf 卡尔曼滤波器结构体指针
+ * @return int8_t 0对应没有错误
  */
 int8_t KF_PredictState(KF_t *kf) {
   if (kf == NULL) return -1;
 
   if (!kf->skip_eq1) {
     if (kf->u_size > 0) {
-      /* �������: xhat'(k) = F��xhat(k-1) + B��u */
+      /* 有控制输入: xhat'(k) = F·xhat(k-1) + B·u */
       kf->temp_vector.numRows = kf->xhat_size;
       kf->temp_vector.numCols = 1;
       kf->mat_status = KF_MatMult(&kf->F, &kf->xhat, &kf->temp_vector);
@@ -340,7 +340,7 @@ int8_t KF_PredictState(KF_t *kf) {
       kf->mat_status =
           KF_MatAdd(&kf->temp_vector, &kf->temp_vector1, &kf->xhatminus);
     } else {
-      /* �޿�������: xhat'(k) = F��xhat(k-1) */
+      /* 无控制输入: xhat'(k) = F·xhat(k-1) */
       kf->mat_status = KF_MatMult(&kf->F, &kf->xhat, &kf->xhatminus);
     }
   }
@@ -349,10 +349,10 @@ int8_t KF_PredictState(KF_t *kf) {
 }
 
 /**
- * @brief ����2���������� - P'(k) = F��P(k-1)��F^T + Q
+ * @brief 步骤2：先验协方差 - P'(k) = F·P(k-1)·F^T + Q
  *
- * @param kf �������˲����ṹ��ָ��
- * @return int8_t 0��Ӧû�д���
+ * @param kf 卡尔曼滤波器结构体指针
+ * @return int8_t 0对应没有错误
  */
 int8_t KF_PredictCovariance(KF_t *kf) {
   if (kf == NULL) return -1;
@@ -362,7 +362,7 @@ int8_t KF_PredictCovariance(KF_t *kf) {
     kf->mat_status = KF_MatMult(&kf->F, &kf->P, &kf->Pminus);
     kf->temp_matrix.numRows = kf->Pminus.numRows;
     kf->temp_matrix.numCols = kf->FT.numCols;
-    /* F��P(k-1)��F^T */
+    /* F·P(k-1)·F^T */
     kf->mat_status = KF_MatMult(&kf->Pminus, &kf->FT, &kf->temp_matrix);
     kf->mat_status = KF_MatAdd(&kf->temp_matrix, &kf->Q, &kf->Pminus);
   }
@@ -371,10 +371,10 @@ int8_t KF_PredictCovariance(KF_t *kf) {
 }
 
 /**
- * @brief ����3������������ - K = P'(k)��H^T / (H��P'(k)��H^T + R)
+ * @brief 步骤3：卡尔曼增益 - K = P'(k)·H^T / (H·P'(k)·H^T + R)
  *
- * @param kf �������˲����ṹ��ָ��
- * @return int8_t 0��Ӧû�д���
+ * @param kf 卡尔曼滤波器结构体指针
+ * @return int8_t 0对应没有错误
  */
 int8_t KF_CalcGain(KF_t *kf) {
   if (kf == NULL) return -1;
@@ -383,23 +383,23 @@ int8_t KF_CalcGain(KF_t *kf) {
     kf->mat_status = KF_MatTrans(&kf->H, &kf->HT);
     kf->temp_matrix.numRows = kf->H.numRows;
     kf->temp_matrix.numCols = kf->Pminus.numCols;
-    /* H��P'(k) */
+    /* H·P'(k) */
     kf->mat_status = KF_MatMult(&kf->H, &kf->Pminus, &kf->temp_matrix);
     kf->temp_matrix1.numRows = kf->temp_matrix.numRows;
     kf->temp_matrix1.numCols = kf->HT.numCols;
-    /* H��P'(k)��H^T */
+    /* H·P'(k)·H^T */
     kf->mat_status = KF_MatMult(&kf->temp_matrix, &kf->HT, &kf->temp_matrix1);
     kf->S.numRows = kf->R.numRows;
     kf->S.numCols = kf->R.numCols;
-    /* S = H��P'(k)��H^T + R */
+    /* S = H·P'(k)·H^T + R */
     kf->mat_status = KF_MatAdd(&kf->temp_matrix1, &kf->R, &kf->S);
     /* S^-1 */
     kf->mat_status = KF_MatInv(&kf->S, &kf->temp_matrix1);
     kf->temp_matrix.numRows = kf->Pminus.numRows;
     kf->temp_matrix.numCols = kf->HT.numCols;
-    /* P'(k)��H^T */
+    /* P'(k)·H^T */
     kf->mat_status = KF_MatMult(&kf->Pminus, &kf->HT, &kf->temp_matrix);
-    /* K = P'(k)��H^T��S^-1 */
+    /* K = P'(k)·H^T·S^-1 */
     kf->mat_status = KF_MatMult(&kf->temp_matrix, &kf->temp_matrix1, &kf->K);
   }
 
@@ -407,10 +407,10 @@ int8_t KF_CalcGain(KF_t *kf) {
 }
 
 /**
- * @brief ����4��״̬���� - xhat(k) = xhat'(k) + K��(z - H��xhat'(k))
+ * @brief 步骤4：状态更新 - xhat(k) = xhat'(k) + K·(z - H·xhat'(k))
  *
- * @param kf �������˲����ṹ��ָ��
- * @return int8_t 0��Ӧû�д���
+ * @param kf 卡尔曼滤波器结构体指针
+ * @return int8_t 0对应没有错误
  */
 int8_t KF_UpdateState(KF_t *kf) {
   if (kf == NULL) return -1;
@@ -418,17 +418,17 @@ int8_t KF_UpdateState(KF_t *kf) {
   if (!kf->skip_eq4) {
     kf->temp_vector.numRows = kf->H.numRows;
     kf->temp_vector.numCols = 1;
-    /* H��xhat'(k) */
+    /* H·xhat'(k) */
     kf->mat_status = KF_MatMult(&kf->H, &kf->xhatminus, &kf->temp_vector);
     kf->temp_vector1.numRows = kf->z.numRows;
     kf->temp_vector1.numCols = 1;
-    /* ��Ϣ: z - H��xhat'(k) */
+    /* 新息: z - H·xhat'(k) */
     kf->mat_status = KF_MatSub(&kf->z, &kf->temp_vector, &kf->temp_vector1);
     kf->temp_vector.numRows = kf->K.numRows;
     kf->temp_vector.numCols = 1;
-    /* K����Ϣ */
+    /* K·新息 */
     kf->mat_status = KF_MatMult(&kf->K, &kf->temp_vector1, &kf->temp_vector);
-    /* xhat = xhat' + K����Ϣ */
+    /* xhat = xhat' + K·新息 */
     kf->mat_status = KF_MatAdd(&kf->xhatminus, &kf->temp_vector, &kf->xhat);
   }
 
@@ -436,10 +436,10 @@ int8_t KF_UpdateState(KF_t *kf) {
 }
 
 /**
- * @brief ����5��������� - P(k) = P'(k) - K��H��P'(k)
+ * @brief 步骤5：协方差更新 - P(k) = P'(k) - K·H·P'(k)
  *
- * @param kf �������˲����ṹ��ָ��
- * @return int8_t 0��Ӧû�д���
+ * @param kf 卡尔曼滤波器结构体指针
+ * @return int8_t 0对应没有错误
  */
 int8_t KF_UpdateCovariance(KF_t *kf) {
   if (kf == NULL) return -1;
@@ -449,11 +449,11 @@ int8_t KF_UpdateCovariance(KF_t *kf) {
     kf->temp_matrix.numCols = kf->H.numCols;
     kf->temp_matrix1.numRows = kf->temp_matrix.numRows;
     kf->temp_matrix1.numCols = kf->Pminus.numCols;
-    /* K��H */
+    /* K·H */
     kf->mat_status = KF_MatMult(&kf->K, &kf->H, &kf->temp_matrix);
-    /* K��H��P'(k) */
+    /* K·H·P'(k) */
     kf->mat_status = KF_MatMult(&kf->temp_matrix, &kf->Pminus, &kf->temp_matrix1);
-    /* P = P' - K��H��P' */
+    /* P = P' - K·H·P' */
     kf->mat_status = KF_MatSub(&kf->Pminus, &kf->temp_matrix1, &kf->P);
   }
 
@@ -461,70 +461,70 @@ int8_t KF_UpdateCovariance(KF_t *kf) {
 }
 
 /**
- * @brief ִ�������Ŀ������˲����ڣ���󷽳̣�
+ * @brief 执行完整的卡尔曼滤波周期（五大方程）
  *
- * ʵ�ֱ�׼KF����֧���û��Զ��庯������������չ��EKF/UKF/ESKF/AUKF����
- * ÿ�����趼����ͨ�� User_Func �ص������滻��
+ * 实现标准KF，并支持用户自定义函数钩子用于扩展（EKF/UKF/ESKF/AUKF）。
+ * 每个步骤都可以通过 User_Func 回调函数替换。
  *
- * @param kf �������˲����ṹ��ָ��
- * @return float* �˲����״̬����ֵָ��
+ * @param kf 卡尔曼滤波器结构体指针
+ * @return float* 滤波后的状态估计值指针
  */
 float *KF_Update(KF_t *kf) {
   if (kf == NULL) return NULL;
 
-  /* ����0: �����ȡ�;������ */
+  /* 步骤0: 量测获取和矩阵调整 */
   KF_Measure(kf);
   if (kf->User_Func0_f != NULL) kf->User_Func0_f(kf);
 
-  /* ����1: ����״̬���� - xhat'(k) = F��xhat(k-1) + B��u */
+  /* 步骤1: 先验状态估计 - xhat'(k) = F·xhat(k-1) + B·u */
   KF_PredictState(kf);
   if (kf->User_Func1_f != NULL) kf->User_Func1_f(kf);
 
-  /* ����2: �������� - P'(k) = F��P(k-1)��F^T + Q */
+  /* 步骤2: 先验协方差 - P'(k) = F·P(k-1)·F^T + Q */
   KF_PredictCovariance(kf);
   if (kf->User_Func2_f != NULL) kf->User_Func2_f(kf);
 
-  /* ������£����ڴ�����Ч����ʱִ�У� */
+  /* 量测更新（仅在存在有效量测时执行） */
   if (kf->measurement_valid_num != 0 || kf->use_auto_adjustment == 0) {
-    /* ����3: ���������� - K = P'(k)��H^T / (H��P'(k)��H^T + R) */
+    /* 步骤3: 卡尔曼增益 - K = P'(k)·H^T / (H·P'(k)·H^T + R) */
     KF_CalcGain(kf);
     if (kf->User_Func3_f != NULL) kf->User_Func3_f(kf);
 
-    /* ����4: ״̬���� - xhat(k) = xhat'(k) + K��(z - H��xhat'(k)) */
+    /* 步骤4: 状态更新 - xhat(k) = xhat'(k) + K·(z - H·xhat'(k)) */
     KF_UpdateState(kf);
     if (kf->User_Func4_f != NULL) kf->User_Func4_f(kf);
 
-    /* ����5: ������� - P(k) = P'(k) - K��H��P'(k) */
+    /* 步骤5: 协方差更新 - P(k) = P'(k) - K·H·P'(k) */
     KF_UpdateCovariance(kf);
   } else {
-    /* ����Ч���� - ��Ԥ�� */
+    /* 无有效量测 - 仅预测 */
     memcpy(kf->xhat_data, kf->xhatminus_data, sizeof(float) * kf->xhat_size);
     memcpy(kf->P_data, kf->Pminus_data,
            sizeof(float) * kf->xhat_size * kf->xhat_size);
   }
 
-  /* �������� */
+  /* 后处理钩子 */
   if (kf->User_Func5_f != NULL) kf->User_Func5_f(kf);
 
-  /* ������������ǿ����С���� */
+  /* 防过度收敛：强制最小方差 */
   for (uint8_t i = 0; i < kf->xhat_size; i++) {
     if (kf->P_data[i * kf->xhat_size + i] < kf->state_min_variance[i])
       kf->P_data[i * kf->xhat_size + i] = kf->state_min_variance[i];
   }
 
-  /* ���ƽ������� */
+  /* 复制结果到输出 */
   memcpy(kf->filtered_value, kf->xhat_data, sizeof(float) * kf->xhat_size);
 
-  /* ���Ӻ������� */
+  /* 附加后处理钩子 */
   if (kf->User_Func6_f != NULL) kf->User_Func6_f(kf);
 
   return kf->filtered_value;
 }
 
 /**
- * @brief ���ÿ������˲���״̬
+ * @brief 重置卡尔曼滤波器状态
  *
- * @param kf �������˲����ṹ��ָ��
+ * @param kf 卡尔曼滤波器结构体指针
  */
 void KF_Reset(KF_t *kf) {
   if (kf == NULL) return;
@@ -536,45 +536,45 @@ void KF_Reset(KF_t *kf) {
 }
 
 /**
- * @brief ����������Ч�Զ�̬���� H, R, K ����
+ * @brief 根据量测有效性动态调整 H, R, K 矩阵
  *
- * �ú������ݵ�ǰ��������Щ������Ч�����㣩���ؽ�������ؾ���
- * ֧�־��в�ͬ�����ʵ��첽��������
+ * 该函数根据当前周期中哪些量测有效（非零）来重建量测相关矩阵。
+ * 支持具有不同采样率的异步传感器。
  *
- * @param kf �������˲����ṹ��ָ��
+ * @param kf 卡尔曼滤波器结构体指针
  */
 static void KF_AdjustHKR(KF_t *kf) {
   kf->measurement_valid_num = 0;
 
-  /* ���Ʋ������������� */
+  /* 复制并重置量测向量 */
   memcpy(kf->z_data, kf->measured_vector, sizeof(float) * kf->z_size);
   memset(kf->measured_vector, 0, sizeof(float) * kf->z_size);
 
-  /* ��� H �� R ���� */
+  /* 清空 H 和 R 矩阵 */
   memset(kf->R_data, 0, sizeof(float) * kf->z_size * kf->z_size);
   memset(kf->H_data, 0, sizeof(float) * kf->xhat_size * kf->z_size);
 
-  /* ʶ����Ч���Ⲣ�ؽ� z, H */
+  /* 识别有效量测并重建 z, H */
   for (uint8_t i = 0; i < kf->z_size; i++) {
-    if (kf->z_data[i] != 0) { /* �����ʾ��Ч���� */
-      /* ����Ч����ѹ���� z */
+    if (kf->z_data[i] != 0) { /* 非零表示有效量测 */
+      /* 将有效量测压缩到 z */
       kf->z_data[kf->measurement_valid_num] = kf->z_data[i];
       kf->temp[kf->measurement_valid_num] = i;
 
-      /* �ؽ� H ������ */
+      /* 重建 H 矩阵行 */
       kf->H_data[kf->xhat_size * kf->measurement_valid_num +
                  kf->measurement_map[i] - 1] = kf->measurement_degree[i];
       kf->measurement_valid_num++;
     }
   }
 
-  /* ����Ч���ⷽ���ؽ� R ���� */
+  /* 用有效量测方差重建 R 矩阵 */
   for (uint8_t i = 0; i < kf->measurement_valid_num; i++) {
     kf->R_data[i * kf->measurement_valid_num + i] =
         kf->mat_r_diagonal_elements[kf->temp[i]];
   }
 
-  /* ��������ά����ƥ����Ч�������� */
+  /* 调整矩阵维度以匹配有效量测数量 */
   kf->H.numRows = kf->measurement_valid_num;
   kf->H.numCols = kf->xhat_size;
   kf->HT.numRows = kf->xhat_size;