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>
This commit is contained in:
2026-05-20 03:07:27 -04:00
parent 5f9b445572
commit 45642fda0e
15 changed files with 1217 additions and 5 deletions
+140
View File
@@ -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()
+107 -4
View File
@@ -15,6 +15,7 @@ instances + a CLI tool don't interleave half-written events.
from __future__ import annotations
import hashlib
import json
from datetime import UTC, datetime
from enum import StrEnum
@@ -23,6 +24,9 @@ from typing import Any
from pydantic import BaseModel, ConfigDict, Field
# Sentinel used as the "previous hash" for the very first entry in a log.
GENESIS_HASH = "0" * 64
class AuditOutcome(StrEnum):
SUCCESS = "success"
@@ -67,26 +71,85 @@ class AuditEvent(BaseModel):
)
extra: dict[str, Any] = Field(default_factory=dict)
# ----- Hash-chain fields (Sprint 8) -------------------------------------
# Set by AuditLog.append(); must not be set by the caller.
prev_hash: str | None = Field(
default=None,
min_length=64,
max_length=64,
pattern=r"^[0-9a-f]{64}$",
description="SHA-256 hex digest of the previous JSONL line (or GENESIS_HASH for first).",
)
line_hash: str | None = Field(
default=None,
min_length=64,
max_length=64,
pattern=r"^[0-9a-f]{64}$",
description="SHA-256 hex digest of (prev_hash + this event's canonical JSON).",
)
def to_jsonl(self) -> str:
"""Render as one JSON line (no trailing newline)."""
return json.dumps(self.model_dump(mode="json"), ensure_ascii=False)
@staticmethod
def _compute_hash(prev_hash: str, payload: str) -> str:
"""Return SHA-256(prev_hash + payload) as a lower-case hex string."""
return hashlib.sha256((prev_hash + payload).encode()).hexdigest()
class AuditLog:
"""Append-only writer to a JSONL audit file."""
"""Append-only writer to a JSONL audit file with SHA-256 hash-chain.
Each appended event is automatically chained: the ``prev_hash`` is set
to the SHA-256 of the previous JSONL line (or GENESIS_HASH for the first
entry), and ``line_hash`` is SHA-256(prev_hash + canonical_json).
The chain is verified by :meth:`verify_chain`.
"""
def __init__(self, path: Path | str) -> None:
self.path = Path(path)
self.path.parent.mkdir(parents=True, exist_ok=True)
# Touch the file so subsequent appends work even on first run.
if not self.path.exists():
self.path.touch()
# Bootstrap: read the last hash from the file tail.
self._last_line_hash: str = self._read_last_hash()
def _read_last_hash(self) -> str:
"""Return the line_hash of the last entry, or GENESIS_HASH if empty."""
if not self.path.exists() or self.path.stat().st_size == 0:
return GENESIS_HASH
last_line = ""
with self.path.open("r", encoding="utf-8") as f:
for line in f:
stripped = line.strip()
if stripped:
last_line = stripped
if not last_line:
return GENESIS_HASH
try:
data = json.loads(last_line)
return data.get("line_hash") or GENESIS_HASH
except (json.JSONDecodeError, KeyError):
return GENESIS_HASH
def append(self, event: AuditEvent) -> None:
"""Append one event to the log. Atomic at the line level (single write())."""
"""Append one event with hash-chain fields filled in."""
prev = self._last_line_hash
# Build the payload (without hash fields) for signing.
payload_dict = event.model_dump(mode="json")
payload_dict.pop("prev_hash", None)
payload_dict.pop("line_hash", None)
canonical = json.dumps(payload_dict, ensure_ascii=False, sort_keys=True)
h = AuditEvent._compute_hash(prev, canonical)
# Create a signed copy.
signed = event.model_copy(update={"prev_hash": prev, "line_hash": h})
line = signed.to_jsonl()
with self.path.open("a", encoding="utf-8") as f:
f.write(event.to_jsonl())
f.write(line)
f.write("\n")
self._last_line_hash = h
def read_all(self) -> list[AuditEvent]:
"""Read every event in chronological order."""
@@ -107,6 +170,46 @@ class AuditLog:
events.append(AuditEvent.model_validate(data))
return events
def verify_chain(self) -> tuple[bool, str]:
"""Verify the hash-chain integrity of the entire log.
Returns ``(True, "ok")`` on success, or ``(False, reason)`` on
the first detected tampering.
"""
prev = GENESIS_HASH
with self.path.open("r", encoding="utf-8") as f:
for line_no, raw in enumerate(f, start=1):
line = raw.strip()
if not line:
continue
try:
data = json.loads(line)
except json.JSONDecodeError:
return False, f"line {line_no}: invalid JSON"
stored_prev = data.get("prev_hash")
stored_hash = data.get("line_hash")
if stored_prev != prev:
return False, (
f"line {line_no}: prev_hash mismatch "
f"(expected {prev[:16]}… got {str(stored_prev)[:16]}…)"
)
# Recompute canonical payload (fields minus hash fields).
payload = {k: v for k, v in data.items()
if k not in ("prev_hash", "line_hash")}
canonical = json.dumps(payload, ensure_ascii=False, sort_keys=True)
expected = AuditEvent._compute_hash(prev, canonical)
if stored_hash != expected:
return False, (
f"line {line_no}: line_hash mismatch -- entry tampered"
)
prev = stored_hash
return True, "ok"
def __len__(self) -> int:
if not self.path.exists():
return 0
+169
View File
@@ -0,0 +1,169 @@
"""2-state heading EKF (Extended Kalman Filter) -- Sprint 8.
Fuses NMEA 2000 heading (PGN 127250) and rate-of-turn (PGN 127251) into a
smoothed, low-latency heading/ROT estimate for the outer PID loop.
State vector x = [heading_deg, rot_dps]
Process model (constant-ROT):
h_{k+1} = h_k + rot_k * dt
r_{k+1} = r_k (ROT modelled as random walk)
Measurements:
z_heading = h_k + v_h (v_h ~ N(0, R_h))
z_rot = r_k + v_r (v_r ~ N(0, R_r))
Angles are kept in the range [0, 360) for the state but the update step
works on signed shortest-arc differences to avoid wrap-around errors.
Usage::
ekf = HeadingEKF()
# On each 10 Hz tick:
ekf.predict(dt_s=0.1)
if new_heading_available:
ekf.update_heading(heading_deg, noise_deg=2.0)
if new_rot_available:
ekf.update_rot(rot_dps, noise_dps=1.0)
h, rot = ekf.heading_deg, ekf.rot_dps
"""
from __future__ import annotations
import math
from dataclasses import dataclass, field
@dataclass
class HeadingEKF:
"""2-state linear Kalman filter for heading and rate-of-turn.
Parameters
----------
heading_deg:
Initial heading estimate (degrees, 0-360).
rot_dps:
Initial rate-of-turn estimate (degrees per second, signed).
process_noise_heading:
Process noise variance for heading (deg²). Larger = trust model less.
process_noise_rot:
Process noise variance for ROT (deg²/s²).
"""
heading_deg: float = 0.0
rot_dps: float = 0.0
# Process noise (Q matrix diagonal)
process_noise_heading: float = 0.01 # deg²
process_noise_rot: float = 0.1 # (deg/s)²
# Covariance matrix P (2×2, stored as flat [p00, p01, p10, p11])
_P: list[float] = field(default_factory=lambda: [1.0, 0.0, 0.0, 1.0])
# ------------------------------------------------------------------
def predict(self, dt_s: float) -> None:
"""Propagate the state and covariance forward by ``dt_s`` seconds."""
h = self.heading_deg
r = self.rot_dps
# State transition: heading += rot * dt
self.heading_deg = (h + r * dt_s) % 360.0
# Jacobian F = [[1, dt], [0, 1]]
dt = dt_s
p00, p01, p10, p11 = self._P
# P = F P Fᵀ + Q
new_p00 = p00 + dt * p10 + dt * p01 + dt * dt * p11 + self.process_noise_heading
new_p01 = p01 + dt * p11
new_p10 = p10 + dt * p11
new_p11 = p11 + self.process_noise_rot
self._P = [new_p00, new_p01, new_p10, new_p11]
def update_heading(self, measured_deg: float, noise_deg: float = 2.0) -> None:
"""Kalman update step for a heading measurement.
Parameters
----------
measured_deg:
Raw heading from PGN 127250, degrees [0, 360).
noise_deg:
Standard deviation of the sensor noise (degrees). Variance = noise²
"""
R = noise_deg * noise_deg
p00, p01, p10, p11 = self._P
# Innovation (shortest arc)
innov = _shortest_arc(measured_deg, self.heading_deg)
# H = [1, 0] (observe heading only)
# S = H P Hᵀ + R = p00 + R
S = p00 + R
if S == 0:
return
# Kalman gain K = P Hᵀ / S → [k0, k1] = [p00/S, p10/S]
k0 = p00 / S
k1 = p10 / S
# Update state
self.heading_deg = (self.heading_deg + k0 * innov) % 360.0
self.rot_dps += k1 * innov
# Update P = (I - K H) P
self._P = [
(1 - k0) * p00,
(1 - k0) * p01,
p10 - k1 * p00,
p11 - k1 * p01,
]
def update_rot(self, measured_rot_dps: float, noise_dps: float = 1.0) -> None:
"""Kalman update step for a rate-of-turn measurement.
Parameters
----------
measured_rot_dps:
Raw ROT from PGN 127251, degrees per second (signed).
noise_dps:
Standard deviation of the ROT sensor noise (deg/s).
"""
R = noise_dps * noise_dps
p00, p01, p10, p11 = self._P
# Innovation
innov = measured_rot_dps - self.rot_dps
# H = [0, 1] (observe ROT only)
# S = p11 + R
S = p11 + R
if S == 0:
return
# Kalman gain: [k0, k1] = [p01/S, p11/S]
k0 = p01 / S
k1 = p11 / S
# Update state
self.heading_deg = (self.heading_deg + k0 * innov) % 360.0
self.rot_dps += k1 * innov
# Update P
self._P = [
p00 - k0 * p01,
(1 - k1) * p01,
p10 - k0 * p11,
(1 - k1) * p11,
]
@property
def covariance(self) -> tuple[float, float, float, float]:
"""Return (p00, p01, p10, p11) — the 2×2 covariance matrix."""
return tuple(self._P) # type: ignore[return-value]
def _shortest_arc(a: float, b: float) -> float:
"""Signed shortest-arc from ``b`` to ``a`` (degrees)."""
diff = (a - b + 180.0) % 360.0 - 180.0
return diff
+71
View File
@@ -0,0 +1,71 @@
"""Hardware ID binding and activation token -- Sprint 8.
The 6-byte ESP32 MAC (read via Modbus INPUT_HWID_MAC_01/23/45) is used to
bind a project license to a specific hardware unit.
Activation token format
-----------------------
The factory generates a token by HMAC-SHA256(key=SECRET, msg=hwid_hex),
where ``hwid_hex`` is the 12-character lower-case hex representation of the
6-byte MAC. The token is the first 16 bytes (32 hex chars) of the HMAC
output.
The SECRET is a per-product deployment key embedded in the Studio binary
(not in the open-source firmware). This file ships the *verification* side
only; token generation happens offline in the factory tooling.
For development / local testing a deterministic stub secret is used
(STUB_SECRET_KEY). The production key must be injected via the environment
variable ``AR_ACTIVATION_KEY``.
"""
from __future__ import annotations
import hashlib
import hmac
import os
STUB_SECRET_KEY = b"AR-Autopilot-Dev-Key-2026"
TOKEN_BYTES = 16 # 32 hex chars
def _get_secret() -> bytes:
key = os.environ.get("AR_ACTIVATION_KEY", "").encode()
return key if key else STUB_SECRET_KEY
def hwid_from_mac_words(mac01: int, mac23: int, mac45: int) -> str:
"""Convert three 16-bit Modbus words to a 12-char lower-case hex HWID string."""
mac = bytes([
(mac01 >> 8) & 0xFF, mac01 & 0xFF,
(mac23 >> 8) & 0xFF, mac23 & 0xFF,
(mac45 >> 8) & 0xFF, mac45 & 0xFF,
])
return mac.hex()
def generate_token(hwid_hex: str) -> str:
"""Generate the activation token for a given HWID.
Should only be called by factory tooling. In the Studio this is
called only during development / bench testing with the stub key.
"""
h = hmac.new(_get_secret(), hwid_hex.lower().encode(), hashlib.sha256)
return h.hexdigest()[:TOKEN_BYTES * 2]
def verify_token(hwid_hex: str, token: str) -> bool:
"""Return True iff the token is valid for the given HWID.
Uses constant-time comparison to resist timing attacks.
"""
expected = generate_token(hwid_hex)
return hmac.compare_digest(expected.lower(), token.lower())
def format_hwid(hwid_hex: str) -> str:
"""Format a 12-char hex HWID as 'AA:BB:CC:DD:EE:FF'."""
if len(hwid_hex) != 12:
raise ValueError(f"HWID must be 12 hex chars, got {len(hwid_hex)}")
h = hwid_hex.upper()
return ":".join(h[i:i+2] for i in range(0, 12, 2))