fast imu

main_odometry.py

@@ -117,7 +117,7 @@ def run_live(port, baud, save_csv=None):
                              'pos_x', 'pos_y', 'pos_z'])
 
     print(f"打开串口 {port} @ {baud} baud ...")
-    ser = serial.Serial(port, baud, timeout=1)
+    ser = serial.Serial(port, baud, timeout=0.01)  # 短超时, 避免 read(1) 阻塞
 
     # 静止标定
     accel_bias, gyro_bias = calibrate(ser)
@@ -135,6 +135,7 @@ def run_live(port, baud, save_csv=None):
         while plt.fignum_exists(viewer.fig.number):
             frame = read_frame(ser)
             if frame is None:
+                time.sleep(0.001)  # 无数据时短暂休眠, 避免忙等
                 continue
 
             ts = frame['timestamp_ms']

trajectory_tracker.py

@@ -50,9 +50,9 @@ class Tracker:
     使用 EKF 四元数进行姿态旋转, 标定加速度计偏置, ZUPT 抑制静止漂移。
     """
 
-    def __init__(self, zupt_threshold_accel=0.10, zupt_threshold_gyro=0.03,
-                 zupt_frames=8, deadzone_threshold=0.02,
-                 var_window_size=20, zupt_var_threshold=0.002):
+    def __init__(self, zupt_threshold_accel=0.20, zupt_threshold_gyro=0.05,
+                 zupt_frames=15, deadzone_threshold=0.03,
+                 var_window_size=30, zupt_var_threshold=0.005):
         """
         Args:
             zupt_threshold_accel: ZUPT ‖a_linear‖ 阈值 (m/s^2)

visualize_3d.py

@@ -2,7 +2,7 @@
 IMU 3D 轨迹实时可视化
 
 matplotlib 3D 窗口, 30Hz 刷新, 显示:
-  - 蓝色轨迹线
+  - 蓝色轨迹线 (自动降采样, 最多 800 点)
   - 当前点红点
   - 原点坐标系指示
   - 等比例坐标轴
@@ -10,19 +10,19 @@ matplotlib 3D 窗口, 30Hz 刷新, 显示:
 
 import numpy as np
 import matplotlib.pyplot as plt
-from matplotlib.animation import FuncAnimation
 
 
 class TrajectoryViewer:
     """3D 轨迹实时显示窗口"""
 
-    def __init__(self, title="IMU 3D Odometry", refresh_interval=33):
+    def __init__(self, title="IMU 3D Odometry", max_trail_points=800):
         """
         Args:
-            title:           窗口标题
-            refresh_interval: 刷新间隔 (ms), 默认 33ms ≈ 30Hz
+            title:            窗口标题
+            max_trail_points: 轨迹线最多显示点数 (降采样, 避免渲染卡顿)
         """
-        self.refresh_interval = refresh_interval
+        self.max_trail_points = max_trail_points
+        self._full_count = 0
 
         self.fig = plt.figure(figsize=(8, 7))
         self.fig.canvas.manager.set_window_title(title)
@@ -44,9 +44,9 @@ class TrajectoryViewer:
 
         self._setup_axes()
 
-        # 动画
-        self.anim = FuncAnimation(self.fig, self._animate, interval=self.refresh_interval,
-                                   cache_frame_data=False, blit=False)
+        # 显示非阻塞窗口
+        plt.show(block=False)
+        plt.pause(0.1)
 
     def _setup_axes(self):
         """初始化坐标轴"""
@@ -56,7 +56,6 @@ class TrajectoryViewer:
         self.ax.set_title("IMU 3D Trajectory (Z-up)")
         self.ax.legend(loc='upper left')
 
-        # 初始范围
         self.ax.set_xlim([-1, 1])
         self.ax.set_ylim([-1, 1])
         self.ax.set_zlim([-1, 1])
@@ -68,34 +67,41 @@ class TrajectoryViewer:
 
         self.ax.grid(True)
 
-    def _animate(self, frame):
-        """动画帧回调 (不做任何事, 数据由外部 update 驱动)"""
-        pass  # 通过 plt.pause 驱动, FuncAnimation 仅用于保持窗口响应
-
     def update(self, history_array):
-        """更新显示的轨迹数据
+        """更新显示的轨迹数据 (自动降采样)
 
         Args:
             history_array: Nx3 numpy array, 位置历史
         """
-        if len(history_array) < 1:
+        n = len(history_array)
+        if n < 1:
             return
 
-        x, y, z = history_array[:, 0], history_array[:, 1], history_array[:, 2]
+        # 降采样: 超过 max_trail_points 时取等间隔子集
+        if n > self.max_trail_points:
+            step = n // self.max_trail_points
+            indices = np.arange(0, n, step)
+            sampled = history_array[indices]
+        else:
+            sampled = history_array
+
+        x, y, z = sampled[:, 0], sampled[:, 1], sampled[:, 2]
 
-        # 更新轨迹线
         self.traj_line.set_data(x, y)
         self.traj_line.set_3d_properties(z)
 
-        # 更新当前点
-        self.current_point.set_data([x[-1]], [y[-1]])
-        self.current_point.set_3d_properties([z[-1]])
+        # 当前点
+        last = history_array[-1]
+        self.current_point.set_data([last[0]], [last[1]])
+        self.current_point.set_3d_properties([last[2]])
 
-        # 自适应坐标轴范围
-        self._auto_scale(x, y, z)
+        # 自适应坐标轴 (每 20 次完整更新做一次, 减少开销)
+        self._full_count += 1
+        if self._full_count % 5 == 0:
+            self._auto_scale(x, y, z)
 
     def _auto_scale(self, x, y, z):
-        """根据数据自动调整坐标轴范围, 保持等比例"""
+        """自适应等比例坐标轴"""
         all_coords = np.concatenate([x, y, z])
         margin = max(np.ptp(all_coords) * 0.2, 0.5)
         mid = (all_coords.min() + all_coords.max()) / 2
@@ -105,10 +111,6 @@ class TrajectoryViewer:
         self.ax.set_ylim([mid - half, mid + half])
         self.ax.set_zlim([mid - half, mid + half])
 
-    def show(self):
-        """阻塞显示窗口"""
-        plt.show()
-
     def close(self):
         """关闭窗口"""
         plt.close(self.fig)