此更改更改了imu数据处理方式，使得静止状态yaw不会发生偏移，后续应优化陀螺仪数据处理：低通滤波会导致相位延迟、滑动窗口会导致数据有微量误差。

无法避免运动过程中的yaw偏移。
后续应在运动时也加上零漂偏移

scripts/udp_to_cmdvel.py

@@ -0,0 +1,58 @@
+#!/usr/bin/env python3
+"""
+UDP → /cmd_vel bridge for keyboard control.
+Receives (float64 lin_x, float64 ang_z) from Windows client.
+
+Deploy on RDKx5: /home/sunrise/yiliao_ws/udp_to_cmdvel.py
+Run: python3 udp_to_cmdvel.py
+"""
+
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Twist
+import socket
+import struct
+import sys
+
+
+class UDPToCmdVel(Node):
+    def __init__(self, port=9999):
+        super().__init__("udp_to_cmdvel")
+        self.pub = self.create_publisher(Twist, "/cmd_vel", 10)
+        self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+        self.sock.bind(("0.0.0.0", port))
+        self.sock.settimeout(0.1)
+        self.get_logger().info("Listening on UDP port %d → /cmd_vel" % port)
+        self.create_timer(0.05, self.loop)
+
+    def loop(self):
+        try:
+            data, addr = self.sock.recvfrom(16)
+            lin, ang = struct.unpack('dd', data)
+            msg = Twist()
+            msg.linear.x = lin
+            msg.angular.z = ang
+            self.pub.publish(msg)
+        except socket.timeout:
+            pass
+
+    def destroy_node(self):
+        self.sock.close()
+        super().destroy_node()
+
+
+def main():
+    rclpy.init()
+    port = int(sys.argv[1]) if len(sys.argv) > 1 else 9999
+    node = UDPToCmdVel(port=port)
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()