MAI/imu_fusion.py

# ============================================================
# 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()
# =============================================================