87 lines
2.8 KiB
Python
87 lines
2.8 KiB
Python
"""IMU unit, axis, bias, and saturation audit."""
|
|
|
|
from __future__ import annotations
|
|
|
|
from dataclasses import dataclass
|
|
|
|
import numpy as np
|
|
|
|
from .contracts import ImuSeries
|
|
|
|
G = 9.80665
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class ImuAuditReport:
|
|
ok: bool
|
|
gyro_bias_rad_s: np.ndarray
|
|
static_acc_mean_m_s2: np.ndarray
|
|
static_acc_norm_m_s2: float
|
|
suggested_up_axis: int
|
|
suggested_up_sign: float
|
|
static_ratio: float
|
|
notes: tuple[str, ...] = ()
|
|
|
|
|
|
def _static_mask(gyro: np.ndarray, acc: np.ndarray) -> np.ndarray:
|
|
gyro_norm = np.linalg.norm(gyro, axis=1)
|
|
acc_norm = np.linalg.norm(acc, axis=1)
|
|
gyro_thr = max(0.02, float(np.percentile(gyro_norm, 20)) * 1.5)
|
|
acc_thr_low = 0.7 * G
|
|
acc_thr_high = 1.3 * G
|
|
return (gyro_norm < gyro_thr) & (acc_norm > acc_thr_low) & (acc_norm < acc_thr_high)
|
|
|
|
|
|
def audit_imu(imu: ImuSeries) -> ImuAuditReport:
|
|
"""Audit normalized IMU samples and estimate a static gyro bias."""
|
|
|
|
notes: list[str] = []
|
|
mask = _static_mask(imu.gyro_rad_s, imu.acc_m_s2)
|
|
static_ratio = float(np.mean(mask)) if mask.size else 0.0
|
|
if static_ratio < 0.02:
|
|
# Fall back to lowest-gyro percentile window.
|
|
gyro_norm = np.linalg.norm(imu.gyro_rad_s, axis=1)
|
|
cutoff = float(np.percentile(gyro_norm, 10))
|
|
mask = gyro_norm <= cutoff
|
|
notes.append("few gravity-consistent static samples; using lowest-gyro percentile")
|
|
static_ratio = float(np.mean(mask))
|
|
|
|
if not np.any(mask):
|
|
notes.append("no static samples found")
|
|
bias = np.zeros(3)
|
|
acc_mean = np.zeros(3)
|
|
acc_norm = 0.0
|
|
up_axis = 2
|
|
up_sign = 1.0
|
|
ok = False
|
|
else:
|
|
bias = np.mean(imu.gyro_rad_s[mask], axis=0)
|
|
acc_mean = np.mean(imu.acc_m_s2[mask], axis=0)
|
|
acc_norm = float(np.linalg.norm(acc_mean))
|
|
up_axis = int(np.argmax(np.abs(acc_mean)))
|
|
up_sign = float(np.sign(acc_mean[up_axis]) or 1.0)
|
|
if abs(acc_norm - G) > 2.5:
|
|
notes.append(
|
|
f"static |acc|={acc_norm:.3f} differs from g={G}; check units (expect m/s^2)"
|
|
)
|
|
gyro_peak = float(np.max(np.linalg.norm(imu.gyro_rad_s, axis=1)))
|
|
if gyro_peak > 20.0:
|
|
notes.append(
|
|
f"peak |gyro|={gyro_peak:.1f} rad/s looks extreme; check whether data is deg/s"
|
|
)
|
|
ok = abs(acc_norm - G) < 3.5 or static_ratio > 0.05
|
|
|
|
notes.append(
|
|
f"suggested up axis index={up_axis} sign={up_sign:+.0f} (0=x,1=y,2=z)"
|
|
)
|
|
return ImuAuditReport(
|
|
ok=ok,
|
|
gyro_bias_rad_s=np.asarray(bias, dtype=float),
|
|
static_acc_mean_m_s2=np.asarray(acc_mean, dtype=float),
|
|
static_acc_norm_m_s2=float(acc_norm),
|
|
suggested_up_axis=up_axis,
|
|
suggested_up_sign=up_sign,
|
|
static_ratio=static_ratio,
|
|
notes=tuple(notes),
|
|
)
|