# ============================================================ # imu_fusion.py # Sensor Fusion: IMU + Camera + Latency Compensation # # Решает три критических проблемы реального перехвата: # # 1) ЛАТЕНТНОСТЬ: между захватом кадра и моментом когда команда # дойдёт до моторов проходит 50-120ms. При Vc=20 м/с это # 1-2.4 метра промаха. Этот модуль экстраполирует состояние # цели ВПЕРЁД на величину латентности. # # 2) СОБСТВЕННОЕ ДВИЖЕНИЕ: без IMU невозможно отличить движение # цели от движения камеры. Optical flow affine — грубое # приближение, ломается при тряске и быстрых манёврах. # IMU даёт точную ориентацию и скорость перехватчика. # # 3) ПРЕДСКАЗАНИЕ ТОЧКИ ВСТРЕЧИ: с IMU мы знаем свою скорость, # а с latency compensation — где цель БУДЕТ, а не где она # БЫЛА. Это позволяет PN работать корректно. # # Поддерживает: # - MAVLink ATTITUDE + LOCAL_POSITION_NED (ArduPilot/PX4) # - MSP_ATTITUDE + MSP_RAW_IMU (Betaflight/INAV) # - Fallback без IMU (только латентность, по Kalman-предсказанию) # ============================================================ import time import math import threading from collections import deque import numpy as np from config import * from config_intercept import * from helpers import clamp, box_center, box_wh, clip_box # ───────────────────────────────────────────────────────────── # Config (добавить в config_intercept.py) # ───────────────────────────────────────────────────────────── # Латентность пайплайна (секунды). # Измеряется как: время захвата кадра → момент исполнения команды мотором. # Типичные значения: # Camera capture + USB: 15-25ms # YOLO inference (async): 8-15ms (но результат приходит на 1-2 кадра позже) # ByteTrack + Kalman: <1ms # Guidance compute: <1ms # Serial/MAVLink send: 1-5ms # FC processing: 5-15ms # ESC + motor response: 10-20ms # ИТОГО: ~50-80ms для ROI, ~80-120ms для fullscan LATENCY_FIXED_MS = 65.0 # фиксированная оценка полной латентности LATENCY_ADAPTIVE = True # адаптивная оценка (подстраивается) LATENCY_EXTRA_TERMINAL_MS = 0.0 # доп. латентность в TERMINAL (0 = не добавлять) LATENCY_MAX_MS = 200.0 # верхний предел (защита от багов) LATENCY_MIN_MS = 20.0 # нижний предел # IMU IMU_ENABLE = True IMU_SOURCE = "mavlink" # "mavlink" | "msp" | "none" IMU_MAVLINK_CONNECTION = "udpin:0.0.0.0:14550" # получаем телеметрию IMU_MSP_PORT = "/dev/ttyUSB0" IMU_MSP_BAUD = 115200 IMU_POLL_HZ = 100.0 # частота опроса IMU # Сглаживание IMU IMU_SMOOTH_ALPHA = 0.30 # EMA для скоростей IMU_GRAVITY = 9.81 # Camera-IMU alignment (поворот IMU относительно камеры) # Для типичного FPV: камера наклонена вперёд на 25-35° CAMERA_TILT_DEG = 30.0 # наклон камеры вперёд (градусы) # Latency measurement (для адаптивной оценки) LATENCY_MEASURE_ENABLE = True LATENCY_MEASURE_WINDOW = 30 # окно усреднения # Debug DRAW_LATENCY_COMP = True DRAW_IMU_STATE = True class IMUState: """Текущее состояние IMU перехватчика.""" __slots__ = [ "timestamp", "roll", "pitch", "yaw", # радианы "roll_rate", "pitch_rate", "yaw_rate", # рад/с "vx", "vy", "vz", # м/с, NED frame "ax", "ay", "az", # м/с², body frame "lat", "lon", "alt", # GPS (если есть) "valid", ] def __init__(self): self.timestamp = 0.0 self.roll = 0.0 self.pitch = 0.0 self.yaw = 0.0 self.roll_rate = 0.0 self.pitch_rate = 0.0 self.yaw_rate = 0.0 self.vx = 0.0 self.vy = 0.0 self.vz = 0.0 self.ax = 0.0 self.ay = 0.0 self.az = 0.0 self.lat = 0.0 self.lon = 0.0 self.alt = 0.0 self.valid = False class LatencyCompensator: """ Компенсация латентности пайплайна. Принцип: вместо того чтобы наводиться на позицию цели "сейчас" (которая на самом деле была latency_ms миллисекунд назад), экстраполируем состояние цели вперёд на величину латентности. Использует: - Kalman/IMM-предсказание для экстраполяции цели - IMU для вычитания собственного движения - Адаптивную оценку латентности по корреляции предсказание/факт """ def __init__(self): self.latency_sec = float(LATENCY_FIXED_MS) / 1000.0 self.adaptive = bool(LATENCY_ADAPTIVE) # Адаптивная оценка латентности self._pred_hist = deque(maxlen=int(LATENCY_MEASURE_WINDOW)) self._measure_enable = bool(LATENCY_MEASURE_ENABLE) # Статистика self.compensated_shift_px = 0.0 self.ego_shift_px = 0.0 def get_latency_sec(self, phase="TRACK"): """Возвращает текущую оценку латентности.""" lat = self.latency_sec if phase == "TERMINAL": lat += float(LATENCY_EXTRA_TERMINAL_MS) / 1000.0 return float(clamp(lat, float(LATENCY_MIN_MS) / 1000.0, float(LATENCY_MAX_MS) / 1000.0)) def compensate(self, kf_state, imu_state, frame_w, frame_h, phase="TRACK"): """ Компенсирует латентность — предсказывает где цель БУДЕТ в момент исполнения команды. Args: kf_state: dict с полями из Kalman/IMM: cx, cy — текущий центр цели (пиксели) vx, vy — скорость цели (пиксели/сек) w, h — размер bbox ax, ay — ускорение (если IMM CA) imu_state: IMUState или None frame_w, frame_h: размеры кадра phase: текущая фаза FSM Returns: dict: comp_cx, comp_cy — компенсированный центр цели comp_box — компенсированный bbox [x1,y1,x2,y2] ego_dx, ego_dy — смещение из-за собственного движения pred_dx, pred_dy — смещение из-за движения цели latency_sec — использованная латентность shift_px — полное смещение (пиксели) """ lat = self.get_latency_sec(phase) cx = float(kf_state.get("cx", frame_w * 0.5)) cy = float(kf_state.get("cy", frame_h * 0.5)) vx = float(kf_state.get("vx", 0.0)) vy = float(kf_state.get("vy", 0.0)) ax = float(kf_state.get("ax", 0.0)) ay = float(kf_state.get("ay", 0.0)) bw = float(kf_state.get("w", 20.0)) bh = float(kf_state.get("h", 20.0)) # ─── 1. Предсказание движения цели ────────────────────── # Линейная экстраполяция + ускорение (если есть) pred_dx = vx * lat + 0.5 * ax * lat * lat pred_dy = vy * lat + 0.5 * ay * lat * lat # ─── 2. Вычитание собственного движения ───────────────── ego_dx = 0.0 ego_dy = 0.0 if imu_state is not None and imu_state.valid: ego_dx, ego_dy = self._compute_ego_shift( imu_state, lat, frame_w, frame_h ) # ─── 3. Компенсированная позиция ──────────────────────── comp_cx = cx + pred_dx - ego_dx comp_cy = cy + pred_dy - ego_dy # Clamp к границам кадра comp_cx = float(clamp(comp_cx, 0.0, float(frame_w - 1))) comp_cy = float(clamp(comp_cy, 0.0, float(frame_h - 1))) # Компенсированный bbox comp_box = np.array([ comp_cx - bw * 0.5, comp_cy - bh * 0.5, comp_cx + bw * 0.5, comp_cy + bh * 0.5, ], dtype=np.float32) comp_box = clip_box(comp_box, frame_w, frame_h) shift = float(np.hypot(pred_dx - ego_dx, pred_dy - ego_dy)) self.compensated_shift_px = shift self.ego_shift_px = float(np.hypot(ego_dx, ego_dy)) return { "comp_cx": float(comp_cx), "comp_cy": float(comp_cy), "comp_box": comp_box, "ego_dx": float(ego_dx), "ego_dy": float(ego_dy), "pred_dx": float(pred_dx), "pred_dy": float(pred_dy), "latency_sec": float(lat), "shift_px": float(shift), } def feed_measurement(self, predicted_center, actual_center, dt): """ Для адаптивной оценки латентности. Вызывается когда YOLO даёт новую детекцию — сравниваем где мы предсказывали цель vs где она реально оказалась. Если prediction overshoots — латентность завышена. Если undershoots — занижена. """ if not self._measure_enable or not self.adaptive: return if predicted_center is None or actual_center is None: return pred = np.array(predicted_center, dtype=np.float32) actual = np.array(actual_center, dtype=np.float32) error = float(np.linalg.norm(pred - actual)) self._pred_hist.append({ "error": error, "dt": float(dt), }) # Пока простая эвристика: если средняя ошибка растёт, # уменьшаем латентность (overshooting) if len(self._pred_hist) >= 10: errors = [p["error"] for p in self._pred_hist] recent = np.mean(errors[-5:]) older = np.mean(errors[:5]) if recent > older * 1.3: self.latency_sec *= 0.95 elif recent < older * 0.7: self.latency_sec *= 1.05 self.latency_sec = float(clamp( self.latency_sec, float(LATENCY_MIN_MS) / 1000.0, float(LATENCY_MAX_MS) / 1000.0, )) def _compute_ego_shift(self, imu, lat, frame_w, frame_h): """ Вычисляет смещение изображения из-за собственного вращения/движения дрона-перехватчика за время lat секунд. Используем угловые скорости IMU → пиксельное смещение через фокусное расстояние камеры. """ focal = float(CAMERA_FOCAL_LENGTH_PX) if focal < 10.0: return 0.0, 0.0 tilt_rad = float(CAMERA_TILT_DEG) * (math.pi / 180.0) # Угловые скорости (body frame → camera frame) # Камера наклонена вперёд, поэтому rotation mapping: # camera_pan ≈ yaw_rate * cos(tilt) + pitch_rate * sin(tilt) # camera_tilt ≈ pitch_rate * cos(tilt) - yaw_rate * sin(tilt) cos_t = math.cos(tilt_rad) sin_t = math.sin(tilt_rad) # Угловая скорость камеры cam_pan_rate = imu.yaw_rate * cos_t + imu.pitch_rate * sin_t cam_tilt_rate = imu.pitch_rate * cos_t - imu.yaw_rate * sin_t cam_roll_rate = imu.roll_rate # Angular velocity → pixel shift # Для pinhole camera: dx_px ≈ focal * d_angle ego_dx = focal * cam_pan_rate * lat # горизонтальное смещение ego_dy = focal * cam_tilt_rate * lat # вертикальное смещение # Roll создаёт вращение вокруг центра — для малых углов: # dx_roll ≈ -(y - cy) * roll_rate * lat # dy_roll ≈ (x - cx) * roll_rate * lat # Это применяется к конкретной точке, здесь пропускаем # (применится в compensate() если нужно) return float(ego_dx), float(ego_dy) class IMUReader: """ Асинхронное чтение IMU с полётного контроллера. Работает в отдельном потоке. """ def __init__(self): self.enabled = bool(IMU_ENABLE) and (IMU_SOURCE != "none") self.source = str(IMU_SOURCE).lower() self.state = IMUState() self._lock = threading.Lock() self._running = False self._thread = None self._conn = None # Сглаживание self._alpha = float(IMU_SMOOTH_ALPHA) self._smooth_vx = 0.0 self._smooth_vy = 0.0 self._smooth_vz = 0.0 def start(self): if not self.enabled: return self._running = True self._thread = threading.Thread(target=self._loop, daemon=True) self._thread.start() def stop(self): self._running = False if self._thread is not None: self._thread.join(timeout=1.0) if self._conn is not None: try: self._conn.close() except Exception: pass def get_state(self): """Потокобезопасное чтение последнего состояния IMU.""" with self._lock: s = IMUState() for attr in IMUState.__slots__: setattr(s, attr, getattr(self.state, attr)) return s def status_line(self): if not self.enabled: return "IMU disabled" if self.state.valid: return (f"IMU ready: {self.source} " f"rpy=({math.degrees(self.state.roll):.1f}, " f"{math.degrees(self.state.pitch):.1f}, " f"{math.degrees(self.state.yaw):.1f})") return f"IMU not ready: {self.source}" def _loop(self): """Главный цикл чтения IMU.""" if self.source == "mavlink": self._loop_mavlink() elif self.source == "msp": self._loop_msp() def _loop_mavlink(self): """Чтение через MAVLink.""" try: from pymavlink import mavutil self._conn = mavutil.mavlink_connection( str(IMU_MAVLINK_CONNECTION), baud=115200, ) except Exception as e: print(f"[imu] MAVLink connection failed: {e}") return poll_interval = 1.0 / max(1.0, float(IMU_POLL_HZ)) while self._running: try: # Читаем ATTITUDE msg = self._conn.recv_match( type=["ATTITUDE", "LOCAL_POSITION_NED", "SCALED_IMU2", "HIGHRES_IMU"], blocking=True, timeout=poll_interval, ) if msg is None: continue with self._lock: msg_type = msg.get_type() if msg_type == "ATTITUDE": self.state.roll = float(msg.roll) self.state.pitch = float(msg.pitch) self.state.yaw = float(msg.yaw) self.state.roll_rate = float(msg.rollspeed) self.state.pitch_rate = float(msg.pitchspeed) self.state.yaw_rate = float(msg.yawspeed) self.state.timestamp = time.perf_counter() self.state.valid = True elif msg_type == "LOCAL_POSITION_NED": raw_vx = float(msg.vx) raw_vy = float(msg.vy) raw_vz = float(msg.vz) a = self._alpha self._smooth_vx = a * self._smooth_vx + (1 - a) * raw_vx self._smooth_vy = a * self._smooth_vy + (1 - a) * raw_vy self._smooth_vz = a * self._smooth_vz + (1 - a) * raw_vz self.state.vx = self._smooth_vx self.state.vy = self._smooth_vy self.state.vz = self._smooth_vz elif msg_type in ("SCALED_IMU2", "HIGHRES_IMU"): if hasattr(msg, "xacc"): self.state.ax = float(msg.xacc) / 1000.0 * IMU_GRAVITY self.state.ay = float(msg.yacc) / 1000.0 * IMU_GRAVITY self.state.az = float(msg.zacc) / 1000.0 * IMU_GRAVITY except Exception: time.sleep(poll_interval) def _loop_msp(self): """Чтение через MSP (Betaflight/INAV).""" try: import serial self._conn = serial.Serial( str(IMU_MSP_PORT), int(IMU_MSP_BAUD), timeout=0.02, ) except Exception as e: print(f"[imu] MSP serial open failed: {e}") return poll_interval = 1.0 / max(1.0, float(IMU_POLL_HZ)) while self._running: try: # Запрашиваем MSP_ATTITUDE (108) attitude = self._msp_request(108, 6) if attitude is not None and len(attitude) >= 6: roll_deci = int.from_bytes(attitude[0:2], 'little', signed=True) pitch_deci = int.from_bytes(attitude[2:4], 'little', signed=True) yaw_deg = int.from_bytes(attitude[4:6], 'little', signed=False) with self._lock: self.state.roll = float(roll_deci) / 10.0 * (math.pi / 180.0) self.state.pitch = float(pitch_deci) / 10.0 * (math.pi / 180.0) self.state.yaw = float(yaw_deg) * (math.pi / 180.0) self.state.timestamp = time.perf_counter() self.state.valid = True # Запрашиваем MSP_RAW_IMU (102) для угловых скоростей raw_imu = self._msp_request(102, 18) if raw_imu is not None and len(raw_imu) >= 18: gx = int.from_bytes(raw_imu[6:8], 'little', signed=True) gy = int.from_bytes(raw_imu[8:10], 'little', signed=True) gz = int.from_bytes(raw_imu[10:12], 'little', signed=True) # Betaflight gyro: raw → °/s зависит от настройки # Типично: raw / 16.4 для ±2000°/s scale = math.pi / (180.0 * 16.4) with self._lock: self.state.roll_rate = float(gx) * scale self.state.pitch_rate = float(gy) * scale self.state.yaw_rate = float(gz) * scale time.sleep(poll_interval) except Exception: time.sleep(poll_interval) def _msp_request(self, cmd, expected_len): """ Отправляет MSP запрос и читает ответ. Формат MSP v1: $M< [0] [cmd] [checksum] Ответ: $M> [size] [cmd] [data...] [checksum] """ if self._conn is None: return None # Запрос (пустой payload) checksum = 0 ^ cmd packet = bytearray([ ord('$'), ord('M'), ord('<'), 0, # size = 0 cmd, checksum & 0xFF, ]) self._conn.write(packet) self._conn.flush() # Чтение ответа header = self._conn.read(5) # $M> size cmd if len(header) < 5: return None if header[0:3] != b'$M>': return None size = header[3] cmd_resp = header[4] if size < expected_len: # Читаем что есть + checksum data = self._conn.read(size + 1) return data[:size] if len(data) >= size else None data = self._conn.read(size + 1) # data + checksum if len(data) < size: return None return data[:size] class SensorFusion: """ Высокоуровневый модуль: объединяет IMU + Latency Compensation. Вызывается один раз в каждом кадре main loop. Принимает текущее состояние трекера и выдаёт компенсированную позицию/bbox для наведения. """ def __init__(self): self.imu_reader = IMUReader() self.latency_comp = LatencyCompensator() self.enabled = True def start(self): self.imu_reader.start() def stop(self): self.imu_reader.stop() def status_line(self): imu_status = self.imu_reader.status_line() lat = self.latency_comp.latency_sec * 1000.0 return f"Sensor fusion: latency={lat:.0f}ms | {imu_status}" def update(self, kf, locked_box, pred_box, frame_w, frame_h, phase="TRACK", confirmed=False): """ Полный пайплайн компенсации. Args: kf: Kalman8D или IMMFilter (с .x, .initialized) locked_box: текущий locked bbox или None pred_box: Kalman-предсказанный bbox или None frame_w, frame_h: размеры effective frame phase: фаза FSM confirmed: цель подтверждена Returns: dict: comp_center — компенсированный центр [cx, cy] comp_box — компенсированный bbox [x1, y1, x2, y2] raw_center — исходный центр (без компенсации) imu_valid — IMU доступен latency_ms — использованная латентность shift_px — смещение компенсации ego_speed — скорость дрона (м/с) ego_yaw_rate — скорость рыскания (°/с) """ result = { "comp_center": None, "comp_box": None, "raw_center": None, "imu_valid": False, "latency_ms": 0.0, "shift_px": 0.0, "ego_speed": 0.0, "ego_yaw_rate": 0.0, } # Определяем исходную позицию цели ref_box = locked_box if locked_box is not None else pred_box if ref_box is None or not kf.initialized: return result center = box_center(ref_box) w, h = box_wh(ref_box) result["raw_center"] = center.copy() # Состояние из Kalman/IMM kf_state = { "cx": float(center[0]), "cy": float(center[1]), "vx": float(kf.x[4, 0]), "vy": float(kf.x[5, 0]), "w": float(w), "h": float(h), "ax": 0.0, "ay": 0.0, } # Если IMM — берём ускорение из CA-модели if hasattr(kf, 'get_acceleration'): ax, ay = kf.get_acceleration() kf_state["ax"] = ax kf_state["ay"] = ay # Читаем IMU imu_state = self.imu_reader.get_state() if self.imu_reader.enabled else None result["imu_valid"] = (imu_state is not None and imu_state.valid) if imu_state is not None and imu_state.valid: speed = float(np.hypot(imu_state.vx, imu_state.vy)) result["ego_speed"] = speed result["ego_yaw_rate"] = float( math.degrees(imu_state.yaw_rate) ) # Компенсация латентности comp = self.latency_comp.compensate( kf_state, imu_state, frame_w, frame_h, phase ) result["comp_center"] = np.array( [comp["comp_cx"], comp["comp_cy"]], dtype=np.float32 ) result["comp_box"] = comp["comp_box"] result["latency_ms"] = comp["latency_sec"] * 1000.0 result["shift_px"] = comp["shift_px"] return result def feed_detection(self, predicted_center, actual_center, dt): """Для адаптивной калибровки латентности.""" self.latency_comp.feed_measurement(predicted_center, actual_center, dt) def draw_overlay(self, frame_bgr, result, sx, sy): """Визуализация компенсации.""" if not DRAW_LATENCY_COMP: return import cv2 raw = result.get("raw_center") comp_center = result.get("comp_center") shift = result.get("shift_px", 0.0) lat = result.get("latency_ms", 0.0) if raw is not None and comp_center is not None: # Линия от raw к compensated rx = int(float(raw[0]) / max(1e-6, sx)) ry = int(float(raw[1]) / max(1e-6, sy)) cx = int(float(comp_center[0]) / max(1e-6, sx)) cy = int(float(comp_center[1]) / max(1e-6, sy)) color = (255, 128, 0) # оранжевый cv2.arrowedLine(frame_bgr, (rx, ry), (cx, cy), color, 2, cv2.LINE_AA) cv2.circle(frame_bgr, (cx, cy), 5, color, -1) # Текст y0 = 320 imu_str = "IMU:OK" if result.get("imu_valid") else "IMU:---" ego = result.get("ego_speed", 0.0) yaw_r = result.get("ego_yaw_rate", 0.0) txt = f"LAT: {lat:.0f}ms shift={shift:.1f}px {imu_str}" cv2.putText(frame_bgr, txt, (20, y0), cv2.FONT_HERSHEY_SIMPLEX, 0.55, (255, 128, 0), 2) if DRAW_IMU_STATE and result.get("imu_valid"): txt2 = f"EGO: v={ego:.1f}m/s yaw_r={yaw_r:.1f}d/s" cv2.putText(frame_bgr, txt2, (20, y0 + 25), cv2.FONT_HERSHEY_SIMPLEX, 0.55, (255, 128, 0), 2) # ============================================================= # ИНТЕГРАЦИЯ В MAIN.PY # ============================================================= # # 1. Импорт: # from imu_fusion import SensorFusion # # 2. Инициализация (после guidance_ctrl): # sensor_fusion = SensorFusion() # sensor_fusion.start() # print(sensor_fusion.status_line()) # # 3. В главном цикле, ПОСЛЕ guidance_state и ПЕРЕД autopilot: # # fusion_result = sensor_fusion.update( # kf=kf, # locked_box=locked_box_eff, # pred_box=pred_box_eff, # frame_w=ew, # frame_h=eh, # phase=intercept_params.get("phase", "TRACK") if intercept_params else "TRACK", # confirmed=confirmed, # ) # # # Используем компенсированную позицию для PN и autopilot: # if fusion_result["comp_center"] is not None: # # Передаём в PN вместо raw center # target_center_for_pn = fusion_result["comp_center"] # target_box_for_guidance = fusion_result["comp_box"] # # 4. При получении новой YOLO-детекции — адаптивная калибровка: # if have_yolo and chosen_valid: # sensor_fusion.feed_detection( # predicted_center=box_center(pred_box_eff) if pred_box_eff is not None else None, # actual_center=box_center(chosen), # dt=dt, # ) # # 5. Отрисовка: # sensor_fusion.draw_overlay(frame_orig, fusion_result, sx, sy) # # 6. Cleanup: # sensor_fusion.stop() # =============================================================