sprint-8: EKF + adaptive tuner + HWID + SHA-256 audit hash-chain

- heading_ekf.py: 2-state Kalman filter fusing PGN 127250 heading and 127251 ROT with shortest-arc innovation and symmetric covariance update - adaptive_tuner.py: gradient-descent outer-loop Kp/Ki adjuster bounded to ±adaptive_max_deviation_pct; oscillation vs steady-state detection - hwid.py: HMAC-SHA256 activation token (verify side); hwid_from_mac_words converts three Modbus uint16 MAC words to 12-char hex HWID - audit.py: SHA-256 hash-chain -- each JSONL line carries prev_hash and line_hash; verify_chain() detects tampering, deletion, insertion - firmware/system/hwid.h+cpp: esp_efuse_mac_get_default wrapper + FNV-32 hash + "AA:BB:CC:DD:EE:FF" formatter - modbus_registers.yaml + generated .h/.py: HWID_MAC_01/23/45 at input addrs 9/10/11 (three 16-bit words = 6-byte MAC) - modbus_slave.cpp: INPUT_HWID_MAC_01/23/45 cases read eFuse MAC - main.cpp: logs HWID string + FNV-32 hash at boot (activation traceability) - tests: 72 new tests (audit signing, EKF, adaptive tuner, HWID) -- 398 total Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-05-20 03:07:27 -04:00
parent 5f9b445572
commit 45642fda0e
15 changed files with 1217 additions and 5 deletions
@@ -0,0 +1,140 @@
+"""Adaptive gain tuner -- Sprint 8.
+
+Watches the steady-state heading error and adjusts the outer-loop Kp/Ki
+within the bounds defined by ``PidConfig.adaptive_max_deviation_pct``.
+
+The strategy is a simple integral-error gradient scheme:
+- If |mean_error| > dead_band for a sustained window, nudge Kp up by step_pct.
+- If the response is oscillating (error sign changes frequently), nudge Kp down.
+- Ki is adjusted proportionally to maintain the ZN ratio Ki = 2*Kp / Tu_est.
+- Kd is not adapted (derivative magnifies noise; ZN auto-tune sets it once).
+
+All changes are bounded to ±adaptive_max_deviation_pct of the base gains
+(brief section 6: "never outside ±50 %").
+
+Usage::
+
+    tuner = AdaptiveTuner(pid_config, base_gains)
+    # On each outer-loop tick (10 Hz):
+    new_gains = tuner.step(heading_error_deg, dt_s=0.1)
+    if new_gains is not None:
+        apply_outer_gains(new_gains)
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+
+from arautopilot.core.pid_config import PidConfig, PidGains
+
+
+@dataclass
+class AdaptiveTuner:
+    """Gradient-descent adaptive gain tuner for the outer PID loop.
+
+    Parameters
+    ----------
+    config:
+        PidConfig that owns the base gains and adaptive bounds.
+    base_gains:
+        The current base gains (set by commissioning or default).
+    dead_band_deg:
+        Error below which no adaptation occurs.
+    window_steps:
+        Number of 10 Hz steps over which the error statistics are computed.
+    step_pct:
+        Fractional Kp adjustment per adaptation event (default 2 %).
+    """
+
+    config: PidConfig
+    base_gains: PidGains
+    dead_band_deg: float = 1.0
+    window_steps: int = 100       # 10 seconds
+    step_pct: float = 0.02        # 2 % per step
+
+    _error_buffer: list[float] = field(default_factory=list)
+    _current_kp: float = field(init=False)
+    _current_ki: float = field(init=False)
+    _current_kd: float = field(init=False)
+
+    def __post_init__(self) -> None:
+        self._current_kp = self.base_gains.kp
+        self._current_ki = self.base_gains.ki
+        self._current_kd = self.base_gains.kd
+
+    # ------------------------------------------------------------------
+
+    @property
+    def current_gains(self) -> PidGains:
+        return PidGains(kp=self._current_kp, ki=self._current_ki, kd=self._current_kd)
+
+    def step(self, error_deg: float, dt_s: float = 0.1) -> PidGains | None:
+        """Feed one heading error sample; return updated gains if adapted.
+
+        Returns ``None`` if no adaptation occurred this step.
+        """
+        if not self.config.adaptive_enabled:
+            return None
+
+        self._error_buffer.append(error_deg)
+        if len(self._error_buffer) > self.window_steps:
+            self._error_buffer.pop(0)
+
+        if len(self._error_buffer) < self.window_steps:
+            return None  # buffer not yet full
+
+        mean_abs = sum(abs(e) for e in self._error_buffer) / len(self._error_buffer)
+
+        # Count sign changes (oscillation indicator)
+        sign_changes = sum(
+            1 for i in range(1, len(self._error_buffer))
+            if (self._error_buffer[i] >= 0) != (self._error_buffer[i - 1] >= 0)
+        )
+        oscillating = sign_changes > self.window_steps * 0.3  # > 30 % sign flips
+
+        # Decision
+        if oscillating:
+            # Reduce Kp to damp oscillation.
+            return self._adjust_kp(-self.step_pct)
+        elif mean_abs > self.dead_band_deg:
+            # Increase Kp to reduce steady-state error.
+            return self._adjust_kp(+self.step_pct)
+
+        return None
+
+    def _adjust_kp(self, delta_frac: float) -> PidGains | None:
+        """Adjust Kp by ``delta_frac`` fraction (signed), clamped to bounds."""
+        new_kp = self._current_kp * (1.0 + delta_frac)
+
+        # Clamp to ±adaptive_max_deviation_pct of base.
+        limit = self.config.adaptive_max_deviation_pct / 100.0
+        lo = self.base_gains.kp * (1.0 - limit)
+        hi = self.base_gains.kp * (1.0 + limit)
+        new_kp = max(lo, min(hi, new_kp))
+        if new_kp == self._current_kp:
+            return None
+
+        # Adjust Ki proportionally (maintain integral-to-proportional ratio).
+        if self.base_gains.kp > 0:
+            ki_ratio = self.base_gains.ki / self.base_gains.kp
+        else:
+            ki_ratio = 0.0
+        new_ki = new_kp * ki_ratio
+
+        # Clamp Ki as well.
+        ki_lo = self.base_gains.ki * (1.0 - limit)
+        ki_hi = self.base_gains.ki * (1.0 + limit)
+        new_ki = max(ki_lo, min(ki_hi, new_ki))
+
+        self._current_kp = new_kp
+        self._current_ki = new_ki
+        # Clear buffer after each adaptation to avoid consecutive nudges.
+        self._error_buffer.clear()
+        return self.current_gains
+
+    def reset(self) -> None:
+        """Reset to base gains."""
+        self._current_kp = self.base_gains.kp
+        self._current_ki = self.base_gains.ki
+        self._current_kd = self.base_gains.kd
+        self._error_buffer.clear()