use crate::inertial::imu_errors::ImuErrorModel;
use crate::inertial::G_M_PER_S2;
use crate::quantum_trade::PositionDrift;
use serde::Deserialize;
const LABEL: &str = "MODELLED INS/TRN coasting error budget. The per-contribution growth \
laws (bias t^2, gyro-bias tilt t^3, velocity random walk t^1.5, angle random walk t^2.5, \
scale factor x travelled distance) are exact for this model and cross-checked against the \
engine's own stochastic dead-reckoner and IMU error model; the IMU grade coefficients are \
representative CLASS figures (Groves 2013 Table 4.1 bands), not a datasheet for a part, and \
the TRN fix residual is a documented input. The crossings are located by bisection on the \
model, never by manuscript arithmetic. Not certified for operational navigation.";
const SQRT_SECONDS_PER_HOUR: f64 = 60.0;
const DEG_PER_HR_TO_RAD_S: f64 = std::f64::consts::PI / 180.0 / 3600.0;
const UG_TO_M_S2: f64 = 1.0e-6 * G_M_PER_S2;
const PPM: f64 = 1.0e-6;
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum ImuGrade {
Navigation,
Tactical,
Industrial,
Consumer,
}
impl ImuGrade {
pub fn all() -> &'static [ImuGrade] {
&[
ImuGrade::Navigation,
ImuGrade::Tactical,
ImuGrade::Industrial,
ImuGrade::Consumer,
]
}
pub fn as_str(self) -> &'static str {
match self {
ImuGrade::Navigation => "navigation",
ImuGrade::Tactical => "tactical",
ImuGrade::Industrial => "industrial",
ImuGrade::Consumer => "consumer",
}
}
pub fn parse(name: &str) -> Result<ImuGrade, String> {
match name {
"navigation" => Ok(ImuGrade::Navigation),
"tactical" => Ok(ImuGrade::Tactical),
"industrial" => Ok(ImuGrade::Industrial),
"consumer" => Ok(ImuGrade::Consumer),
other => Err(format!(
"unknown imu_grade {other:?}; expected one of navigation, tactical, \
industrial, consumer"
)),
}
}
pub fn params(self) -> ImuParams {
match self {
ImuGrade::Navigation => ImuParams {
grade: "navigation",
accel_bias_ug: 25.0,
accel_vrw_m_s_per_sqrt_hr: 0.007,
accel_scale_factor_ppm: 100.0,
gyro_bias_deg_per_hr: 0.01,
gyro_arw_deg_per_sqrt_hr: 0.002,
},
ImuGrade::Tactical => ImuParams {
grade: "tactical",
accel_bias_ug: 300.0,
accel_vrw_m_s_per_sqrt_hr: 0.06,
accel_scale_factor_ppm: 300.0,
gyro_bias_deg_per_hr: 1.0,
gyro_arw_deg_per_sqrt_hr: 0.05,
},
ImuGrade::Industrial => ImuParams {
grade: "industrial",
accel_bias_ug: 3000.0,
accel_vrw_m_s_per_sqrt_hr: 0.3,
accel_scale_factor_ppm: 2000.0,
gyro_bias_deg_per_hr: 20.0,
gyro_arw_deg_per_sqrt_hr: 0.3,
},
ImuGrade::Consumer => ImuParams {
grade: "consumer",
accel_bias_ug: 25000.0,
accel_vrw_m_s_per_sqrt_hr: 1.5,
accel_scale_factor_ppm: 10000.0,
gyro_bias_deg_per_hr: 200.0,
gyro_arw_deg_per_sqrt_hr: 2.0,
},
}
}
}
#[derive(Clone, Copy, Debug)]
pub struct ImuParams {
pub grade: &'static str,
pub accel_bias_ug: f64,
pub accel_vrw_m_s_per_sqrt_hr: f64,
pub accel_scale_factor_ppm: f64,
pub gyro_bias_deg_per_hr: f64,
pub gyro_arw_deg_per_sqrt_hr: f64,
}
#[derive(Clone, Copy, Debug)]
pub struct ImuParamsSi {
pub accel_bias_m_s2: f64,
pub accel_vrw_m_s_per_sqrt_s: f64,
pub accel_scale_factor: f64,
pub gyro_bias_rad_s: f64,
pub gyro_arw_rad_per_sqrt_s: f64,
}
impl ImuParams {
pub fn si(&self) -> ImuParamsSi {
ImuParamsSi {
accel_bias_m_s2: self.accel_bias_ug * UG_TO_M_S2,
accel_vrw_m_s_per_sqrt_s: self.accel_vrw_m_s_per_sqrt_hr / SQRT_SECONDS_PER_HOUR,
accel_scale_factor: self.accel_scale_factor_ppm * PPM,
gyro_bias_rad_s: self.gyro_bias_deg_per_hr * DEG_PER_HR_TO_RAD_S,
gyro_arw_rad_per_sqrt_s: self.gyro_arw_deg_per_sqrt_hr * (std::f64::consts::PI / 180.0)
/ SQRT_SECONDS_PER_HOUR,
}
}
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum ContributionClass {
Deterministic,
Stochastic,
}
impl ContributionClass {
pub fn as_str(self) -> &'static str {
match self {
ContributionClass::Deterministic => "deterministic",
ContributionClass::Stochastic => "stochastic",
}
}
}
#[derive(Clone, Debug)]
pub struct Contribution {
pub name: &'static str,
pub class: ContributionClass,
pub exponent: f64,
pub coefficient: f64,
pub law: &'static str,
}
impl Contribution {
pub fn error_m(&self, t: f64) -> f64 {
if t <= 0.0 {
return if self.exponent == 0.0 {
self.coefficient
} else {
0.0
};
}
self.coefficient * t.powf(self.exponent)
}
pub fn closed_form_crossing_s(&self, threshold_m: f64) -> Option<f64> {
if threshold_m <= 0.0 || self.coefficient <= 0.0 || self.exponent <= 0.0 {
return None;
}
Some((threshold_m / self.coefficient).powf(1.0 / self.exponent))
}
}
struct MonomialDrift {
coefficient: f64,
exponent: f64,
}
impl PositionDrift for MonomialDrift {
fn drift_m(&self, t: f64) -> f64 {
if t <= 0.0 {
return 0.0;
}
self.coefficient * t.powf(self.exponent)
}
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum Combination {
Rss,
LinearSum,
DetSumStochRss,
}
impl Combination {
pub fn all() -> &'static [Combination] {
&[
Combination::Rss,
Combination::LinearSum,
Combination::DetSumStochRss,
]
}
pub fn as_str(self) -> &'static str {
match self {
Combination::Rss => "rss",
Combination::LinearSum => "linear-sum",
Combination::DetSumStochRss => "det-sum-stoch-rss",
}
}
pub fn parse(name: &str) -> Result<Combination, String> {
match name {
"rss" => Ok(Combination::Rss),
"linear-sum" => Ok(Combination::LinearSum),
"det-sum-stoch-rss" => Ok(Combination::DetSumStochRss),
other => Err(format!(
"unknown combination {other:?}; expected one of rss, linear-sum, \
det-sum-stoch-rss"
)),
}
}
pub fn combine(self, parts: &[f64], classes: &[ContributionClass]) -> f64 {
match self {
Combination::Rss => parts.iter().map(|p| p * p).sum::<f64>().sqrt(),
Combination::LinearSum => parts.iter().sum(),
Combination::DetSumStochRss => {
let mut det = 0.0;
let mut stoch2 = 0.0;
for (p, c) in parts.iter().zip(classes.iter()) {
match c {
ContributionClass::Deterministic => det += *p,
ContributionClass::Stochastic => stoch2 += p * p,
}
}
det + stoch2.sqrt()
}
}
}
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum TrnFixMode {
None,
PositionOnly,
FullReset,
}
impl TrnFixMode {
pub fn as_str(self) -> &'static str {
match self {
TrnFixMode::None => "none",
TrnFixMode::PositionOnly => "position-only",
TrnFixMode::FullReset => "full-reset",
}
}
pub fn parse(name: &str) -> Result<TrnFixMode, String> {
match name {
"none" => Ok(TrnFixMode::None),
"position-only" => Ok(TrnFixMode::PositionOnly),
"full-reset" => Ok(TrnFixMode::FullReset),
other => Err(format!(
"unknown trn_fix_mode {other:?}; expected one of none, position-only, \
full-reset"
)),
}
}
}
#[derive(Clone, Debug)]
pub struct CoastModel {
pub imu: ImuParamsSi,
pub speed_m_s: f64,
pub ref_accel_m_s2: f64,
pub combination: Combination,
pub fix_residual_m: f64,
contributions: Vec<Contribution>,
}
impl CoastModel {
pub fn new(
imu: ImuParamsSi,
speed_m_s: f64,
ref_accel_m_s2: f64,
combination: Combination,
fix_residual_m: f64,
) -> Self {
let mut contributions = vec![
Contribution {
name: "accel_bias",
class: ContributionClass::Deterministic,
exponent: 2.0,
coefficient: 0.5 * imu.accel_bias_m_s2,
law: "0.5 * b_a * t^2",
},
Contribution {
name: "gyro_bias_tilt",
class: ContributionClass::Deterministic,
exponent: 3.0,
coefficient: G_M_PER_S2 * imu.gyro_bias_rad_s / 6.0,
law: "g * b_g * t^3 / 6",
},
Contribution {
name: "scale_factor_cruise",
class: ContributionClass::Deterministic,
exponent: 1.0,
coefficient: imu.accel_scale_factor * speed_m_s,
law: "s * v * t",
},
Contribution {
name: "scale_factor_accel",
class: ContributionClass::Deterministic,
exponent: 2.0,
coefficient: 0.5 * imu.accel_scale_factor * ref_accel_m_s2,
law: "0.5 * s * a * t^2",
},
Contribution {
name: "velocity_random_walk",
class: ContributionClass::Stochastic,
exponent: 1.5,
coefficient: imu.accel_vrw_m_s_per_sqrt_s / 3.0f64.sqrt(),
law: "sigma_vrw * t^1.5 / sqrt(3)",
},
Contribution {
name: "angle_random_walk",
class: ContributionClass::Stochastic,
exponent: 2.5,
coefficient: G_M_PER_S2 * imu.gyro_arw_rad_per_sqrt_s / 20.0f64.sqrt(),
law: "g * sigma_arw * t^2.5 / sqrt(20)",
},
];
if fix_residual_m > 0.0 {
contributions.push(Contribution {
name: "trn_fix_residual",
class: ContributionClass::Stochastic,
exponent: 0.0,
coefficient: fix_residual_m,
law: "r (constant)",
});
}
Self {
imu,
speed_m_s,
ref_accel_m_s2,
combination,
fix_residual_m,
contributions,
}
}
pub fn contributions(&self) -> &[Contribution] {
&self.contributions
}
pub fn breakdown_m(&self, t: f64) -> Vec<f64> {
self.contributions.iter().map(|c| c.error_m(t)).collect()
}
fn classes(&self) -> Vec<ContributionClass> {
self.contributions.iter().map(|c| c.class).collect()
}
pub fn travelled_distance_m(&self, t: f64) -> f64 {
self.speed_m_s * t + 0.5 * self.ref_accel_m_s2 * t * t
}
pub fn dominant_at(&self, t: f64) -> Option<(&'static str, f64)> {
let mut best: Option<(&'static str, f64)> = None;
for c in &self.contributions {
let e = c.error_m(t);
if e > 0.0 && best.map_or(true, |(_, b)| e > b) {
best = Some((c.name, e));
}
}
best
}
}
impl PositionDrift for CoastModel {
fn drift_m(&self, t: f64) -> f64 {
let parts = self.breakdown_m(t);
self.combination.combine(&parts, &self.classes())
}
}
#[derive(Clone, Debug)]
pub struct Crossing {
pub threshold_m: f64,
pub coast_s: Option<f64>,
pub status: &'static str,
}
pub fn locate_crossing<D: PositionDrift>(model: &D, threshold_m: f64) -> Crossing {
if threshold_m.is_nan() || threshold_m <= 0.0 {
return Crossing {
threshold_m,
coast_s: None,
status: "threshold-not-positive",
};
}
if model.drift_m(0.0) >= threshold_m {
return Crossing {
threshold_m,
coast_s: Some(0.0),
status: "already-exceeded-at-zero",
};
}
let t = model.inertial_holdover_s(threshold_m);
if t.is_finite() && t > 0.0 {
Crossing {
threshold_m,
coast_s: Some(t),
status: "reached",
}
} else if t == 0.0 {
Crossing {
threshold_m,
coast_s: Some(0.0),
status: "already-exceeded-at-zero",
}
} else {
Crossing {
threshold_m,
coast_s: None,
status: "never-reached",
}
}
}
fn bisect_in_bracket<F: Fn(f64) -> f64>(f: F, target: f64, lo: f64, hi: f64) -> Option<f64> {
if lo.is_nan() || hi.is_nan() || lo >= hi || !target.is_finite() {
return None;
}
let (flo, fhi) = (f(lo), f(hi));
if !flo.is_finite() || !fhi.is_finite() || flo > target || fhi < target {
return None;
}
let (mut a, mut b) = (lo, hi);
for _ in 0..80 {
let mid = 0.5 * (a + b);
if f(mid) < target {
a = mid;
} else {
b = mid;
}
}
Some(0.5 * (a + b))
}
#[derive(Clone, Debug)]
pub struct TrnCoast {
pub fix_interval_s: f64,
pub intervals: usize,
pub interval_errors_m: Vec<f64>,
pub peak_error_m: f64,
}
pub const MAX_TRN_INTERVALS: usize = 4096;
impl CoastModel {
fn position_only_interval_parts(&self, t0: f64, tau: f64) -> Vec<f64> {
self.contributions
.iter()
.map(|c| match c.name {
"velocity_random_walk" => {
let s = self.imu.accel_vrw_m_s_per_sqrt_s;
s * (t0 * tau * tau + tau * tau * tau / 3.0).max(0.0).sqrt()
}
"angle_random_walk" => {
let s = self.imu.gyro_arw_rad_per_sqrt_s;
G_M_PER_S2
* s
* (t0 * tau.powi(4) / 4.0 + tau.powi(5) / 20.0)
.max(0.0)
.sqrt()
}
"trn_fix_residual" => c.coefficient,
_ => {
if c.exponent == 0.0 {
c.coefficient
} else {
c.coefficient * ((t0 + tau).powf(c.exponent) - t0.powf(c.exponent))
}
}
})
.collect()
}
pub fn trn_coast(&self, mode: TrnFixMode, fix_interval_s: f64, mission_s: f64) -> TrnCoast {
if mode == TrnFixMode::None || fix_interval_s <= 0.0 || mission_s <= 0.0 {
let e = self.drift_m(mission_s.max(0.0));
return TrnCoast {
fix_interval_s,
intervals: 0,
interval_errors_m: vec![e],
peak_error_m: e,
};
}
let classes = self.classes();
let n = ((mission_s / fix_interval_s).floor() as usize).clamp(1, MAX_TRN_INTERVALS);
let mut errs = Vec::with_capacity(n);
for k in 0..n {
let t0 = k as f64 * fix_interval_s;
let parts = match mode {
TrnFixMode::FullReset => self.breakdown_m(fix_interval_s),
TrnFixMode::PositionOnly => self.position_only_interval_parts(t0, fix_interval_s),
TrnFixMode::None => unreachable!("the none mode returned above"),
};
errs.push(self.combination.combine(&parts, &classes));
}
let peak = errs.iter().copied().fold(0.0f64, f64::max);
TrnCoast {
fix_interval_s,
intervals: n,
interval_errors_m: errs,
peak_error_m: peak,
}
}
pub fn max_fix_interval_s(
&self,
mode: TrnFixMode,
mission_s: f64,
threshold_m: f64,
) -> (Option<f64>, &'static str) {
if mode == TrnFixMode::None || mission_s <= 0.0 || threshold_m <= 0.0 {
return (None, "not-applicable");
}
let lo = mission_s / MAX_TRN_INTERVALS as f64;
let peak = |tau: f64| self.trn_coast(mode, tau, mission_s).peak_error_m;
if peak(mission_s) <= threshold_m {
return (Some(mission_s), "holds-at-every-interval");
}
if peak(lo) > threshold_m {
return (None, "never-holds");
}
match bisect_in_bracket(peak, threshold_m, lo, mission_s) {
Some(t) => (Some(t), "bisected"),
None => (None, "not-bracketed"),
}
}
}
pub const DEFAULT_CROSSING_THRESHOLDS_M: [f64; 2] = [10.0, 50.0];
pub const DEFAULT_SPEED_M_S: f64 = 250.0;
pub const DEFAULT_TRN_FIX_RESIDUAL_M: f64 = 17.0;
pub const DEFAULT_TRN_FIX_INTERVAL_S: f64 = 300.0;
pub const DEFAULT_MISSION_DURATION_S: f64 = 3600.0;
pub const DEFAULT_DRIFT_BAND_LO_M_PER_S: f64 = 0.001;
pub const DEFAULT_DRIFT_BAND_HI_M_PER_S: f64 = 0.050;
#[derive(Clone, Debug, Default, Deserialize)]
pub struct InsTrnCoastScenario {
pub imu_grade: Option<String>,
pub accel_bias_ug: Option<f64>,
pub accel_vrw_m_s_per_sqrt_hr: Option<f64>,
pub accel_scale_factor_ppm: Option<f64>,
pub gyro_bias_deg_per_hr: Option<f64>,
pub gyro_arw_deg_per_sqrt_hr: Option<f64>,
pub speed_m_s: Option<f64>,
pub ref_accel_m_s2: Option<f64>,
pub combination: Option<String>,
pub crossing_thresholds_m: Option<Vec<f64>>,
pub grades: Option<Vec<String>>,
pub drift_band_lo_m_per_s: Option<f64>,
pub drift_band_hi_m_per_s: Option<f64>,
pub trn_fix_mode: Option<String>,
pub trn_fix_interval_s: Option<f64>,
pub trn_fix_residual_m: Option<f64>,
pub mission_duration_s: Option<f64>,
}
impl InsTrnCoastScenario {
fn resolve_imu(&self) -> Result<ImuParams, String> {
let grade = ImuGrade::parse(self.imu_grade.as_deref().unwrap_or("navigation"))?;
let mut p = grade.params();
let overridden = self.accel_bias_ug.is_some()
|| self.accel_vrw_m_s_per_sqrt_hr.is_some()
|| self.accel_scale_factor_ppm.is_some()
|| self.gyro_bias_deg_per_hr.is_some()
|| self.gyro_arw_deg_per_sqrt_hr.is_some();
if let Some(v) = self.accel_bias_ug {
p.accel_bias_ug = v;
}
if let Some(v) = self.accel_vrw_m_s_per_sqrt_hr {
p.accel_vrw_m_s_per_sqrt_hr = v;
}
if let Some(v) = self.accel_scale_factor_ppm {
p.accel_scale_factor_ppm = v;
}
if let Some(v) = self.gyro_bias_deg_per_hr {
p.gyro_bias_deg_per_hr = v;
}
if let Some(v) = self.gyro_arw_deg_per_sqrt_hr {
p.gyro_arw_deg_per_sqrt_hr = v;
}
if overridden {
p.grade = "custom";
}
for (name, v) in [
("accel_bias_ug", p.accel_bias_ug),
("accel_vrw_m_s_per_sqrt_hr", p.accel_vrw_m_s_per_sqrt_hr),
("accel_scale_factor_ppm", p.accel_scale_factor_ppm),
("gyro_bias_deg_per_hr", p.gyro_bias_deg_per_hr),
("gyro_arw_deg_per_sqrt_hr", p.gyro_arw_deg_per_sqrt_hr),
] {
if !v.is_finite() || v < 0.0 {
return Err(format!("{name} must be finite and non-negative (got {v})"));
}
}
Ok(p)
}
fn resolve_thresholds(&self) -> Result<Vec<f64>, String> {
let ts = match &self.crossing_thresholds_m {
None => DEFAULT_CROSSING_THRESHOLDS_M.to_vec(),
Some(v) if v.is_empty() => {
return Err("crossing_thresholds_m must name at least one threshold".into())
}
Some(v) => v.clone(),
};
for t in &ts {
if !t.is_finite() || *t <= 0.0 {
return Err(format!(
"crossing_thresholds_m entries must be finite and positive (got {t})"
));
}
}
Ok(ts)
}
fn resolve_grades(&self) -> Result<Vec<ImuGrade>, String> {
match &self.grades {
None => Ok(ImuGrade::all().to_vec()),
Some(v) if v.is_empty() => Err("grades must name at least one IMU class".into()),
Some(v) => v.iter().map(|g| ImuGrade::parse(g)).collect(),
}
}
fn resolve_motion(&self) -> Result<(f64, f64), String> {
let v = self.speed_m_s.unwrap_or(DEFAULT_SPEED_M_S);
let a = self.ref_accel_m_s2.unwrap_or(0.0);
if !v.is_finite() || v < 0.0 {
return Err(format!(
"speed_m_s must be finite and non-negative (got {v})"
));
}
if !a.is_finite() || a < 0.0 {
return Err(format!(
"ref_accel_m_s2 must be finite and non-negative (got {a})"
));
}
Ok((v, a))
}
fn resolve_trn(&self) -> Result<(TrnFixMode, f64, f64, f64), String> {
let mode = TrnFixMode::parse(self.trn_fix_mode.as_deref().unwrap_or("none"))?;
let interval = self
.trn_fix_interval_s
.unwrap_or(DEFAULT_TRN_FIX_INTERVAL_S);
let residual = self
.trn_fix_residual_m
.unwrap_or(DEFAULT_TRN_FIX_RESIDUAL_M);
let mission = self
.mission_duration_s
.unwrap_or(DEFAULT_MISSION_DURATION_S);
for (name, v) in [
("trn_fix_interval_s", interval),
("trn_fix_residual_m", residual),
("mission_duration_s", mission),
] {
if !v.is_finite() || v < 0.0 {
return Err(format!("{name} must be finite and non-negative (got {v})"));
}
}
if mission <= 0.0 {
return Err("mission_duration_s must be positive".into());
}
Ok((mode, interval, residual, mission))
}
fn resolve_band(&self) -> Result<(f64, f64), String> {
let lo = self
.drift_band_lo_m_per_s
.unwrap_or(DEFAULT_DRIFT_BAND_LO_M_PER_S);
let hi = self
.drift_band_hi_m_per_s
.unwrap_or(DEFAULT_DRIFT_BAND_HI_M_PER_S);
if !lo.is_finite() || !hi.is_finite() || lo <= 0.0 || hi <= lo {
return Err(format!(
"drift band must satisfy 0 < lo < hi (got lo={lo}, hi={hi})"
));
}
Ok((lo, hi))
}
pub fn model(&self) -> Result<CoastModel, String> {
let imu = self.resolve_imu()?.si();
let (v, a) = self.resolve_motion()?;
let comb = Combination::parse(self.combination.as_deref().unwrap_or("rss"))?;
Ok(CoastModel::new(imu, v, a, comb, 0.0))
}
pub fn trn_model(&self) -> Result<CoastModel, String> {
let imu = self.resolve_imu()?.si();
let (v, a) = self.resolve_motion()?;
let comb = Combination::parse(self.combination.as_deref().unwrap_or("rss"))?;
let (mode, _, residual, _) = self.resolve_trn()?;
let r = if mode == TrnFixMode::None {
0.0
} else {
residual
};
Ok(CoastModel::new(imu, v, a, comb, r))
}
pub fn run_json(&self) -> Result<(String, String), String> {
let doc = self.compute()?;
let summary = doc.summary.clone();
let json = serde_json::to_string_pretty(&doc.json)
.map_err(|e| format!("serializing ins-trn-coast result: {e}"))?;
Ok((json, summary))
}
fn compute(&self) -> Result<Computed, String> {
let params = self.resolve_imu()?;
let si = params.si();
let (speed, ref_accel) = self.resolve_motion()?;
let comb = Combination::parse(self.combination.as_deref().unwrap_or("rss"))?;
let thresholds = self.resolve_thresholds()?;
let grades = self.resolve_grades()?;
let (band_lo, band_hi) = self.resolve_band()?;
let (mode, fix_interval, fix_residual, mission) = self.resolve_trn()?;
let model = self.model()?;
let trn_model = self.trn_model()?;
let mut crossings = Vec::new();
for &th in &thresholds {
let c = locate_crossing(&model, th);
let (err_at, dist, rate, dominant, dom_err, breakdown) = match c.coast_s {
Some(t) => {
let parts = model.breakdown_m(t);
let lin: f64 = parts.iter().sum();
let rows: Vec<serde_json::Value> = model
.contributions()
.iter()
.zip(parts.iter())
.map(|(c, e)| {
serde_json::json!({
"name": c.name,
"class": c.class.as_str(),
"exponent": c.exponent,
"error_m": e,
"fraction_of_linear_sum": if lin > 0.0 { e / lin } else { 0.0 },
})
})
.collect();
let (dn, de) = model.dominant_at(t).unwrap_or(("none", 0.0));
(
Some(model.drift_m(t)),
Some(model.travelled_distance_m(t)),
Some(th / t),
dn,
Some(de),
rows,
)
}
None => (None, None, None, "none", None, Vec::new()),
};
crossings.push(serde_json::json!({
"threshold_m": th,
"coast_s": c.coast_s,
"status": c.status,
"method": "bisection on the combined model",
"error_at_coast_m": err_at,
"travelled_distance_m": dist,
"implied_mean_drift_rate_m_per_s": rate,
"in_swept_drift_band": rate.map(|r| r >= band_lo && r <= band_hi),
"dominant_contribution": dominant,
"dominant_contribution_error_m": dom_err,
"breakdown": breakdown,
}));
}
let mut contribution_rows = Vec::new();
for c in model.contributions() {
let rows: Vec<serde_json::Value> = thresholds
.iter()
.map(|&th| {
let closed = c.closed_form_crossing_s(th);
let bis = if c.coefficient > 0.0 && c.exponent > 0.0 {
locate_crossing(
&MonomialDrift {
coefficient: c.coefficient,
exponent: c.exponent,
},
th,
)
.coast_s
} else {
None
};
let rel = match (closed, bis) {
(Some(a), Some(b)) => {
let d = a.abs().max(b.abs());
if d > 0.0 {
Some((a - b).abs() / d)
} else {
Some(0.0)
}
}
_ => None,
};
serde_json::json!({
"threshold_m": th,
"closed_form_s": closed,
"bisection_s": bis,
"rel_diff": rel,
"status": if closed.is_some() { "both" } else { "no-crossing-for-this-contribution" },
})
})
.collect();
contribution_rows.push(serde_json::json!({
"name": c.name,
"class": c.class.as_str(),
"exponent": c.exponent,
"coefficient_si": c.coefficient,
"law": c.law,
"crossings": rows,
}));
}
let sensitivity: Vec<serde_json::Value> = Combination::all()
.iter()
.map(|&k| {
let m = CoastModel::new(si, speed, ref_accel, k, 0.0);
let rows: Vec<serde_json::Value> = thresholds
.iter()
.map(|&th| {
let c = locate_crossing(&m, th);
serde_json::json!({
"threshold_m": th,
"coast_s": c.coast_s,
"status": c.status,
})
})
.collect();
serde_json::json!({ "combination": k.as_str(), "crossings": rows })
})
.collect();
let mut grade_rows = Vec::new();
let mut band_rows = Vec::new();
for g in &grades {
let gm = CoastModel::new(g.params().si(), speed, ref_accel, comb, 0.0);
let rows: Vec<serde_json::Value> = thresholds
.iter()
.map(|&th| {
let c = locate_crossing(&gm, th);
let (dom, rate) = match c.coast_s {
Some(t) => (
gm.dominant_at(t).map(|(n, _)| n).unwrap_or("none"),
Some(th / t),
),
None => ("none", None),
};
serde_json::json!({
"threshold_m": th,
"coast_s": c.coast_s,
"status": c.status,
"dominant_contribution": dom,
"implied_mean_drift_rate_m_per_s": rate,
"in_swept_drift_band": rate.map(|r| r >= band_lo && r <= band_hi),
})
})
.collect();
grade_rows.push(serde_json::json!({
"grade": g.as_str(),
"accel_bias_ug": g.params().accel_bias_ug,
"gyro_bias_deg_per_hr": g.params().gyro_bias_deg_per_hr,
"crossings": rows,
}));
let rate_of = |t: f64| if t > 0.0 { gm.drift_m(t) / t } else { 0.0 };
let (lo_b, hi_b) = (1.0e-3, 1.0e6);
let entry = bisect_in_bracket(rate_of, band_lo, lo_b, hi_b);
let exit = bisect_in_bracket(rate_of, band_hi, lo_b, hi_b);
band_rows.push(serde_json::json!({
"grade": g.as_str(),
"band_entry_s": entry,
"band_exit_s": exit,
"rate_at_bracket_lo_m_per_s": rate_of(lo_b),
"rate_at_bracket_hi_m_per_s": rate_of(hi_b),
"status": band_status(rate_of(lo_b), rate_of(hi_b), band_lo, band_hi, entry, exit),
}));
}
let trn = trn_model.trn_coast(mode, fix_interval, mission);
let trn_thresholds: Vec<serde_json::Value> = thresholds
.iter()
.map(|&th| {
let (iv, st) = trn_model.max_fix_interval_s(mode, mission, th);
serde_json::json!({
"threshold_m": th,
"peak_within_threshold": trn.peak_error_m <= th,
"max_fix_interval_s": iv,
"max_fix_interval_status": st,
})
})
.collect();
let monotone = trn
.interval_errors_m
.windows(2)
.all(|w| w[1] >= w[0] - 1.0e-12);
let summary = {
let show = |i: usize| -> String {
crossings
.get(i)
.and_then(|c| c["coast_s"].as_f64())
.map(|t| format!("{t:.1} s"))
.unwrap_or_else(|| "not reached".into())
};
let dom = |i: usize| -> String {
crossings
.get(i)
.and_then(|c| c["dominant_contribution"].as_str())
.unwrap_or("none")
.to_string()
};
let heads: Vec<String> = (0..thresholds.len().min(2))
.map(|i| format!("{:.0} m at {} ({})", thresholds[i], show(i), dom(i)))
.collect();
format!(
"ins-trn-coast | {} IMU, {:.0} m/s, combination {} | {} | TRN {} peak {:.1} m over {:.0} s",
params.grade,
speed,
comb.as_str(),
heads.join(" | "),
mode.as_str(),
trn.peak_error_m,
mission,
)
};
let json = serde_json::json!({
"kind": "ins-trn-coast",
"label": LABEL,
"combination": comb.as_str(),
"combination_note": "A modelling choice, not a derivation: rss reads every \
coefficient as a 1-sigma spec, linear-sum is the coherent worst case, det-sum-stoch-rss \
adds the systematic terms and root-sum-squares the independently driven random walks. \
combination_sensitivity below reports the headline crossings under all three.",
"units": units_block(),
"imu": {
"grade": params.grade,
"provenance": "representative CLASS coefficients (Groves 2013 Table 4.1 \
bands), not a datasheet for a specific part",
"accel_bias_ug": params.accel_bias_ug,
"accel_bias_m_s2": si.accel_bias_m_s2,
"accel_vrw_m_s_per_sqrt_hr": params.accel_vrw_m_s_per_sqrt_hr,
"accel_vrw_m_s_per_sqrt_s": si.accel_vrw_m_s_per_sqrt_s,
"accel_scale_factor_ppm": params.accel_scale_factor_ppm,
"gyro_bias_deg_per_hr": params.gyro_bias_deg_per_hr,
"gyro_bias_rad_s": si.gyro_bias_rad_s,
"gyro_arw_deg_per_sqrt_hr": params.gyro_arw_deg_per_sqrt_hr,
"gyro_arw_rad_per_sqrt_s": si.gyro_arw_rad_per_sqrt_s,
},
"motion": {
"speed_m_s": speed,
"ref_accel_m_s2": ref_accel,
"note": "the travelled distance the scale-factor error mis-scales is \
v*t + 0.5*a*t^2; a static platform (v = a = 0) has no scale-factor contribution",
},
"contributions": model.contributions().iter().map(|c| serde_json::json!({
"name": c.name,
"class": c.class.as_str(),
"exponent": c.exponent,
"coefficient_si": c.coefficient,
"law": c.law,
})).collect::<Vec<_>>(),
"crossings": crossings,
"contribution_crossings": contribution_rows,
"combination_sensitivity": sensitivity,
"grade_table": grade_rows,
"swept_drift_band": {
"lo_m_per_s": band_lo,
"hi_m_per_s": band_hi,
"rate_definition": "mean drift rate = modelled position error / coast duration",
"rows": band_rows,
},
"trn": {
"fix_mode": mode.as_str(),
"fix_interval_s": fix_interval,
"fix_residual_m": if mode == TrnFixMode::None { 0.0 } else { fix_residual },
"fix_residual_provenance": "input; the default is the matching sigma of the \
shipped terrain-nav configuration, hypot(altimeter 8 m, DEM 15 m), not an achieved \
batch-matcher residual",
"mission_duration_s": mission,
"intervals_evaluated": trn.intervals,
"peak_error_m": trn.peak_error_m,
"peak_is_at_the_last_interval": monotone,
"interval_errors_m": trn.interval_errors_m,
"max_interval_scan": MAX_TRN_INTERVALS,
"thresholds": trn_thresholds,
"bounds_the_coast": "a terrain fix supplies a POSITION correction with a \
residual; position-only leaves velocity error and tilt integrating across the fix, \
full-reset re-zeroes the whole error state as AccelModel::reset does",
"scope": "this block, and only this block, carries the fix residual. The \
headline crossings above are the FREE-INERTIAL coast durations, so an aiding residual \
cannot silently shorten them.",
},
});
Ok(Computed { json, summary })
}
}
fn band_status(
rate_lo: f64,
rate_hi: f64,
band_lo: f64,
band_hi: f64,
entry: Option<f64>,
exit: Option<f64>,
) -> &'static str {
match (entry, exit) {
(Some(_), Some(_)) => "band-spanned",
(Some(_), None) => "enters-the-band-but-the-bracket-never-reaches-the-upper-edge",
(None, Some(_)) => "already-above-the-lower-edge-when-the-coast-starts",
(None, None) if rate_lo > band_hi => "already-above-the-upper-edge-when-the-coast-starts",
(None, None) if rate_hi < band_lo => "never-reaches-the-lower-edge-within-the-bracket",
(None, None) => "band-not-bracketed-over-1e-3-to-1e6-s",
}
}
struct Computed {
json: serde_json::Value,
summary: String,
}
const UNITS: &[(&str, &str, &str, Option<&str>)] = &[
("imu.accel_bias_ug", "ug", "input", Some("1 ug = 9.80665e-6 m/s^2")),
("imu.accel_bias_m_s2", "m/s^2", "computed", None),
("imu.accel_vrw_m_s_per_sqrt_hr", "(m/s)/sqrt(hr)", "input", None),
("imu.accel_vrw_m_s_per_sqrt_s", "(m/s)/sqrt(s)", "computed", Some("square of this is the white acceleration PSD q_va")),
("imu.accel_scale_factor_ppm", "ppm", "input", None),
("imu.gyro_bias_deg_per_hr", "deg/hr", "input", None),
("imu.gyro_bias_rad_s", "rad/s", "computed", None),
("imu.gyro_arw_deg_per_sqrt_hr", "deg/sqrt(hr)", "input", None),
("imu.gyro_arw_rad_per_sqrt_s", "rad/sqrt(s)", "computed", Some("square of this is the white angular-rate PSD q_arw")),
("motion.speed_m_s", "m/s", "input", None),
("motion.ref_accel_m_s2", "m/s^2", "input", None),
("contributions.exponent", "1", "modelled", Some("the power of coast duration this contribution grows with")),
("contributions.coefficient_si", "m/s^exponent", "computed", None),
("crossings.threshold_m", "m", "input", None),
("crossings.coast_s", "s", "computed", Some("bisection on the combined model; null with a status when the model never reaches the threshold")),
("crossings.error_at_coast_m", "m", "computed", Some("the model re-evaluated at the located crossing; a residual check on the bisection")),
("crossings.travelled_distance_m", "m", "computed", None),
("crossings.implied_mean_drift_rate_m_per_s", "m/s", "computed", Some("threshold / crossing duration; the quantity a duty-cycle study sweeps")),
("crossings.dominant_contribution_error_m", "m", "computed", Some("the error the single largest contribution had accumulated at the crossing; `dominant_contribution` names which one")),
("crossings.breakdown.error_m", "m", "computed", None),
("crossings.breakdown.exponent", "1", "modelled", Some("the power of coast duration that contribution grows with, repeated on the breakdown row")),
("crossings.breakdown.fraction_of_linear_sum", "1", "computed", None),
("contribution_crossings.closed_form_s", "s", "closed-form", Some("exact algebraic inversion of that contribution's monomial")),
("contribution_crossings.bisection_s", "s", "computed", Some("the engine's bisection on the same monomial")),
("contribution_crossings.rel_diff", "1", "internal-consistency", None),
("contribution_crossings.exponent", "1", "modelled", Some("the power of coast duration this contribution grows with")),
("contribution_crossings.coefficient_si", "m/s^exponent", "computed", None),
("contribution_crossings.crossings.threshold_m", "m", "input", None),
("contribution_crossings.crossings.closed_form_s", "s", "closed-form", Some("exact algebraic inversion of that contribution's monomial")),
("contribution_crossings.crossings.bisection_s", "s", "computed", Some("the engine's bisection on the same monomial")),
("contribution_crossings.crossings.rel_diff", "1", "internal-consistency", Some("|bisection - closed form| / closed form; the agreement of the two routes to the same crossing")),
("combination_sensitivity.crossings.threshold_m", "m", "input", None),
("combination_sensitivity.crossings.coast_s", "s", "computed", None),
("grade_table.accel_bias_ug", "ug", "spec", Some("the representative accelerometer-bias coefficient of that IMU grade band, in the units a datasheet quotes; a class figure, not a measured unit")),
("grade_table.gyro_bias_deg_per_hr", "deg/hr", "spec", Some("the representative gyro-bias coefficient of that IMU grade band; a class figure, not a measured unit")),
("grade_table.crossings.threshold_m", "m", "input", None),
("grade_table.crossings.coast_s", "s", "computed", None),
("grade_table.crossings.implied_mean_drift_rate_m_per_s", "m/s", "computed", None),
("swept_drift_band.lo_m_per_s", "m/s", "input", None),
("swept_drift_band.hi_m_per_s", "m/s", "input", None),
("swept_drift_band.rows.band_entry_s", "s", "computed", Some("bisection for the coast duration whose mean drift rate equals the band's lower edge")),
("swept_drift_band.rows.band_exit_s", "s", "computed", None),
("swept_drift_band.rows.rate_at_bracket_lo_m_per_s", "m/s", "computed", None),
("swept_drift_band.rows.rate_at_bracket_hi_m_per_s", "m/s", "computed", None),
("trn.fix_interval_s", "s", "input", None),
("trn.fix_residual_m", "m", "input", None),
("trn.mission_duration_s", "s", "input", None),
("trn.intervals_evaluated", "count", "computed", Some("0 in fix_mode none, where the single reported error is the free-inertial value at the mission duration")),
("trn.peak_error_m", "m", "computed", Some("maximum over every evaluated inter-fix interval, scanned rather than assumed to be the last")),
("trn.interval_errors_m", "m", "computed", None),
("trn.max_interval_scan", "count", "constant", Some("the fixed cap on how many inter-fix intervals are evaluated, and the divisor of the bisection bracket's lower end")),
("trn.thresholds.threshold_m", "m", "input", None),
("trn.thresholds.max_fix_interval_s", "s", "computed", Some("bisection over [mission/4096, mission]; null with a status when unbracketed")),
];
fn units_block() -> serde_json::Value {
let mut m = serde_json::Map::with_capacity(UNITS.len());
for (field, unit, provenance, note) in UNITS {
let mut e = serde_json::Map::new();
e.insert("unit".into(), serde_json::Value::from(*unit));
e.insert("provenance".into(), serde_json::Value::from(*provenance));
if let Some(n) = note {
e.insert("note".into(), serde_json::Value::from(*n));
}
m.insert((*field).to_string(), serde_json::Value::Object(e));
}
serde_json::Value::Object(m)
}
pub fn scale_factor_reference_error_m(
scale_ppm: f64,
accel: f64,
ramp_s: f64,
cruise_s: f64,
dt: f64,
) -> f64 {
let m = ImuErrorModel::ideal().with_scale_accel_ppm([scale_ppm, 0.0, 0.0]);
let (mut v_err, mut p_err, mut t) = (0.0f64, 0.0f64, 0.0f64);
let total = ramp_s + cruise_s;
while t < total - 0.5 * dt {
let f_true = if t < ramp_s { accel } else { 0.0 };
let (_, f_meas) = m.distort([0.0; 3], [f_true, 0.0, 0.0], t);
v_err += (f_meas[0] - f_true) * dt;
p_err += v_err * dt;
t += dt;
}
p_err
}
#[cfg(test)]
mod tests {
use super::*;
use crate::inertial::AccelModel;
use crate::quantum_trade::ClassicalInsBudget;
use rand::SeedableRng;
use rand_chacha::ChaCha8Rng;
fn rel(a: f64, b: f64) -> f64 {
let d = a.abs().max(b.abs());
if d == 0.0 {
0.0
} else {
(a - b).abs() / d
}
}
fn single(name: &str) -> CoastModel {
let zero = ImuParamsSi {
accel_bias_m_s2: 0.0,
accel_vrw_m_s_per_sqrt_s: 0.0,
accel_scale_factor: 0.0,
gyro_bias_rad_s: 0.0,
gyro_arw_rad_per_sqrt_s: 0.0,
};
let (mut imu, mut v, mut a) = (zero, 0.0, 0.0);
match name {
"accel_bias" => imu.accel_bias_m_s2 = 3.0e-4,
"gyro_bias_tilt" => imu.gyro_bias_rad_s = 5.0e-8,
"velocity_random_walk" => imu.accel_vrw_m_s_per_sqrt_s = 1.0e-4,
"angle_random_walk" => imu.gyro_arw_rad_per_sqrt_s = 6.0e-7,
"scale_factor_cruise" => {
imu.accel_scale_factor = 1.0e-4;
v = 200.0;
}
"scale_factor_accel" => {
imu.accel_scale_factor = 1.0e-4;
a = 2.0;
}
other => panic!("no single-contribution fixture for {other}"),
}
CoastModel::new(imu, v, a, Combination::Rss, 0.0)
}
#[test]
fn doubling_the_coast_scales_each_contribution_by_two_to_its_own_power() {
let expected: &[(&str, f64)] = &[
("accel_bias", 2.0),
("gyro_bias_tilt", 3.0),
("scale_factor_cruise", 1.0),
("scale_factor_accel", 2.0),
("velocity_random_walk", 1.5),
("angle_random_walk", 2.5),
];
for (name, p) in expected {
let m = single(name);
for t in [10.0f64, 137.0, 900.0] {
let ratio = m.drift_m(2.0 * t) / m.drift_m(t);
assert!(
rel(ratio, 2.0f64.powf(*p)) < 1.0e-12,
"{name} at t={t}: doubling scaled the error by {ratio}, expected 2^{p} = {}",
2.0f64.powf(*p)
);
}
}
}
#[test]
fn a_pure_bias_error_quadruples_and_a_pure_random_walk_error_grows_by_two_to_the_three_halves()
{
let bias = single("accel_bias");
assert!(rel(bias.drift_m(600.0) / bias.drift_m(300.0), 4.0) < 1.0e-12);
let vrw = single("velocity_random_walk");
assert!(rel(vrw.drift_m(600.0) / vrw.drift_m(300.0), 2.0f64.powf(1.5)) < 1.0e-12);
}
#[test]
fn the_exponents_the_document_publishes_are_the_ones_the_curves_actually_follow() {
let m = CoastModel::new(
ImuGrade::Tactical.params().si(),
180.0,
1.5,
Combination::Rss,
0.0,
);
for c in m.contributions() {
if c.coefficient <= 0.0 {
continue;
}
let measured = (c.error_m(800.0) / c.error_m(400.0)).log2();
assert!(
rel(measured, c.exponent) < 1.0e-12,
"{} reports exponent {} but its curve measures {measured}",
c.name,
c.exponent
);
}
}
#[test]
fn every_contributions_closed_form_crossing_agrees_with_the_engines_bisection() {
let m = CoastModel::new(
ImuGrade::Navigation.params().si(),
250.0,
0.0,
Combination::Rss,
0.0,
);
let mut checked = 0;
for c in m.contributions() {
for th in [1.0, 10.0, 50.0, 500.0] {
let Some(closed) = c.closed_form_crossing_s(th) else {
continue;
};
let bisected = locate_crossing(
&MonomialDrift {
coefficient: c.coefficient,
exponent: c.exponent,
},
th,
)
.coast_s
.expect(
"a contribution with a closed-form crossing is bracketed by the \
doubling search, so the bisection returns a finite duration",
);
assert!(
rel(closed, bisected) < 1.0e-9,
"{}: closed form {closed} s vs bisection {bisected} s",
c.name
);
checked += 1;
}
}
assert!(
checked >= 16,
"only {checked} contribution crossings checked"
);
}
#[test]
fn the_located_crossing_puts_the_model_on_the_threshold_it_searched_for() {
let m = CoastModel::new(
ImuGrade::Tactical.params().si(),
120.0,
0.0,
Combination::Rss,
0.0,
);
for th in [10.0, 50.0, 200.0] {
let t = locate_crossing(&m, th)
.coast_s
.expect("a tactical-grade coast reaches every threshold in this list");
assert!(
rel(m.drift_m(t), th) < 1.0e-9,
"threshold {th} m located at {t} s, where the model reads {} m",
m.drift_m(t)
);
}
}
#[test]
fn the_bias_law_matches_the_engines_stochastic_dead_reckoner_stepped_forward() {
let (b, dt, t_end) = (3.0e-4f64, 0.01f64, 100.0f64);
let mut sim = AccelModel::new("bias", "test", b, 0.0);
let mut rng = ChaCha8Rng::seed_from_u64(1);
let n = (t_end / dt).round() as usize;
for _ in 0..n {
sim.step(dt, &mut rng);
}
let analytic = 0.5 * b * t_end * t_end;
assert!(
rel(sim.pos(), analytic) < 2.0e-4,
"stepped {} m vs closed form {analytic} m",
sim.pos()
);
}
#[test]
fn the_gyro_tilt_law_matches_the_engines_stochastic_dead_reckoner_stepped_forward() {
let (bg, dt, t_end) = (5.0e-6f64, 0.01f64, 100.0f64);
let mut sim = AccelModel::new("gyro", "test", 0.0, 0.0).with_gyro(bg, 0.0);
let mut rng = ChaCha8Rng::seed_from_u64(2);
let n = (t_end / dt).round() as usize;
for _ in 0..n {
sim.step(dt, &mut rng);
}
let analytic = G_M_PER_S2 * bg * t_end.powi(3) / 6.0;
assert!(
rel(sim.pos(), analytic) < 1.0e-3,
"stepped {} m vs closed form {analytic} m",
sim.pos()
);
}
fn accel_model_rms_m(build: impl Fn() -> AccelModel, runs: u64, dt: f64, t_end: f64) -> f64 {
let n = (t_end / dt).round() as usize;
let mut sum2 = 0.0;
for s in 0..runs {
let mut m = build();
let mut rng = ChaCha8Rng::seed_from_u64(0xC0A5_7000 + s);
for _ in 0..n {
m.step(dt, &mut rng);
}
sum2 += m.pos() * m.pos();
}
(sum2 / runs as f64).sqrt()
}
#[test]
fn the_velocity_random_walk_law_matches_a_monte_carlo_of_the_engines_dead_reckoner() {
let (vrw, dt, t_end, runs) = (2.0e-3f64, 0.05f64, 100.0f64, 300u64);
let measured = accel_model_rms_m(
|| AccelModel::new("vrw", "test", 0.0, vrw * vrw),
runs,
dt,
t_end,
);
let analytic = vrw * t_end.powf(1.5) / 3.0f64.sqrt();
assert!(
rel(measured, analytic) < 0.10,
"Monte-Carlo RMS {measured} m vs closed form {analytic} m over {runs} seeds"
);
}
#[test]
fn the_angle_random_walk_law_matches_a_monte_carlo_of_the_engines_dead_reckoner() {
let (arw, dt, t_end, runs) = (1.0e-4f64, 0.05f64, 100.0f64, 300u64);
let measured = accel_model_rms_m(
|| AccelModel::new("arw", "test", 0.0, 0.0).with_gyro(0.0, arw * arw),
runs,
dt,
t_end,
);
let analytic = G_M_PER_S2 * arw * t_end.powf(2.5) / 20.0f64.sqrt();
assert!(
rel(measured, analytic) < 0.10,
"Monte-Carlo RMS {measured} m vs closed form {analytic} m over {runs} seeds"
);
}
#[test]
fn the_scale_factor_law_matches_a_double_integration_of_the_engines_imu_error_model() {
let (ppm, accel, ramp, cruise, dt) = (500.0f64, 2.0f64, 40.0f64, 300.0f64, 1.0e-3f64);
let reference = scale_factor_reference_error_m(ppm, accel, ramp, cruise, dt);
let v = accel * ramp;
let distance = 0.5 * accel * ramp * ramp + v * cruise;
let analytic = ppm * PPM * distance;
assert!(
rel(reference, analytic) < 2.0e-3,
"ImuErrorModel double integration {reference} m vs s*distance {analytic} m"
);
}
#[test]
fn the_model_reduces_to_the_engines_existing_classical_ins_budget() {
let (bias, ppm, a_ref, psd) = (2.5e-4f64, 200.0f64, 1.5f64, 4.0e-8f64);
let existing = ClassicalInsBudget {
bias_m_s2: bias,
scale_factor_ppm: ppm,
ref_accel_m_s2: a_ref,
vrw_psd: psd,
};
let mine = CoastModel::new(
ImuParamsSi {
accel_bias_m_s2: bias,
accel_vrw_m_s_per_sqrt_s: psd.sqrt(),
accel_scale_factor: ppm * PPM,
gyro_bias_rad_s: 0.0,
gyro_arw_rad_per_sqrt_s: 0.0,
},
0.0,
a_ref,
Combination::Rss,
0.0,
);
for t in [1.0, 60.0, 600.0, 3600.0] {
assert!(
rel(mine.drift_m(t), existing.drift_m(t)) < 1.0e-12,
"t={t}: this model {} m vs ClassicalInsBudget {} m",
mine.drift_m(t),
existing.drift_m(t)
);
}
for th in [10.0, 50.0] {
let a = locate_crossing(&mine, th).coast_s.expect("reached");
let b = existing.inertial_holdover_s(th);
assert!(rel(a, b) < 1.0e-9, "threshold {th}: {a} s vs {b} s");
}
}
#[test]
fn the_three_combinations_order_the_crossings_rss_latest_and_linear_sum_earliest() {
let si = ImuGrade::Navigation.params().si();
let t = |k: Combination| {
locate_crossing(&CoastModel::new(si, 250.0, 0.5, k, 0.0), 10.0)
.coast_s
.expect("a navigation-grade coast reaches 10 m under every combination")
};
let (rss, mixed, sum) = (
t(Combination::Rss),
t(Combination::DetSumStochRss),
t(Combination::LinearSum),
);
assert!(
rss >= mixed && mixed >= sum,
"expected rss >= det-sum-stoch-rss >= linear-sum, got {rss} / {mixed} / {sum}"
);
for u in [1.0f64, 100.0, 1000.0] {
let m = |k: Combination| CoastModel::new(si, 250.0, 0.5, k, 0.0).drift_m(u);
assert!(m(Combination::Rss) <= m(Combination::DetSumStochRss) + 1.0e-12);
assert!(m(Combination::DetSumStochRss) <= m(Combination::LinearSum) + 1.0e-12);
}
}
#[test]
fn a_full_reset_fix_makes_every_inter_fix_excursion_identical() {
let m = CoastModel::new(
ImuGrade::Tactical.params().si(),
200.0,
0.0,
Combination::Rss,
17.0,
);
let c = m.trn_coast(TrnFixMode::FullReset, 120.0, 3600.0);
assert_eq!(c.intervals, 30);
let first = c.interval_errors_m[0];
for e in &c.interval_errors_m {
assert!(rel(*e, first) < 1.0e-12, "{e} vs {first}");
}
assert!(rel(c.peak_error_m, first) < 1.0e-12);
}
#[test]
fn a_position_only_fix_lets_each_excursion_exceed_the_last_and_the_peak_is_the_final_one() {
let m = CoastModel::new(
ImuGrade::Tactical.params().si(),
200.0,
0.0,
Combination::Rss,
17.0,
);
let c = m.trn_coast(TrnFixMode::PositionOnly, 120.0, 3600.0);
assert_eq!(c.intervals, 30);
for w in c.interval_errors_m.windows(2) {
assert!(
w[1] >= w[0],
"excursion sequence fell: {} -> {}",
w[0],
w[1]
);
}
let last = *c
.interval_errors_m
.last()
.expect("30 intervals were evaluated");
assert!(rel(c.peak_error_m, last) < 1.0e-12);
let full = m.trn_coast(TrnFixMode::FullReset, 120.0, 3600.0);
assert!(
c.peak_error_m > full.peak_error_m,
"position-only peak {} m should exceed full-reset peak {} m",
c.peak_error_m,
full.peak_error_m
);
}
#[test]
fn the_first_position_only_interval_reproduces_the_free_inertial_coast() {
let m = CoastModel::new(
ImuGrade::Navigation.params().si(),
250.0,
0.0,
Combination::Rss,
0.0,
);
let tau = 300.0;
let c = m.trn_coast(TrnFixMode::PositionOnly, tau, tau * 4.0);
assert!(
rel(c.interval_errors_m[0], m.drift_m(tau)) < 1.0e-12,
"{} vs {}",
c.interval_errors_m[0],
m.drift_m(tau)
);
}
#[test]
fn the_largest_fix_interval_holding_a_threshold_puts_the_peak_on_that_threshold() {
let cases: &[(TrnFixMode, ImuGrade, f64)] = &[
(TrnFixMode::FullReset, ImuGrade::Tactical, 3600.0),
(TrnFixMode::PositionOnly, ImuGrade::Navigation, 600.0),
];
for (mode, grade, mission) in cases {
let m = CoastModel::new(grade.params().si(), 200.0, 0.0, Combination::Rss, 5.0);
let (iv, status) = m.max_fix_interval_s(*mode, *mission, 50.0);
assert_eq!(status, "bisected", "{} mode: {status}", mode.as_str());
let iv = iv.expect("a bisected interval is present");
let peak = m.trn_coast(*mode, iv, *mission).peak_error_m;
assert!(
rel(peak, 50.0) < 1.0e-6,
"{} mode: interval {iv} s gives peak {peak} m, not 50 m",
mode.as_str()
);
}
}
#[test]
fn a_position_only_fix_cannot_bound_a_tactical_hour_at_any_fix_rate() {
let m = CoastModel::new(
ImuGrade::Tactical.params().si(),
200.0,
0.0,
Combination::Rss,
5.0,
);
let (iv, status) = m.max_fix_interval_s(TrnFixMode::PositionOnly, 3600.0, 50.0);
assert_eq!(status, "never-holds");
assert!(iv.is_none());
let tightest = 3600.0 / MAX_TRN_INTERVALS as f64;
let peak = m
.trn_coast(TrnFixMode::PositionOnly, tightest, 3600.0)
.peak_error_m;
assert!(
peak > 50.0,
"the tightest evaluated interval already had to breach 50 m, got {peak} m"
);
let (full_iv, full_status) = m.max_fix_interval_s(TrnFixMode::FullReset, 3600.0, 50.0);
assert_eq!(full_status, "bisected");
assert!(full_iv.expect("a bisected interval") > tightest);
}
#[test]
fn a_fix_residual_above_the_threshold_is_reported_as_never_holding_not_as_a_zero() {
let m = CoastModel::new(
ImuGrade::Navigation.params().si(),
250.0,
0.0,
Combination::Rss,
30.0,
);
let (iv, status) = m.max_fix_interval_s(TrnFixMode::FullReset, 3600.0, 10.0);
assert_eq!(status, "never-holds");
assert!(iv.is_none());
let c = locate_crossing(&m, 10.0);
assert_eq!(c.status, "already-exceeded-at-zero");
assert_eq!(c.coast_s, Some(0.0));
}
fn run(src: &str) -> serde_json::Value {
let scn: InsTrnCoastScenario = toml::from_str(src).expect("scenario parses");
let (json, _) = scn.run_json().expect("scenario runs");
serde_json::from_str(&json).expect("result is JSON")
}
#[test]
fn the_default_scenario_reports_a_ten_metre_and_a_fifty_metre_crossing() {
let v = run("kind = \"ins-trn-coast\"\n");
let cs = v["crossings"].as_array().expect("crossings array");
assert_eq!(cs.len(), 2);
assert_eq!(cs[0]["threshold_m"].as_f64(), Some(10.0));
assert_eq!(cs[1]["threshold_m"].as_f64(), Some(50.0));
for c in cs {
assert_eq!(c["status"].as_str(), Some("reached"));
let t = c["coast_s"].as_f64().expect("a located crossing");
assert!(t > 0.0 && t.is_finite());
}
assert!(
cs[1]["coast_s"].as_f64() > cs[0]["coast_s"].as_f64(),
"the 50 m crossing must come after the 10 m one"
);
}
#[test]
fn the_crossing_thresholds_are_inputs_and_moving_them_moves_the_crossings() {
let a = run("kind = \"ins-trn-coast\"\n");
let b = run("kind = \"ins-trn-coast\"\ncrossing_thresholds_m = [5.0, 25.0, 100.0]\n");
assert_eq!(b["crossings"].as_array().expect("array").len(), 3);
assert!(
b["crossings"][0]["coast_s"].as_f64() < a["crossings"][0]["coast_s"].as_f64(),
"a 5 m threshold must be crossed before a 10 m one"
);
}
#[test]
fn every_published_field_carries_a_unit_and_a_provenance_class() {
let v = run("kind = \"ins-trn-coast\"\ntrn_fix_mode = \"position-only\"\n");
let units = v["units"].as_object().expect("a units block");
assert!(units.len() >= 35, "only {} units declared", units.len());
for (field, e) in units {
assert!(
e["unit"].is_string(),
"{field} declares no unit in the units block"
);
let p = e["provenance"].as_str().unwrap_or_default();
assert!(
crate::field_schema::ProvenanceClass::parse(p).is_some(),
"{field} carries an unrecognised provenance class {p:?}"
);
}
let audit = crate::field_schema::audit_document(&v);
assert!(
audit.missing.is_empty() && audit.malformed.is_empty(),
"missing {:?}, malformed {:?}",
audit.missing,
audit.malformed
);
}
#[test]
fn the_document_names_the_combination_it_used_and_reports_all_three() {
let v = run("kind = \"ins-trn-coast\"\ncombination = \"linear-sum\"\n");
assert_eq!(v["combination"].as_str(), Some("linear-sum"));
let s = v["combination_sensitivity"]
.as_array()
.expect("sensitivity array");
assert_eq!(s.len(), 3);
let names: Vec<&str> = s.iter().filter_map(|r| r["combination"].as_str()).collect();
assert_eq!(names, vec!["rss", "linear-sum", "det-sum-stoch-rss"]);
}
#[test]
fn each_crossing_names_the_contribution_that_dominates_it() {
let v = run("kind = \"ins-trn-coast\"\n");
for c in v["crossings"].as_array().expect("array") {
let dom = c["dominant_contribution"]
.as_str()
.expect("a dominant contribution");
assert_ne!(dom, "none");
let rows = c["breakdown"].as_array().expect("a breakdown");
let best = rows
.iter()
.max_by(|a, b| {
a["error_m"]
.as_f64()
.unwrap_or(0.0)
.total_cmp(&b["error_m"].as_f64().unwrap_or(0.0))
})
.expect("a largest row");
assert_eq!(
best["name"].as_str(),
Some(dom),
"the named dominant contribution is not the largest row"
);
}
}
#[test]
fn a_navigation_grade_coast_is_accelerometer_bias_limited_at_both_default_thresholds() {
let v = run("kind = \"ins-trn-coast\"\n");
for c in v["crossings"].as_array().expect("array") {
assert_eq!(
c["dominant_contribution"].as_str(),
Some("accel_bias"),
"threshold {:?}",
c["threshold_m"]
);
}
}
#[test]
fn the_gyro_tilt_term_overtakes_the_bias_term_at_the_epoch_their_coefficients_predict() {
let m = CoastModel::new(
ImuGrade::Navigation.params().si(),
250.0,
0.0,
Combination::Rss,
0.0,
);
let cs = m.contributions();
let bias = cs
.iter()
.find(|c| c.name == "accel_bias")
.expect("accel_bias is always present");
let gyro = cs
.iter()
.find(|c| c.name == "gyro_bias_tilt")
.expect("gyro_bias_tilt is always present");
let t_star = bias.coefficient / gyro.coefficient;
assert!(
rel(bias.error_m(t_star), gyro.error_m(t_star)) < 1.0e-12,
"the two curves do not meet at {t_star} s"
);
assert!(gyro.error_m(0.5 * t_star) < bias.error_m(0.5 * t_star));
assert!(gyro.error_m(2.0 * t_star) > bias.error_m(2.0 * t_star));
}
#[test]
fn a_static_platform_has_no_scale_factor_contribution_at_all() {
let v = run("kind = \"ins-trn-coast\"\nspeed_m_s = 0.0\n");
for c in v["contributions"].as_array().expect("array") {
if c["name"].as_str() == Some("scale_factor_cruise")
|| c["name"].as_str() == Some("scale_factor_accel")
{
assert_eq!(c["coefficient_si"].as_f64(), Some(0.0));
}
}
}
#[test]
fn a_coarser_imu_grade_crosses_every_threshold_sooner() {
let v = run("kind = \"ins-trn-coast\"\n");
let rows = v["grade_table"].as_array().expect("grade table");
assert_eq!(rows.len(), 4);
for i in 1..rows.len() {
for k in 0..2 {
let prev = rows[i - 1]["crossings"][k]["coast_s"]
.as_f64()
.expect("a located crossing");
let cur = rows[i]["crossings"][k]["coast_s"]
.as_f64()
.expect("a located crossing");
assert!(
cur < prev,
"grade {:?} crosses later than {:?}",
rows[i]["grade"],
rows[i - 1]["grade"]
);
}
}
}
#[test]
fn the_swept_drift_band_question_is_answered_per_grade_with_a_status_never_a_blank() {
let v = run("kind = \"ins-trn-coast\"\n");
let b = &v["swept_drift_band"];
assert_eq!(b["lo_m_per_s"].as_f64(), Some(0.001));
assert_eq!(b["hi_m_per_s"].as_f64(), Some(0.050));
for r in b["rows"].as_array().expect("band rows") {
let s = r["status"].as_str().expect("a status");
assert!(!s.is_empty());
if r["band_entry_s"].is_null() && r["band_exit_s"].is_null() {
assert_ne!(s, "band-spanned");
}
}
}
#[test]
fn a_slow_platform_puts_a_navigation_grade_unit_inside_the_swept_band() {
let v = run("kind = \"ins-trn-coast\"\nspeed_m_s = 5.0\n");
let row = &v["swept_drift_band"]["rows"][0];
assert_eq!(row["grade"].as_str(), Some("navigation"));
assert_eq!(row["status"].as_str(), Some("band-spanned"));
let entry = row["band_entry_s"].as_f64().expect("a band entry");
let exit = row["band_exit_s"].as_f64().expect("a band exit");
assert!(entry < exit && entry > 0.0);
}
#[test]
fn the_trn_block_reports_a_bounded_peak_and_the_largest_interval_that_holds_it() {
let v = run("kind = \"ins-trn-coast\"\nimu_grade = \"tactical\"\n\
trn_fix_mode = \"position-only\"\ntrn_fix_residual_m = 5.0\n\
trn_fix_interval_s = 60.0\n");
let t = &v["trn"];
assert_eq!(t["fix_mode"].as_str(), Some("position-only"));
assert_eq!(t["intervals_evaluated"].as_u64(), Some(60));
let peak = t["peak_error_m"].as_f64().expect("a peak");
assert!(peak.is_finite() && peak > 0.0);
let free = run("kind = \"ins-trn-coast\"\nimu_grade = \"tactical\"\n")["trn"]
["peak_error_m"]
.as_f64()
.expect("a free-inertial peak");
assert!(
peak < free,
"TRN aiding must bound the coast: {peak} m vs free-inertial {free} m"
);
}
#[test]
fn an_unknown_grade_combination_or_fix_mode_is_an_error_naming_the_accepted_set() {
for (src, needle) in [
(
"kind = \"ins-trn-coast\"\nimu_grade = \"military\"\n",
"navigation",
),
(
"kind = \"ins-trn-coast\"\ncombination = \"quadrature\"\n",
"rss",
),
(
"kind = \"ins-trn-coast\"\ntrn_fix_mode = \"tight\"\n",
"position-only",
),
] {
let scn: InsTrnCoastScenario = toml::from_str(src).expect("parses");
let e = scn
.run_json()
.expect_err("an unknown name must be an error");
assert!(
e.contains(needle),
"error {e:?} does not name the accepted set"
);
}
}
#[test]
fn a_per_coefficient_override_relabels_the_grade_as_custom() {
let v = run("kind = \"ins-trn-coast\"\naccel_bias_ug = 40.0\n");
assert_eq!(v["imu"]["grade"].as_str(), Some("custom"));
assert_eq!(v["imu"]["accel_bias_ug"].as_f64(), Some(40.0));
}
#[test]
fn a_negative_or_non_finite_input_is_refused_rather_than_coasted_on() {
for src in [
"kind = \"ins-trn-coast\"\naccel_bias_ug = -1.0\n",
"kind = \"ins-trn-coast\"\nspeed_m_s = -10.0\n",
"kind = \"ins-trn-coast\"\ncrossing_thresholds_m = [0.0]\n",
"kind = \"ins-trn-coast\"\ncrossing_thresholds_m = []\n",
"kind = \"ins-trn-coast\"\ndrift_band_lo_m_per_s = 0.1\ndrift_band_hi_m_per_s = 0.01\n",
"kind = \"ins-trn-coast\"\nmission_duration_s = 0.0\n",
] {
let scn: InsTrnCoastScenario = toml::from_str(src).expect("parses");
assert!(
scn.run_json().is_err(),
"input {src:?} should have been refused"
);
}
}
#[test]
fn the_result_document_carries_no_non_finite_number() {
fn walk(v: &serde_json::Value, path: &str) {
match v {
serde_json::Value::Number(n) => {
let x = n.as_f64().unwrap_or(f64::NAN);
assert!(x.is_finite(), "non-finite number at {path}");
}
serde_json::Value::Array(a) => {
for (i, e) in a.iter().enumerate() {
walk(e, &format!("{path}[{i}]"));
}
}
serde_json::Value::Object(o) => {
for (k, e) in o {
walk(e, &format!("{path}.{k}"));
}
}
_ => {}
}
}
for src in [
"kind = \"ins-trn-coast\"\n",
"kind = \"ins-trn-coast\"\nspeed_m_s = 0.0\nimu_grade = \"consumer\"\n",
"kind = \"ins-trn-coast\"\ntrn_fix_mode = \"full-reset\"\n",
"kind = \"ins-trn-coast\"\ntrn_fix_mode = \"position-only\"\ntrn_fix_residual_m = 0.0\n",
"kind = \"ins-trn-coast\"\naccel_bias_ug = 0.0\naccel_vrw_m_s_per_sqrt_hr = 0.0\n\
accel_scale_factor_ppm = 0.0\ngyro_bias_deg_per_hr = 0.0\n\
gyro_arw_deg_per_sqrt_hr = 0.0\n",
] {
walk(&run(src), "root");
}
}
#[test]
fn an_error_free_imu_never_reaches_a_threshold_and_says_so_instead_of_reporting_zero() {
let v = run(
"kind = \"ins-trn-coast\"\naccel_bias_ug = 0.0\naccel_vrw_m_s_per_sqrt_hr = 0.0\n\
accel_scale_factor_ppm = 0.0\ngyro_bias_deg_per_hr = 0.0\n\
gyro_arw_deg_per_sqrt_hr = 0.0\n",
);
for c in v["crossings"].as_array().expect("array") {
assert_eq!(c["status"].as_str(), Some("never-reached"));
assert!(c["coast_s"].is_null());
assert!(c["implied_mean_drift_rate_m_per_s"].is_null());
}
}
#[test]
fn the_scenario_runs_through_the_engines_public_dispatch_and_is_reproducible() {
let src = "kind = \"ins-trn-coast\"\nimu_grade = \"tactical\"\n\
trn_fix_mode = \"position-only\"\n";
let a = crate::api::run_toml(src).expect("dispatch reaches the pack");
let b = crate::api::run_toml(src).expect("second run");
assert_eq!(a.json, b.json, "the same source produced different JSON");
assert!(a.summary.starts_with("ins-trn-coast |"));
assert!(!a.svg.is_empty());
assert_eq!(
crate::api::ScenarioKind::classify(src).expect("classifies"),
crate::api::ScenarioKind::InsTrnCoast
);
}
}