AR-Autopilot/arautopilot/core/autotuner.py

"""Relay-feedback Ziegler-Nichols auto-tuner -- Sprint 7.

Implements the closed-loop relay auto-tuning method (Åström & Hägglund 1984)
to identify the ultimate gain ``Ku`` and ultimate period ``Tu`` from a single
relay experiment, then converts them to PID gains using the ZN formulas.

The relay oscillation amplitude ``d`` is chosen so the actuator stays within
a safe excitation band during commissioning (typically ±5 % of full-scale
rudder output).

Usage::

    tuner = RelayAutoTuner(relay_amplitude=5.0, min_cycles=3)

    # Feed the error signal at the control rate (10 Hz outer loop).
    for step in simulation:
        output = tuner.step(error_deg)

    result = tuner.result()
    if result.converged:
        gains = result.to_pid_gains()

Theory
------
The relay switches its output between ``+d`` and ``-d`` based on the sign of
the process error. The resulting limit cycle has amplitude ``a`` (peak
deviation of the output) and period ``Tu``. The ultimate gain is:

    Ku = 4d / (π a)

ZN formulas for PID (tuned for setpoint tracking):

    Kp = 0.6 · Ku
    Ki = 1.2 · Ku / Tu      (= 2 · Kp / Tu)
    Kd = 0.075 · Ku · Tu    (= Kp · Tu / 8)
"""

from __future__ import annotations

import math
from dataclasses import dataclass, field

from arautopilot.core.pid_config import PidGains


@dataclass
class TunerResult:
    """Outcome of one relay experiment."""

    converged: bool = False
    ku: float = 0.0           # ultimate gain
    tu_s: float = 0.0         # ultimate period, seconds
    cycles_detected: int = 0  # full half-cycles ÷ 2

    # Raw measurements
    amplitude: float = 0.0   # peak-to-peak process amplitude / 2 (deg)
    relay_amplitude: float = 0.0  # relay output magnitude used

    def to_pid_gains(self) -> PidGains:
        """Convert Ku / Tu to ZN-formula PID gains."""
        if not self.converged:
            raise RuntimeError("Tuner has not converged yet")
        kp = 0.6 * self.ku
        ki = 1.2 * self.ku / self.tu_s if self.tu_s > 0 else 0.0
        kd = 0.075 * self.ku * self.tu_s
        return PidGains(kp=round(kp, 4), ki=round(ki, 4), kd=round(kd, 4))


class RelayAutoTuner:
    """Closed-loop relay auto-tuner (10 Hz outer-loop step clock).

    Parameters
    ----------
    relay_amplitude:
        Magnitude of the relay output in degrees of rudder.
    min_cycles:
        Minimum number of complete oscillation cycles before declaring
        convergence.  Typically 3–5 for a reliable estimate.
    dt_s:
        Time step in seconds (default 0.1 for 10 Hz outer loop).
    max_steps:
        Safety cut-off: abort if the experiment runs longer than this many
        control steps.
    """

    def __init__(
        self,
        relay_amplitude: float = 5.0,
        min_cycles: int = 3,
        dt_s: float = 0.1,
        max_steps: int = 6000,  # 10 min at 10 Hz
    ) -> None:
        if relay_amplitude <= 0:
            raise ValueError("relay_amplitude must be positive")
        if min_cycles < 1:
            raise ValueError("min_cycles must be >= 1")

        self._d: float = relay_amplitude
        self._min_cycles: int = min_cycles
        self._dt: float = dt_s
        self._max_steps: int = max_steps

        # State
        self._relay_output: float = self._d  # starts positive
        self._step_count: int = 0
        self._done: bool = False

        # Peak tracking for amplitude estimation
        self._peak_buffer: list[float] = []   # absolute peak values
        self._last_sign: int = 0              # sign of the last error sample

        # Half-cycle timing
        self._half_cycle_start: int = 0
        self._half_period_steps: list[float] = []  # in steps

        # Running amplitude samples (one per half-cycle)
        self._amp_peaks: list[float] = []    # max abs(error) per half-cycle
        self._current_max: float = 0.0

    # ------------------------------------------------------------------

    def step(self, error: float) -> float:
        """Feed one error sample; return the relay output (degrees of rudder).

        Parameters
        ----------
        error:
            Heading error in degrees (setpoint – measured).

        Returns
        -------
        float
            Rudder command in degrees to apply for this step.
        """
        if self._done:
            return 0.0

        self._step_count += 1
        self._current_max = max(self._current_max, abs(error))

        sign = 1 if error >= 0 else -1

        # Detect zero-crossing (sign change) → half-cycle boundary.
        if self._last_sign != 0 and sign != self._last_sign:
            half_len = self._step_count - self._half_cycle_start
            self._half_period_steps.append(half_len)
            self._amp_peaks.append(self._current_max)
            self._current_max = 0.0
            self._half_cycle_start = self._step_count

            # Toggle relay on sign change.
            self._relay_output = self._d if sign > 0 else -self._d

            # Check convergence: need 2*min_cycles half-cycles for min_cycles
            # full cycles.
            if len(self._half_period_steps) >= 2 * self._min_cycles:
                self._done = True

        self._last_sign = sign

        # Safety cut-off
        if self._step_count >= self._max_steps:
            self._done = True

        return self._relay_output

    @property
    def is_done(self) -> bool:
        return self._done

    def result(self) -> TunerResult:
        """Return the tuning result.  Call only after ``is_done`` is True."""
        n_half = len(self._half_period_steps)
        if n_half < 2:
            return TunerResult(converged=False, relay_amplitude=self._d)

        # Average the last ``2*min_cycles`` half-period measurements
        # (discard the first half-cycle which is often distorted).
        use = self._half_period_steps[-2 * self._min_cycles:]
        tu_s = (sum(use) / len(use)) * 2 * self._dt  # full period

        # Amplitude: average peak-to-peak / 2 of last min_cycles cycles
        amp_use = self._amp_peaks[-2 * self._min_cycles:]
        amplitude = sum(amp_use) / len(amp_use)

        if amplitude <= 0:
            return TunerResult(converged=False, relay_amplitude=self._d)

        # Åström-Hägglund: Ku = 4d / (π a)
        ku = 4.0 * self._d / (math.pi * amplitude)

        return TunerResult(
            converged=True,
            ku=round(ku, 4),
            tu_s=round(tu_s, 3),
            cycles_detected=n_half // 2,
            amplitude=round(amplitude, 3),
            relay_amplitude=self._d,
        )