update imu

trajectory_tracker.py

@@ -66,6 +66,7 @@ class Tracker:
         self.velocity = np.zeros(3)
 
         self.position_history = [self.position.copy()]
+        self._max_history = 800
 
         self.zupt_threshold_accel = zupt_threshold_accel
         self.zupt_threshold_gyro = zupt_threshold_gyro
@@ -83,8 +84,9 @@ class Tracker:
         self._linear_var_window = []
         self._prev_accel_linear = np.zeros(3)
 
-        # 当前四元数 (用于外部查询)
+        # 当前四元数和旋转矩阵 (用于外部查询)
         self.qw, self.qx, self.qy, self.qz = 1.0, 0.0, 0.0, 0.0
+        self.R = np.eye(3)
 
     def update(self, gyro, accel, qw, qx, qy, qz, dt):
         """处理一帧 IMU 数据
@@ -104,7 +106,7 @@ class Tracker:
         accel_corrected = accel - self.accel_bias
 
         # 1. 四元数 → 旋转矩阵
-        R = quat_to_rotation(qw, qx, qy, qz)
+        self.R = R = quat_to_rotation(qw, qx, qy, qz)
 
         # 2. 机体加速度 → 世界加速度 → 重力补偿
         a_world = rotate_accel(accel_corrected, R)
@@ -113,9 +115,10 @@ class Tracker:
         # 3. 加速度死区
         a_linear = apply_deadzone(a_linear, self.deadzone_threshold)
 
-        # 4. ZUPT 静止检测
+        # 4. ZUPT 静止/纯旋转检测
         gyro_norm = np.linalg.norm(gyro)
         linear_magnitude = np.linalg.norm(a_linear)
+        accel_mag = np.linalg.norm(accel_corrected)
 
         self._linear_var_window.append(a_linear.copy())
         if len(self._linear_var_window) > self.var_window_size:
@@ -125,13 +128,22 @@ class Tracker:
         if len(self._linear_var_window) >= self.var_window_size:
             linear_variance = np.var(self._linear_var_window, axis=0).mean()
 
+        # 静止: gyro 低 + a_linear 小 + 方差低
         is_static = (
             gyro_norm < self.zupt_threshold_gyro
             and linear_magnitude < self.zupt_threshold_accel
             and linear_variance < self._zupt_var_threshold
         )
 
-        if is_static:
+        # 纯旋转: a_linear 小 + 方差低 + body accel 幅值 ≈ 纯重力 (无平移)
+        is_rotation_only = (
+            not is_static
+            and linear_magnitude < self.zupt_threshold_accel
+            and linear_variance < self._zupt_var_threshold
+            and abs(accel_mag - 9.81) < 0.3
+        )
+
+        if is_static or is_rotation_only:
             self._zupt_counter += 1
         else:
             self._zupt_counter = 0
@@ -150,6 +162,8 @@ class Tracker:
                 self.position = self.position + self.velocity * dt
 
         self.position_history.append(self.position.copy())
+        if len(self.position_history) > self._max_history:
+            self.position_history = self.position_history[-self._max_history:]
         return self.position
 
     @staticmethod