use std::collections::{BTreeMap, BTreeSet};
use serde::{Deserialize, Serialize};
use super::live::LiveCfg;
use super::maritime::{MarineEpoch, MarineMonitors, MaritimeConfig};
use super::platform::PlatformCfg;
use super::score::{score_from_ratios, ScoreCfg, TrustScore};
use super::{LogEpoch, ReportedFix, Timeline};
use crate::allan::overlapping_adev;
use crate::frames::{geodetic_to_ecef, Geodetic};
use crate::gnss_sim::{Meteo, C_M_PER_S};
use crate::orbit::los_unit;
use crate::pvt::{assemble_epoch, solve_spp, AtmosModel};
use crate::rinex::RinexEphemeris;
use crate::rinex_obs::RinexObs;
use crate::security::{min_detectable_offset_ns, SPOOF_DETECT_K};
use crate::spoof_monitors::parity_raim_test;
const PRED_MAX: usize = 60;
const PRED_MIN: usize = 10;
const RESET_TOL_M: f64 = 100.0;
const MERGE_TOL_S: f64 = 1e-3;
const RUN_GAP_FACTOR: f64 = 1.5;
#[derive(Clone, Debug, Serialize, Deserialize, PartialEq)]
#[serde(deny_unknown_fields, default)]
pub struct MonitorConfig {
pub calibration_s: f64,
pub cn0_drop_db: f64,
pub cn0_min_sats: usize,
pub agc_k_sigma: f64,
pub jam_ind_threshold: f64,
pub sats_lost: usize,
pub position_jump_m: f64,
pub mask_deg: f64,
pub raim_sigma_m: f64,
pub raim_pfa: f64,
pub clock_monitor: bool,
#[serde(skip_deserializing, skip_serializing_if = "PlatformCfg::is_static")]
pub platform: PlatformCfg,
#[serde(skip_deserializing, skip_serializing_if = "MaritimeConfig::is_default")]
pub maritime: MaritimeConfig,
#[serde(skip_deserializing, skip_serializing_if = "ScoreCfg::is_default")]
pub score: ScoreCfg,
#[serde(skip_deserializing, skip_serializing_if = "LiveCfg::is_default")]
pub live: LiveCfg,
}
impl Default for MonitorConfig {
fn default() -> Self {
Self {
calibration_s: 60.0,
cn0_drop_db: 6.0,
cn0_min_sats: 4,
agc_k_sigma: 5.0,
jam_ind_threshold: 80.0,
sats_lost: 4,
position_jump_m: 50.0,
mask_deg: 10.0,
raim_sigma_m: 3.0,
raim_pfa: 1e-5,
clock_monitor: true,
platform: PlatformCfg::default(),
maritime: MaritimeConfig::default(),
score: ScoreCfg::default(),
live: LiveCfg::default(),
}
}
}
impl MonitorConfig {
pub fn validate(&self) -> Result<(), String> {
self.platform.validate()?;
self.maritime.validate()?;
self.score.validate()?;
self.live.validate()?;
let pos = |name: &str, v: f64| -> Result<(), String> {
if v.is_finite() && v > 0.0 {
Ok(())
} else {
Err(format!(
"monitor config: {name} must be finite and > 0 (got {v})"
))
}
};
pos("calibration_s", self.calibration_s)?;
pos("cn0_drop_db", self.cn0_drop_db)?;
pos("agc_k_sigma", self.agc_k_sigma)?;
pos("position_jump_m", self.position_jump_m)?;
pos("raim_sigma_m", self.raim_sigma_m)?;
if !self.jam_ind_threshold.is_finite() {
return Err("monitor config: jam_ind_threshold must be finite".into());
}
if !(self.mask_deg.is_finite() && (-90.0..90.0).contains(&self.mask_deg)) {
return Err(format!(
"monitor config: mask_deg must be in [-90, 90) (got {})",
self.mask_deg
));
}
if !(self.raim_pfa > 0.0 && self.raim_pfa < 1.0) {
return Err(format!(
"monitor config: raim_pfa must be in (0, 1) (got {})",
self.raim_pfa
));
}
if self.cn0_min_sats == 0 {
return Err("monitor config: cn0_min_sats must be at least 1".into());
}
if self.sats_lost == 0 {
return Err("monitor config: sats_lost must be at least 1".into());
}
Ok(())
}
}
#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash, Serialize, Deserialize, PartialOrd, Ord)]
#[serde(rename_all = "kebab-case")]
pub enum Monitor {
Cn0Drop,
Agc,
JamInd,
LossOfLock,
PositionJump,
Raim,
Clock,
SolveFailure,
Kinematic,
HeadingCourse,
SpeedLog,
SeaLevel,
Cn0Spread,
Cn0Rise,
TimeConsistency,
Osnma,
}
#[derive(Clone, Copy, Debug, PartialEq, Eq, Serialize, Deserialize)]
#[serde(rename_all = "kebab-case")]
pub enum TrustState {
Calibrating,
Nominal,
Degraded,
Untrusted,
}
#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)]
pub struct EpochTrust {
pub t_s: f64,
pub n_sats: usize,
pub cn0_mean_dbhz: Option<f64>,
pub cn0_drop_db: Option<f64>,
pub agc: Option<f64>,
pub agc_z: Option<f64>,
pub jam_ind: Option<f64>,
pub position_offset_m: Option<f64>,
pub raim_stat: Option<f64>,
pub raim_thr: Option<f64>,
pub clock_innov_ns: Option<f64>,
pub clock_bound_ns: Option<f64>,
#[serde(default, skip_serializing_if = "Option::is_none")]
pub marine: Option<MarineEpoch>,
#[serde(default, skip_serializing_if = "Option::is_none")]
pub score: Option<TrustScore>,
pub alarms: Vec<Monitor>,
pub state: TrustState,
}
#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)]
pub struct AlarmRun {
pub monitor: Monitor,
pub start_s: f64,
pub end_s: f64,
pub epochs: usize,
}
#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)]
pub struct Baseline {
pub calibration_epochs: usize,
pub cn0_median_dbhz: Option<f64>,
pub tracked_median: Option<f64>,
pub agc_mean: Option<f64>,
pub agc_sd: Option<f64>,
pub jam_ind_median: Option<f64>,
pub clock_bound_ns: Option<f64>,
}
#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)]
pub struct TrustResult {
pub monitors_run: Vec<Monitor>,
pub baseline: Baseline,
pub epochs: Vec<EpochTrust>,
pub runs: Vec<AlarmRun>,
pub first_alarm_s: Option<f64>,
}
pub struct EngineFixInput<'a> {
pub obs: &'a RinexObs,
pub ephs: &'a [RinexEphemeris],
}
#[derive(Clone, Debug, Default)]
struct EngineEpoch {
t_s: f64,
clock_s: Option<f64>,
raim_stat: Option<f64>,
raim_thr: Option<f64>,
raim_alarm: bool,
solve_failure: bool,
clock_innov_s: Option<f64>,
clock_decided: bool,
clock_alarm: bool,
}
fn median(v: &[f64]) -> Option<f64> {
if v.is_empty() {
return None;
}
let mut s = v.to_vec();
s.sort_by(f64::total_cmp);
let n = s.len();
Some(if n % 2 == 1 {
s[n / 2]
} else {
0.5 * (s[n / 2 - 1] + s[n / 2])
})
}
fn mean(v: &[f64]) -> Option<f64> {
if v.is_empty() {
None
} else {
Some(v.iter().sum::<f64>() / v.len() as f64)
}
}
fn line_fit(ts: &[f64], ys: &[f64]) -> (f64, f64, f64) {
let n = ts.len() as f64;
let t0 = ts.iter().sum::<f64>() / n;
let y0 = ys.iter().sum::<f64>() / n;
let (mut sxx, mut sxy) = (0.0, 0.0);
for (t, y) in ts.iter().zip(ys) {
sxx += (t - t0) * (t - t0);
sxy += (t - t0) * (y - y0);
}
let b = if sxx > 0.0 { sxy / sxx } else { 0.0 };
(y0, b, t0)
}
fn line_predict(ts: &[f64], ys: &[f64], t: f64) -> f64 {
let (a, b, t0) = line_fit(ts, ys);
a + b * (t - t0)
}
fn horizontal_offset_m(reference: (f64, f64, f64), fix: &ReportedFix) -> f64 {
let (lat0, lon0, h0) = reference;
let r = geodetic_to_ecef(Geodetic {
lat_rad: lat0.to_radians(),
lon_rad: lon0.to_radians(),
alt_m: h0,
});
let p = geodetic_to_ecef(Geodetic {
lat_rad: fix.lat_deg.to_radians(),
lon_rad: fix.lon_deg.to_radians(),
alt_m: h0,
});
let d = [p[0] - r[0], p[1] - r[1], p[2] - r[2]];
let (sl, cl) = lat0.to_radians().sin_cos();
let (so, co) = lon0.to_radians().sin_cos();
let east = -so * d[0] + co * d[1];
let north = -sl * co * d[0] - sl * so * d[1] + cl * d[2];
east.hypot(north)
}
fn tracks_satellites(e: &LogEpoch) -> bool {
!e.cn0.is_empty() || (e.agc.is_none() && e.jam_ind.is_none() && e.fix.is_none())
}
fn distinct_sats(e: &LogEpoch) -> usize {
e.cn0
.iter()
.map(|c| c.sat.as_str())
.collect::<BTreeSet<_>>()
.len()
}
fn engine_pass(
input: &EngineFixInput<'_>,
cfg: &MonitorConfig,
) -> Result<(Vec<EngineEpoch>, Option<f64>), String> {
let obs = input.obs;
let apriori = obs.header.approx_xyz.ok_or_else(|| {
"engine fix: the RINEX observation header has no APPROX POSITION XYZ to start the \
solve from"
.to_string()
})?;
let Some(first) = obs.epochs.first() else {
return Ok((Vec::new(), None));
};
let t_first = first.time.seconds_from_gps_epoch();
let atmos = AtmosModel {
iono: Default::default(),
meteo: Meteo::default(),
};
let mut epochs = Vec::with_capacity(obs.epochs.len());
let mut reset_offset = 0.0_f64;
let mut prev_raw: Option<f64> = None;
for idx in 0..obs.epochs.len() {
let t_s = obs.epochs[idx].time.seconds_from_gps_epoch() - t_first;
let meas: Vec<_> =
assemble_epoch(obs, idx, input.ephs, apriori, &atmos, cfg.mask_deg, true)
.into_iter()
.map(|(_, m)| m)
.collect();
let n = meas.len();
let mut out = EngineEpoch {
t_s,
..Default::default()
};
if n >= 4 {
match solve_spp(&meas, apriori) {
Some(fix) => {
let raw = fix.clock_bias_m / C_M_PER_S;
if let Some(p) = prev_raw {
let d = raw - p;
let k = (d / 1e-3).round();
if k != 0.0 && ((d - k * 1e-3) * C_M_PER_S).abs() < RESET_TOL_M {
reset_offset -= k * 1e-3;
}
}
prev_raw = Some(raw);
out.clock_s = Some(raw + reset_offset);
if n >= 5 {
let mut rows = Vec::with_capacity(n);
let mut resid = Vec::with_capacity(n);
for m in &meas {
let Some(u) = los_unit(fix.ecef, m.sat_ecef) else {
continue;
};
let dx = m.sat_ecef[0] - fix.ecef[0];
let dy = m.sat_ecef[1] - fix.ecef[1];
let dz = m.sat_ecef[2] - fix.ecef[2];
let pred = (dx * dx + dy * dy + dz * dz).sqrt() + fix.clock_bias_m
- m.sat_clock_m
+ m.iono_m
+ m.tropo_m;
rows.push([u[0], u[1], u[2], 1.0]);
resid.push(m.pseudorange_m - pred);
}
match parity_raim_test(&rows, &resid, cfg.raim_sigma_m, cfg.raim_pfa) {
Some(r) => {
out.raim_stat = Some(r.statistic);
out.raim_thr = Some(r.threshold);
out.raim_alarm = r.alert;
}
None => out.solve_failure = true,
}
}
}
None => {
if n >= 5 {
out.solve_failure = true;
}
}
}
}
epochs.push(out);
}
if !cfg.clock_monitor {
return Ok((epochs, None));
}
let cal: Vec<(f64, f64)> = epochs
.iter()
.filter(|e| e.t_s < cfg.calibration_s)
.filter_map(|e| e.clock_s.map(|c| (e.t_s, c)))
.collect();
let (ts, ys): (Vec<f64>, Vec<f64>) = cal.iter().copied().unzip();
let tau0 = median(&ts.windows(2).map(|w| w[1] - w[0]).collect::<Vec<_>>())
.filter(|d| d.is_finite() && *d > 0.0)
.unwrap_or(1.0);
let (a, b, t0) = if ts.len() >= 2 {
line_fit(&ts, &ys)
} else {
(0.0, 0.0, 0.0)
};
let r = if ts.len() > 2 {
ts.iter()
.zip(&ys)
.map(|(t, y)| (y - (a + b * (t - t0))).powi(2))
.sum::<f64>()
/ (ts.len() as f64 - 2.0)
} else {
f64::NAN
};
let adev1 = if ys.len() > 3 {
overlapping_adev(&ys, tau0, 1)
} else {
f64::NAN
};
let adev10 = if ys.len() > 21 {
overlapping_adev(&ys, tau0, 10)
} else {
f64::NAN
};
let q_wf = adev1 * adev1 * tau0;
let rw_level = adev10 / (10.0_f64 / 3.0).sqrt();
let q_rw = 3.0 * rw_level * rw_level / tau0;
let bound_ns = min_detectable_offset_ns(q_wf, q_rw, r, tau0, 1.0, SPOOF_DETECT_K);
if !(bound_ns.is_finite() && ts.len() >= PRED_MIN) {
return Ok((epochs, None));
}
let mut history = cal;
for e in epochs.iter_mut() {
if e.t_s < cfg.calibration_s {
continue;
}
if let Some(c) = e.clock_s {
if history.len() >= PRED_MIN {
let tail = &history[history.len().saturating_sub(PRED_MAX)..];
let (hts, hys): (Vec<f64>, Vec<f64>) = tail.iter().copied().unzip();
let innov = c - line_predict(&hts, &hys, e.t_s);
e.clock_innov_s = Some(innov);
e.clock_decided = true;
e.clock_alarm = innov.abs() * 1e9 > bound_ns;
}
if !(e.clock_alarm || e.raim_alarm || e.solve_failure) {
history.push((e.t_s, c));
}
}
}
Ok((epochs, Some(bound_ns)))
}
struct Slot {
t_s: f64,
tl: Option<usize>,
en: Option<usize>,
}
fn merge_slots(tl: &Timeline, en: &[EngineEpoch]) -> Vec<Slot> {
let mut a: Vec<usize> = (0..tl.epochs.len()).collect();
a.sort_by(|&i, &j| tl.epochs[i].t_s.total_cmp(&tl.epochs[j].t_s));
let mut b: Vec<usize> = (0..en.len()).collect();
b.sort_by(|&i, &j| en[i].t_s.total_cmp(&en[j].t_s));
let mut out = Vec::with_capacity(a.len().max(b.len()));
let (mut i, mut j) = (0, 0);
loop {
match (a.get(i), b.get(j)) {
(Some(&ia), Some(&jb)) => {
let (ta, tb) = (tl.epochs[ia].t_s, en[jb].t_s);
if (ta - tb).abs() <= MERGE_TOL_S {
out.push(Slot {
t_s: ta,
tl: Some(ia),
en: Some(jb),
});
i += 1;
j += 1;
} else if ta < tb {
out.push(Slot {
t_s: ta,
tl: Some(ia),
en: None,
});
i += 1;
} else {
out.push(Slot {
t_s: tb,
tl: None,
en: Some(jb),
});
j += 1;
}
}
(Some(&ia), None) => {
out.push(Slot {
t_s: tl.epochs[ia].t_s,
tl: Some(ia),
en: None,
});
i += 1;
}
(None, Some(&jb)) => {
out.push(Slot {
t_s: en[jb].t_s,
tl: None,
en: Some(jb),
});
j += 1;
}
(None, None) => break,
}
}
out
}
pub fn run_monitors(
tl: &Timeline,
engine: Option<EngineFixInput<'_>>,
cfg: &MonitorConfig,
) -> Result<TrustResult, String> {
cfg.validate()?;
if tl.epochs.iter().any(|e| !e.t_s.is_finite()) {
return Err("receiver timeline has a non-finite epoch time".into());
}
let (en, clock_bound_ns) = match &engine {
Some(input) => engine_pass(input, cfg)?,
None => (Vec::new(), None),
};
let slots = merge_slots(tl, &en);
let cal_s = cfg.calibration_s;
let calibration_epochs = slots.iter().filter(|s| s.t_s < cal_s).count();
if calibration_epochs < 3 {
let span = slots.last().map(|s| s.t_s).unwrap_or(0.0);
return Err(format!(
"the log is shorter than the calibration window: {calibration_epochs} epoch(s) \
before {cal_s} s (log spans {span:.1} s); at least 3 are needed to form the baseline"
));
}
let is_rewound = |s: &Slot| {
cfg.platform.is_vessel()
&& s.tl
.is_some_and(|i| tl.epochs[i].marine.as_ref().is_some_and(|m| m.time_rewound))
};
let cal_tl: Vec<&LogEpoch> = slots
.iter()
.filter(|s| s.t_s < cal_s && !is_rewound(s))
.filter_map(|s| s.tl.map(|i| &tl.epochs[i]))
.collect();
let mut key_samples: BTreeMap<(String, String), Vec<f64>> = BTreeMap::new();
let mut all_cn0 = Vec::new();
for e in &cal_tl {
for c in &e.cn0 {
key_samples
.entry((c.sat.clone(), c.band.clone()))
.or_default()
.push(c.cn0_dbhz);
all_cn0.push(c.cn0_dbhz);
}
}
let key_median: BTreeMap<(String, String), f64> = key_samples
.iter()
.filter_map(|(k, v)| median(v).map(|m| (k.clone(), m)))
.collect();
let tracked: Vec<f64> = cal_tl
.iter()
.filter(|e| tracks_satellites(e))
.map(|e| distinct_sats(e) as f64)
.collect();
let agc_cal: Vec<f64> = cal_tl.iter().filter_map(|e| e.agc).collect();
let agc_mean = mean(&agc_cal);
let agc_sd = agc_mean.map(|m| {
let sd = if agc_cal.len() > 1 {
(agc_cal.iter().map(|x| (x - m).powi(2)).sum::<f64>() / (agc_cal.len() - 1) as f64)
.sqrt()
} else {
0.0
};
sd.max(1.0).max(0.01 * m.abs())
});
let jam_cal: Vec<f64> = cal_tl.iter().filter_map(|e| e.jam_ind).collect();
let fixes_cal: Vec<ReportedFix> = cal_tl.iter().filter_map(|e| e.fix).collect();
let fix_ref = fixes_cal.first().map(|f0| {
let n = fixes_cal.len() as f64;
let lat = fixes_cal.iter().map(|f| f.lat_deg).sum::<f64>() / n;
let dlon = fixes_cal
.iter()
.map(|f| (f.lon_deg - f0.lon_deg + 540.0).rem_euclid(360.0) - 180.0)
.sum::<f64>()
/ n;
let h = fixes_cal.iter().map(|f| f.height_m).sum::<f64>() / n;
(lat, f0.lon_deg + dlon, h)
});
let baseline = Baseline {
calibration_epochs,
cn0_median_dbhz: median(&all_cn0),
tracked_median: median(&tracked),
agc_mean,
agc_sd,
jam_ind_median: median(&jam_cal),
clock_bound_ns,
};
let has_cn0 = tl.epochs.iter().any(|e| !e.cn0.is_empty());
let has_jam = tl.epochs.iter().any(|e| e.jam_ind.is_some());
let mut run_set = BTreeSet::new();
if has_cn0 && !key_median.is_empty() {
run_set.insert(Monitor::Cn0Drop);
}
if has_cn0 && baseline.tracked_median.is_some() {
run_set.insert(Monitor::LossOfLock);
}
if agc_mean.is_some() {
run_set.insert(Monitor::Agc);
}
if has_jam {
run_set.insert(Monitor::JamInd);
}
if fix_ref.is_some() && !cfg.platform.is_vessel() {
run_set.insert(Monitor::PositionJump);
}
if engine.is_some() {
run_set.insert(Monitor::Raim);
run_set.insert(Monitor::SolveFailure);
if clock_bound_ns.is_some() {
run_set.insert(Monitor::Clock);
}
}
let mut marine_of: Vec<Option<MarineEpoch>> = vec![None; slots.len()];
if let Some(limits) = cfg.platform.limits() {
if limits.heading_sensor
&& !tl
.epochs
.iter()
.any(|e| e.marine.as_ref().is_some_and(|m| m.heading_deg.is_some()))
{
return Err(
"platform declares heading_sensor = true but the log carries no HDT, THS or VHW \
heading"
.into(),
);
}
let mut mm = MarineMonitors::new(&cfg.maritime, limits, cal_s);
for (slot, out) in slots.iter().zip(marine_of.iter_mut()) {
if let Some(i) = slot.tl {
let m = mm.push(&tl.epochs[i]);
if slot.t_s >= cal_s {
for (mon, _) in &m.ratios {
run_set.insert(*mon);
}
}
*out = Some(m);
}
}
}
let mut epochs = Vec::with_capacity(slots.len());
let mut decided: Vec<BTreeSet<Monitor>> = Vec::with_capacity(slots.len());
let mut held: BTreeMap<Monitor, (f64, f64)> = BTreeMap::new();
for (slot_index, s) in slots.iter().enumerate() {
let post = s.t_s >= cal_s || is_rewound(s);
let mut alarms = BTreeSet::new();
let mut dec = BTreeSet::new();
let mut lost_ratio: Option<f64> = None;
let mut et = EpochTrust {
t_s: s.t_s,
n_sats: 0,
cn0_mean_dbhz: None,
cn0_drop_db: None,
agc: None,
agc_z: None,
jam_ind: None,
position_offset_m: None,
raim_stat: None,
raim_thr: None,
clock_innov_ns: None,
clock_bound_ns: None,
marine: None,
score: None,
alarms: Vec::new(),
state: TrustState::Calibrating,
};
let mut decide = |m: Monitor, alarm: bool, alarms: &mut BTreeSet<Monitor>| {
if post && run_set.contains(&m) {
dec.insert(m);
if alarm {
alarms.insert(m);
}
}
};
if let Some(i) = s.tl {
let e = &tl.epochs[i];
et.n_sats = distinct_sats(e);
et.cn0_mean_dbhz = mean(&e.cn0.iter().map(|c| c.cn0_dbhz).collect::<Vec<_>>());
let diffs: Vec<f64> = e
.cn0
.iter()
.filter_map(|c| {
key_median
.get(&(c.sat.clone(), c.band.clone()))
.map(|b| b - c.cn0_dbhz)
})
.collect();
if diffs.len() >= cfg.cn0_min_sats {
let d = mean(&diffs).unwrap_or(0.0);
et.cn0_drop_db = Some(d);
decide(Monitor::Cn0Drop, d >= cfg.cn0_drop_db, &mut alarms);
}
if let Some(med) = baseline.tracked_median {
if tracks_satellites(e) {
let lost = med - et.n_sats as f64;
lost_ratio = Some(lost / cfg.sats_lost as f64);
decide(
Monitor::LossOfLock,
lost >= cfg.sats_lost as f64,
&mut alarms,
);
}
}
et.agc = e.agc;
if let (Some(a), Some(m), Some(sd)) = (e.agc, agc_mean, agc_sd) {
let z = (a - m) / sd;
et.agc_z = Some(z);
decide(Monitor::Agc, z.abs() > cfg.agc_k_sigma, &mut alarms);
}
et.jam_ind = e.jam_ind;
if let Some(j) = e.jam_ind {
decide(Monitor::JamInd, j > cfg.jam_ind_threshold, &mut alarms);
}
if let (Some(f), Some(r), true) =
(e.fix, fix_ref, run_set.contains(&Monitor::PositionJump))
{
let off = horizontal_offset_m(r, &f);
et.position_offset_m = Some(off);
decide(
Monitor::PositionJump,
off > cfg.position_jump_m,
&mut alarms,
);
}
}
if let Some(j) = s.en {
let e = &en[j];
et.raim_stat = e.raim_stat;
et.raim_thr = e.raim_thr;
if e.raim_stat.is_some() {
decide(Monitor::Raim, e.raim_alarm, &mut alarms);
}
decide(Monitor::SolveFailure, e.solve_failure, &mut alarms);
et.clock_innov_ns = e.clock_innov_s.map(|x| x * 1e9);
if e.clock_decided {
et.clock_bound_ns = clock_bound_ns;
decide(Monitor::Clock, e.clock_alarm, &mut alarms);
}
}
if let Some(Some(me)) = marine_of.get(slot_index) {
if post {
for (mon, r) in &me.ratios {
decide(*mon, *r >= 1.0, &mut alarms);
}
}
et.marine = Some(me.clone());
}
if post {
let fix_alarm = alarms.iter().any(|m| {
matches!(
m,
Monitor::Raim
| Monitor::Clock
| Monitor::SolveFailure
| Monitor::PositionJump
| Monitor::Kinematic
| Monitor::HeadingCourse
| Monitor::SpeedLog
| Monitor::SeaLevel
| Monitor::TimeConsistency
| Monitor::Osnma
)
});
if cfg.platform.is_vessel() {
let mut ratios = core_ratios(&et, cfg, lost_ratio, &alarms, &run_set);
if let Some(me) = &et.marine {
ratios.extend(me.ratios.iter().copied());
}
for (m, r) in &ratios {
held.insert(*m, (et.t_s, *r));
}
for (m, (t, r)) in &held {
if !ratios.contains_key(m) && et.t_s - t <= cfg.score.evidence_hold_s {
ratios.insert(*m, *r);
}
}
for (m, r) in &ratios {
dec.insert(*m);
let strict = matches!(m, Monitor::Agc | Monitor::JamInd);
if *r > 1.0 || (*r == 1.0 && !strict) {
alarms.insert(*m);
}
}
let sc = score_from_ratios(&ratios, &cfg.score);
et.state = sc.band;
et.score = Some(sc);
} else {
let severe_cn0 = run_set.contains(&Monitor::Cn0Drop)
&& et.cn0_drop_db.is_some_and(|d| d >= 2.0 * cfg.cn0_drop_db);
et.state = if fix_alarm || severe_cn0 {
TrustState::Untrusted
} else if !alarms.is_empty() {
TrustState::Degraded
} else {
TrustState::Nominal
};
}
}
et.alarms = alarms.into_iter().collect();
epochs.push(et);
decided.push(dec);
}
let runs = alarm_runs(&run_set, &epochs, &decided);
let first_alarm_s = epochs
.iter()
.find(|e| e.t_s >= cal_s && !e.alarms.is_empty())
.map(|e| e.t_s);
Ok(TrustResult {
monitors_run: run_set.into_iter().collect(),
baseline,
epochs,
runs,
first_alarm_s,
})
}
fn core_ratios(
et: &EpochTrust,
cfg: &MonitorConfig,
lost_ratio: Option<f64>,
alarms: &BTreeSet<Monitor>,
run_set: &BTreeSet<Monitor>,
) -> BTreeMap<Monitor, f64> {
let mut r = BTreeMap::new();
let mut put = |m: Monitor, v: Option<f64>| {
if let (true, Some(v)) = (run_set.contains(&m), v) {
r.insert(m, v);
}
};
put(
Monitor::Cn0Drop,
et.cn0_drop_db.map(|d| d / cfg.cn0_drop_db),
);
put(Monitor::LossOfLock, lost_ratio);
put(Monitor::Agc, et.agc_z.map(|z| z.abs() / cfg.agc_k_sigma));
put(
Monitor::JamInd,
et.jam_ind.map(|j| j / cfg.jam_ind_threshold),
);
put(
Monitor::Raim,
match (et.raim_stat, et.raim_thr) {
(Some(s), Some(t)) if t > 0.0 => Some(s / t),
_ => None,
},
);
put(
Monitor::Clock,
match (et.clock_innov_ns, et.clock_bound_ns) {
(Some(i), Some(b)) if b > 0.0 => Some(i.abs() / b),
_ => None,
},
);
put(
Monitor::SolveFailure,
alarms.contains(&Monitor::SolveFailure).then_some(1.5),
);
r
}
fn alarm_runs(
run_set: &BTreeSet<Monitor>,
epochs: &[EpochTrust],
decided: &[BTreeSet<Monitor>],
) -> Vec<AlarmRun> {
let mut runs = Vec::new();
for &m in run_set {
let pts: Vec<(f64, bool)> = epochs
.iter()
.zip(decided)
.filter(|(_, d)| d.contains(&m))
.map(|(e, _)| (e.t_s, e.alarms.contains(&m)))
.collect();
let spacing = median(&pts.windows(2).map(|w| w[1].0 - w[0].0).collect::<Vec<_>>());
let gap_limit = spacing.map_or(f64::INFINITY, |s| RUN_GAP_FACTOR * s);
let mut cur: Option<AlarmRun> = None;
let mut prev_t: Option<f64> = None;
for &(t, alarm) in &pts {
let contiguous = prev_t.is_some_and(|p| t - p <= gap_limit);
if alarm {
match cur.as_mut() {
Some(r) if contiguous => {
r.end_s = t;
r.epochs += 1;
}
_ => {
if let Some(r) = cur.take() {
runs.push(r);
}
cur = Some(AlarmRun {
monitor: m,
start_s: t,
end_s: t,
epochs: 1,
});
}
}
} else if let Some(r) = cur.take() {
runs.push(r);
}
prev_t = Some(t);
}
if let Some(r) = cur {
runs.push(r);
}
}
runs.sort_by(|a, b| {
a.start_s
.total_cmp(&b.start_s)
.then(a.monitor.cmp(&b.monitor))
});
runs
}
#[cfg(test)]
mod tests {
use super::super::SatCn0;
use super::*;
const LAT: f64 = 69.29;
const LON: f64 = 16.03;
fn epoch(t: f64, n: usize, drop: impl Fn(usize) -> f64) -> LogEpoch {
LogEpoch {
t_s: t,
cn0: (0..n)
.map(|k| SatCn0 {
sat: format!("G{:02}", k + 1),
band: "L1".into(),
cn0_dbhz: 40.0 + (k % 5) as f64 - drop(k),
})
.collect(),
..Default::default()
}
}
fn timeline(epochs: Vec<LogEpoch>) -> Timeline {
let mut obs = BTreeSet::new();
for e in &epochs {
if !e.cn0.is_empty() {
obs.insert("cn0");
}
if e.agc.is_some() {
obs.insert("agc");
}
if e.jam_ind.is_some() {
obs.insert("jam_ind");
}
if e.fix.is_some() {
obs.insert("fix");
}
}
Timeline {
epochs,
start_label: None,
observables: obs.into_iter().map(String::from).collect(),
skipped_records: 0,
}
}
fn fix(north_m: f64) -> ReportedFix {
ReportedFix {
lat_deg: LAT + north_m / 111_200.0,
lon_deg: LON,
height_m: 50.0,
n_used: Some(8),
}
}
fn clean(t: usize) -> LogEpoch {
let mut e = epoch(t as f64, 8, |k| 0.3 * (((t + k) % 3) as f64 - 1.0));
e.agc = Some(3000.0 + ((t % 7) as f64 - 3.0) * 2.0);
e.jam_ind = Some(10.0 + (t % 4) as f64);
e.fix = Some(fix(((t % 5) as f64 - 2.0) * 0.8));
e
}
fn state_at(r: &TrustResult, t: f64) -> TrustState {
r.epochs.iter().find(|e| e.t_s == t).unwrap().state
}
fn runs_of(r: &TrustResult, m: Monitor) -> Vec<&AlarmRun> {
r.runs.iter().filter(|x| x.monitor == m).collect()
}
#[test]
fn clean_log_raises_no_alarm() {
let tl = timeline((0..240).map(clean).collect());
let r = run_monitors(&tl, None, &MonitorConfig::default()).unwrap();
assert!(r.runs.is_empty(), "{:?}", r.runs);
assert_eq!(r.first_alarm_s, None);
assert_eq!(r.baseline.calibration_epochs, 60);
assert_eq!(r.baseline.tracked_median, Some(8.0));
for e in &r.epochs {
let want = if e.t_s < 60.0 {
TrustState::Calibrating
} else {
TrustState::Nominal
};
assert_eq!(e.state, want, "t = {}", e.t_s);
assert!(e.alarms.is_empty());
}
assert_eq!(
r.monitors_run,
vec![
Monitor::Cn0Drop,
Monitor::Agc,
Monitor::JamInd,
Monitor::LossOfLock,
Monitor::PositionJump
]
);
}
#[test]
fn cn0_drop_degrades_then_untrusts() {
let tl = timeline(
(0..240)
.map(|t| {
let d = if t >= 180 {
20.0
} else if t >= 120 {
10.0
} else {
0.0
};
epoch(t as f64, 8, |k| if k < 6 { d } else { 0.0 })
})
.collect(),
);
let r = run_monitors(&tl, None, &MonitorConfig::default()).unwrap();
let runs = runs_of(&r, Monitor::Cn0Drop);
assert_eq!(runs.len(), 1, "{runs:?}");
assert_eq!(runs[0].start_s, 120.0);
assert_eq!(runs[0].end_s, 239.0);
assert_eq!(runs[0].epochs, 120);
assert_eq!(r.first_alarm_s, Some(120.0));
assert_eq!(state_at(&r, 119.0), TrustState::Nominal);
assert_eq!(state_at(&r, 150.0), TrustState::Degraded);
assert_eq!(state_at(&r, 200.0), TrustState::Untrusted);
let e = r.epochs.iter().find(|e| e.t_s == 150.0).unwrap();
assert!((e.cn0_drop_db.unwrap() - 7.5).abs() < 1e-9);
}
#[test]
fn agc_step_raises_agc() {
let tl = timeline(
(0..240)
.map(|t| {
let mut e = clean(t);
if t >= 150 {
e.agc = Some(2400.0);
}
e
})
.collect(),
);
let r = run_monitors(&tl, None, &MonitorConfig::default()).unwrap();
let runs = runs_of(&r, Monitor::Agc);
assert_eq!(runs.len(), 1);
assert_eq!(runs[0].start_s, 150.0);
assert_eq!(r.first_alarm_s, Some(150.0));
assert_eq!(state_at(&r, 160.0), TrustState::Degraded);
assert!((r.baseline.agc_sd.unwrap() - 30.0).abs() < 1.0);
}
#[test]
fn jam_indicator_raises_jam_ind() {
let tl = timeline(
(0..240)
.map(|t| {
let mut e = clean(t);
if (130..170).contains(&t) {
e.jam_ind = Some(200.0);
}
e
})
.collect(),
);
let r = run_monitors(&tl, None, &MonitorConfig::default()).unwrap();
let runs = runs_of(&r, Monitor::JamInd);
assert_eq!(runs.len(), 1);
assert_eq!(
(runs[0].start_s, runs[0].end_s, runs[0].epochs),
(130.0, 169.0, 40)
);
assert_eq!(state_at(&r, 140.0), TrustState::Degraded);
assert_eq!(state_at(&r, 200.0), TrustState::Nominal);
}
#[test]
fn lost_satellites_raise_loss_of_lock() {
let tl = timeline(
(0..240)
.map(|t| epoch(t as f64, if t >= 100 { 4 } else { 10 }, |_| 0.0))
.collect(),
);
let r = run_monitors(&tl, None, &MonitorConfig::default()).unwrap();
assert_eq!(r.baseline.tracked_median, Some(10.0));
let runs = runs_of(&r, Monitor::LossOfLock);
assert_eq!(runs.len(), 1);
assert_eq!(runs[0].start_s, 100.0);
assert!(runs_of(&r, Monitor::Cn0Drop).is_empty());
assert_eq!(state_at(&r, 120.0), TrustState::Degraded);
}
#[test]
fn position_jump_is_untrusted() {
let tl = timeline(
(0..240)
.map(|t| {
let mut e = clean(t);
if t >= 140 {
e.fix = Some(fix(500.0));
}
e
})
.collect(),
);
let r = run_monitors(&tl, None, &MonitorConfig::default()).unwrap();
let runs = runs_of(&r, Monitor::PositionJump);
assert_eq!(runs.len(), 1);
assert_eq!(runs[0].start_s, 140.0);
assert_eq!(state_at(&r, 150.0), TrustState::Untrusted);
let off = r
.epochs
.iter()
.find(|e| e.t_s == 150.0)
.unwrap()
.position_offset_m
.unwrap();
assert!((off - 500.0).abs() < 5.0, "{off}");
}
#[test]
fn short_log_is_an_error() {
let tl = timeline((0..2).map(clean).collect());
let err = run_monitors(&tl, None, &MonitorConfig::default()).unwrap_err();
assert!(err.contains("shorter than the calibration window"), "{err}");
}
#[test]
fn monitors_run_lists_only_present_observables() {
let tl = timeline((0..120).map(|t| epoch(t as f64, 8, |_| 0.0)).collect());
let r = run_monitors(&tl, None, &MonitorConfig::default()).unwrap();
assert_eq!(r.monitors_run, vec![Monitor::Cn0Drop, Monitor::LossOfLock]);
assert_eq!(r.baseline.agc_mean, None);
assert_eq!(r.baseline.clock_bound_ns, None);
assert!(r.epochs.iter().all(|e| e.agc_z.is_none()));
}
#[test]
fn missing_epoch_splits_a_run() {
let tl = timeline(
(0..240)
.filter(|&t| t != 150)
.map(|t| {
let mut e = clean(t);
if t >= 140 {
e.jam_ind = Some(200.0);
}
e
})
.collect(),
);
let r = run_monitors(&tl, None, &MonitorConfig::default()).unwrap();
let runs = runs_of(&r, Monitor::JamInd);
assert_eq!(runs.len(), 2);
assert_eq!((runs[0].start_s, runs[0].end_s), (140.0, 149.0));
assert_eq!(runs[1].start_s, 151.0);
}
#[test]
fn config_round_trips_and_rejects_unknown_fields() {
let cfg: MonitorConfig = serde_json::from_str(r#"{"cn0_drop_db": 8.0}"#).unwrap();
assert_eq!(cfg.cn0_drop_db, 8.0);
assert_eq!(cfg.calibration_s, 60.0);
assert!(serde_json::from_str::<MonitorConfig>(r#"{"bogus": 1}"#).is_err());
let bad = MonitorConfig {
raim_pfa: 0.0,
..Default::default()
};
assert!(bad.validate().is_err());
}
#[test]
fn a_declared_vessel_does_not_run_the_static_position_jump_monitor() {
let moving = |t: usize| {
let mut e = clean(t);
e.fix = Some(fix(t as f64 * 8.0));
e
};
let tl = timeline((0..240).map(moving).collect());
let st = run_monitors(&tl, None, &MonitorConfig::default()).unwrap();
assert!(st.monitors_run.contains(&Monitor::PositionJump));
assert!(!runs_of(&st, Monitor::PositionJump).is_empty());
let cfg = MonitorConfig {
platform: PlatformCfg {
kind: super::super::platform::PlatformKind::Vessel,
..Default::default()
},
..MonitorConfig::default()
};
let v = run_monitors(&tl, None, &cfg).unwrap();
assert!(!v.monitors_run.contains(&Monitor::PositionJump));
assert!(v.epochs.iter().all(|e| e.position_offset_m.is_none()));
assert!(v.runs.is_empty(), "{:?}", v.runs);
}
}