Skip to main content

hpr_flightdata/
smoother.rs

1//! The smoother: a flight's height and vertical speed at every sample, each with its standard
2//! error, from the barometer, the axial accelerometer and a GNSS receiver together, and a health
3//! card saying how each sensor fared ([M7.2d][m7-2d]).
4//!
5//! **What it is for.** The [readings] take one number each from one track. The
6//! smoother takes the whole flight from every sensor the log holds, and says at each moment how
7//! far the truth may be: its 90% interval is the estimate ± [`INTERVAL_Z`] standard errors. On
8//! HPR Sim's synthetic flights, whose truth is known, it is checked at burnout, the top speed,
9//! apogee and a drawn moment (`cargo xtask synthetic`). There the three sets' cells hold between
10//! 94.2% and 99.4% of the truths inside the 90% intervals. Two logger cells hold 88.7% and 89.2%,
11//! which the check allows. Its rules were set with those sets in view.
12//!
13//! **How far to trust it.** It has not been compared with a real flight's truth. Its errors are
14//! the sensors' errors as the synthetic flights draw them, from the makers' datasheets and
15//! measured public logs ([ADR-280][adr-280] §3, the plan for the synthetic flights). A real logger
16//! whose errors are larger or of another kind, such as a port that reads the flow differently or
17//! a swing correlated over more than a second or two, can leave the truth outside the interval
18//! more often than it says; the health card shows where its sensors disagreed. It needs a barometer: a GNSS track alone is never trusted, as its receiver
19//! lags a climb by up to hundreds of meters while flagging its fix valid
20//! (`docs/research/gnss-in-boost.md`).
21//!
22//! **Method** (`docs/research/altimeter-smoothing-methods.md`, "What this suggests" 1–2, 5;
23//! [ADR-300][adr-300], the smoother and its check). A Kalman filter runs forward over every sample, then a Rauch–Tung–Striebel pass back
24//! (`kalman`). Its state is the logger's height above the pad, its vertical speed and
25//! acceleration, and the sensors' own errors: the accelerometer's bias, scale and a slowly
26//! changing model error (the rocket's tilt puts its axis off the vertical); the barometer's slow
27//! correlated noise, its drift and the drift's rate, and the static port's pressure coefficient in
28//! the fall; the day's air off the standard atmosphere as a share of the height, which only a GNSS
29//! receiver measures; and the receiver's offset. The acceleration is a white-jerk state that the
30//! accelerometer measures (`a + g` along the axis, gravity's reaction in it), so a log with no
31//! accelerometer, or one clipped, uses the same model with nothing measuring it. On the pad the
32//! height, speed and acceleration are held at zero until liftoff, found from the accelerometer
33//! where there is one.
34//!
35//! The climb's port coefficient can't be learned from the log, so it is held at its prior, and
36//! the log is smoothed again with it a standard deviation off, replaying the first run's choices;
37//! each standard error is widened by the difference ([`smooth_with`]).
38//!
39//! - **The barometer** is de-weighted from [`TRANSONIC_FROM_MACH`] to [`TRANSONIC_FULL_MACH`] and
40//!   above, where its port's pressure coefficient jumps (`docs/research/altimeter-mach-error.md`
41//!   §1–2), for [`CHARGE_WINDOW_S`] after each charge the log records, and around its own apogee
42//!   ([`APOGEE_CHARGE_SPAN_M`]), where a charge fires whether or not the log names it.
43//! - **The GNSS receiver** is fused at zero weight from liftoff until its vertical speed has
44//!   agreed with the smoother's for [`GNSS_AGREE_WINDOW_S`] (`gnss-in-boost.md` §5), then with
45//!   its variance inflated for [`GNSS_SETTLE_S`]; never on its fix flag.
46//! - **Every sensor** is gated: a sample whose squared innovation passes [`GATE_SIGMAS`]² of its
47//!   variance is rejected and listed, and a sensor rejected for [`GATE_RESET_S`] is taken back
48//!   with the height's and speed's uncertainty widened by the gap.
49//!
50//! [m7-2d]: https://hpr.fusionspace.co/decisions-and-roadmap.html#m7-2d
51//! [adr-280]: https://github.com/nrdptel/fusionspace-eridanus/blob/main/docs/decisions/0280-m7-2-in-seven-the-synthetic-flights-first.md
52//! [adr-300]: https://github.com/nrdptel/fusionspace-eridanus/blob/main/docs/decisions/0300-the-smoother-and-its-check.md
53
54mod kalman;
55
56use hpr_atmos::Ussa76;
57use hpr_core::gravity::STANDARD_GRAVITY_MPS2;
58
59use schemars::JsonSchema;
60use serde::{Deserialize, Serialize};
61use std::collections::VecDeque;
62
63use crate::log::{Channel, FlightLog, Quantity};
64use crate::readings::{
65    self, ACCEL_BIAS_M_S2, ACCEL_SCALE_SHARE, AIR_TEMPERATURE_SPREAD_K, BAROMETER_GAIN, Clip,
66    DRIFT_M_PER_S, GRAVITY_TOLERANCE_G, Gravity, RAIL_TILT_DEG, Reading, Readings, Source,
67    standard_temperature_k,
68};
69use kalman::{Matrix, N, PACKED, Vector};
70
71const G0: f64 = STANDARD_GRAVITY_MPS2;
72
73// The state's entries.
74/// Height above the pad, m.
75const H: usize = 0;
76/// Vertical speed, m/s, up.
77const V: usize = 1;
78/// Vertical acceleration, m/s², up.
79const A: usize = 2;
80/// The accelerometer's bias, m/s².
81const BIAS: usize = 3;
82/// The accelerometer's scale error.
83const SCALE: usize = 4;
84/// The accelerometer's model error, m/s²: its axis off the vertical, and the drag a sideways
85/// speed adds along it.
86const TILT: usize = 5;
87/// The barometer's slow correlated noise, m.
88const SLOW: usize = 6;
89/// The barometer's drift, m.
90const DRIFT: usize = 7;
91/// The drift's rate, m/s.
92const DRIFT_RATE: usize = 8;
93/// The day's air off the standard atmosphere, as a share of the height: `−ΔT/T_pad`. Only a GNSS
94/// receiver's geometric heights measure it; without one it is held at its prior and its spread
95/// carried into the output.
96const AIR: usize = 9;
97/// The GNSS receiver's altitude less the height above the pad, m.
98const GNSS: usize = 10;
99/// The static port's pressure coefficient in a fall: its own, as a fall's attitude isn't the
100/// climb's, and one shared would let a long fast fall set the climb's.
101const PORT_FALL: usize = 11;
102
103/// The standard normal quantile at 95%: the estimate ± this many standard errors is a two-sided
104/// 90% interval.
105pub const INTERVAL_Z: f64 = 1.644_853_626_951_472_2;
106
107/// A sample is rejected when its innovation passes this many of its standard deviations: PX4's
108/// default barometer gate, 5 (its EKF2 parameter `EKF2_BARO_GATE`; the smoothing note §7).
109pub const GATE_SIGMAS: f64 = 5.0;
110
111/// A sensor rejected this long is taken back, s: HPR Sim's choice, longer than a charge's pulse
112/// (0.3 s on the public Pnut log the synthetic flights copy) and shorter than any transonic span.
113pub const GATE_RESET_S: f64 = 1.0;
114
115/// The barometer's fast correlated noise, 1σ, m: an AR(1) process of `φ = 0.5` at 50 Hz, measured
116/// on public logs (`docs/research/altimeter-sensor-noise.md` §6).
117pub const FAST_NOISE_M: f64 = 0.5;
118
119/// Its time constant, s: `−1/(50 ln 0.5)`.
120pub const FAST_NOISE_TAU_S: f64 = 0.028_853_900_817_779_27;
121
122/// The barometer's slow correlated noise, 1σ, m: the top of the 0.24–0.29 m of a first-order
123/// Gauss–Markov term measured on a BMP085 at rest (`altimeter-sensor-noise.md` §3).
124pub const SLOW_NOISE_M: f64 = 0.29;
125
126/// Its time constant, s: within the 0.65–1.26 s measured beside it.
127pub const SLOW_NOISE_TAU_S: f64 = 1.0;
128
129/// The prior on the port's pressure coefficient: its mean. The ogive-cylinder's measured
130/// coefficient a caliber or more behind the nose's shoulder is −0.02 to 0.00 below Mach 0.9
131/// (`altimeter-mach-error.md` §1–2); HPR Sim's prior is centered there…
132pub const PORT_PRIOR_MEAN: f64 = -0.01;
133
134/// …with this standard deviation, half the range again either side. The climb's coefficient is
135/// held there and enters each interval by the consider run ([`smooth_with`]); learned from the
136/// log, it came out biased by 0.010 claiming 0.001 on the synthetic flights' barometer-only
137/// loggers ([ADR-300], the smoother and its check).
138///
139/// [ADR-300]: https://github.com/nrdptel/fusionspace-eridanus/blob/main/docs/decisions/0300-the-smoother-and-its-check.md
140pub const PORT_PRIOR_SD: f64 = 0.02;
141
142/// The Mach number, estimated high (`mach_high`), at which the barometer begins to be
143/// de-weighted: below it the port's coefficient holds within 0.01 of its subsonic value on the
144/// tunnel's ogive-cylinder a caliber or more behind the shoulder (`altimeter-mach-error.md`
145/// §1–2). HPR Sim's margin below that note's 0.9.
146pub const TRANSONIC_FROM_MACH: f64 = 0.8;
147
148/// The Mach number from which the barometer is fully de-weighted.
149pub const TRANSONIC_FULL_MACH: f64 = 0.9;
150
151/// How far the port's coefficient may move from its subsonic value once fully de-weighted: the
152/// tunnel's largest jump, −0.07 to −0.20 at the shoulder from Mach 0.7 to 1.0, rounded up.
153pub const TRANSONIC_PORT_SHIFT: f64 = 0.2;
154
155/// The time over which a de-weighted span's error is correlated, s: the second or so a climb takes
156/// from Mach 0.8 through 1.0, and longer than a charge's pulse. HPR Sim's choice.
157pub const DEWEIGHT_TAU_S: f64 = 1.0;
158
159/// How much a first-order Gauss–Markov error of time constant `tau_s` weighs as one sample's white
160/// variance when samples are `dt_s` apart: `(1 + φ)/(1 − φ)`, `φ = e^{−Δt/τ}`, the variance of the
161/// mean of many such samples over that of white ones (the smoothing note §6).
162fn correlated_factor(dt_s: f64, tau_s: f64) -> f64 {
163    let phi = (-dt_s / tau_s).exp();
164    (1.0 + phi) / (1.0 - phi)
165}
166
167/// How long after a charge the log records the barometer is de-weighted, s: the public Pnut
168/// log's pulse lasts 0.3 s.
169pub const CHARGE_WINDOW_S: f64 = 0.5;
170
171/// A charge is expected at apogee whether or not the log names it (a dual-deploy drogue, or a
172/// motor's ejection timed for it), so the barometer is de-weighted from the first time its
173/// running median comes within this of its highest to the last, and [`CHARGE_WINDOW_S`] after,
174/// m: a coast passes within 5 m of its apogee for about a second either side, `√(2·5/g)`, wider
175/// than the running median's noise lets the top be placed. HPR Sim's choice.
176pub const APOGEE_CHARGE_SPAN_M: f64 = 5.0;
177
178/// The barometer's standard deviation in that window, m: twice the Pnut pulse's 16 m.
179pub const CHARGE_SD_M: f64 = 32.0;
180
181/// Words in an event's name that mark a charge.
182pub const CHARGE_WORDS: [&str; 7] = [
183    "drogue", "main", "charge", "deploy", "pyro", "eject", "apogee",
184];
185
186/// The accelerometer's model error as a share of its reading: the axis `θ` off the vertical
187/// costs `1 − cos θ` of the reading. Under power the axis is within a rail's 20°
188/// ([`RAIL_TILT_DEG`]). In the coast it pitches over as the climb slows while the sideways speed
189/// through the air stays, `tan θ = v_side/v`: at most the burnout's `sin 20°` share of the top
190/// speed plus a [`WIND_LIMIT_M_S`] wind, so the share grows toward one at apogee, where the
191/// reading is small.
192pub fn tilt_share(climb_m_s: f64, top_speed_m_s: f64, powered: bool) -> f64 {
193    let rail = 1.0 - RAIL_TILT_DEG.to_radians().cos();
194    if powered {
195        return rail;
196    }
197    let side = RAIL_TILT_DEG.to_radians().sin() * top_speed_m_s + WIND_LIMIT_M_S;
198    let climb = climb_m_s.max(0.0);
199    let coast = 1.0 - climb / climb.hypot(side);
200    rail.max(coast)
201}
202
203/// The largest wind a high-power flight leaves the pad in, m/s: 20 mph, the most the NAR
204/// high-power safety code and NFPA 1127 allow.
205pub const WIND_LIMIT_M_S: f64 = 8.9408;
206
207/// The model error's floor, m/s²: the drag a sideways speed puts along the axis near apogee, where
208/// the rocket may lie far over. HPR Sim's choice.
209pub const TILT_FLOOR_M_S2: f64 = 0.5;
210
211/// The model error's time constant, s. HPR Sim's choice: a tilt changes over seconds.
212pub const TILT_TAU_S: f64 = 2.0;
213
214/// The white jerk's spectral density on the pad, m²/s⁵.
215pub const JERK_PAD: f64 = 1.0;
216
217/// …while the motor may be burning, m²/s⁵: lets the acceleration rise by 100 m/s² within 0.05 s
218/// at 3σ, as a motor's thrust does.
219pub const JERK_POWERED: f64 = 1.0e5;
220
221/// …in the coast to apogee, m²/s⁵: the drag changes slowly.
222pub const JERK_COAST: f64 = 10.0;
223
224/// …in the descent, m²/s⁵: a canopy's opening changes the speed within a second or two.
225pub const JERK_DESCENT: f64 = 100.0;
226
227/// How long the motor may still burn after the acceleration first falls below `−g/2`, s.
228pub const POWERED_HOLD_S: f64 = 1.0;
229
230/// A climb is under way, for finding its burnout and apogee, once its speed has passed this, m/s.
231pub const APOGEE_SPEED_M_S: f64 = 10.0;
232
233/// The pad's hold on the height, speed and acceleration: their standard deviation there.
234pub const PAD_SD: f64 = 0.01;
235
236/// The GNSS receiver's speed must agree with the smoother's this long before it is fused, s
237/// (`gnss-in-boost.md` §5: "for at least 2 s").
238pub const GNSS_AGREE_WINDOW_S: f64 = 2.0;
239
240/// …within this, m/s ("within a few m/s"), and within twice their combined standard error.
241pub const GNSS_AGREE_M_S: f64 = 3.0;
242
243/// A gap between fixes this long ends the receiver's trust, s: a dropout.
244pub const GNSS_GAP_S: f64 = 2.0;
245
246/// After trust, its variance is inflated this long, s…
247pub const GNSS_SETTLE_S: f64 = 5.0;
248
249/// …this many times.
250pub const GNSS_SETTLE_FACTOR: f64 = 4.0;
251
252/// The receiver's altitude noise when its pad can't say, m; its pad's scatter is floored at
253/// [`GNSS_SD_FLOOR_M`].
254pub const GNSS_SD_DEFAULT_M: f64 = 10.0;
255
256/// The least its noise is taken as, m.
257pub const GNSS_SD_FLOOR_M: f64 = 2.0;
258
259/// The prior on its offset, 1σ, m, set again each time it is trusted.
260pub const GNSS_OFFSET_SD_M: f64 = 1000.0;
261
262/// The offset's random walk, m²/s: a receiver's lead or lag changes as the acceleration does.
263pub const GNSS_OFFSET_WALK: f64 = 4.0;
264
265/// The pad's height above sea level taken where the log can't say, m, for the coldest air and
266/// slowest sound the barometer's de-weighting allows: higher than most launch sites.
267pub const UNKNOWN_PAD_MSL_M: f64 = 3000.0;
268
269/// The fewest barometer samples on the pad that zero it and measure its noise. A log with fewer,
270/// as one written from launch detection is, is smoothed on an assumed pad ([`Health::assumed_pad`]).
271pub const PAD_MIN_SAMPLES: usize = 5;
272
273/// The prior on the height at a launch-started log's first sample, 1σ, m, about that sample: wide
274/// enough that the barometer alone places it, whatever the offset state below holds.
275pub const ASSUMED_HEIGHT_SD_M: f64 = 100.0;
276
277/// The prior on the speed at a launch-started log's first sample, 1σ, m/s, about zero: not about
278/// the barometer's rate there, whose running median has too few samples at a log's edge to set one
279/// garbled sample aside. A rocket that has climbed the 49 m (160 ft) a StratoLogger's launch detection
280/// allows at 30 g is doing about 170 m/s, under two of these. HPR Sim's choice.
281pub const ASSUMED_SPEED_SD_M_S: f64 = 100.0;
282
283/// The prior on the acceleration at a launch-started log's first sample, 1σ, m/s²: 30 g, about
284/// zero, which holds a motor's first spike on most hobby rockets. HPR Sim's choice.
285pub const ASSUMED_ACCELERATION_SD_M_S2: f64 = 300.0;
286
287/// How long a launch-started log's barometer is settled for, s, at least [`ASSUMED_SETTLE_SAMPLES`]:
288/// a filter this wide takes any sample as the truth, and has narrowed by then. Over this span each
289/// sample fused is the median of the five about it, which leaves a climb as it was and takes out
290/// one or two bad samples, and the first two, which have no such window, aren't weighed. On a log
291/// with a pad, the pad's narrow priors reject those samples at the gate.
292pub const ASSUMED_SETTLE_S: f64 = 1.0;
293
294/// The fewest samples a launch-started log's barometer is settled over.
295pub const ASSUMED_SETTLE_SAMPLES: usize = 10;
296
297/// The most steps the smoother takes, every sensor's samples together: two sensors at 500 Hz for
298/// 33 minutes. The synthetic flights' longest 500 Hz logs pass 500,000. The filter holds two covariances a step for the pass
299/// back, over 1 kB, so a step count without a limit is a memory limit a file could reach: a file just
300/// under this one takes tens of seconds and gigabytes, by a run in review, not a committed test.
301pub const MAX_STEPS: usize = 2_000_000;
302
303/// The vertical speed past which a smoothed track is withheld as [`Reason::Diverged`], m/s:
304/// Earth's escape speed at its surface, `√(2 GM/R)` with `GM` = 398,600 km³/s² and the mean radius
305/// `R` = 6,371.0 km, the 11.186 km/s of NASA's *Earth Fact Sheet* (D. R. Williams, NASA Space
306/// Science Data Coordinated Archive, Goddard Space Flight Center). Nothing that climbs on a hobby
307/// motor and comes back passes it, so an estimate that does is the filter's, not the rocket's.
308///
309/// It is a bound on a diverged estimate, not a test of a good one. A take-back
310/// ([`GATE_RESET_S`]) reads a sensor's gap as motion the filter missed and widens the speed's
311/// and the acceleration's variances by it. With two height sensors that disagree past their
312/// gates and can both be taken back, the other sensor's samples, fused against those variances,
313/// swing the speed, and the next take-back can meet a wider gap, without limit:
314/// [`GnssTrust::FromLiftoff`] does it on some of the synthetic flights ([#1590]).
315/// [`GnssTrust::Agreement`] never takes the receiver back, and the synthetic flights' check
316/// fails if it withholds one of their logs so. Three of the sample logs' 1,058 records still
317/// run away under it, by a road not traced yet ([#1073]), and are withheld
318/// (`validation/reports/log-import.md`, *The smoother*).
319///
320/// [#1073]: https://github.com/nrdptel/fusionspace-eridanus/issues/1073
321///
322/// [#1590]: https://github.com/nrdptel/fusionspace-eridanus/issues/1590
323pub const DIVERGED_SPEED_M_S: f64 = 11_186.0;
324
325/// How the GNSS receiver is trusted.
326#[derive(Debug, Clone, Copy, PartialEq, Eq, Default, Serialize, Deserialize)]
327#[serde(rename_all = "snake_case")]
328#[non_exhaustive]
329pub enum GnssTrust {
330    /// At zero weight from liftoff until its speed agrees with the smoother's.
331    #[default]
332    Agreement,
333    /// At its full weight from liftoff, its offset set on the pad: never right for a flight, a
334    /// mutation that the synthetic flights' check shows fails.
335    FromLiftoff,
336}
337
338/// How the smoother runs.
339#[derive(Debug, Clone, Copy, PartialEq, Eq, Default, Serialize, Deserialize)]
340pub struct Options {
341    /// How the GNSS receiver is trusted.
342    pub gnss_trust: GnssTrust,
343}
344
345/// Why a log isn't smoothed.
346#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize, JsonSchema)]
347#[serde(rename_all = "snake_case")]
348#[non_exhaustive]
349pub enum Reason {
350    /// The log has no barometer: no height track, or a GNSS receiver's alone.
351    NoBarometer,
352    /// The readings found no liftoff.
353    NoLiftoff,
354    /// More than [`MAX_STEPS`] samples.
355    TooLong,
356    /// The estimate left the finite numbers somewhere in the log, or its vertical speed passed
357    /// Earth's escape speed, 11,186 m/s ([`DIVERGED_SPEED_M_S`]): it is withheld whole rather
358    /// than given with a hole, or with numbers no rocket flew.
359    Diverged,
360}
361
362/// A log that isn't smoothed, and why.
363#[derive(Debug, Clone, PartialEq, Serialize, Deserialize, JsonSchema)]
364pub struct Withheld {
365    /// The reason, as a code.
366    pub reason: Reason,
367    /// The reason in words.
368    pub detail: String,
369}
370
371/// The sensors.
372#[derive(
373    Debug, Clone, Copy, PartialEq, Eq, Hash, PartialOrd, Ord, Serialize, Deserialize, JsonSchema,
374)]
375#[serde(rename_all = "snake_case")]
376#[non_exhaustive]
377pub enum Sensor {
378    /// The barometer's height track.
379    Barometer,
380    /// The axial accelerometer.
381    Accelerometer,
382    /// The GNSS receiver's altitude.
383    Gnss,
384}
385
386/// How liftoff was found.
387#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize, JsonSchema)]
388#[serde(rename_all = "snake_case")]
389pub enum LiftoffFrom {
390    /// The accelerometer's first rise off its pad.
391    Accelerometer,
392    /// The barometer's track, as the readings find it.
393    Barometer,
394}
395
396/// Liftoff, as the smoother holds it.
397#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize, JsonSchema)]
398pub struct Liftoff {
399    /// The last moment on the pad, s on the log's clock.
400    pub time_s: f64,
401    /// The bound on its error, s: from the accelerometer, two of its intervals; from the
402    /// barometer, the reading's.
403    pub bound_s: f64,
404    /// How it was found.
405    pub from: LiftoffFrom,
406}
407
408/// Why the barometer was de-weighted.
409#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize, JsonSchema)]
410#[serde(rename_all = "snake_case")]
411pub enum Deweighted {
412    /// At or above [`TRANSONIC_FROM_MACH`].
413    Transonic,
414    /// Within [`CHARGE_WINDOW_S`] after a charge.
415    Charge,
416}
417
418/// A span over which the barometer was de-weighted.
419#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize, JsonSchema)]
420pub struct Span {
421    /// Its first sample, s on the log's clock.
422    pub start_s: f64,
423    /// Its last sample, s.
424    pub end_s: f64,
425    /// Why.
426    pub why: Deweighted,
427}
428
429/// A rejected sample.
430#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize, JsonSchema)]
431pub struct Rejected {
432    /// The sensor.
433    pub sensor: Sensor,
434    /// When, s on the log's clock.
435    pub time_s: f64,
436    /// Its test ratio: the squared innovation over its variance, over [`GATE_SIGMAS`]²; above 1
437    /// is rejected.
438    pub ratio: f64,
439}
440
441/// A sensor's largest test ratio.
442#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize, JsonSchema)]
443pub struct Largest {
444    /// The ratio.
445    pub ratio: f64,
446    /// When, s on the log's clock.
447    pub time_s: f64,
448    /// The Mach number then, from the filter's vertical speed and the standard atmosphere's
449    /// sound at its height.
450    pub mach: f64,
451}
452
453/// How one sensor fared.
454#[derive(Debug, Clone, PartialEq, Serialize, Deserialize, JsonSchema)]
455pub struct SensorHealth {
456    /// The sensor.
457    pub sensor: Sensor,
458    /// Its samples.
459    pub samples: usize,
460    /// Those fused.
461    pub used: usize,
462    /// Those rejected by the gate.
463    pub rejected: usize,
464    /// Those not weighed: a GNSS fix before the receiver is trusted, an accelerometer's clipped
465    /// sample or one past apogee, a launch-started log's first two barometer samples.
466    pub unweighed: usize,
467    /// How many times the sensor was taken back after [`GATE_RESET_S`] of rejections.
468    pub resets: usize,
469    /// The share of the samples tested that the gate passed; `None` if none was tested.
470    pub inside_gate_share: Option<f64>,
471    /// The largest test ratio, where it was.
472    pub largest: Option<Largest>,
473    /// The mean normalized innovation squared over the samples fused, `ν²/S`, `S` with one
474    /// sample's own variance as the gate weighs it: about 1 when the sensor's noise is what the
475    /// smoother takes it to be, below 1 when it takes it as noisier.
476    pub mean_nis: Option<f64>,
477}
478
479/// A log's health card: how each sensor fared against the others (the smoothing note §7, after
480/// PX4's and ArduPilot's estimator status).
481#[derive(Debug, Clone, PartialEq, Serialize, Deserialize, JsonSchema)]
482pub struct Health {
483    /// Each sensor the log holds.
484    pub sensors: Vec<SensorHealth>,
485    /// The spans the barometer was de-weighted over.
486    pub deweighted: Vec<Span>,
487    /// Every sample rejected, in time order.
488    pub rejected: Vec<Rejected>,
489    /// When the GNSS receiver was first trusted, s on the log's clock; `None` if never, or if the
490    /// log has none.
491    pub gnss_trusted_s: Option<f64>,
492    /// The barometer's height less the accelerometer's alone, integrated from liftoff with its
493    /// pad's reading taken out, at the smoothed apogee, m; `None` with no accelerometer.
494    pub baro_minus_inertial_at_apogee_m: Option<f64>,
495    /// Steps where the pass back couldn't solve and kept the forward filter's estimate.
496    pub unsmoothed_steps: usize,
497    /// Whether the log holds fewer than [`PAD_MIN_SAMPLES`] barometer samples on the pad, as one
498    /// written from launch detection does. The pad is then assumed: the height stands on the
499    /// readings' zero (`Readings::pad_altitude_m`), the barometer's noise comes from the flight's
500    /// own scatter, and no sample is held still.
501    pub assumed_pad: bool,
502    /// Whether the log's accelerometer had too little of the pad to take its level from, and was
503    /// set aside: it isn't in [`Health::sensors`].
504    pub accelerometer_set_aside: bool,
505    /// The smoothed apogee's height less the reading's, m; `None` where the apogee isn't read.
506    pub apogee_less_reading_m: Option<f64>,
507    /// Whether that gap passes the reading's bound: the reading's error stays within its bound, so
508    /// the track has gone wrong around its top, as it can where a long transonic span leaves it to
509    /// the motion model. The bound holds a floor's truth only from below, and a smoothed apogee
510    /// above a floor doesn't count.
511    pub apogee_past_reading_bound: bool,
512}
513
514impl Sensor {
515    /// Its name.
516    pub fn name(self) -> &'static str {
517        match self {
518            Self::Barometer => "barometer",
519            Self::Accelerometer => "accelerometer",
520            Self::Gnss => "GNSS",
521        }
522    }
523}
524
525impl Health {
526    /// The card as text: a line each if the pad was assumed or the accelerometer set aside, a line
527    /// for each sensor, then the barometer's de-weighted spans, when
528    /// the GNSS receiver was first trusted, the barometer's height less the accelerometer's at
529    /// apogee, and the pass back. Times are on the log's clock; numbers are to three significant
530    /// figures.
531    pub fn card(&self) -> String {
532        let mut out = String::new();
533        if self.assumed_pad {
534            out.push_str(
535                "pad assumed: too little of it in the log; heights on the readings' zero, noise \
536                 from the flight's scatter\n",
537            );
538        }
539        if self.accelerometer_set_aside {
540            out.push_str("accelerometer set aside: too little of the pad to take its level from\n");
541        }
542        for s in &self.sensors {
543            out.push_str(&format!(
544                "{}: {} samples, {} fused, {} rejected, {} not weighed, {} {}",
545                s.sensor.name(),
546                s.samples,
547                s.used,
548                s.rejected,
549                s.unweighed,
550                s.resets,
551                if s.resets == 1 { "reset" } else { "resets" }
552            ));
553            if let Some(share) = s.inside_gate_share {
554                // Rounded down, so a share short of all reads short of 100%.
555                let tenths = (1000.0 * share).floor() / 10.0;
556                out.push_str(&format!("; {tenths:.1}% inside the gate"));
557            }
558            if let Some(nis) = s.mean_nis {
559                out.push_str(&format!("; mean NIS {}", figures(nis)));
560            }
561            if let Some(l) = s.largest {
562                out.push_str(&format!(
563                    "; largest test ratio {} at {} s, Mach {:.2}",
564                    figures(l.ratio),
565                    figures(l.time_s),
566                    l.mach
567                ));
568            }
569            out.push('\n');
570        }
571        if self.apogee_past_reading_bound
572            && let Some(gap) = self.apogee_less_reading_m
573        {
574            out.push_str(&format!(
575                "smoothed apogee {} m from the reading, past the reading's bound: go by the \
576                 reading\n",
577                figures(gap)
578            ));
579        }
580        out.push_str("barometer de-weighted: ");
581        if self.deweighted.is_empty() {
582            out.push_str("never");
583        } else {
584            let spans: Vec<String> = self
585                .deweighted
586                .iter()
587                .map(|span| {
588                    let why = match span.why {
589                        Deweighted::Transonic => "transonic",
590                        Deweighted::Charge => "charge",
591                    };
592                    format!("{why} {}–{} s", figures(span.start_s), figures(span.end_s))
593                })
594                .collect();
595            out.push_str(&spans.join(", "));
596        }
597        out.push('\n');
598        if self.sensors.iter().any(|s| s.sensor == Sensor::Gnss) {
599            match self.gnss_trusted_s {
600                Some(t) => out.push_str(&format!("GNSS trusted from {} s\n", figures(t))),
601                None => out.push_str("GNSS never trusted\n"),
602            }
603        }
604        if let Some(gap) = self.baro_minus_inertial_at_apogee_m {
605            out.push_str(&format!(
606                "barometer less accelerometer at apogee: {} m\n",
607                figures(gap)
608            ));
609        }
610        if self.unsmoothed_steps == 0 {
611            out.push_str("pass back: every step smoothed\n");
612        } else {
613            out.push_str(&format!(
614                "pass back: {} steps kept the forward estimate\n",
615                self.unsmoothed_steps
616            ));
617        }
618        out
619    }
620}
621
622/// `x` to three significant figures, without an exponent; at least its whole part.
623fn figures(x: f64) -> String {
624    if !x.is_finite() {
625        return format!("{x}");
626    }
627    if x == 0.0 {
628        return "0".to_owned();
629    }
630    #[allow(
631        clippy::cast_possible_truncation,
632        reason = "x is finite and not zero (returned above), so its decimal exponent is within ±324"
633    )]
634    let digits = x.abs().log10().floor() as i32;
635    #[allow(clippy::cast_sign_loss, reason = "not negative after max(0)")]
636    let decimals = (2 - digits).max(0) as usize;
637    let text = format!("{x:.decimals$}");
638    if text
639        .trim_start_matches('-')
640        .chars()
641        .all(|c| c == '0' || c == '.')
642    {
643        "0".to_owned()
644    } else {
645        text
646    }
647}
648
649/// A flight smoothed.
650#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)]
651pub struct Smoothed {
652    /// Each moment, s on the log's clock: every sample of every sensor.
653    pub time_s: Vec<f64>,
654    /// The height above the pad, m.
655    pub height_m: Vec<f64>,
656    /// Its standard error, m.
657    pub height_sd_m: Vec<f64>,
658    /// The vertical speed, m/s, up.
659    pub speed_m_s: Vec<f64>,
660    /// Its standard error, m/s.
661    pub speed_sd_m_s: Vec<f64>,
662    /// Liftoff.
663    pub liftoff: Liftoff,
664    /// The smoothed apogee: when the smoothed height after liftoff is highest, s on the log's
665    /// clock.
666    pub apogee_time_s: f64,
667    /// Whether the smoothed apogee is a floor: the log ends on the climb
668    /// ([`Readings::ends_on_the_climb`]), whether or not the reading's apogee is withheld, so the top of the track isn't the rocket's and its standard error isn't about the
669    /// rocket's top.
670    pub apogee_is_floor: bool,
671    /// The health card.
672    pub health: Health,
673}
674
675/// One moment's smoothed state.
676#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
677pub struct State {
678    /// The height above the pad, m.
679    pub height_m: f64,
680    /// Its standard error, m.
681    pub height_sd_m: f64,
682    /// The vertical speed, m/s.
683    pub speed_m_s: f64,
684    /// Its standard error, m/s.
685    pub speed_sd_m_s: f64,
686}
687
688impl Smoothed {
689    /// The state at `time_s` on the log's clock, linear between the moments either side; `None`
690    /// outside the log.
691    pub fn at(&self, time_s: f64) -> Option<State> {
692        let n = self.time_s.len();
693        // A value read back from elsewhere may hold columns of other lengths.
694        let columns = [
695            &self.height_m,
696            &self.height_sd_m,
697            &self.speed_m_s,
698            &self.speed_sd_m_s,
699        ];
700        if n == 0
701            || columns.iter().any(|c| c.len() != n)
702            || !(self.time_s[0]..=self.time_s[n - 1]).contains(&time_s)
703        {
704            return None;
705        }
706        let after = self.time_s.partition_point(|&t| t < time_s);
707        if after == 0 || self.time_s[after] == time_s {
708            return Some(self.state(after));
709        }
710        let before = after - 1;
711        let w = (time_s - self.time_s[before]) / (self.time_s[after] - self.time_s[before]);
712        let lerp = |v: &[f64]| v[before] + w * (v[after] - v[before]);
713        Some(State {
714            height_m: lerp(&self.height_m),
715            height_sd_m: lerp(&self.height_sd_m),
716            speed_m_s: lerp(&self.speed_m_s),
717            speed_sd_m_s: lerp(&self.speed_sd_m_s),
718        })
719    }
720
721    fn state(&self, i: usize) -> State {
722        State {
723            height_m: self.height_m[i],
724            height_sd_m: self.height_sd_m[i],
725            speed_m_s: self.speed_m_s[i],
726            speed_sd_m_s: self.speed_sd_m_s[i],
727        }
728    }
729}
730
731/// Smooths a flight log, the GNSS receiver trusted by agreement.
732pub fn smooth(log: &FlightLog) -> Result<Smoothed, Withheld> {
733    smooth_with(log, &readings::read(log), Options::default())
734}
735
736/// Smooths a flight log whose readings are already taken.
737///
738/// The climb's port coefficient isn't learned ([`PORT_PRIOR_SD`]): the log is smoothed at its
739/// prior mean and again a standard deviation off, and each standard error `σ` becomes
740/// `hypot(σ, x_off − x)`, the smoother's linear response to the coefficient's spread (a consider
741/// analysis: S. F. Schmidt, 1966; D. Simon, *Optimal State Estimation*, Wiley, 2006, §7.6). The
742/// second run replays the first's choices, so the difference is the coefficient's effect alone.
743pub fn smooth_with(
744    log: &FlightLog,
745    readings: &Readings,
746    options: Options,
747) -> Result<Smoothed, Withheld> {
748    let inputs = Inputs::gather(log, readings)?;
749    // The barometer's row is linear in the coefficient, so one side's step is the other's.
750    let (mut smoothed, choices) = run(&inputs, options, PORT_PRIOR_MEAN, None, true);
751    // Only the consider run's means are used, so its pass back carries no covariance.
752    let (off, _) = run(
753        &inputs,
754        options,
755        PORT_PRIOR_MEAN - PORT_PRIOR_SD,
756        Some(&choices),
757        false,
758    );
759    let pairs = [
760        (&mut smoothed.height_sd_m, &smoothed.height_m, &off.height_m),
761        (
762            &mut smoothed.speed_sd_m_s,
763            &smoothed.speed_m_s,
764            &off.speed_m_s,
765        ),
766    ];
767    for (sd, at_mean, at_off) in pairs {
768        for ((sd, a), b) in sd.iter_mut().zip(at_mean).zip(at_off) {
769            *sd = sd.hypot(b - a);
770        }
771    }
772    smoothed.apogee_is_floor = readings.ends_on_the_climb;
773    if let (Reading::Read(apogee), Some(state)) =
774        (&readings.apogee, smoothed.at(smoothed.apogee_time_s))
775    {
776        let gap = state.height_m - apogee.height_m;
777        smoothed.health.apogee_less_reading_m = Some(gap);
778        smoothed.health.apogee_past_reading_bound = if apogee.is_floor {
779            -gap > apogee.bound_m
780        } else {
781            gap.abs() > apogee.bound_m
782        };
783    }
784    let outputs = [
785        &smoothed.height_m,
786        &smoothed.height_sd_m,
787        &smoothed.speed_m_s,
788        &smoothed.speed_sd_m_s,
789    ];
790    if let Some(k) = (0..smoothed.time_s.len()).find(|&k| outputs.iter().any(|o| !o[k].is_finite()))
791    {
792        return Err(Withheld {
793            reason: Reason::Diverged,
794            detail: format!(
795                "the smoother's estimate isn't a finite number at {} s",
796                smoothed.time_s[k]
797            ),
798        });
799    }
800    if let Some(k) =
801        (0..smoothed.time_s.len()).find(|&k| smoothed.speed_m_s[k].abs() > DIVERGED_SPEED_M_S)
802    {
803        return Err(Withheld {
804            reason: Reason::Diverged,
805            detail: format!(
806                "the smoother's vertical speed reads {:.0} m/s at {:.2} s, past Earth's escape \
807                 speed, {:.0} m/s either way: the estimate ran away",
808                smoothed.speed_m_s[k], smoothed.time_s[k], DIVERGED_SPEED_M_S
809            ),
810        });
811    }
812    Ok(smoothed)
813}
814
815/// A barometer sample.
816#[derive(Debug, Clone, Copy)]
817struct Baro {
818    time_s: f64,
819    /// Above the pad, m.
820    height_m: f64,
821    /// The running median about it ([`REGRESSOR_HALF_S`]), m.
822    smoothed_m: f64,
823    /// The running medians' least-squares slope over [`RATE_HALF_S`] either side, m/s: of the
824    /// medians, so one garbled sample doesn't bend its neighbors' rates.
825    rate_m_s: f64,
826    /// The heights' own white scatter about it ([`scatter`]), as a variance, m².
827    scatter_var_m2: f64,
828    /// Whether it is left out of the fit: a launch-started log's first samples ([`settle`]).
829    unweighed: bool,
830}
831
832/// An accelerometer sample.
833#[derive(Debug, Clone, Copy)]
834struct Accel {
835    time_s: f64,
836    /// Along the axis, nose up, gravity's reaction in it, m/s².
837    value_m_s2: f64,
838    /// The running median about it ([`REGRESSOR_HALF_S`]), m/s²: the reading the part's scale
839    /// multiplies.
840    smoothed_m_s2: f64,
841    clipped: bool,
842}
843
844/// What the smoother takes from a log.
845#[derive(Debug, Clone)]
846struct Inputs {
847    baro: Vec<Baro>,
848    /// The barometer's white noise and step, as a variance at the pad, m².
849    baro_white_var: f64,
850    /// The pad's height above sea level, m, on the standard atmosphere; `None` where the log
851    /// can't say.
852    pad_msl_m: Option<f64>,
853    /// Whether the pad is assumed ([`Health::assumed_pad`]).
854    assumed_pad: bool,
855    /// Whether the log's accelerometer was set aside ([`Health::accelerometer_set_aside`]).
856    accelerometer_set_aside: bool,
857    /// How far the zero the heights stand on may be from the pad on an assumed pad, 1σ, m; 0 where
858    /// the zero is the logger's own or the pad's samples set it.
859    zero_sd_m: f64,
860    accel: Vec<Accel>,
861    /// The accelerometer's noise on the pad, 1σ, m/s².
862    accel_sd: f64,
863    /// Its median on the pad, m/s².
864    accel_pad: f64,
865    gnss: Vec<(f64, f64)>,
866    gnss_sd: f64,
867    liftoff: Liftoff,
868    /// The pad's hold ends here, s.
869    pad_end_s: f64,
870    /// The spans the barometer is de-weighted in for the charges the log names, s.
871    charges: Vec<(f64, f64)>,
872    /// The landing, where the barometer settles, s.
873    landing_s: Option<f64>,
874    /// From here the barometer's own scatter weighs its samples ([`scatter`]): the end of the
875    /// span about its top in which a charge is expected ([`APOGEE_CHARGE_SPAN_M`]), s.
876    descent_from_s: f64,
877}
878
879/// The landing: the first time past the barometer's highest reading from which its running median
880/// stays within [`LANDED_SPAN_M`] for [`LANDED_S`]. `None` if it never does.
881fn landing(baro: &[Baro], liftoff_s: f64) -> Option<f64> {
882    let top = baro
883        .iter()
884        .enumerate()
885        .filter(|(_, b)| b.time_s > liftoff_s)
886        .max_by(|(_, a), (_, b)| a.smoothed_m.total_cmp(&b.smoothed_m))?
887        .0;
888    let end_s = baro.last()?.time_s;
889    // The window's highest and lowest kept as it slides, each sample in and out once, so a burst
890    // of samples close together isn't walked over for each of them.
891    let (mut highs, mut lows) = (VecDeque::<usize>::new(), VecDeque::<usize>::new());
892    let mut next = top;
893    for i in top..baro.len() {
894        let start = baro[i];
895        if end_s < start.time_s + LANDED_S {
896            return None;
897        }
898        while next < baro.len() && baro[next].time_s <= start.time_s + LANDED_S {
899            let m = baro[next].smoothed_m;
900            while highs.back().is_some_and(|&k| baro[k].smoothed_m <= m) {
901                highs.pop_back();
902            }
903            highs.push_back(next);
904            while lows.back().is_some_and(|&k| baro[k].smoothed_m >= m) {
905                lows.pop_back();
906            }
907            lows.push_back(next);
908            next += 1;
909        }
910        while highs.front().is_some_and(|&k| k < i) {
911            highs.pop_front();
912        }
913        while lows.front().is_some_and(|&k| k < i) {
914            lows.pop_front();
915        }
916        let high = highs
917            .front()
918            .map_or(start.smoothed_m, |&k| baro[k].smoothed_m);
919        let low = lows
920            .front()
921            .map_or(start.smoothed_m, |&k| baro[k].smoothed_m);
922        if high - start.smoothed_m <= LANDED_SPAN_M && start.smoothed_m - low <= LANDED_SPAN_M {
923            return Some(start.time_s);
924        }
925    }
926    None
927}
928
929/// The median of `values`, which mustn't be empty.
930fn median(values: &mut [f64]) -> f64 {
931    values.sort_by(f64::total_cmp);
932    let n = values.len();
933    if n % 2 == 1 {
934        values[n / 2]
935    } else {
936        0.5 * (values[n / 2 - 1] + values[n / 2])
937    }
938}
939
940/// The median and the median absolute deviation scaled to a normal σ (× 1.4826).
941fn robust(values: &[f64]) -> (f64, f64) {
942    let mut v = values.to_vec();
943    let m = median(&mut v);
944    let mut dev: Vec<f64> = values.iter().map(|x| (x - m).abs()).collect();
945    (m, 1.4826 * median(&mut dev))
946}
947
948/// The smallest gap between distinct finite values, or 0 if there are fewer than two.
949fn step_of(values: &[f64]) -> f64 {
950    let mut v: Vec<f64> = values.iter().copied().filter(|x| x.is_finite()).collect();
951    v.sort_by(f64::total_cmp);
952    v.dedup();
953    let gap = v
954        .windows(2)
955        .map(|w| w[1] - w[0])
956        .fold(f64::INFINITY, f64::min);
957    if gap.is_finite() { gap } else { 0.0 }
958}
959
960impl Inputs {
961    fn gather(log: &FlightLog, readings: &Readings) -> Result<Self, Withheld> {
962        let withheld = |reason: Reason, detail: String| Withheld { reason, detail };
963        let track = readings::track(log).map_err(|(_, detail)| {
964            withheld(
965                Reason::NoBarometer,
966                format!("the smoother needs a barometer's height track: {detail}"),
967            )
968        })?;
969        if track.source == Source::Gnss {
970            return Err(withheld(
971                Reason::NoBarometer,
972                "the log's only height is a GNSS receiver's, which lags a climb with its fix \
973                 flagged valid; the smoother needs a barometer to check it against"
974                    .to_owned(),
975            ));
976        }
977        // Before anything is worked out from the samples: a file's size sets how long this takes.
978        let accel_samples_held = readings.accelerometer.as_ref().map_or(0, |settled| {
979            log.channels_of(Quantity::Acceleration)
980                .find(|c| c.column == settled.column)
981                .map_or(0, |c| c.values.len())
982        });
983        let gnss_samples_held = log
984            .channels_of(Quantity::GnssAltitude)
985            .next()
986            .map_or(0, |c| c.values.len());
987        let steps = track.time_s.len() + accel_samples_held + gnss_samples_held;
988        if steps > MAX_STEPS {
989            return Err(withheld(
990                Reason::TooLong,
991                format!(
992                    "the log holds {steps} samples for the smoother, more than the {MAX_STEPS} \
993                     it takes"
994                ),
995            ));
996        }
997        let read_liftoff = match &readings.liftoff {
998            Reading::Read(liftoff) => *liftoff,
999            Reading::Withheld(w) => {
1000                return Err(withheld(
1001                    Reason::NoLiftoff,
1002                    format!("the readings found no liftoff: {}", w.detail),
1003                ));
1004            }
1005        };
1006        let pad = readings.pad_altitude_m.unwrap_or(0.0);
1007        let baro: Vec<Baro> = track
1008            .time_s
1009            .iter()
1010            .zip(&track.height_m)
1011            .filter(|(t, h)| t.is_finite() && h.is_finite())
1012            .map(|(&time_s, &h)| Baro {
1013                time_s,
1014                height_m: h - pad,
1015                smoothed_m: 0.0,
1016                rate_m_s: 0.0,
1017                scatter_var_m2: 0.0,
1018                unweighed: false,
1019            })
1020            .collect();
1021        let mut baro = baro;
1022        annotate(&mut baro);
1023        let pad_msl_m = match track.source {
1024            Source::Pressure => Some(pad),
1025            Source::AltitudeChannel if !track.zeroed => Some(pad),
1026            _ => None,
1027        };
1028
1029        // The accelerometer the readings settled, gravity put back and turned nose up.
1030        let mut accel = Vec::new();
1031        let mut accel_sd = 0.0;
1032        let mut accel_pad = G0;
1033        let mut accelerometer_set_aside = false;
1034        let mut liftoff = Liftoff {
1035            time_s: read_liftoff.time_s,
1036            bound_s: read_liftoff.bound_s,
1037            from: LiftoffFrom::Barometer,
1038        };
1039        if let Some(settled) = &readings.accelerometer
1040            && let Some(channel) = log
1041                .channels_of(Quantity::Acceleration)
1042                .find(|c| c.column == settled.column)
1043        {
1044            let (top, range) = match &readings.max_acceleration {
1045                Reading::Read(max) => (Some((max.acceleration_m_s2, max.clip)), max.range_m_s2),
1046                Reading::Withheld(_) => (None, None),
1047            };
1048            accel = accel_samples(channel, settled.gravity, settled.turned_over);
1049            let values: Vec<f64> = accel.iter().map(|a| a.value_m_s2).collect();
1050            let times: Vec<f64> = accel.iter().map(|a| a.time_s).collect();
1051            for (a, m) in accel.iter_mut().zip(smoothed(&values, &times)) {
1052                a.smoothed_m_s2 = m;
1053            }
1054            mark_clipped(&mut accel, top, range, settled.step_m_s2);
1055            let pad_until = read_liftoff.time_s - read_liftoff.bound_s - 0.2;
1056            let on_pad: Vec<f64> = accel
1057                .iter()
1058                .filter(|s| s.time_s <= pad_until && !s.clipped)
1059                .map(|s| s.value_m_s2)
1060                .collect();
1061            if on_pad.len() >= PAD_MIN_SAMPLES {
1062                let (level, sd) = robust(&on_pad);
1063                accel_pad = level;
1064                accel_sd = sd.max(settled.step_m_s2 / 12_f64.sqrt()).max(1.0e-3);
1065                if let Some(found) =
1066                    accel_liftoff(&accel, level, accel_sd, settled.step_m_s2, &read_liftoff)
1067                {
1068                    liftoff = found;
1069                }
1070            } else {
1071                accelerometer_set_aside = !accel.is_empty();
1072                accel.clear();
1073            }
1074        }
1075
1076        let found_pad_end_s = match liftoff.from {
1077            LiftoffFrom::Accelerometer => liftoff.time_s,
1078            LiftoffFrom::Barometer => liftoff.time_s - liftoff.bound_s,
1079        };
1080        let on_pad: Vec<f64> = baro
1081            .iter()
1082            .filter(|s| s.time_s <= found_pad_end_s)
1083            .map(|s| s.height_m)
1084            .collect();
1085        let assumed_pad = on_pad.len() < PAD_MIN_SAMPLES;
1086        if assumed_pad {
1087            settle(&mut baro);
1088            annotate(&mut baro);
1089        }
1090        let apogee_span = apogee_charge(&baro, liftoff.time_s);
1091        // Past the barometer's own top and the charge expected there, not from the filter's
1092        // apogee: the charge's pulse can hold the filter's decision back while the swing has
1093        // begun.
1094        let descent_from_s = apogee_span.map_or(f64::INFINITY, |(_, end)| end);
1095
1096        let step = step_of(&track.height_m);
1097        // The pad's scatter holds the fast and slow correlated noise too, which the smoother
1098        // carries apart; what is left is the part's white noise. A step's own variance is the
1099        // least it can be.
1100        let correlated = FAST_NOISE_M * FAST_NOISE_M + SLOW_NOISE_M * SLOW_NOISE_M;
1101        let measured_var = if assumed_pad {
1102            flight_noise_var(&baro, descent_from_s, pad_msl_m.unwrap_or(0.0), correlated)
1103        } else {
1104            let (_, pad_sd) = robust(&on_pad);
1105            pad_sd * pad_sd - correlated
1106        };
1107        let baro_white_var = measured_var.max(step * step / 12.0).max(1.0e-6);
1108        // With no pad, nothing is held still: a sample near the zero may already be moving.
1109        let pad_end_s = if assumed_pad {
1110            f64::NEG_INFINITY
1111        } else {
1112            found_pad_end_s
1113        };
1114        // A zero the logger set is the pad's; one that is the pad's only by a mapping's or a
1115        // column's word sits within `PAD_ZERO_M` of it ([`readings::PAD_ZERO_M`]), which the
1116        // readings' bounds hold and the height's standard error must too.
1117        let zero_sd_m = if assumed_pad && readings::zero_by_word(log, &track) {
1118            readings::PAD_ZERO_M / 3_f64.sqrt()
1119        } else {
1120            0.0
1121        };
1122
1123        let mut gnss: Vec<(f64, f64)> = Vec::new();
1124        // A receiver that logs past the barometer's last sample would leave the track there to
1125        // the motion model and a receiver not yet trusted, so its fixes are taken within the
1126        // barometer's span.
1127        let baro_end_s = baro.last().map_or(f64::NEG_INFINITY, |b| b.time_s);
1128        if let Some(channel) = log.channels_of(Quantity::GnssAltitude).next() {
1129            for (&t, &z) in channel.time_s.iter().zip(&channel.values) {
1130                if t.is_finite()
1131                    && z.is_finite()
1132                    && t <= baro_end_s
1133                    && gnss.last().is_none_or(|&(last, _)| t > last)
1134                {
1135                    gnss.push((t, z));
1136                }
1137            }
1138        }
1139        let gnss_pad: Vec<f64> = gnss
1140            .iter()
1141            .filter(|(t, _)| *t <= pad_end_s)
1142            .map(|(_, z)| *z)
1143            .collect();
1144        let gnss_sd = if gnss_pad.len() >= 3 {
1145            robust(&gnss_pad).1.max(GNSS_SD_FLOOR_M)
1146        } else {
1147            GNSS_SD_DEFAULT_M
1148        };
1149
1150        let mut charges: Vec<(f64, f64)> = log
1151            .events
1152            .iter()
1153            .filter(|e| {
1154                let name = e.name.to_lowercase();
1155                e.time_s.is_finite() && CHARGE_WORDS.iter().any(|w| name.contains(w))
1156            })
1157            .map(|e| (e.time_s, e.time_s + CHARGE_WINDOW_S))
1158            .collect();
1159        charges.extend(apogee_span);
1160
1161        let landing_s = landing(&baro, liftoff.time_s);
1162        Ok(Self {
1163            landing_s,
1164            descent_from_s,
1165            baro,
1166            baro_white_var,
1167            pad_msl_m,
1168            assumed_pad,
1169            accelerometer_set_aside,
1170            zero_sd_m,
1171            accel,
1172            accel_sd,
1173            accel_pad,
1174            gnss,
1175            gnss_sd,
1176            liftoff,
1177            pad_end_s,
1178            charges,
1179        })
1180    }
1181}
1182
1183/// Works out each sample's running median, rate and scatter from the heights.
1184fn annotate(baro: &mut [Baro]) {
1185    let heights: Vec<f64> = baro.iter().map(|b| b.height_m).collect();
1186    let times: Vec<f64> = baro.iter().map(|b| b.time_s).collect();
1187    let medians = smoothed(&heights, &times);
1188    let rates = slopes(&medians, &times, RATE_HALF_S);
1189    for (((b, &m), rate), var) in baro
1190        .iter_mut()
1191        .zip(&medians)
1192        .zip(rates)
1193        .zip(scatter(&heights, &medians, &times))
1194    {
1195        b.smoothed_m = m;
1196        b.rate_m_s = rate;
1197        b.scatter_var_m2 = var;
1198    }
1199}
1200
1201/// Settles a launch-started log's first samples ([`ASSUMED_SETTLE_S`]). The first two take the
1202/// median of the first five too, or they would bend the rates and running medians around them.
1203fn settle(baro: &mut [Baro]) {
1204    let Some(first_s) = baro.first().map(|b| b.time_s) else {
1205        return;
1206    };
1207    if baro.len() < 5 {
1208        return;
1209    }
1210    let by_time = baro.partition_point(|b| b.time_s < first_s + ASSUMED_SETTLE_S);
1211    let settling = by_time.max(ASSUMED_SETTLE_SAMPLES).min(baro.len());
1212    let raw: Vec<f64> = baro[..(settling + 2).min(baro.len())]
1213        .iter()
1214        .map(|b| b.height_m)
1215        .collect();
1216    for (k, b) in baro.iter_mut().enumerate().take(settling) {
1217        let centered = k.max(2);
1218        if k < 2 {
1219            b.unweighed = true;
1220        }
1221        if let Some(window) = raw.get(centered - 2..=centered + 2) {
1222            b.height_m = median(&mut window.to_vec());
1223        }
1224    }
1225}
1226
1227/// The barometer's white noise on a log with no pad, as a variance at the pad's air, m²: the median
1228/// over the samples past `descent_from_s` (all of them if there are none) of the flight's own
1229/// scatter ([`scatter`]), brought to the pad's air as [`Inputs`]' noise is thinned from it, with the
1230/// correlated noise the smoother carries apart taken out. Under a canopy the scatter also holds the
1231/// swing, so this errs noisy, the side that widens an interval; a log that runs on past landing has
1232/// a median nearer the part's own noise.
1233fn flight_noise_var(baro: &[Baro], descent_from_s: f64, pad_msl_m: f64, correlated: f64) -> f64 {
1234    let rho_pad = standard_density(pad_msl_m);
1235    let at_pad = |b: &Baro| {
1236        let thin = (rho_pad / standard_density(pad_msl_m + b.smoothed_m)).powi(2);
1237        b.scatter_var_m2 / thin - correlated
1238    };
1239    let mut past: Vec<f64> = baro
1240        .iter()
1241        .filter(|b| b.time_s >= descent_from_s)
1242        .map(at_pad)
1243        .collect();
1244    if past.is_empty() {
1245        past = baro.iter().map(at_pad).collect();
1246    }
1247    if past.is_empty() {
1248        return 0.0;
1249    }
1250    median(&mut past)
1251}
1252
1253/// The span around the barometer's apogee in which a charge is expected
1254/// ([`APOGEE_CHARGE_SPAN_M`]); `None` if the track has no sample after liftoff.
1255fn apogee_charge(baro: &[Baro], liftoff_s: f64) -> Option<(f64, f64)> {
1256    let after = || baro.iter().filter(|b| b.time_s > liftoff_s);
1257    let top = after().map(|b| b.smoothed_m).max_by(f64::total_cmp)?;
1258    let near = || after().filter(|b| b.smoothed_m >= top - APOGEE_CHARGE_SPAN_M);
1259    let first = near().next()?.time_s;
1260    let last = near().next_back()?.time_s;
1261    Some((first, last + CHARGE_WINDOW_S))
1262}
1263
1264/// The channel's samples, gravity put back and nose up.
1265fn accel_samples(channel: &Channel, gravity: Gravity, turned_over: bool) -> Vec<Accel> {
1266    let mut out: Vec<Accel> = Vec::with_capacity(channel.values.len());
1267    for (&t, &v) in channel.time_s.iter().zip(&channel.values) {
1268        if !(t.is_finite() && v.is_finite()) || out.last().is_some_and(|last| t <= last.time_s) {
1269            continue;
1270        }
1271        let mut value = if turned_over { -v } else { v };
1272        if gravity == Gravity::Out {
1273            value += G0;
1274        }
1275        out.push(Accel {
1276            time_s: t,
1277            value_m_s2: value,
1278            smoothed_m_s2: value,
1279            clipped: false,
1280        });
1281    }
1282    out
1283}
1284
1285/// The half-span of the running median a scale state's regressor is taken from, s. A scale
1286/// multiplies the quantity it scales; taken from the state, the product makes the filter extended,
1287/// and its linearization finds information along directions the sensors can't see (the scale and
1288/// the acceleration it multiplies, with the barometer de-weighted); taken from the raw sample, the
1289/// sample's own noise is fitted by the scale (a pad's noise, read as a height that scales). The
1290/// median keeps the track's shape and takes most of its noise out. HPR Sim's choice.
1291pub const REGRESSOR_HALF_S: f64 = 0.05;
1292
1293/// The most samples either side a scale state's running median is taken over: [`REGRESSOR_HALF_S`]
1294/// at 5 kHz, past any logger's rate the project has seen. A file's clock could otherwise ask for a
1295/// window of 50,000 samples, kept sorted, for each of its samples.
1296pub const REGRESSOR_MAX_HALF: usize = 250;
1297
1298/// The barometer has landed once its running median stays within this many meters ...
1299pub const LANDED_SPAN_M: f64 = 5.0;
1300
1301/// ... for this many seconds: under the slowest main canopies, 2 m/s, a descent moves 6 m in it.
1302pub const LANDED_S: f64 = 3.0;
1303
1304/// The impact is taken back this long before the landing's first settled reading, s: the running
1305/// median lags a stop by its half-width, and a ballistic stop takes a few hundredths of a second.
1306pub const LANDING_LEAD_S: f64 = 0.1;
1307
1308/// The barometer's own climb rate is taken over this many seconds either side of a sample, for
1309/// the Mach number its de-weighting ends at, the burnout's and apogee's rules, and the speed the
1310/// port's term is taken at, which keeps the barometer's row linear in the states: its noise there
1311/// is a few m/s.
1312pub const RATE_HALF_S: f64 = 0.25;
1313
1314/// The time over which the barometer's scatter in flight beyond the pad's is correlated, s: a
1315/// rocket swinging under its canopy on `L` = 3–15 m of cord and lines is a pendulum of period
1316/// `2π √(L/g)`, 3.5–7.8 s, and a first-order error of `τ = √(L/g)`, 0.55–1.24 s, averages away no
1317/// faster than its swing does. HPR Sim's estimate, taken past that range because the scatter's
1318/// quadratic takes part of a correlated error's variance out: with 2 s, descents swinging by
1319/// 3–6 m correlated over up to a second stay inside their 90% intervals in the tests, and one
1320/// correlated over 3 s doesn't. The noise note gives no correlation time for the swing.
1321pub const SWING_TAU_S: f64 = 2.0;
1322
1323/// A barometer's scatter is taken over this many seconds either side of a sample, and over at
1324/// least [`SCATTER_MIN_SAMPLES`] samples either side: long against [`SWING_TAU_S`], so a
1325/// quadratic through the window can't follow a swing and take its variance out, and long enough
1326/// that a charge's pulse or a spike is a minority of it. Where the track bends faster than a
1327/// quadratic over six seconds, as a fall settles to its canopy's speed or the main opens, the
1328/// scatter reads the bend as noise too, and the barometer is weighed less there than it could be.
1329pub const SCATTER_HALF_S: f64 = 3.0;
1330
1331/// The fewest samples either side a barometer's scatter is taken over.
1332pub const SCATTER_MIN_SAMPLES: usize = 5;
1333
1334/// The most samples a scatter's window is read at, evenly spaced: enough for its median to
1335/// scatter by a tenth, and a fast logger's window no slower than a slow one's.
1336pub const SCATTER_MAX_SAMPLES: usize = 201;
1337
1338/// Each sample's scatter, as a variance (m²): the heights' robust spread about the least-squares
1339/// quadratic through their running `medians` over [`SCATTER_HALF_S`] either side (so a spike
1340/// doesn't pull it), the median absolute residual scaled to a normal σ (× 1.4826). A quadratic follows a coast's or a steady descent's track to well
1341/// under a meter, so what is left is the barometer's noise in flight.
1342///
1343/// The pad's scatter measures the sensor; in flight a rocket swinging under its canopy and the
1344/// gusts on it add their own, correlated over a second or so: 0.3–7.5 m robust σ about
1345/// two-second quadratic fits on two public dual-altimeter pairs' descents
1346/// (`docs/research/altimeter-sensor-noise.md` §3). From the track's top on, past the charge expected
1347/// there, where those were measured, the
1348/// smoother weighs each sample by the pad's noise and the scatter beyond it, correlated over
1349/// [`SWING_TAU_S`], so a quiet pad doesn't make a swinging descent's samples fail the gate or
1350/// average a swing away. Before apogee the pad's noise and the transonic and charge de-weighting
1351/// stand alone. The fast
1352/// noise ([`FAST_NOISE_M`]), white at a logger's rates, is in the scatter whole and is taken out
1353/// of it there, as the pad's is.
1354fn scatter(values: &[f64], medians: &[f64], times: &[f64]) -> Vec<f64> {
1355    let n = values.len();
1356    let (mut residuals, mut picked) = (Vec::new(), Vec::new());
1357    (0..n)
1358        .map(|i| {
1359            // By search, not by walking out from the sample: a burst of samples close together
1360            // would make that a walk over the burst for each of them.
1361            let from = times[..i]
1362                .partition_point(|t| times[i] - t > SCATTER_HALF_S)
1363                .min(i.saturating_sub(SCATTER_MIN_SAMPLES));
1364            let to = (i + times[i..].partition_point(|t| t - times[i] <= SCATTER_HALF_S))
1365                .saturating_sub(1)
1366                .max((i + SCATTER_MIN_SAMPLES).min(n.saturating_sub(1)));
1367            // Every `stride`-th sample of the window.
1368            let stride = (to - from) / SCATTER_MAX_SAMPLES + 1;
1369            picked.clear();
1370            picked.extend((from..=to).step_by(stride));
1371            let t: Vec<f64> = picked.iter().map(|&j| times[j]).collect();
1372            let m: Vec<f64> = picked.iter().map(|&j| medians[j]).collect();
1373            let Some(fit) = quadratic(&m, &t, times[i]) else {
1374                return 0.0;
1375            };
1376            residuals.clear();
1377            residuals.extend(picked.iter().map(|&j| {
1378                let tau = times[j] - times[i];
1379                (values[j] - (fit[0] + fit[1] * tau + fit[2] * tau * tau)).abs()
1380            }));
1381            let sd = 1.4826 * selected_median(&mut residuals);
1382            sd * sd
1383        })
1384        .collect()
1385}
1386
1387/// The median of `values`, which mustn't be empty, by selection rather than a sort: the same
1388/// number as [`median`]'s, in linear time.
1389fn selected_median(values: &mut [f64]) -> f64 {
1390    let n = values.len();
1391    let (lower, &mut upper, _) = values.select_nth_unstable_by(n / 2, f64::total_cmp);
1392    if n % 2 == 1 {
1393        upper
1394    } else {
1395        // The lower middle is the largest of the half below.
1396        0.5 * (lower.iter().copied().fold(f64::NEG_INFINITY, f64::max) + upper)
1397    }
1398}
1399
1400/// The least-squares quadratic `c₀ + c₁ τ + c₂ τ²`, `τ = t − center`, through the points, by
1401/// Cramer's rule on the normal equations; `None` with fewer than four points or three distinct
1402/// times' worth of spread.
1403fn quadratic(values: &[f64], times: &[f64], center: f64) -> Option<[f64; 3]> {
1404    if values.len() < 4 {
1405        return None;
1406    }
1407    // Σ τᵏ for k = 0..4 and Σ z τᵏ for k = 0..2, τ scaled by the window's half-width so the
1408    // determinant's terms are of one size.
1409    let scale = times
1410        .iter()
1411        .map(|t| (t - center).abs())
1412        .fold(0.0, f64::max)
1413        .max(1.0e-9);
1414    let (mut s, mut b) = ([0.0; 5], [0.0; 3]);
1415    for (&z, &t) in values.iter().zip(times) {
1416        let u = (t - center) / scale;
1417        let mut power = 1.0;
1418        for (k, sk) in s.iter_mut().enumerate() {
1419            *sk += power;
1420            if k < 3 {
1421                b[k] += z * power;
1422            }
1423            power *= u;
1424        }
1425    }
1426    let m = [[s[0], s[1], s[2]], [s[1], s[2], s[3]], [s[2], s[3], s[4]]];
1427    let det = |m: &[[f64; 3]; 3]| {
1428        m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1])
1429            - m[0][1] * (m[1][0] * m[2][2] - m[1][2] * m[2][0])
1430            + m[0][2] * (m[1][0] * m[2][1] - m[1][1] * m[2][0])
1431    };
1432    let d = det(&m);
1433    if d.is_nan() || d.abs() <= 1.0e-9 * s[0].powi(3) {
1434        return None;
1435    }
1436    let mut c = [0.0; 3];
1437    for (k, ck) in c.iter_mut().enumerate() {
1438        let mut mk = m;
1439        for row in 0..3 {
1440            mk[row][k] = b[row];
1441        }
1442        *ck = det(&mk) / d;
1443    }
1444    Some([c[0], c[1] / scale, c[2] / (scale * scale)])
1445}
1446
1447/// Each sample's least-squares slope of `values` against `times` over `half_s` either side; zero
1448/// where the window holds fewer than two distinct times.
1449fn slopes(values: &[f64], times: &[f64], half_s: f64) -> Vec<f64> {
1450    let origin = times.first().copied().unwrap_or(0.0);
1451    let (mut from, mut to) = (0, 0);
1452    let (mut n, mut st, mut sz, mut stt, mut stz) = (0.0, 0.0, 0.0, 0.0, 0.0);
1453    let mut out = Vec::with_capacity(values.len());
1454    for &center in times {
1455        while to < times.len() && times[to] <= center + half_s {
1456            let t = times[to] - origin;
1457            n += 1.0;
1458            st += t;
1459            sz += values[to];
1460            stt += t * t;
1461            stz += t * values[to];
1462            to += 1;
1463        }
1464        while times[from] < center - half_s {
1465            let t = times[from] - origin;
1466            n -= 1.0;
1467            st -= t;
1468            sz -= values[from];
1469            stt -= t * t;
1470            stz -= t * values[from];
1471            from += 1;
1472        }
1473        let spread = n * stt - st * st;
1474        out.push(if n >= 2.0 && spread > 1.0e-12 * n * n {
1475            (n * stz - st * sz) / spread
1476        } else {
1477            0.0
1478        });
1479    }
1480    out
1481}
1482
1483/// The running median of `values` over [`REGRESSOR_HALF_S`] either side, by the median interval.
1484fn smoothed(values: &[f64], times: &[f64]) -> Vec<f64> {
1485    if values.len() < 3 {
1486        return values.to_vec();
1487    }
1488    let mut gaps: Vec<f64> = times.windows(2).map(|w| w[1] - w[0]).collect();
1489    let interval = median(&mut gaps).max(1.0e-6);
1490    #[allow(
1491        clippy::cast_possible_truncation,
1492        clippy::cast_sign_loss,
1493        reason = "the interval is at least 10⁻⁶ s, so this is 0 to 5 × 10⁴"
1494    )]
1495    let half = ((REGRESSOR_HALF_S / interval).round() as usize).clamp(1, REGRESSOR_MAX_HALF);
1496    crate::filter::running_median(values, half)
1497}
1498
1499/// Marks the samples the part couldn't read: at its stated range less two steps and
1500/// [`GRAVITY_TOLERANCE_G`], as the readings take a top as clipped, or on a flat top the readings
1501/// found.
1502fn mark_clipped(
1503    accel: &mut [Accel],
1504    top: Option<(f64, Option<Clip>)>,
1505    range: Option<f64>,
1506    step: f64,
1507) {
1508    let limit = range.map(|r| r - 2.0 * step - GRAVITY_TOLERANCE_G * G0);
1509    let flat = match top {
1510        Some((value, Some(Clip::Flat))) => Some(value - step),
1511        _ => None,
1512    };
1513    for sample in accel {
1514        let reading = sample.value_m_s2;
1515        sample.clipped =
1516            limit.is_some_and(|l| reading.abs() >= l) || flat.is_some_and(|f| reading >= f);
1517    }
1518}
1519
1520/// Liftoff from the accelerometer: the last sample before the first run of three that rise past
1521/// five of the pad's σ and a step above its level, and at least 3 m/s², within a second of the
1522/// barometer's liftoff and its bound; walked back while the samples stay above twice σ and a
1523/// step. On the pad an accelerometer reads 1 g however hard the motor pushes, until the rocket
1524/// moves.
1525fn accel_liftoff(
1526    accel: &[Accel],
1527    level: f64,
1528    sd: f64,
1529    step: f64,
1530    read: &readings::Liftoff,
1531) -> Option<Liftoff> {
1532    let rise = (5.0 * sd + step).max(3.0);
1533    let from = read.time_s - read.bound_s - 1.0;
1534    let to = read.time_s + read.bound_s + 1.0;
1535    let above = |s: &Accel| s.clipped || s.value_m_s2 - level > rise;
1536    let start = accel.iter().position(|s| s.time_s >= from)?;
1537    let mut first = None;
1538    for i in start..accel.len().saturating_sub(2) {
1539        if accel[i].time_s > to {
1540            break;
1541        }
1542        if above(&accel[i]) && above(&accel[i + 1]) && above(&accel[i + 2]) {
1543            first = Some(i);
1544            break;
1545        }
1546    }
1547    let mut i = first?;
1548    while i > 0 && accel[i - 1].value_m_s2 - level > 2.0 * sd + step {
1549        i -= 1;
1550    }
1551    let last_on_pad = i.checked_sub(1)?;
1552    let interval = accel[i].time_s - accel[last_on_pad].time_s;
1553    Some(Liftoff {
1554        time_s: accel[last_on_pad].time_s,
1555        bound_s: 2.0 * interval,
1556        from: LiftoffFrom::Accelerometer,
1557    })
1558}
1559
1560/// `T_pad ∫ dz/T(z)` from the pad to `height_m` above it, on the standard atmosphere, m: the height
1561/// a barometer's error from the day's air scales with. On a day `ΔT` off the standard, a logger
1562/// converting pressure by the standard atmosphere reads `h − (ΔT/T_pad) ψ`, from the hypsometric
1563/// equation `dz = −(R T/g) dp/p` (`altimeter-height-formulas.md`).
1564fn air_regressor(height_m: f64, pad_msl_m: f64) -> f64 {
1565    let t_pad = standard_temperature_k(pad_msl_m);
1566    let integral = |z: f64| -> f64 {
1567        // ∫₀^z dz/T from sea level.
1568        let troposphere = z.min(11_000.0);
1569        let lower = -(standard_temperature_k(troposphere) / 288.15).ln() / 0.0065;
1570        lower + (z - 11_000.0).max(0.0) / 216.65
1571    };
1572    t_pad * (integral(pad_msl_m + height_m) - integral(pad_msl_m))
1573}
1574
1575/// The standard atmosphere's density at a height above sea level, kg/m³.
1576fn standard_density(height_msl_m: f64) -> f64 {
1577    Ussa76::standard()
1578        .sample(height_msl_m)
1579        .map_or(1.225, |s| s.air.density_kg_m3)
1580}
1581
1582/// The speed of sound at a temperature, m/s.
1583fn sound(temperature_k: f64) -> f64 {
1584    (1.4 * 287.052_87 * temperature_k).sqrt()
1585}
1586
1587/// The Mach number estimated high, for de-weighting: the vertical speed two of its standard
1588/// errors high, in air [`AIR_TEMPERATURE_SPREAD_K`] colder than the standard, the axis up to
1589/// [`RAIL_TILT_DEG`] off the vertical.
1590fn mach_high(speed: f64, speed_sd: f64, height_msl_m: f64) -> f64 {
1591    let temperature = standard_temperature_k(height_msl_m) - AIR_TEMPERATURE_SPREAD_K;
1592    (speed.abs() + 2.0 * speed_sd) / (sound(temperature) * RAIL_TILT_DEG.to_radians().cos())
1593}
1594
1595/// The barometer's transonic span: opened once both the filter's speed and the barometer's own
1596/// rate say transonic, and closed on the rate alone, as in the span the filter's speed spreads
1597/// and would hold it open, or open it again, on its own spread.
1598#[derive(Debug, Clone, Copy, Default)]
1599struct Transonic {
1600    open: bool,
1601    /// Since when the barometer's rate has said subsonic, in an open span.
1602    clear_since_s: Option<f64>,
1603}
1604
1605impl Transonic {
1606    /// The port coefficient's possible shift at this sample, and the speed it multiplies.
1607    fn weigh(&mut self, t: f64, v: f64, v_sd: f64, rate_m_s: f64, height_msl_m: f64) -> (f64, f64) {
1608        if !self.open {
1609            let filter = mach_high(v, v_sd, height_msl_m);
1610            let rate = mach_high(rate_m_s, 0.0, height_msl_m);
1611            let shift = transonic_shift(filter.min(rate));
1612            self.open = shift >= TRANSONIC_PORT_SHIFT;
1613            return (
1614                shift,
1615                (v.abs() + 2.0 * v_sd).min(rate_m_s.abs().max(v.abs())),
1616            );
1617        }
1618        // Open, the speed is the barometer's or the span's least, whichever is more.
1619        let cold = standard_temperature_k(height_msl_m) - AIR_TEMPERATURE_SPREAD_K;
1620        let least = TRANSONIC_FULL_MACH * sound(cold) * RAIL_TILT_DEG.to_radians().cos();
1621        if mach_high(rate_m_s, 0.0, height_msl_m) < TRANSONIC_FROM_MACH {
1622            let since = *self.clear_since_s.get_or_insert(t);
1623            if t - since >= TRANSONIC_CLEAR_S {
1624                self.open = false;
1625                self.clear_since_s = None;
1626                return (0.0, 0.0);
1627            }
1628        } else {
1629            self.clear_since_s = None;
1630        }
1631        (TRANSONIC_PORT_SHIFT, rate_m_s.abs().max(least))
1632    }
1633}
1634
1635/// The burn is taken as over once the barometer's rate has fallen this share below its peak: a
1636/// coast's drag and gravity take a tenth of the speed within a few seconds, while the rate's own
1637/// noise is a few m/s. HPR Sim's choice.
1638pub const RATE_DROP_SHARE: f64 = 0.1;
1639
1640/// An open transonic span closes once the barometer's rate has said subsonic this long, s: a
1641/// shock crossing the port swings the rate both ways for a few tenths of a second.
1642pub const TRANSONIC_CLEAR_S: f64 = 1.0;
1643
1644/// The port's coefficient's possible shift at that Mach number.
1645fn transonic_shift(mach: f64) -> f64 {
1646    let w = ((mach - TRANSONIC_FROM_MACH) / (TRANSONIC_FULL_MACH - TRANSONIC_FROM_MACH))
1647        .clamp(0.0, 1.0);
1648    w * TRANSONIC_PORT_SHIFT
1649}
1650
1651/// What happened to one sample at the gate.
1652#[derive(Debug, Clone, Copy, PartialEq)]
1653enum Gated {
1654    Used,
1655    Rejected,
1656}
1657
1658/// A sensor's running tally.
1659#[derive(Debug, Clone)]
1660struct Tally {
1661    health: SensorHealth,
1662    nis_sum: f64,
1663    rejecting_since: Option<f64>,
1664}
1665
1666impl Tally {
1667    fn new(sensor: Sensor) -> Self {
1668        Self {
1669            health: SensorHealth {
1670                sensor,
1671                samples: 0,
1672                used: 0,
1673                rejected: 0,
1674                unweighed: 0,
1675                resets: 0,
1676                inside_gate_share: None,
1677                largest: None,
1678                mean_nis: None,
1679            },
1680            nis_sum: 0.0,
1681            rejecting_since: None,
1682        }
1683    }
1684
1685    fn finish(mut self) -> SensorHealth {
1686        let tested = self.health.used + self.health.rejected;
1687        if tested > 0 {
1688            self.health.inside_gate_share = Some(self.health.used as f64 / tested as f64);
1689        }
1690        if self.health.used > 0 {
1691            self.health.mean_nis = Some(self.nis_sum / self.health.used as f64);
1692        }
1693        self.health
1694    }
1695}
1696
1697/// The filter's running state and what it records for the pass back.
1698struct Filter {
1699    x: Vector,
1700    p: Matrix,
1701    /// What this step added to the covariance after its prediction was stored: a sensor taken
1702    /// back, a GNSS receiver newly trusted. It is added to the stored prediction too, as process
1703    /// noise of the step, so the pass back sees the covariance the filter used.
1704    jump: Matrix,
1705    /// The states no measurement moves now: the day's air without a GNSS receiver, and from the
1706    /// first apogee the accelerometer's, which nothing after it measures; a landing's impact would otherwise move
1707    /// them through their correlations with the speed and carry that back over the climb.
1708    held: [bool; N],
1709    rejected: Vec<Rejected>,
1710    pad_msl_for_sound_m: f64,
1711    /// Each sample's fate at the gate, in the order fused.
1712    gates: Vec<Gate>,
1713    /// A run's gate outcomes to replay, and the next one's index.
1714    replay: Option<(Vec<Gate>, usize)>,
1715}
1716
1717/// A sample's fate at the gate.
1718#[derive(Debug, Clone, Copy, PartialEq)]
1719enum Gate {
1720    Used,
1721    Rejected,
1722    /// Rejected for [`GATE_RESET_S`] and taken back, the state widened.
1723    TakenBack,
1724}
1725
1726/// The discrete choices a run makes from its own estimates: each step's phase and whether the
1727/// first apogee has passed, each barometer sample's transonic de-weighting, each GNSS fix's
1728/// trust, and each sample's fate at the gate. The consider run replays the first run's, so the
1729/// difference between the two is the port coefficient's effect alone, not that of a span closed
1730/// a sample later or a fix rejected in one run and not the other.
1731#[derive(Debug, Clone, Default, PartialEq)]
1732struct Choices {
1733    /// Each step after the first: its phase, and whether the first apogee has passed.
1734    steps: Vec<(Phase, bool)>,
1735    /// Each barometer sample's transonic shift and the speed it multiplies.
1736    transonic: Vec<(f64, f64)>,
1737    /// Each GNSS fix: trusted, trust begun, settling.
1738    gnss: Vec<(bool, bool, bool)>,
1739    /// Each sample's fate at the gate, in the order fused.
1740    gates: Vec<Gate>,
1741}
1742
1743/// A measurement's variance: as fused, and as one sample's own for the gate.
1744#[derive(Debug, Clone, Copy)]
1745struct Variance {
1746    /// As fused: a correlated error's variance grown by its correlation over the samples it
1747    /// spans, so a fast logger's many samples don't average it away.
1748    fused: f64,
1749    /// One sample's own, which its innovation is gated against: grown, it would let a pulse
1750    /// through on a fast logger that a slow one rejects.
1751    gate: f64,
1752}
1753
1754impl Variance {
1755    /// A white error's, the same both ways.
1756    fn white(variance: f64) -> Self {
1757        Self {
1758            fused: variance,
1759            gate: variance,
1760        }
1761    }
1762}
1763
1764impl Filter {
1765    /// Gates and fuses one scalar measurement.
1766    fn fuse(
1767        &mut self,
1768        tally: &mut Tally,
1769        time_s: f64,
1770        h: &Vector,
1771        y: f64,
1772        variance: Variance,
1773        reset_after_s: Option<f64>,
1774    ) -> Gated {
1775        let r = variance.fused;
1776        let s = kalman::innovation_variance(&self.p, h, variance.gate);
1777        let nis = y * y / s;
1778        let ratio = nis / (GATE_SIGMAS * GATE_SIGMAS);
1779        if tally.health.largest.is_none_or(|l| ratio > l.ratio) {
1780            let height_msl = self.pad_msl_for_sound_m + self.x[H];
1781            let mach = self.x[V].abs() / sound(standard_temperature_k(height_msl));
1782            tally.health.largest = Some(Largest {
1783                ratio,
1784                time_s,
1785                mach,
1786            });
1787        }
1788        let replayed = self.replay.as_mut().and_then(|(gates, at)| {
1789            let gate = gates.get(*at).copied();
1790            *at += 1;
1791            gate
1792        });
1793        let gate = replayed.unwrap_or_else(|| {
1794            if ratio <= 1.0 {
1795                return Gate::Used;
1796            }
1797            // An innovation past the numbers can't size the widening a reset takes.
1798            if !y.is_finite() {
1799                return Gate::Rejected;
1800            }
1801            let since = *tally.rejecting_since.get_or_insert(time_s);
1802            if reset_after_s.is_some_and(|after| time_s - since >= after) {
1803                Gate::TakenBack
1804            } else {
1805                Gate::Rejected
1806            }
1807        });
1808        self.gates.push(gate);
1809        match gate {
1810            Gate::Used => {}
1811            Gate::Rejected => {
1812                tally.health.rejected += 1;
1813                self.rejected.push(Rejected {
1814                    sensor: tally.health.sensor,
1815                    time_s,
1816                    ratio,
1817                });
1818                return Gated::Rejected;
1819            }
1820            Gate::TakenBack => {
1821                // The height, speed and acceleration widened by what the innovation says they
1822                // missed, the gap closed over the time rejected.
1823                self.widen(H, y * y);
1824                self.widen(V, (y / GATE_RESET_S).powi(2));
1825                self.widen(A, (2.0 * y / (GATE_RESET_S * GATE_RESET_S)).powi(2));
1826                tally.health.resets += 1;
1827            }
1828        }
1829        tally.rejecting_since = None;
1830        kalman::update(&mut self.x, &mut self.p, h, y, r, &self.held);
1831        tally.health.used += 1;
1832        // Against one sample's own variance, as the gate weighs it: near 1 when the sensor's
1833        // noise is what the smoother takes it to be, though a correlated error is fused grown.
1834        tally.nis_sum += nis;
1835        Gated::Used
1836    }
1837}
1838
1839impl Filter {
1840    /// Adds `variance` to a state's, as process noise of this step.
1841    fn widen(&mut self, state: usize, variance: f64) {
1842        self.p[state][state] += variance;
1843        self.jump[state][state] += variance;
1844    }
1845}
1846
1847/// The transition over `dt` seconds.
1848fn transition(dt: f64) -> Matrix {
1849    let mut f = kalman::identity();
1850    f[H][V] = dt;
1851    f[H][A] = 0.5 * dt * dt;
1852    f[V][A] = dt;
1853    f[TILT][TILT] = (-dt / TILT_TAU_S).exp();
1854    f[SLOW][SLOW] = (-dt / SLOW_NOISE_TAU_S).exp();
1855    f[DRIFT][DRIFT_RATE] = dt;
1856    f
1857}
1858
1859/// The process noise over `dt` seconds.
1860fn process_noise(dt: f64, jerk: f64, tilt_sd: f64, gnss_walk: f64) -> Matrix {
1861    let mut q = kalman::ZERO;
1862    let (d2, d3, d4, d5) = (dt * dt, dt.powi(3), dt.powi(4), dt.powi(5));
1863    q[H][H] = jerk * d5 / 20.0;
1864    q[H][V] = jerk * d4 / 8.0;
1865    q[H][A] = jerk * d3 / 6.0;
1866    q[V][V] = jerk * d3 / 3.0;
1867    q[V][A] = jerk * d2 / 2.0;
1868    q[A][A] = jerk * dt;
1869    q[V][H] = q[H][V];
1870    q[A][H] = q[H][A];
1871    q[A][V] = q[V][A];
1872    q[TILT][TILT] = tilt_sd * tilt_sd * (1.0 - (-2.0 * dt / TILT_TAU_S).exp());
1873    q[SLOW][SLOW] = SLOW_NOISE_M * SLOW_NOISE_M * (1.0 - (-2.0 * dt / SLOW_NOISE_TAU_S).exp());
1874    q[GNSS][GNSS] = gnss_walk * dt;
1875    q
1876}
1877
1878/// One step's grid: what is measured then.
1879#[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord)]
1880enum Measured {
1881    Accel(usize),
1882    Baro(usize),
1883    Gnss(usize),
1884}
1885
1886/// The grid: every sample's time, and the samples taken then, in time order.
1887fn grid(inputs: &Inputs) -> Vec<(f64, Vec<Measured>)> {
1888    let mut all: Vec<(f64, Measured)> = Vec::new();
1889    all.extend(
1890        inputs
1891            .accel
1892            .iter()
1893            .enumerate()
1894            .map(|(i, s)| (s.time_s, Measured::Accel(i))),
1895    );
1896    all.extend(
1897        inputs
1898            .baro
1899            .iter()
1900            .enumerate()
1901            .map(|(i, s)| (s.time_s, Measured::Baro(i))),
1902    );
1903    all.extend(
1904        inputs
1905            .gnss
1906            .iter()
1907            .enumerate()
1908            .map(|(i, s)| (s.0, Measured::Gnss(i))),
1909    );
1910    all.sort_by(|a, b| a.0.total_cmp(&b.0).then(a.1.cmp(&b.1)));
1911    let mut out: Vec<(f64, Vec<Measured>)> = Vec::new();
1912    for (t, m) in all {
1913        match out.last_mut() {
1914            Some((last, list)) if *last == t => list.push(m),
1915            _ => out.push((t, vec![m])),
1916        }
1917    }
1918    out
1919}
1920
1921/// The phase that sets the white jerk.
1922#[derive(Debug, Clone, Copy, PartialEq)]
1923enum Phase {
1924    Pad,
1925    Powered,
1926    Coast,
1927    Descent,
1928}
1929
1930/// The GNSS receiver's trust, as the filter runs.
1931#[derive(Debug, Clone)]
1932struct Trust {
1933    mode: GnssTrust,
1934    trusted: bool,
1935    /// When the receiver was last trusted.
1936    since_s: Option<f64>,
1937    /// The first time it was.
1938    first_s: Option<f64>,
1939    /// The start of the current run of agreeing fixes.
1940    agree_from_s: Option<f64>,
1941    /// The fixes after liftoff, kept for the window's slope.
1942    recent: Vec<(f64, f64)>,
1943    misses: usize,
1944}
1945
1946impl Trust {
1947    /// Weighs a fix at `t` against the filter's speed `v` (standard error `v_sd`); returns whether
1948    /// the receiver is trusted now, and whether trust began at this fix.
1949    fn weigh(
1950        &mut self,
1951        t: f64,
1952        z: f64,
1953        v: f64,
1954        v_sd: f64,
1955        sd: f64,
1956        liftoff_s: f64,
1957    ) -> (bool, bool) {
1958        if self.mode == GnssTrust::FromLiftoff {
1959            return (true, false);
1960        }
1961        if t <= liftoff_s {
1962            return (false, false);
1963        }
1964        if self
1965            .recent
1966            .last()
1967            .is_some_and(|&(last, _)| t - last > GNSS_GAP_S)
1968        {
1969            self.recent.clear();
1970            self.agree_from_s = None;
1971            self.trusted = false;
1972        }
1973        self.recent.push((t, z));
1974        self.recent.retain(|&(s, _)| t - s <= GNSS_AGREE_WINDOW_S);
1975        if self.trusted {
1976            return (true, false);
1977        }
1978        let agrees = self.recent.len() >= 3 && {
1979            let n = self.recent.len() as f64;
1980            let mean_t = self.recent.iter().map(|p| p.0).sum::<f64>() / n;
1981            let mean_z = self.recent.iter().map(|p| p.1).sum::<f64>() / n;
1982            let stt: f64 = self.recent.iter().map(|p| (p.0 - mean_t).powi(2)).sum();
1983            let stz: f64 = self
1984                .recent
1985                .iter()
1986                .map(|p| (p.0 - mean_t) * (p.1 - mean_z))
1987                .sum();
1988            let slope = stz / stt;
1989            let slope_sd = sd / stt.sqrt();
1990            let gap = (slope - v).abs();
1991            gap <= GNSS_AGREE_M_S && gap <= 2.0 * (slope_sd * slope_sd + v_sd * v_sd).sqrt()
1992        };
1993        if !agrees {
1994            self.agree_from_s = None;
1995            return (false, false);
1996        }
1997        let from = *self.agree_from_s.get_or_insert(t);
1998        if t - from >= GNSS_AGREE_WINDOW_S {
1999            self.trusted = true;
2000            self.since_s = Some(t);
2001            self.first_s.get_or_insert(t);
2002            return (true, true);
2003        }
2004        (false, false)
2005    }
2006}
2007
2008/// One run of the filter and the pass back, at the climb's port coefficient `climb_port`,
2009/// replaying `replay`'s choices if given. Without `spread` the pass back carries the means alone:
2010/// the smoothed means need only each step's gain, from the filtered and predicted covariances, not
2011/// the smoothed covariance, so they are the same numbers, and the standard errors are NaN.
2012fn run(
2013    inputs: &Inputs,
2014    options: Options,
2015    climb_port: f64,
2016    replay: Option<&Choices>,
2017    spread: bool,
2018) -> (Smoothed, Choices) {
2019    let mut choices = Choices::default();
2020    let steps = grid(inputs);
2021    let n = steps.len();
2022    let pad_msl_for_air_m = inputs.pad_msl_m.unwrap_or(0.0);
2023    let pad_msl_for_sound_m = inputs.pad_msl_m.unwrap_or(UNKNOWN_PAD_MSL_M);
2024    let rho_pad = standard_density(pad_msl_for_air_m);
2025
2026    // The prior.
2027    let mut x = [0.0; N];
2028    let mut p = kalman::ZERO;
2029    p[H][H] = 1.0;
2030    p[V][V] = 1.0;
2031    p[A][A] = 1.0;
2032    let has_accel = !inputs.accel.is_empty();
2033    let has_gnss = !inputs.gnss.is_empty();
2034    // States a log can't move keep a unit variance and are never touched.
2035    p[BIAS][BIAS] = if has_accel {
2036        (ACCEL_BIAS_M_S2 / 3_f64.sqrt()).powi(2)
2037    } else {
2038        1.0
2039    };
2040    p[SCALE][SCALE] = if has_accel {
2041        (ACCEL_SCALE_SHARE / 3_f64.sqrt()).powi(2)
2042    } else {
2043        1.0
2044    };
2045    p[TILT][TILT] = if has_accel {
2046        TILT_FLOOR_M_S2.powi(2)
2047    } else {
2048        1.0
2049    };
2050    p[SLOW][SLOW] = SLOW_NOISE_M * SLOW_NOISE_M;
2051    p[DRIFT][DRIFT] = 1.0;
2052    p[DRIFT_RATE][DRIFT_RATE] = (DRIFT_M_PER_S / 3_f64.sqrt()).powi(2);
2053    let air_sd = (AIR_TEMPERATURE_SPREAD_K / standard_temperature_k(pad_msl_for_air_m)
2054        + BAROMETER_GAIN)
2055        / 3_f64.sqrt();
2056    // The states are on the barometer's scale, `h_b = h + k ψ(h)`, `k` the day's air (`−ΔT/T_pad`
2057    // and the sensor's gain): a barometer can't tell it from a trajectory scaled to fit, and the
2058    // dynamics that could (a coast's speed against its accelerometer) carry model errors of the
2059    // same size, so an estimate of it from them walks off by many of its own standard deviations.
2060    // Only a GNSS receiver's geometric heights measure it; without one it is held at its prior
2061    // and its spread carried into the output.
2062    p[AIR][AIR] = air_sd * air_sd;
2063    if inputs.assumed_pad {
2064        // Nothing holds the rocket still at the first sample: it may be moving and under thrust,
2065        // and the barometer alone places it. The zero's own error is an offset the height can't
2066        // be told from, so it goes on the drift and reaches the height's standard error.
2067        if let Some(first) = inputs.baro.iter().find(|b| !b.unweighed) {
2068            x[H] = first.height_m;
2069        }
2070        p[H][H] = ASSUMED_HEIGHT_SD_M * ASSUMED_HEIGHT_SD_M;
2071        p[V][V] = ASSUMED_SPEED_SD_M_S * ASSUMED_SPEED_SD_M_S;
2072        p[A][A] = ASSUMED_ACCELERATION_SD_M_S2 * ASSUMED_ACCELERATION_SD_M_S2;
2073        p[DRIFT][DRIFT] = p[DRIFT][DRIFT].max(inputs.zero_sd_m * inputs.zero_sd_m);
2074    }
2075    let mut held = [false; N];
2076    held[AIR] = !has_gnss;
2077    if has_accel {
2078        // On that scale, `a_b = (1 + k) a`, the accelerometer reads
2079        // `a + g + s f + b = a_b + g + (s − k) f + (b + k g)` to first order in `k`: its scale and
2080        // bias take the air's spread, anti-correlated so a pad's `s g + b` keeps its own.
2081        p[SCALE][SCALE] += air_sd * air_sd;
2082        p[BIAS][BIAS] += (G0 * air_sd).powi(2);
2083        p[SCALE][BIAS] -= G0 * air_sd * air_sd;
2084        p[BIAS][SCALE] = p[SCALE][BIAS];
2085    }
2086    x[PORT_FALL] = PORT_PRIOR_MEAN;
2087    p[PORT_FALL][PORT_FALL] = PORT_PRIOR_SD * PORT_PRIOR_SD;
2088    if has_gnss {
2089        x[GNSS] = inputs.gnss[0].1;
2090        p[GNSS][GNSS] = GNSS_OFFSET_SD_M * GNSS_OFFSET_SD_M;
2091    } else {
2092        p[GNSS][GNSS] = 1.0;
2093    }
2094
2095    let mut filter = Filter {
2096        x,
2097        p,
2098        jump: kalman::ZERO,
2099        held,
2100        rejected: Vec::new(),
2101        pad_msl_for_sound_m,
2102        gates: Vec::new(),
2103        replay: replay.map(|c| (c.gates.clone(), 0)),
2104    };
2105    let mut baro_tally = Tally::new(Sensor::Barometer);
2106    let mut accel_tally = Tally::new(Sensor::Accelerometer);
2107    let mut gnss_tally = Tally::new(Sensor::Gnss);
2108    let mut trust = Trust {
2109        mode: options.gnss_trust,
2110        trusted: false,
2111        since_s: None,
2112        first_s: None,
2113        agree_from_s: None,
2114        recent: Vec::new(),
2115        misses: 0,
2116    };
2117    let mut deweighted: Vec<Span> = Vec::new();
2118
2119    let mut x_pred: Vec<Vector> = Vec::with_capacity(n);
2120    let mut p_pred: Vec<f64> = Vec::with_capacity(n * PACKED);
2121    let mut x_filt: Vec<Vector> = Vec::with_capacity(n);
2122    let mut held_at: Vec<[bool; N]> = Vec::with_capacity(n);
2123    let mut p_filt: Vec<f64> = Vec::with_capacity(n * PACKED);
2124
2125    let mut powered_until: Option<f64> = None;
2126    // Past the first apogee the rocket may lie over, tumble or fall nose down, and its axis says
2127    // nothing of the vertical: the accelerometer is set aside from then.
2128    let mut past_apogee = false;
2129    let mut top_speed = 0.0_f64;
2130    let mut peak_low = 0.0_f64;
2131    let mut rate_peak = 0.0_f64;
2132    let mut last_rate = 0.0_f64;
2133    let mut landed = false;
2134    let mut transonic = Transonic::default();
2135    let mut last_f = G0;
2136    let mut last_baro_s: Option<f64> = None;
2137
2138    for (k, (t, measured)) in steps.iter().enumerate() {
2139        let t = *t;
2140        if k > 0 {
2141            let dt = t - steps[k - 1].0;
2142            let phase = if t <= inputs.pad_end_s {
2143                Phase::Pad
2144            } else {
2145                // The burn ends when the climb surely slows faster than half a g, or its speed is
2146                // surely past its peak, each by two standard errors, or the barometer's own rate
2147                // has fallen [`RATE_DROP_SHARE`] off its peak outside a transonic span (where the
2148                // filter's spread, wide under the burn's jerk, would never let the first two
2149                // say so); and the first apogee comes when the climb stops. None of these before
2150                // the rocket is off the pad and climbing, as a barometer's liftoff window opens on
2151                // noise: a burnout found early would hold the coast's jerk through the burn.
2152                let a_high = filter.x[A] + 2.0 * filter.p[A][A].max(0.0).sqrt();
2153                let v_sd = filter.p[V][V].max(0.0).sqrt();
2154                top_speed = top_speed.max(filter.x[V]);
2155                peak_low = peak_low.max(filter.x[V] - 2.0 * v_sd);
2156                let replayed = replay.and_then(|c| c.steps.get(k - 1)).copied();
2157                let climbing = t > inputs.liftoff.time_s && top_speed > APOGEE_SPEED_M_S;
2158                let rate_fell = !transonic.open
2159                    && rate_peak > APOGEE_SPEED_M_S
2160                    && last_rate < rate_peak * (1.0 - RATE_DROP_SHARE);
2161                let slowing =
2162                    a_high < -0.5 * G0 || filter.x[V] + 2.0 * v_sd < peak_low || rate_fell;
2163                if powered_until.is_none() && climbing && slowing {
2164                    powered_until = Some(t + POWERED_HOLD_S);
2165                }
2166                let apogee = match replayed {
2167                    Some((_, past)) => past,
2168                    None => {
2169                        past_apogee
2170                            || powered_until.is_some()
2171                                && filter.x[V] + 2.0 * v_sd <= 0.0
2172                                && last_rate <= 0.0
2173                    }
2174                };
2175                if apogee && !past_apogee {
2176                    past_apogee = true;
2177                    for state in [BIAS, SCALE, TILT] {
2178                        filter.held[state] = true;
2179                    }
2180                }
2181                match (replayed, powered_until) {
2182                    (Some((phase, _)), _) => phase,
2183                    (None, None) => Phase::Powered,
2184                    (None, Some(until)) if t <= until => Phase::Powered,
2185                    _ if filter.x[V] > 0.0 => Phase::Coast,
2186                    _ => Phase::Descent,
2187                }
2188            };
2189            choices.steps.push((phase, past_apogee));
2190            let measured_a = has_accel && !past_apogee;
2191            let jerk = match phase {
2192                Phase::Pad => JERK_PAD,
2193                Phase::Powered => JERK_POWERED,
2194                // The impact stops the rocket in a few hundredths of a second, and what it does
2195                // on the ground (a bounce, a drag across the field) no model of a flight holds.
2196                _ if measured_a || landed => JERK_POWERED,
2197                Phase::Coast => JERK_COAST,
2198                Phase::Descent => JERK_DESCENT,
2199            };
2200            let tilt_sd = if has_accel {
2201                tilt_share(filter.x[V], top_speed, phase == Phase::Powered) * last_f.abs()
2202                    + TILT_FLOOR_M_S2
2203            } else {
2204                0.0
2205            };
2206            let gnss_walk = if has_gnss { GNSS_OFFSET_WALK } else { 0.0 };
2207            let f = transition(dt);
2208            filter.x = kalman::apply(&f, &filter.x);
2209            filter.p =
2210                kalman::propagate(&f, &filter.p, &process_noise(dt, jerk, tilt_sd, gnss_walk));
2211        }
2212        x_pred.push(filter.x);
2213        kalman::pack(&filter.p, &mut p_pred);
2214
2215        // The impact stops the rocket faster than any jerk the model allows; its speed and
2216        // acceleration let go there, so the stop moves them and not the sensors' constants, and
2217        // the pass back doesn't carry it over the descent.
2218        if let Some(landing_s) = inputs.landing_s
2219            && !landed
2220            && t >= landing_s - LANDING_LEAD_S
2221        {
2222            landed = true;
2223            let v = filter.x[V].abs() + 3.0 * filter.p[V][V].max(0.0).sqrt();
2224            filter.widen(V, v * v);
2225            filter.widen(A, (v / LANDING_LEAD_S).powi(2));
2226        }
2227
2228        if t <= inputs.pad_end_s {
2229            for state in [H, V, A] {
2230                let mut h = [0.0; N];
2231                h[state] = 1.0;
2232                let y = -filter.x[state];
2233                kalman::update(
2234                    &mut filter.x,
2235                    &mut filter.p,
2236                    &h,
2237                    y,
2238                    PAD_SD * PAD_SD,
2239                    &filter.held,
2240                );
2241            }
2242        }
2243
2244        for m in measured {
2245            match *m {
2246                Measured::Accel(i) => {
2247                    let s = inputs.accel[i];
2248                    accel_tally.health.samples += 1;
2249                    if s.clipped || past_apogee {
2250                        accel_tally.health.unweighed += 1;
2251                        continue;
2252                    }
2253                    last_f = s.value_m_s2;
2254                    // z = a + g + s z̃ + b + m, the scale on the running median z̃.
2255                    let x = filter.x;
2256                    let mut h = [0.0; N];
2257                    h[A] = 1.0;
2258                    h[SCALE] = s.smoothed_m_s2;
2259                    h[BIAS] = 1.0;
2260                    h[TILT] = 1.0;
2261                    let predicted = x[A] + G0 + x[SCALE] * s.smoothed_m_s2 + x[BIAS] + x[TILT];
2262                    let y = s.value_m_s2 - predicted;
2263                    filter.fuse(
2264                        &mut accel_tally,
2265                        t,
2266                        &h,
2267                        y,
2268                        Variance::white(inputs.accel_sd * inputs.accel_sd),
2269                        Some(GATE_RESET_S),
2270                    );
2271                }
2272                Measured::Baro(i) => {
2273                    let s = inputs.baro[i];
2274                    baro_tally.health.samples += 1;
2275                    if t > inputs.liftoff.time_s {
2276                        last_rate = s.rate_m_s;
2277                        if !transonic.open {
2278                            rate_peak = rate_peak.max(s.rate_m_s);
2279                        }
2280                    }
2281                    if s.unweighed {
2282                        baro_tally.health.unweighed += 1;
2283                        continue;
2284                    }
2285                    let dt_baro = last_baro_s.map_or(0.05, |last| (t - last).max(1.0e-4));
2286                    last_baro_s = Some(t);
2287                    // The air at the running median's height: a garbled sample's own would
2288                    // put it in no air at all.
2289                    let height_msl = pad_msl_for_air_m + s.smoothed_m;
2290                    let thin = (rho_pad / standard_density(height_msl)).powi(2);
2291                    let fast = FAST_NOISE_M * FAST_NOISE_M;
2292                    // A de-weighted span's error is correlated over a second, not white.
2293                    let held = correlated_factor(dt_baro, DEWEIGHT_TAU_S);
2294                    // The pad's white noise, thinned with the air, and what the flight's own
2295                    // scatter holds beyond it and the correlated noise the pad's was taken less
2296                    // of (the fast, added below, and the slow, a state): correlated, so fused as
2297                    // such.
2298                    let pad = inputs.baro_white_var * thin;
2299                    let slow = SLOW_NOISE_M * SLOW_NOISE_M;
2300                    let excess = if t >= inputs.descent_from_s {
2301                        (s.scatter_var_m2 - (fast + slow) * thin - pad).max(0.0)
2302                    } else {
2303                        0.0
2304                    };
2305                    let mut r = Variance {
2306                        fused: pad
2307                            + excess * correlated_factor(dt_baro, SWING_TAU_S)
2308                            + fast * correlated_factor(dt_baro, FAST_NOISE_TAU_S) * thin,
2309                        gate: pad + excess + fast * thin,
2310                    };
2311                    let v = filter.x[V];
2312                    let v_sd = filter.p[V][V].max(0.0).sqrt();
2313                    // A descent fast enough to be transonic is a failed recovery's, and a filter
2314                    // that lost the track would de-weight the barometer that can find it again.
2315                    let replayed = replay.and_then(|c| c.transonic.get(choices.transonic.len()));
2316                    let (shift, v_high) = match replayed {
2317                        Some(&chosen) => chosen,
2318                        None if past_apogee => (0.0, 0.0),
2319                        None => transonic.weigh(
2320                            t,
2321                            v,
2322                            v_sd,
2323                            s.rate_m_s,
2324                            pad_msl_for_sound_m + s.height_m,
2325                        ),
2326                    };
2327                    choices.transonic.push((shift, v_high));
2328                    if shift > 0.0 {
2329                        let error = (shift * v_high * v_high / (2.0 * G0)).powi(2);
2330                        r.fused += error * held;
2331                        r.gate += error;
2332                        note_span(&mut deweighted, t, Deweighted::Transonic);
2333                    }
2334                    if inputs
2335                        .charges
2336                        .iter()
2337                        .any(|&(from, to)| (from..=to).contains(&t))
2338                    {
2339                        r.fused += CHARGE_SD_M * CHARGE_SD_M * held;
2340                        r.gate += CHARGE_SD_M * CHARGE_SD_M;
2341                        note_span(&mut deweighted, t, Deweighted::Charge);
2342                    }
2343                    // z = h_b + e + d − Cp v̂²/(2g), the port's error `Cp q/(ρ g)` on the predicted
2344                    // speed v̂: the climb's coefficient given, the fall's learned (a fall's
2345                    // attitude isn't the climb's); under a canopy it is small.
2346                    let x = filter.x;
2347                    let q_term = if landed {
2348                        0.0
2349                    } else {
2350                        s.rate_m_s * s.rate_m_s / (2.0 * G0)
2351                    };
2352                    let mut h = [0.0; N];
2353                    h[H] = 1.0;
2354                    h[SLOW] = 1.0;
2355                    h[DRIFT] = 1.0;
2356                    let coefficient = if past_apogee {
2357                        h[PORT_FALL] = -q_term;
2358                        x[PORT_FALL]
2359                    } else {
2360                        climb_port
2361                    };
2362                    // The term's slope in the speed, `−Cp v/g`: left out, a speed gone high would
2363                    // move only the height to explain what its own square predicts.
2364                    let predicted = x[H] + x[SLOW] + x[DRIFT] - coefficient * q_term;
2365                    let y = s.height_m - predicted;
2366                    filter.fuse(&mut baro_tally, t, &h, y, r, Some(GATE_RESET_S));
2367                }
2368                Measured::Gnss(i) => {
2369                    let (_, z) = inputs.gnss[i];
2370                    gnss_tally.health.samples += 1;
2371                    let v_sd = filter.p[V][V].max(0.0).sqrt();
2372                    let replayed = replay.and_then(|c| c.gnss.get(choices.gnss.len()));
2373                    let (trusted, began, settling) = match replayed {
2374                        Some(&chosen) => chosen,
2375                        None => {
2376                            let (trusted, began) = trust.weigh(
2377                                t,
2378                                z,
2379                                filter.x[V],
2380                                v_sd,
2381                                inputs.gnss_sd,
2382                                inputs.liftoff.time_s,
2383                            );
2384                            let settling =
2385                                trust.since_s.is_some_and(|since| t - since < GNSS_SETTLE_S);
2386                            (trusted, began, settling)
2387                        }
2388                    };
2389                    choices.gnss.push((trusted, began, settling));
2390                    if !trusted {
2391                        gnss_tally.health.unweighed += 1;
2392                        continue;
2393                    }
2394                    if began {
2395                        // The offset let go: whatever the receiver's lag left in it is forgotten.
2396                        filter.widen(GNSS, GNSS_OFFSET_SD_M * GNSS_OFFSET_SD_M);
2397                    }
2398                    let r = inputs.gnss_sd
2399                        * inputs.gnss_sd
2400                        * if settling { GNSS_SETTLE_FACTOR } else { 1.0 };
2401                    // z = h_b − k ψ(h_b) + o: the true height, and the receiver's offset.
2402                    let psi = air_regressor(filter.x[H].max(0.0), pad_msl_for_air_m);
2403                    let mut h = [0.0; N];
2404                    h[H] = 1.0;
2405                    h[AIR] = -psi;
2406                    h[GNSS] = 1.0;
2407                    let y = z - (filter.x[H] - filter.x[AIR] * psi + filter.x[GNSS]);
2408                    let reset_after_s =
2409                        (trust.mode == GnssTrust::FromLiftoff).then_some(GATE_RESET_S);
2410                    match filter.fuse(&mut gnss_tally, t, &h, y, Variance::white(r), reset_after_s)
2411                    {
2412                        Gated::Used => trust.misses = 0,
2413                        Gated::Rejected => {
2414                            trust.misses += 1;
2415                            if trust.mode == GnssTrust::Agreement && trust.misses >= 2 {
2416                                trust.trusted = false;
2417                                trust.agree_from_s = None;
2418                                trust.misses = 0;
2419                            }
2420                        }
2421                    }
2422                }
2423            }
2424        }
2425        if filter.jump != kalman::ZERO {
2426            let stored = &mut p_pred[k * PACKED..(k + 1) * PACKED];
2427            let mut at = 0;
2428            for i in 0..N {
2429                for j in i..N {
2430                    stored[at] += filter.jump[i][j];
2431                    at += 1;
2432                }
2433            }
2434            filter.jump = kalman::ZERO;
2435        }
2436        held_at.push(filter.held);
2437        x_filt.push(filter.x);
2438        kalman::pack(&filter.p, &mut p_filt);
2439    }
2440
2441    // The pass back.
2442    let mut height_m = vec![0.0; n];
2443    let mut height_sd_m = vec![0.0; n];
2444    let mut speed_m_s = vec![0.0; n];
2445    let mut speed_sd_m_s = vec![0.0; n];
2446    let mut unsmoothed_steps = 0;
2447    if n > 0 {
2448        let mut xs = x_filt[n - 1];
2449        let mut ps = kalman::unpack(&p_filt[(n - 1) * PACKED..n * PACKED]);
2450        // Off the barometer's scale: `h = h_b − k ψ(h_b)` and `v = v_b (1 − k ψ')`, `ψ' = T_pad/T`,
2451        // each with its variance `c P cᵀ` along its row `c`.
2452        let t_pad = standard_temperature_k(pad_msl_for_air_m);
2453        let mut put = |k: usize, xs: &Vector, ps: &Matrix| {
2454            let h_b = xs[H].max(0.0);
2455            let psi = air_regressor(h_b, pad_msl_for_air_m);
2456            let slope = t_pad / standard_temperature_k(pad_msl_for_air_m + h_b);
2457            let along = |i: usize, ci: f64, cair: f64| -> f64 {
2458                ci * ci * ps[i][i] + 2.0 * ci * cair * ps[i][AIR] + cair * cair * ps[AIR][AIR]
2459            };
2460            height_m[k] = xs[H] - xs[AIR] * psi;
2461            let gain = 1.0 - xs[AIR] * slope;
2462            speed_m_s[k] = xs[V] * gain;
2463            if spread {
2464                height_sd_m[k] = along(H, 1.0, -psi).max(0.0).sqrt();
2465                speed_sd_m_s[k] = along(V, gain, -slope * xs[V]).max(0.0).sqrt();
2466            } else {
2467                height_sd_m[k] = f64::NAN;
2468                speed_sd_m_s[k] = f64::NAN;
2469            }
2470        };
2471        put(n - 1, &xs, &ps);
2472        for k in (0..n - 1).rev() {
2473            let dt = steps[k + 1].0 - steps[k].0;
2474            let f = transition(dt);
2475            let pf = kalman::unpack(&p_filt[k * PACKED..(k + 1) * PACKED]);
2476            let pp = kalman::unpack(&p_pred[(k + 1) * PACKED..(k + 2) * PACKED]);
2477            let c = kalman::mul(&f, &pf);
2478            match kalman::solve(&pp, &c) {
2479                Some(gain_t) => {
2480                    // G = (P_pred⁻¹ F P_f)ᵀ.
2481                    let mut gain = kalman::ZERO;
2482                    for i in 0..N {
2483                        for j in 0..N {
2484                            gain[i][j] = gain_t[j][i];
2485                        }
2486                    }
2487                    // A held state's estimate isn't moved going back either.
2488                    for (row, &held) in gain.iter_mut().zip(&held_at[k]) {
2489                        if held {
2490                            *row = [0.0; N];
2491                        }
2492                    }
2493                    let mut dx = [0.0; N];
2494                    for i in 0..N {
2495                        dx[i] = xs[i] - x_pred[k + 1][i];
2496                    }
2497                    let correction = kalman::apply(&gain, &dx);
2498                    let mut next = x_filt[k];
2499                    for i in 0..N {
2500                        next[i] += correction[i];
2501                    }
2502                    xs = next;
2503                    if !spread {
2504                        put(k, &xs, &ps);
2505                        continue;
2506                    }
2507                    let mut dp = ps;
2508                    for i in 0..N {
2509                        for j in 0..N {
2510                            dp[i][j] -= pp[i][j];
2511                        }
2512                    }
2513                    let gdp = kalman::mul(&gain, &dp);
2514                    let mut next_p = kalman::mul_transposed(&gdp, &gain);
2515                    for i in 0..N {
2516                        for j in 0..N {
2517                            next_p[i][j] += pf[i][j];
2518                        }
2519                    }
2520                    kalman::symmetrize(&mut next_p);
2521                    ps = next_p;
2522                }
2523                None => {
2524                    unsmoothed_steps += 1;
2525                    xs = x_filt[k];
2526                    ps = pf;
2527                }
2528            }
2529            put(k, &xs, &ps);
2530        }
2531    }
2532
2533    let times: Vec<f64> = steps.iter().map(|s| s.0).collect();
2534    let apogee_index = (0..n)
2535        .filter(|&k| times[k] > inputs.liftoff.time_s)
2536        .max_by(|&a, &b| height_m[a].total_cmp(&height_m[b]).then(b.cmp(&a)))
2537        .unwrap_or(0);
2538    let apogee_time_s = times.get(apogee_index).copied().unwrap_or(f64::NAN);
2539    let baro_minus_inertial_at_apogee_m = inertial_gap(inputs, apogee_time_s);
2540
2541    let mut sensors = vec![baro_tally.finish()];
2542    if has_accel {
2543        sensors.push(accel_tally.finish());
2544    }
2545    if has_gnss {
2546        sensors.push(gnss_tally.finish());
2547    }
2548    let mut rejected = filter.rejected;
2549    rejected.sort_by(|a, b| a.time_s.total_cmp(&b.time_s).then(a.sensor.cmp(&b.sensor)));
2550    choices.gates = filter.gates;
2551    let smoothed = Smoothed {
2552        time_s: times,
2553        height_m,
2554        height_sd_m,
2555        speed_m_s,
2556        speed_sd_m_s,
2557        liftoff: inputs.liftoff,
2558        apogee_time_s,
2559        apogee_is_floor: false,
2560        health: Health {
2561            sensors,
2562            deweighted,
2563            rejected,
2564            gnss_trusted_s: trust.first_s,
2565            baro_minus_inertial_at_apogee_m,
2566            unsmoothed_steps,
2567            assumed_pad: inputs.assumed_pad,
2568            accelerometer_set_aside: inputs.accelerometer_set_aside,
2569            apogee_less_reading_m: None,
2570            apogee_past_reading_bound: false,
2571        },
2572    };
2573    (smoothed, choices)
2574}
2575
2576/// Adds a de-weighted sample to the spans: it extends the last span of its kind if it follows it
2577/// within a second.
2578fn note_span(spans: &mut Vec<Span>, t: f64, why: Deweighted) {
2579    if let Some(span) = spans.iter_mut().rev().find(|s| s.why == why)
2580        && t - span.end_s <= 1.0
2581    {
2582        span.end_s = t;
2583        return;
2584    }
2585    spans.push(Span {
2586        start_s: t,
2587        end_s: t,
2588        why,
2589    });
2590}
2591
2592/// The barometer's height less the accelerometer's alone at `apogee_s`: the accelerometer's
2593/// readings less its pad's level, integrated twice from liftoff by the trapezoid rule, a clipped
2594/// sample held at the last good one.
2595fn inertial_gap(inputs: &Inputs, apogee_s: f64) -> Option<f64> {
2596    if inputs.accel.is_empty() || !apogee_s.is_finite() {
2597        return None;
2598    }
2599    let start = inputs.liftoff.time_s;
2600    let (mut v, mut h) = (0.0, 0.0);
2601    let mut last: Option<(f64, f64)> = None;
2602    for s in inputs
2603        .accel
2604        .iter()
2605        .filter(|s| s.time_s >= start && s.time_s <= apogee_s)
2606    {
2607        let a = if s.clipped {
2608            last.map_or(0.0, |(_, a)| a)
2609        } else {
2610            s.value_m_s2 - inputs.accel_pad
2611        };
2612        if let Some((t0, a0)) = last {
2613            let dt = s.time_s - t0;
2614            let v_next = v + 0.5 * (a0 + a) * dt;
2615            h += 0.5 * (v + v_next) * dt;
2616            v = v_next;
2617        }
2618        last = Some((s.time_s, a));
2619    }
2620    let baro = inputs.baro.iter().min_by(|a, b| {
2621        (a.time_s - apogee_s)
2622            .abs()
2623            .total_cmp(&(b.time_s - apogee_s).abs())
2624    })?;
2625    Some(baro.height_m - h)
2626}
2627
2628#[cfg(test)]
2629mod tests {
2630    use super::*;
2631    use crate::log::{LogFormat, Stated};
2632
2633    const DT: f64 = 0.05;
2634    const LIFTOFF_S: f64 = 5.0;
2635    const BURNOUT_S: f64 = 7.0;
2636    /// 5 g for 2 s: 98.1 m/s and 98.1 m at burnout.
2637    const BOOST_M_S2: f64 = 5.0 * G0;
2638    const DESCENT_M_S: f64 = 15.0;
2639
2640    /// An invented flight: on the pad to 5 s, 5 g to 7 s, a coast without drag to apogee at 17 s
2641    /// (588.4 m), a fall to 15 m/s and down at that to the ground at 57.0 s, and 16 s on it.
2642    /// Height and vertical speed.
2643    fn truth(t: f64) -> (f64, f64) {
2644        let v_burnout = BOOST_M_S2 * (BURNOUT_S - LIFTOFF_S);
2645        let h_burnout = 0.5 * BOOST_M_S2 * (BURNOUT_S - LIFTOFF_S).powi(2);
2646        let apogee_s = BURNOUT_S + v_burnout / G0;
2647        let apogee_m = h_burnout + v_burnout * v_burnout / (2.0 * G0);
2648        if t <= LIFTOFF_S {
2649            (0.0, 0.0)
2650        } else if t <= BURNOUT_S {
2651            let c = t - LIFTOFF_S;
2652            (0.5 * BOOST_M_S2 * c * c, BOOST_M_S2 * c)
2653        } else if t <= apogee_s {
2654            let c = t - BURNOUT_S;
2655            (
2656                h_burnout + v_burnout * c - 0.5 * G0 * c * c,
2657                v_burnout - G0 * c,
2658            )
2659        } else {
2660            let c = t - apogee_s;
2661            let fall_s = DESCENT_M_S / G0;
2662            if c < fall_s {
2663                return (apogee_m - 0.5 * G0 * c * c, -G0 * c);
2664            }
2665            let h = apogee_m - 0.5 * G0 * fall_s * fall_s - DESCENT_M_S * (c - fall_s);
2666            if h > 0.0 {
2667                (h, -DESCENT_M_S)
2668            } else {
2669                (0.0, 0.0)
2670            }
2671        }
2672    }
2673
2674    /// A noise sample uniform in ±0.5 m (0.29 m standard deviation), the same every run.
2675    fn noise(state: &mut u64) -> f64 {
2676        *state = state
2677            .wrapping_mul(6_364_136_223_846_793_005)
2678            .wrapping_add(1_442_695_040_888_963_407);
2679        (*state >> 11) as f64 / (1_u64 << 53) as f64 - 0.5
2680    }
2681
2682    /// The flight logged by a 20 Hz barometer-only altimeter.
2683    fn log() -> FlightLog {
2684        let times: Vec<f64> = (0..1460).map(|i| f64::from(i) * DT).collect();
2685        let mut state = 7;
2686        let altitude_m = times
2687            .iter()
2688            .map(|&t| truth(t).0 + noise(&mut state))
2689            .collect();
2690        FlightLog {
2691            format: LogFormat::PerfectFlitePf2,
2692            logger: "PerfectFlite Pnut".to_owned(),
2693            serial_number: None,
2694            firmware: None,
2695            flight_number: None,
2696            stated: Stated::default(),
2697            time_s: times,
2698            altitude_m,
2699            vertical_speed_m_s: None,
2700            temperature_k: None,
2701            battery_v: None,
2702            channels: Vec::new(),
2703            events: Vec::new(),
2704            notes: Vec::new(),
2705        }
2706    }
2707
2708    /// The same flight with a GNSS receiver at 10 Hz on a pad 300 m above sea level, its altitude
2709    /// lagging the truth by 1.5 s throughout, as a receiver's filter does in a climb.
2710    fn with_gnss(mut log: FlightLog) -> FlightLog {
2711        let times: Vec<f64> = (0..730).map(|i| f64::from(i) * 0.1).collect();
2712        let values = times.iter().map(|&t| 300.0 + truth(t - 1.5).0).collect();
2713        log.channels
2714            .push(Channel::new("gnss", Quantity::GnssAltitude, times, values));
2715        log
2716    }
2717
2718    fn smoothed(log: &FlightLog, options: Options) -> Smoothed {
2719        smooth_with(log, &readings::read(log), options).expect("the invented log smooths")
2720    }
2721
2722    fn smoothed_track(values: &[f64], times: &[f64]) -> Vec<f64> {
2723        super::smoothed(values, times)
2724    }
2725
2726    /// Asserts the truth lies inside the 90% interval at `t`, and the interval is no wider than
2727    /// `height_sd_m` and `speed_sd_m_s`.
2728    fn holds_at(smoothed: &Smoothed, t: f64, height_sd_m: f64, speed_sd_m_s: f64) {
2729        let state = smoothed.at(t).expect("inside the log");
2730        let (h, v) = truth(t);
2731        assert!(
2732            (state.height_m - h).abs() <= INTERVAL_Z * state.height_sd_m,
2733            "height at {t} s: {} ± {} against {h}",
2734            state.height_m,
2735            state.height_sd_m
2736        );
2737        assert!(
2738            (state.speed_m_s - v).abs() <= INTERVAL_Z * state.speed_sd_m_s,
2739            "speed at {t} s: {} ± {} against {v}",
2740            state.speed_m_s,
2741            state.speed_sd_m_s
2742        );
2743        assert!(
2744            state.height_sd_m <= height_sd_m,
2745            "height sd at {t} s: {}",
2746            state.height_sd_m
2747        );
2748        assert!(
2749            state.speed_sd_m_s <= speed_sd_m_s,
2750            "speed sd at {t} s: {}",
2751            state.speed_sd_m_s
2752        );
2753    }
2754
2755    #[test]
2756    fn an_invented_flight_lies_inside_its_intervals() {
2757        let smoothed = smoothed(&log(), Options::default());
2758        let liftoff = smoothed.liftoff;
2759        assert!(
2760            (liftoff.time_s - LIFTOFF_S).abs() <= liftoff.bound_s,
2761            "{liftoff:?}"
2762        );
2763        // The intervals are bounded by the day's air, up to about 4% of the height without a
2764        // GNSS receiver, and at burnout by the climb's port coefficient's spread, 0.02 of
2765        // `v²/2g`, 9.8 m. On the ground the acceleration is let go ([`JERK_POWERED`]), so the
2766        // speed is the barometer's noise's alone.
2767        for (t, height_sd_m, speed_sd_m_s) in [
2768            (6.0, 15.0, 15.0),
2769            (BURNOUT_S, 20.0, 15.0),
2770            (12.0, 25.0, 5.0),
2771            (17.0, 30.0, 3.0),
2772            (30.0, 25.0, 2.0),
2773            (50.0, 10.0, 2.0),
2774            (70.0, 2.0, 5.0),
2775        ] {
2776            holds_at(&smoothed, t, height_sd_m, speed_sd_m_s);
2777        }
2778        assert!(
2779            (smoothed.apogee_time_s - 17.0).abs() < 0.5,
2780            "{}",
2781            smoothed.apogee_time_s
2782        );
2783        assert_eq!(smoothed.health.unsmoothed_steps, 0);
2784        assert!(
2785            smoothed.health.rejected.is_empty(),
2786            "{:?}",
2787            smoothed.health.rejected
2788        );
2789        // The log names no charge, and the barometer is de-weighted across its apogee anyway.
2790        assert!(
2791            smoothed
2792                .health
2793                .deweighted
2794                .iter()
2795                .any(|s| s.why == Deweighted::Charge && s.start_s <= 17.0 && s.end_s >= 17.0),
2796            "{:?}",
2797            smoothed.health.deweighted
2798        );
2799    }
2800
2801    #[test]
2802    fn the_selected_median_is_the_sorted_one() {
2803        let mut state = 5;
2804        for n in [1, 2, 3, 4, 11, 40, 41] {
2805            let values: Vec<f64> = (0..n).map(|_| noise(&mut state)).collect();
2806            assert_eq!(
2807                selected_median(&mut values.clone()),
2808                median(&mut values.clone()),
2809                "{n}"
2810            );
2811        }
2812    }
2813
2814    #[test]
2815    fn the_scatter_is_the_noise_about_a_curving_track() {
2816        // Normal noise of σ 0.5 m, by Box and Muller from the uniform draws; about a coast's
2817        // parabola the quadratic takes the track out whole, so the scatter is the noise's
2818        // variance, 0.25 m², less the fit's three parameters' share of 41 samples, 7%. A squared
2819        // MAD of 41 samples is skewed, so its median over the windows sits below that: 0.19 on
2820        // this draw. Read low by a quarter, the gate is still 4.3σ wide.
2821        let times: Vec<f64> = (0..400).map(|i| f64::from(i) * DT).collect();
2822        let mut state = 11;
2823        let values: Vec<f64> = times
2824            .iter()
2825            .map(|&t| {
2826                let (u, w) = (noise(&mut state) + 0.5, noise(&mut state) + 0.5);
2827                let normal = (-2.0 * (1.0 - u).ln()).sqrt() * (std::f64::consts::TAU * w).cos();
2828                900.0 + 80.0 * t - 0.5 * G0 * t * t + 0.5 * normal
2829            })
2830            .collect();
2831        let var = scatter(&values, &super::smoothed(&values, &times), &times);
2832        let mut sorted = var.clone();
2833        let middle = median(&mut sorted);
2834        assert!((0.7..1.05).contains(&(middle / 0.25)), "{middle}");
2835        // A noiseless track's is nothing, even curving, but at its top, where the running median
2836        // the quadratic is fitted to is a neighbor's, g Δt²/2 = 1.2 cm low at 20 Hz. Within a
2837        // window of the series' ends the median's first and last samples, a step off, pull the
2838        // fit too: a real log starts and ends at rest.
2839        let clean: Vec<f64> = times.iter().map(|&t| 80.0 * t - 0.5 * G0 * t * t).collect();
2840        let medians = super::smoothed(&clean, &times);
2841        let var = scatter(&clean, &medians, &times);
2842        let end = times.last().copied().expect("samples") - SCATTER_HALF_S;
2843        let largest = var
2844            .iter()
2845            .zip(&times)
2846            .filter(|&(_, &t)| t > SCATTER_HALF_S && t < end)
2847            .map(|(&v, _)| v)
2848            .fold(0.0, f64::max);
2849        assert!(largest < 0.012 * 0.012, "{largest}");
2850    }
2851
2852    #[test]
2853    fn a_quiet_pad_does_not_make_a_swinging_descent_fail_the_gate() {
2854        // The pad reads one value throughout, so the pad gives the barometer no noise at all;
2855        // under the canopy the rocket's swing adds 6 m at 0.8 Hz to what it reads.
2856        let mut log = log();
2857        let apogee_s = 17.0;
2858        for (z, &t) in log.altitude_m.iter_mut().zip(&log.time_s) {
2859            if t <= LIFTOFF_S {
2860                *z = 0.0;
2861            } else if t > apogee_s + 3.0 && truth(t).0 > 0.0 {
2862                *z += 6.0 * (std::f64::consts::TAU * 0.8 * t).sin();
2863            }
2864        }
2865        let smoothed = smoothed(&log, Options::default());
2866        assert!(
2867            smoothed.health.rejected.is_empty(),
2868            "{:?}",
2869            smoothed.health.rejected
2870        );
2871        assert_eq!(smoothed.health.unsmoothed_steps, 0);
2872        // The swing reads as ±30 m/s of rate, and as an error correlated over seconds it is
2873        // smoothed by the motion model, not averaged away: the speed's interval is a few m/s.
2874        for t in [30.0, 40.0, 50.0] {
2875            holds_at(&smoothed, t, 30.0, 6.0);
2876        }
2877    }
2878
2879    /// The share of the descent's heights, 50 s to 56 s every half second, outside their 90%
2880    /// intervals over `seeds` logs at `rate_hz` whose barometer adds, from 20 s on, a first-order
2881    /// error of `sd_m` and `tau_s`: a rocket swinging under its canopy.
2882    fn swinging_misses(rate_hz: f64, sd_m: f64, tau_s: f64, seeds: u64) -> f64 {
2883        let (mut missed, mut counted) = (0_u32, 0_u32);
2884        for seed in 1..=seeds {
2885            let mut log = log();
2886            let n = (73.0 * rate_hz) as u32;
2887            log.time_s = (0..n).map(|i| f64::from(i) / rate_hz).collect();
2888            let mut state = seed * 7919;
2889            let phi = (-1.0 / (rate_hz * tau_s)).exp();
2890            let mut swing = 0.0;
2891            log.altitude_m = log
2892                .time_s
2893                .iter()
2894                .map(|&t| {
2895                    let (u, w) = (noise(&mut state) + 0.5, noise(&mut state) + 0.5);
2896                    let normal = (-2.0 * (1.0 - u).ln()).sqrt() * (std::f64::consts::TAU * w).cos();
2897                    swing = phi * swing + sd_m * (1.0 - phi * phi).sqrt() * normal;
2898                    let pad = 0.29 * normal;
2899                    truth(t).0
2900                        + if t > 20.0 && truth(t).0 > 0.0 {
2901                            swing
2902                        } else {
2903                            pad
2904                        }
2905                })
2906                .collect();
2907            let smoothed = smoothed(&log, Options::default());
2908            for k in 0..=12 {
2909                let t = 50.0 + 0.5 * f64::from(k);
2910                let state = smoothed.at(t).expect("inside the log");
2911                counted += 1;
2912                if (state.height_m - truth(t).0).abs() > INTERVAL_Z * state.height_sd_m {
2913                    missed += 1;
2914                }
2915            }
2916        }
2917        f64::from(missed) / f64::from(counted)
2918    }
2919
2920    #[test]
2921    fn a_swinging_descent_correlated_over_a_second_stays_inside_its_intervals() {
2922        // A 90% interval misses a tenth on average. Over 40 logs these missed 0%, 8.5% and 5%.
2923        for (rate_hz, sd_m, tau_s) in [(20.0, 3.0, 0.3), (20.0, 6.0, 1.0), (100.0, 6.0, 1.0)] {
2924            let share = swinging_misses(rate_hz, sd_m, tau_s, 40);
2925            assert!(share <= 0.1, "{rate_hz} Hz, {sd_m} m, {tau_s} s: {share}");
2926        }
2927        // The limit: a swing correlated over 3 s is partly read as the rocket's motion, and its
2928        // intervals miss more than they claim, 26% on these.
2929        let share = swinging_misses(20.0, 6.0, 3.0, 40);
2930        assert!(share > 0.1, "{share}");
2931    }
2932
2933    #[test]
2934    fn a_smoothed_track_read_back_with_columns_of_other_lengths_gives_nothing() {
2935        let mut smoothed = smoothed(&log(), Options::default());
2936        assert!(smoothed.at(30.0).is_some());
2937        smoothed.height_m.truncate(10);
2938        assert!(smoothed.at(30.0).is_none());
2939    }
2940
2941    #[test]
2942    fn a_receiver_logging_past_the_barometer_is_cut_at_its_end() {
2943        // The receiver logs 20 s past the barometer's last sample at 72.95 s.
2944        let mut log = log();
2945        let times: Vec<f64> = (0..930).map(|i| f64::from(i) * 0.1).collect();
2946        let values = times.iter().map(|&t| 300.0 + truth(t).0).collect();
2947        log.channels
2948            .push(Channel::new("gnss", Quantity::GnssAltitude, times, values));
2949        let smoothed = smoothed(&log, Options::default());
2950        let end = log.time_s.last().copied().expect("samples");
2951        assert!(
2952            smoothed.time_s.iter().all(|&t| t <= end),
2953            "{:?}",
2954            smoothed.time_s.last()
2955        );
2956        assert!(
2957            (smoothed.apogee_time_s - 17.0).abs() < 0.5,
2958            "{}",
2959            smoothed.apogee_time_s
2960        );
2961    }
2962
2963    #[test]
2964    fn a_spike_is_rejected_at_the_gate_and_leaves_the_track_alone() {
2965        let mut log = log();
2966        let at = log
2967            .time_s
2968            .iter()
2969            .position(|&t| t >= 40.0)
2970            .expect("in the log");
2971        log.altitude_m[at] += 200.0;
2972        let smoothed = smoothed(&log, Options::default());
2973        let rejected: Vec<f64> = smoothed
2974            .health
2975            .rejected
2976            .iter()
2977            .filter(|r| r.sensor == Sensor::Barometer)
2978            .map(|r| r.time_s)
2979            .collect();
2980        assert_eq!(rejected, vec![log.time_s[at]]);
2981        holds_at(&smoothed, log.time_s[at], 20.0, 2.0);
2982    }
2983
2984    #[test]
2985    fn a_garbled_sample_is_rejected_and_bends_nothing_around_it() {
2986        // A height a logger's garbled word reads as: 2 km, a centimeter count past an int32's
2987        // range, and an f32's largest. The rates the port's term and the transonic span read
2988        // are the running medians', so the sample's neighbors keep theirs.
2989        for spike in [2000.0, 1.0e7, 3.0e38] {
2990            let mut log = log();
2991            let at = log
2992                .time_s
2993                .iter()
2994                .position(|&t| t >= 10.0)
2995                .expect("in the log");
2996            log.altitude_m[at] = spike;
2997            let smoothed = smoothed(&log, Options::default());
2998            let rejected: Vec<f64> = smoothed.health.rejected.iter().map(|r| r.time_s).collect();
2999            assert_eq!(rejected, vec![log.time_s[at]], "{spike}");
3000            assert!(
3001                smoothed
3002                    .health
3003                    .deweighted
3004                    .iter()
3005                    .all(|s| s.why != Deweighted::Transonic),
3006                "{spike}: {:?}",
3007                smoothed.health.deweighted
3008            );
3009            for t in [10.0, 12.0, 30.0] {
3010                holds_at(&smoothed, t, 25.0, 5.0);
3011            }
3012        }
3013    }
3014
3015    #[test]
3016    fn a_lagging_gnss_receiver_is_trusted_only_once_its_speed_agrees() {
3017        let log = with_gnss(log());
3018        let trusted = smoothed(&log, Options::default());
3019        let from = trusted.health.gnss_trusted_s.expect("the descent agrees");
3020        // Its speed lags the coast's by 1.5 s of gravity, 14.7 m/s, until the descent's steady
3021        // 15 m/s, which the truth reaches 1.5 s after apogee and the receiver 1.5 s later.
3022        assert!(from > 17.0 + 1.5 + 1.5, "{from}");
3023        let gnss = |s: &Smoothed| {
3024            s.health
3025                .sensors
3026                .iter()
3027                .find(|h| h.sensor == Sensor::Gnss)
3028                .cloned()
3029                .expect("a GNSS row")
3030        };
3031        assert!(gnss(&trusted).unweighed > 0);
3032        holds_at(&trusted, 12.0, 20.0, 5.0);
3033
3034        // Trusted from liftoff, the lag is fused through the climb.
3035        let options = Options {
3036            gnss_trust: GnssTrust::FromLiftoff,
3037        };
3038        let from_liftoff = smoothed(&log, options);
3039        assert_eq!(gnss(&from_liftoff).unweighed, 0);
3040    }
3041
3042    /// The flight with a 5 Hz GNSS receiver whose altitude runs off the truth at 20 m/s from
3043    /// apogee on, as no receiver's does: two height sensors that disagree past their gates.
3044    fn with_a_receiver_that_runs_off(mut log: FlightLog) -> FlightLog {
3045        let times: Vec<f64> = (0..365).map(|i| f64::from(i) * 0.2).collect();
3046        let values = times
3047            .iter()
3048            .map(|&t| 300.0 + truth(t).0 + 20.0 * (t - 17.0).max(0.0))
3049            .collect();
3050        log.channels
3051            .push(Channel::new("gnss", Quantity::GnssAltitude, times, values));
3052        log
3053    }
3054
3055    #[test]
3056    fn a_track_past_escape_speed_is_withheld_as_diverged() {
3057        // Trusted from liftoff, the receiver and the barometer are both taken back, each
3058        // take-back read as motion missed, and the smoothed speed runs away past escape speed
3059        // on a flight whose top speed is 98.1 m/s (#1590). The track is withheld, not given.
3060        let log = with_a_receiver_that_runs_off(log());
3061        let options = Options {
3062            gnss_trust: GnssTrust::FromLiftoff,
3063        };
3064        let withheld =
3065            smooth_with(&log, &readings::read(&log), options).expect_err("a speed no rocket flew");
3066        assert_eq!(withheld.reason, Reason::Diverged);
3067        assert!(
3068            withheld
3069                .detail
3070                .contains("past Earth's escape speed, 11186 m/s either way: the estimate ran away"),
3071            "{}",
3072            withheld.detail
3073        );
3074
3075        // Trusted by agreement, the receiver is never taken back, and the same log smooths: the
3076        // top speed is the flight's, and the truth lies inside the interval on the way down.
3077        let smoothed = smoothed(&log, Options::default());
3078        let top = smoothed
3079            .speed_m_s
3080            .iter()
3081            .fold(0.0_f64, |top, v| top.max(v.abs()));
3082        assert!((top - 98.1).abs() < 2.0, "{top}");
3083        holds_at(&smoothed, 30.0, 20.0, 5.0);
3084    }
3085
3086    #[test]
3087    fn the_diverged_speed_is_earths_escape_speed() {
3088        // √(2 GM/R): GM = 398,600.4418 km³/s² (IERS Conventions 2010, table 1.1), the mean
3089        // radius 6,371.0 km; NASA's fact sheet prints 11.186 km/s.
3090        let escape_m_s = (2.0 * 3.986_004_418e14_f64 / 6.371e6).sqrt();
3091        assert!(
3092            (DIVERGED_SPEED_M_S - escape_m_s).abs() < 0.5,
3093            "{escape_m_s}"
3094        );
3095        // Past any flight in HPR Sim's envelope, Mach 3.5 at sea level, by a factor of nine.
3096        const { assert!(DIVERGED_SPEED_M_S > 9.0 * 3.5 * 340.3) };
3097    }
3098
3099    #[test]
3100    fn the_landing_is_where_the_track_settles() {
3101        let baro: Vec<Baro> = (0..1460)
3102            .map(|i| {
3103                let t = f64::from(i) * DT;
3104                let h = truth(t).0;
3105                Baro {
3106                    time_s: t,
3107                    height_m: h,
3108                    smoothed_m: h,
3109                    rate_m_s: truth(t).1,
3110                    scatter_var_m2: 0.0,
3111                    unweighed: false,
3112                }
3113            })
3114            .collect();
3115        // Down at 15 m/s, it lands at 57.0 s; the track is within 5 m of the ground from 56.7 s,
3116        // and stays there.
3117        let landing = landing(&baro, LIFTOFF_S).expect("it lands");
3118        assert!((56.65..=56.75).contains(&landing), "{landing}");
3119        // Cut before the ground holds for 3 s, it hasn't landed.
3120        assert_eq!(landing_cut(&baro, 58.0), None);
3121
3122        fn landing_cut(baro: &[Baro], end_s: f64) -> Option<f64> {
3123            let cut: Vec<Baro> = baro.iter().copied().filter(|b| b.time_s <= end_s).collect();
3124            super::landing(&cut, LIFTOFF_S)
3125        }
3126    }
3127
3128    #[test]
3129    fn slopes_are_the_derivative_of_a_parabola_inside_the_window() {
3130        // Sixteenths of a second, so the window's edges fall on samples exactly.
3131        let times: Vec<f64> = (0..200).map(|i| f64::from(i) / 16.0).collect();
3132        let values: Vec<f64> = times.iter().map(|t| 3.0 + 7.0 * t - 2.0 * t * t).collect();
3133        let rates = slopes(&values, &times, RATE_HALF_S);
3134        // A least-squares line over a window symmetric about its center has a parabola's slope
3135        // there exactly.
3136        for (i, (&t, &rate)) in times.iter().zip(&rates).enumerate().skip(4).take(192) {
3137            assert!((rate - (7.0 - 4.0 * t)).abs() < 1e-9, "{i}: {rate}");
3138        }
3139    }
3140
3141    #[test]
3142    fn the_tilt_share_is_the_rails_under_power_and_grows_as_the_climb_slows() {
3143        let rail = 1.0 - RAIL_TILT_DEG.to_radians().cos();
3144        assert!((tilt_share(300.0, 300.0, true) - rail).abs() < 1e-15);
3145        assert!((tilt_share(5.0, 300.0, true) - rail).abs() < 1e-15);
3146        // In the coast, the sideways speed is the rail's share of the top speed and the wind.
3147        let side = RAIL_TILT_DEG.to_radians().sin() * 300.0 + WIND_LIMIT_M_S;
3148        let at = |v: f64| 1.0 - v / v.hypot(side);
3149        assert!((tilt_share(100.0, 300.0, false) - at(100.0)).abs() < 1e-15);
3150        assert!(tilt_share(10.0, 300.0, false) > tilt_share(100.0, 300.0, false));
3151        // Never less than the rail's.
3152        assert!((tilt_share(1.0e4, 300.0, false) - rail).abs() < 1e-15);
3153    }
3154
3155    #[test]
3156    fn a_transonic_span_opens_on_both_speeds_and_closes_after_a_subsonic_second() {
3157        let cold = standard_temperature_k(0.0) - AIR_TEMPERATURE_SPREAD_K;
3158        let mach_one = sound(cold) * RAIL_TILT_DEG.to_radians().cos();
3159        let mut span = Transonic::default();
3160        // Subsonic by both: closed.
3161        assert_eq!(
3162            span.weigh(0.0, 0.5 * mach_one, 1.0, 0.5 * mach_one, 0.0).0,
3163            0.0
3164        );
3165        // The filter says transonic, the barometer's rate doesn't: still closed.
3166        assert_eq!(
3167            span.weigh(0.1, 0.95 * mach_one, 1.0, 0.5 * mach_one, 0.0).0,
3168            0.0
3169        );
3170        assert!(!span.open);
3171        // Both: open, at the full shift.
3172        let (shift, _) = span.weigh(0.2, 0.95 * mach_one, 1.0, 0.95 * mach_one, 0.0);
3173        assert_eq!(shift, TRANSONIC_PORT_SHIFT);
3174        assert!(span.open);
3175        // The rate subsonic for less than a second, then transonic again: still open.
3176        assert_eq!(
3177            span.weigh(0.3, 0.0, 1.0, 0.5 * mach_one, 0.0).0,
3178            TRANSONIC_PORT_SHIFT
3179        );
3180        assert_eq!(
3181            span.weigh(1.2, 0.0, 1.0, 0.95 * mach_one, 0.0).0,
3182            TRANSONIC_PORT_SHIFT
3183        );
3184        // Subsonic from 1.25 s: open until 2.25 s, closed there.
3185        assert_eq!(
3186            span.weigh(1.25, 0.0, 1.0, 0.5 * mach_one, 0.0).0,
3187            TRANSONIC_PORT_SHIFT
3188        );
3189        assert_eq!(
3190            span.weigh(2.0, 0.0, 1.0, 0.5 * mach_one, 0.0).0,
3191            TRANSONIC_PORT_SHIFT
3192        );
3193        assert_eq!(span.weigh(2.25, 0.0, 1.0, 0.5 * mach_one, 0.0), (0.0, 0.0));
3194        assert!(!span.open);
3195    }
3196
3197    #[test]
3198    fn a_charge_is_expected_where_the_track_tops_out() {
3199        let baro: Vec<Baro> = (0..1460)
3200            .map(|i| {
3201                let t = f64::from(i) * DT;
3202                let h = truth(t).0;
3203                Baro {
3204                    time_s: t,
3205                    height_m: h,
3206                    smoothed_m: h,
3207                    rate_m_s: 0.0,
3208                    scatter_var_m2: 0.0,
3209                    unweighed: false,
3210                }
3211            })
3212            .collect();
3213        // Within 5 m of the top for √(2·5/g) = 1.01 s either side of apogee at 17 s.
3214        let (from, to) = apogee_charge(&baro, LIFTOFF_S).expect("a top");
3215        assert!((15.95..=16.0).contains(&from), "{from}");
3216        assert!((18.0..=18.05).contains(&(to - CHARGE_WINDOW_S)), "{to}");
3217    }
3218
3219    #[test]
3220    fn the_consider_run_replays_the_first_runs_choices_and_widens_by_its_difference() {
3221        let log = with_gnss(log());
3222        let readings = readings::read(&log);
3223        let inputs = Inputs::gather(&log, &readings).expect("it gathers");
3224        let options = Options::default();
3225        let (mean, choices) = run(&inputs, options, PORT_PRIOR_MEAN, None, true);
3226        let (off, replayed) = run(
3227            &inputs,
3228            options,
3229            PORT_PRIOR_MEAN - PORT_PRIOR_SD,
3230            Some(&choices),
3231            false,
3232        );
3233        assert_eq!(replayed, choices);
3234        // The means without the covariance's pass back are the same numbers as with it.
3235        let (off_spread, _) = run(
3236            &inputs,
3237            options,
3238            PORT_PRIOR_MEAN - PORT_PRIOR_SD,
3239            Some(&choices),
3240            true,
3241        );
3242        for (a, b) in [
3243            (&off.height_m, &off_spread.height_m),
3244            (&off.speed_m_s, &off_spread.speed_m_s),
3245        ] {
3246            assert!(a.iter().zip(b).all(|(x, y)| x.to_bits() == y.to_bits()));
3247        }
3248        assert!(off.height_sd_m.iter().all(|sd| sd.is_nan()));
3249        assert!(off.speed_sd_m_s.iter().all(|sd| sd.is_nan()));
3250        let smoothed = smooth_with(&log, &readings, options).expect("it smooths");
3251        for k in 0..mean.time_s.len() {
3252            let widened = mean.height_sd_m[k].hypot(off.height_m[k] - mean.height_m[k]);
3253            assert_eq!(smoothed.height_sd_m[k], widened);
3254        }
3255        // At burnout the port's spread, 0.02 of `v²/2g` = 9.8 m, is most of the interval.
3256        let k = mean.time_s.partition_point(|&t| t < BURNOUT_S);
3257        let difference = (off.height_m[k] - mean.height_m[k]).abs();
3258        assert!((7.0..=12.0).contains(&difference), "{difference}");
3259    }
3260
3261    /// The flight as a logger that writes only from launch would have written it: nothing before
3262    /// liftoff, its clock and zero as they were.
3263    fn from_launch(mut log: FlightLog) -> FlightLog {
3264        let first = log.time_s.partition_point(|t| *t < LIFTOFF_S);
3265        log.time_s.drain(..first);
3266        log.altitude_m.drain(..first);
3267        log
3268    }
3269
3270    #[test]
3271    fn a_log_written_from_launch_is_smoothed_on_an_assumed_pad() {
3272        let whole = smoothed(&log(), Options::default());
3273        assert!(!whole.health.assumed_pad);
3274        assert!(!whole.health.card().contains("pad assumed"));
3275
3276        let started = smoothed(&from_launch(log()), Options::default());
3277        assert!(started.health.assumed_pad);
3278        assert!(started.health.card().starts_with("pad assumed: "));
3279        assert_eq!(started.health.unsmoothed_steps, 0);
3280        for t in [LIFTOFF_S + 1.0, BURNOUT_S, 20.0, 40.0, 60.0] {
3281            holds_at(&started, t, 80.0, 25.0);
3282        }
3283        // A log that starts well up the climb has no zero the readings can take, and the smoother
3284        // takes no liftoff from what they withhold.
3285        let mut later = log();
3286        let first = later.time_s.partition_point(|t| *t < LIFTOFF_S + 1.0);
3287        later.time_s.drain(..first);
3288        later.altitude_m.drain(..first);
3289        let withheld = smooth(&later).unwrap_err();
3290        assert_eq!(withheld.reason, Reason::NoLiftoff);
3291    }
3292
3293    /// A public `.pf2` log's second sample read −65,535 ft, and its first rate, taken over the few
3294    /// samples at the log's edge, set the track off at 29 km/s until the prior left the rate out.
3295    #[test]
3296    fn a_garbled_sample_at_the_start_of_a_log_from_launch_moves_nothing() {
3297        let mut garbled = from_launch(log());
3298        garbled.altitude_m[1] = -65_535.0 * crate::perfectflite::FOOT_M;
3299        let started = smoothed(&garbled, Options::default());
3300        assert!(started.health.assumed_pad);
3301        assert_eq!(started.health.sensors[0].unweighed, 2);
3302        let top = started.height_m.iter().copied().fold(f64::MIN, f64::max);
3303        assert!(
3304            top < 1.05 * truth(BURNOUT_S + BOOST_M_S2 * (BURNOUT_S - LIFTOFF_S) / G0).0,
3305            "{top}"
3306        );
3307        assert!(started.speed_m_s.iter().all(|v| v.abs() < 500.0));
3308        holds_at(&started, 20.0, 80.0, 25.0);
3309    }
3310
3311    /// One or two bad samples where a log from launch starts: a wide prior would take them as the
3312    /// truth, so they don't reach it. Found in review, at errors of ±10 m to +1,000 m. The apogee
3313    /// is held to a standard error of the clean log's, not to its value.
3314    #[test]
3315    fn bad_first_samples_of_a_log_from_launch_move_nothing() {
3316        let clean = smoothed(&from_launch(log()), Options::default());
3317        let clean_top = clean.at(clean.apogee_time_s).unwrap();
3318        let clean_speed_m_s = readings::read(&from_launch(log()))
3319            .max_speed
3320            .value()
3321            .unwrap()
3322            .speed_m_s;
3323        let clean_truth_m = (0..2_000)
3324            .map(|i| truth(f64::from(i) * 0.05).0)
3325            .fold(0.0, f64::max);
3326        for at in [vec![0], vec![1], vec![0, 1], vec![2], vec![1, 2]] {
3327            for error in [
3328                -1000.0, -100.0, -20.0, -10.0, 10.0, 20.0, 100.0, 150.0, 200.0, 300.0, 1000.0,
3329            ] {
3330                let mut bad = from_launch(log());
3331                for &k in &at {
3332                    bad.altitude_m[k] += error;
3333                }
3334                let got = smoothed(&bad, Options::default());
3335                let top = got.at(got.apogee_time_s).unwrap();
3336                // The readings' own pad stays where it was, and with it the reading (#1076).
3337                let read = readings::read(&bad);
3338                let pad = read.pad_altitude_m.unwrap();
3339                assert!(pad.abs() < 10.0, "{at:?} {error}: a pad of {pad}");
3340                let apogee = read.apogee.value().unwrap();
3341                let truth_m = clean_truth_m;
3342                assert!(
3343                    (apogee.height_m - truth_m).abs() <= apogee.bound_m,
3344                    "{at:?} {error}: {} within {} of {truth_m}",
3345                    apogee.height_m,
3346                    apogee.bound_m
3347                );
3348                // The top speed is read as on the clean log, and the card says the pad is assumed.
3349                let speed = read.max_speed.value().unwrap();
3350                assert!(
3351                    (speed.speed_m_s - clean_speed_m_s).abs() <= speed.bound_m_s,
3352                    "{at:?} {error}: {speed:?}"
3353                );
3354                assert!(got.health.assumed_pad, "{at:?} {error}");
3355                assert!(
3356                    (top.height_m - clean_top.height_m).abs() < clean_top.height_sd_m,
3357                    "{at:?} {error}: {} against {}",
3358                    top.height_m,
3359                    clean_top.height_m
3360                );
3361                assert!(
3362                    (got.apogee_time_s - clean.apogee_time_s).abs() < 2.0,
3363                    "{at:?} {error}: at {} against {}",
3364                    got.apogee_time_s,
3365                    clean.apogee_time_s
3366                );
3367                assert!(
3368                    got.speed_m_s.iter().all(|v| v.abs() < 150.0),
3369                    "{at:?} {error}: a speed of {:?}",
3370                    got.speed_m_s.iter().map(|v| v.abs()).fold(0.0, f64::max)
3371                );
3372                holds_at(&got, 20.0, 80.0, 25.0);
3373            }
3374        }
3375        assert_eq!(clean.health.sensors[0].unweighed, 2);
3376        // Three in a row are the median of five: settling doesn't reach them, as ADR-308 says.
3377        let mut bad = from_launch(log());
3378        for k in 0..3 {
3379            bad.altitude_m[k] -= 1000.0;
3380        }
3381        let got = smoothed(&bad, Options::default());
3382        assert!(got.speed_m_s.iter().any(|v| v.abs() > 150.0));
3383    }
3384
3385    /// Two bad samples at the start of a log that holds its pad: the pad's median and the readings
3386    /// stay where they were, from the log's start or a sample in (#1076).
3387    #[test]
3388    fn bad_first_samples_of_a_log_with_its_pad_move_nothing() {
3389        let clean = readings::read(&log());
3390        let clean_apogee = clean.apogee.value().unwrap();
3391        for at in [vec![0, 1], vec![1, 2]] {
3392            for error in [100.0, 300.0, 1000.0] {
3393                let mut bad = log();
3394                for &k in &at {
3395                    bad.altitude_m[k] += error;
3396                }
3397                let read = readings::read(&bad);
3398                let apogee = read.apogee.value().unwrap();
3399                assert!(
3400                    (apogee.height_m - clean_apogee.height_m).abs() <= clean_apogee.bound_m,
3401                    "{at:?} {error}: {} against {}",
3402                    apogee.height_m,
3403                    clean_apogee.height_m
3404                );
3405                let (pad, clean_pad) =
3406                    (read.pad_altitude_m.unwrap(), clean.pad_altitude_m.unwrap());
3407                assert!(
3408                    (pad - clean_pad).abs() < 0.5,
3409                    "{at:?} {error}: {pad} against {clean_pad}"
3410                );
3411                assert!(!smoothed(&bad, Options::default()).health.assumed_pad);
3412            }
3413        }
3414    }
3415
3416    /// `log`, every `every`th sample of it.
3417    fn thinned(log: &FlightLog, every: usize) -> FlightLog {
3418        let mut thin = log.clone();
3419        thin.time_s = log.time_s.iter().copied().step_by(every).collect();
3420        thin.altitude_m = log.altitude_m.iter().copied().step_by(every).collect();
3421        thin
3422    }
3423
3424    /// Two bad samples at the start, on a log from launch or one that holds its pad, at 20, 10, 5
3425    /// and 2.5 samples a second, on this rocket: the apogee reading holds its bound of the truth,
3426    /// where the slower rates' short median used to let them through (#1076). Bad levels sweep the
3427    /// stretch under the top where the arc lingers, which isn't a pad.
3428    ///
3429    /// Low ones aren't set aside: at 5 and 2.5 samples a second the reading errs high, the safe
3430    /// side for a waiver, never low. At 2.5 from launch a first pair the climb itself reaches among
3431    /// the samples they are judged by, 53 to 75 m on this rocket, is left out (#1103).
3432    #[test]
3433    fn bad_first_samples_hold_the_apogee_at_four_rates() {
3434        let truth_m = (0..2_000)
3435            .map(|i| truth(f64::from(i) * 0.05).0)
3436            .fold(0.0, f64::max);
3437        let near_top = (0..60).map(|below| truth_m - f64::from(below));
3438        let errors: Vec<f64> = [-1000.0, -100.0, 100.0, 300.0, 1000.0]
3439            .into_iter()
3440            .chain(near_top)
3441            .collect();
3442        // Every bad level from the pad's margin up, as the first samples' own heights.
3443        let levels: Vec<f64> = (15..700).map(f64::from).collect();
3444        for every in [1, 2, 4, 8] {
3445            for (start, launch) in [(log(), false), (from_launch(log()), true)] {
3446                let clean = thinned(&start, every);
3447                for &error in &errors {
3448                    for at in [vec![0, 1], vec![0, 2], vec![1, 2]] {
3449                        let mut bad = clean.clone();
3450                        for &k in &at {
3451                            bad.altitude_m[k] += error;
3452                        }
3453                        let what = format!("every {every}, from launch {launch}, {at:?} {error}");
3454                        let read = readings::read(&bad);
3455                        let apogee = read
3456                            .apogee
3457                            .value()
3458                            .unwrap_or_else(|| panic!("{what}: {:?}", read.apogee));
3459                        let off_m = apogee.height_m - truth_m;
3460                        let high_allowed = error < 0.0 && every >= 4;
3461                        assert!(
3462                            off_m >= -apogee.bound_m && (high_allowed || off_m <= apogee.bound_m),
3463                            "{what}: {} within {} of {truth_m}",
3464                            apogee.height_m,
3465                            apogee.bound_m
3466                        );
3467                    }
3468                }
3469                for &level in &levels {
3470                    for at in [
3471                        vec![0],
3472                        vec![1],
3473                        vec![2],
3474                        vec![0, 1],
3475                        vec![0, 2],
3476                        vec![1, 2],
3477                    ] {
3478                        if launch && every == 8 && at == [0, 1] && (53.0..=75.0).contains(&level) {
3479                            continue;
3480                        }
3481                        let mut bad = clean.clone();
3482                        for &k in &at {
3483                            bad.altitude_m[k] = level;
3484                        }
3485                        let read = readings::read(&bad);
3486                        let what =
3487                            format!("every {every}, from launch {launch}, {at:?} at {level}");
3488                        let apogee = read
3489                            .apogee
3490                            .value()
3491                            .unwrap_or_else(|| panic!("{what}: {:?}", read.apogee));
3492                        assert!(
3493                            (apogee.height_m - truth_m).abs() <= apogee.bound_m,
3494                            "{what}: {} within {} of {truth_m}",
3495                            apogee.height_m,
3496                            apogee.bound_m
3497                        );
3498                    }
3499                }
3500            }
3501        }
3502    }
3503
3504    /// The same, with a barometer noisier than the readings' tuning and a slow clock after the
3505    /// climb: two bad samples stay out on every seed, and a low sample among those after them
3506    /// changes nothing (#1076).
3507    #[test]
3508    fn bad_first_samples_are_told_through_noise_and_a_slow_tail() {
3509        let truth_m = (0..2_000)
3510            .map(|i| truth(f64::from(i) * 0.05).0)
3511            .fold(0.0, f64::max);
3512        let mut state = 11;
3513        let mut gaussian = move || (0..12).map(|_| noise(&mut state)).sum::<f64>();
3514        for sd_m in [2.0, 3.0] {
3515            for seed in 0..100 {
3516                for (start, every) in [(from_launch(log()), 1), (log(), 2)] {
3517                    let mut noisy = thinned(&start, every);
3518                    for h in &mut noisy.altitude_m {
3519                        *h += sd_m * gaussian();
3520                    }
3521                    let read_clean = readings::read(&noisy);
3522                    let clean_m = read_clean.apogee.value().unwrap().height_m;
3523                    let mut bad = noisy.clone();
3524                    bad.altitude_m[0] += 300.0;
3525                    bad.altitude_m[1] += 300.0;
3526                    let read = readings::read(&bad);
3527                    let apogee = read.apogee.value().unwrap();
3528                    assert!(
3529                        (apogee.height_m - truth_m).abs() <= apogee.bound_m,
3530                        "{sd_m} m, seed {seed}, every {every}: {} (clean {clean_m}) within {} of \
3531                         {truth_m}",
3532                        apogee.height_m,
3533                        apogee.bound_m
3534                    );
3535                }
3536            }
3537        }
3538        // 50 samples a second up to the top, one a second after: the median step is a second, and
3539        // five fast samples are no pad. A level on the climb stays out.
3540        let dt = 0.02;
3541        let fast = ((LIFTOFF_S + 11.6) / dt) as u32;
3542        let mut time: Vec<f64> = (0..fast).map(|i| f64::from(i) * dt).collect();
3543        let last = *time.last().unwrap();
3544        time.extend((1..=3_000).map(|i| last + f64::from(i)));
3545        let mut slow = log();
3546        slow.altitude_m = time.iter().map(|t| truth(*t).0).collect();
3547        slow.time_s = time;
3548        let start = slow.time_s.partition_point(|t| *t < LIFTOFF_S - 0.5);
3549        slow.time_s.drain(..start);
3550        slow.altitude_m.drain(..start);
3551        for level_m in [200.0, 300.0, 500.0] {
3552            let mut bad = slow.clone();
3553            bad.altitude_m[0] = level_m;
3554            bad.altitude_m[1] = level_m;
3555            let read = readings::read(&bad);
3556            let apogee = read.apogee.value().unwrap();
3557            assert!(
3558                (apogee.height_m - truth_m).abs() <= apogee.bound_m,
3559                "{level_m}: {} within {} of {truth_m}",
3560                apogee.height_m,
3561                apogee.bound_m
3562            );
3563        }
3564    }
3565
3566    /// Good first samples, a low stretch far below them, then seconds on the pad, at 20 and 10
3567    /// samples a second: the pad's reach takes in the stretch, so the apogee is withheld, not read
3568    /// from it. A small stretch leaves the reading as on the clean log (#1076, #1101).
3569    #[test]
3570    fn a_low_stretch_that_returns_to_the_pad_reads_no_wrong_height() {
3571        for (every, low) in [(1, 3..15), (2, 2..10)] {
3572            let clean = thinned(&log(), every);
3573            let clean_height = readings::read(&clean).apogee.value().unwrap().height_m;
3574            let with_low = |depth_m: f64| {
3575                let mut bad = clean.clone();
3576                for k in low.clone() {
3577                    bad.altitude_m[k] -= depth_m;
3578                }
3579                readings::read(&bad).apogee
3580            };
3581            assert!(
3582                matches!(with_low(1_000.0), readings::Reading::Withheld(w) if w.reason == readings::Reason::NoClimb)
3583            );
3584            let height = with_low(30.0).value().unwrap().height_m;
3585            assert!(
3586                (height - clean_height).abs() < 5.0,
3587                "{height} against {clean_height}"
3588            );
3589        }
3590    }
3591
3592    /// A burst of samples close together costs each of them no walk over the burst: a walk for each
3593    /// ran for minutes on a burst of 900,000 (ADR-308). The clock times this test alone, and nothing
3594    /// it reads reaches a result.
3595    #[test]
3596    #[allow(clippy::disallowed_types, reason = "the clock times this test alone")]
3597    fn a_burst_of_samples_is_not_walked_over_for_each_of_them() {
3598        let n: u32 = 900_000;
3599        let times: Vec<f64> = (0..n).map(|i| 100.0 + f64::from(i) * 1.0e-9).collect();
3600        let heights: Vec<f64> = (0..n).map(|i| f64::from(i % 7) * 0.1).collect();
3601        let began = std::time::Instant::now();
3602        let medians = smoothed_track(&heights, &times);
3603        assert_eq!(medians.len(), heights.len());
3604        let spread = scatter(&heights, &medians, &times);
3605        assert_eq!(spread.len(), heights.len());
3606        // Within the span until one sample past it: every start has to look to the end.
3607        let mut baro: Vec<Baro> = times
3608            .iter()
3609            .zip(&heights)
3610            .map(|(&time_s, &height_m)| Baro {
3611                time_s,
3612                height_m,
3613                smoothed_m: height_m,
3614                rate_m_s: 0.0,
3615                scatter_var_m2: 0.0,
3616                unweighed: false,
3617            })
3618            .collect();
3619        let last = Baro {
3620            time_s: 102.0,
3621            smoothed_m: 500.0,
3622            ..baro[0]
3623        };
3624        baro.push(last);
3625        baro.push(Baro {
3626            time_s: 110.0,
3627            smoothed_m: 500.0,
3628            ..baro[0]
3629        });
3630        assert_eq!(landing(&baro, 0.0), None);
3631        assert!(
3632            began.elapsed().as_secs_f64() < 60.0,
3633            "{:?}",
3634            began.elapsed()
3635        );
3636    }
3637
3638    /// The landing as a sliding window finds what the rule says, wherever the settling starts.
3639    #[test]
3640    fn the_sliding_landing_is_the_rule_written_out() {
3641        let baro_of = |medians: &[f64]| -> Vec<Baro> {
3642            medians
3643                .iter()
3644                .enumerate()
3645                .map(|(i, &m)| Baro {
3646                    time_s: i as f64 * 0.5,
3647                    height_m: m,
3648                    smoothed_m: m,
3649                    rate_m_s: 0.0,
3650                    scatter_var_m2: 0.0,
3651                    unweighed: false,
3652                })
3653                .collect()
3654        };
3655        let by_rule = |baro: &[Baro]| -> Option<f64> {
3656            let top = baro
3657                .iter()
3658                .enumerate()
3659                .filter(|(_, b)| b.time_s > -1.0)
3660                .max_by(|(_, a), (_, b)| a.smoothed_m.total_cmp(&b.smoothed_m))?
3661                .0;
3662            let end_s = baro.last()?.time_s;
3663            (top..baro.len()).find_map(|i| {
3664                let start = baro[i];
3665                if end_s < start.time_s + LANDED_S {
3666                    return None;
3667                }
3668                baro[i..]
3669                    .iter()
3670                    .take_while(|b| b.time_s <= start.time_s + LANDED_S)
3671                    .all(|b| (b.smoothed_m - start.smoothed_m).abs() <= LANDED_SPAN_M)
3672                    .then_some(start.time_s)
3673            })
3674        };
3675        let mut state = 11;
3676        for _ in 0..300 {
3677            let mut height = 200.0;
3678            let medians: Vec<f64> = (0..60)
3679                .map(|_| {
3680                    height += 12.0 * noise(&mut state) - 1.5;
3681                    height
3682                })
3683                .collect();
3684            let baro = baro_of(&medians);
3685            assert_eq!(landing(&baro, -1.0), by_rule(&baro), "{medians:?}");
3686        }
3687    }
3688
3689    /// A zero that is the pad's only by a word may be off by `PAD_ZERO_M`: the height's standard
3690    /// error holds that, and the speed's doesn't change.
3691    #[test]
3692    fn a_zero_by_a_word_reaches_the_heights_standard_error() {
3693        let own = from_launch(log());
3694        let mut by_word = own.clone();
3695        by_word.format = LogFormat::GenericTable;
3696        let t = 20.0;
3697        let own_state = smoothed(&own, Options::default()).at(t).unwrap();
3698        let word_state = smoothed(&by_word, Options::default()).at(t).unwrap();
3699        let widened = word_state.height_sd_m * word_state.height_sd_m
3700            - own_state.height_sd_m * own_state.height_sd_m;
3701        let expected = readings::PAD_ZERO_M * readings::PAD_ZERO_M / 3.0;
3702        assert!(
3703            (widened / expected - 1.0).abs() < 0.1,
3704            "{widened} against {expected}"
3705        );
3706        assert!((word_state.speed_sd_m_s / own_state.speed_sd_m_s - 1.0).abs() < 0.02);
3707    }
3708
3709    #[test]
3710    fn a_flights_own_scatter_stands_for_a_pads_noise() {
3711        // Past the descent's start the scatter holds the white noise, here 3 m, less the correlated
3712        // noise the smoother carries apart; at sea level, no air thins it.
3713        let baro: Vec<Baro> = (0..100)
3714            .map(|i| Baro {
3715                time_s: f64::from(i),
3716                height_m: 0.0,
3717                smoothed_m: 0.0,
3718                rate_m_s: 0.0,
3719                scatter_var_m2: if i < 50 { 400.0 } else { 9.0 },
3720                unweighed: false,
3721            })
3722            .collect();
3723        let var = flight_noise_var(&baro, 50.0, 0.0, 1.0);
3724        assert!((var - 8.0).abs() < 1e-9, "{var}");
3725        // No sample past it, as a log ending at its top: the whole flight's.
3726        let var = flight_noise_var(&baro, f64::INFINITY, 0.0, 1.0);
3727        assert!((var - 0.5 * (9.0 + 400.0) + 1.0).abs() < 1e-9, "{var}");
3728        assert_eq!(flight_noise_var(&[], 0.0, 0.0, 1.0), 0.0);
3729    }
3730
3731    #[test]
3732    fn a_log_past_the_step_limit_is_withheld_before_any_work() {
3733        let short = log();
3734        let read = readings::read(&short);
3735        let mut long = short;
3736        long.time_s = (0..=MAX_STEPS).map(|i| i as f64 * 0.001).collect();
3737        long.altitude_m = vec![0.0; MAX_STEPS + 1];
3738        let withheld = smooth_with(&long, &read, Options::default()).unwrap_err();
3739        assert_eq!(withheld.reason, Reason::TooLong);
3740        // A receiver's fixes count too: the track alone is well under.
3741        let mut with_fixes = log();
3742        let held = with_fixes.time_s.len();
3743        with_fixes.channels.push(Channel::new(
3744            "gnss",
3745            Quantity::GnssAltitude,
3746            (0..=MAX_STEPS - held).map(|i| i as f64 * 0.001).collect(),
3747            vec![0.0; MAX_STEPS - held + 1],
3748        ));
3749        let withheld = smooth_with(&with_fixes, &read, Options::default()).unwrap_err();
3750        assert_eq!(withheld.reason, Reason::TooLong);
3751    }
3752
3753    /// A log cut on the climb: the smoothed top is the track's, not the rocket's (#1089).
3754    #[test]
3755    fn a_log_cut_on_the_climb_makes_the_smoothed_apogee_a_floor() {
3756        let whole = log();
3757        assert!(!smoothed(&whole, Options::default()).apogee_is_floor);
3758        let mut cut = whole.clone();
3759        let keep = cut.time_s.iter().position(|t| *t > 12.0).unwrap();
3760        cut.time_s.truncate(keep);
3761        cut.altitude_m.truncate(keep);
3762        assert!(readings::read(&cut).apogee.value().unwrap().is_floor);
3763        assert!(smoothed(&cut, Options::default()).apogee_is_floor);
3764        // A dip in the climb withholds the reading's apogee; the smoother still reads, and the
3765        // log still ends on the climb.
3766        let n = cut.altitude_m.len();
3767        for height in &mut cut.altitude_m[n - 60..n - 40] {
3768            *height -= 150.0;
3769        }
3770        let read = readings::read(&cut);
3771        assert_eq!(read.apogee.reason(), Some(readings::Reason::DisturbedTop));
3772        let dipped = smooth_with(&cut, &read, Options::default()).unwrap();
3773        assert!(dipped.apogee_is_floor);
3774    }
3775
3776    #[test]
3777    fn a_smoothed_apogee_past_the_readings_bound_says_so_on_the_card() {
3778        let log = log();
3779        let mut read = readings::read(&log);
3780        let clean = smooth_with(&log, &read, Options::default()).unwrap();
3781        assert!(!clean.health.apogee_past_reading_bound);
3782        assert!(clean.health.apogee_less_reading_m.unwrap().abs() < 100.0);
3783
3784        let Reading::Read(apogee) = &mut read.apogee else {
3785            panic!("the invented log's apogee reads");
3786        };
3787        let bound = apogee.bound_m;
3788        // The reading low by twice its bound.
3789        apogee.height_m -= 2.0 * bound;
3790        let high = smooth_with(&log, &read, Options::default()).unwrap();
3791        assert!(high.health.apogee_past_reading_bound);
3792        assert!(high.health.apogee_less_reading_m.unwrap() > bound);
3793        assert!(
3794            high.health
3795                .card()
3796                .contains("past the reading's bound: go by the reading")
3797        );
3798        // A floor's truth may lie any amount above it: the same gap says nothing there.
3799        let Reading::Read(apogee) = &mut read.apogee else {
3800            panic!("the invented log's apogee reads");
3801        };
3802        apogee.is_floor = true;
3803        let above_floor = smooth_with(&log, &read, Options::default()).unwrap();
3804        assert!(!above_floor.health.apogee_past_reading_bound);
3805        // Below a floor by more than its bound it does.
3806        let Reading::Read(apogee) = &mut read.apogee else {
3807            panic!("the invented log's apogee reads");
3808        };
3809        apogee.height_m += 4.0 * bound;
3810        let below_floor = smooth_with(&log, &read, Options::default()).unwrap();
3811        assert!(below_floor.health.apogee_past_reading_bound);
3812    }
3813
3814    #[test]
3815    fn the_health_card_reads_each_sensor_and_what_was_done() {
3816        let health = Health {
3817            sensors: vec![SensorHealth {
3818                sensor: Sensor::Barometer,
3819                samples: 2000,
3820                used: 1997,
3821                rejected: 3,
3822                unweighed: 0,
3823                resets: 1,
3824                inside_gate_share: Some(0.9985),
3825                largest: Some(Largest {
3826                    ratio: 12.345,
3827                    time_s: 17.04,
3828                    mach: 0.004,
3829                }),
3830                mean_nis: Some(0.7012),
3831            }],
3832            deweighted: vec![Span {
3833                start_s: 9.5,
3834                end_s: 14.25,
3835                why: Deweighted::Transonic,
3836            }],
3837            rejected: Vec::new(),
3838            gnss_trusted_s: None,
3839            baro_minus_inertial_at_apogee_m: None,
3840            unsmoothed_steps: 0,
3841            assumed_pad: false,
3842            accelerometer_set_aside: false,
3843            apogee_less_reading_m: None,
3844            apogee_past_reading_bound: false,
3845        };
3846        assert_eq!(
3847            health.card(),
3848            "barometer: 2000 samples, 1997 fused, 3 rejected, 0 not weighed, 1 reset; 99.8% \
3849             inside the gate; mean NIS 0.701; largest test ratio 12.3 at 17.0 s, Mach 0.00\n\
3850             barometer de-weighted: transonic 9.50–14.2 s\n\
3851             pass back: every step smoothed\n"
3852        );
3853        let assumed = Health {
3854            sensors: Vec::new(),
3855            assumed_pad: true,
3856            accelerometer_set_aside: true,
3857            apogee_less_reading_m: Some(-412.0),
3858            apogee_past_reading_bound: true,
3859            ..health
3860        };
3861        assert_eq!(
3862            assumed.card(),
3863            "pad assumed: too little of it in the log; heights on the readings' zero, noise from \
3864             the flight's scatter\n\
3865             accelerometer set aside: too little of the pad to take its level from\n\
3866             smoothed apogee -412 m from the reading, past the reading's bound: go by the reading\n\
3867             barometer de-weighted: transonic 9.50–14.2 s\n\
3868             pass back: every step smoothed\n"
3869        );
3870    }
3871}