1mod 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
73const H: usize = 0;
76const V: usize = 1;
78const A: usize = 2;
80const BIAS: usize = 3;
82const SCALE: usize = 4;
84const TILT: usize = 5;
87const SLOW: usize = 6;
89const DRIFT: usize = 7;
91const DRIFT_RATE: usize = 8;
93const AIR: usize = 9;
97const GNSS: usize = 10;
99const PORT_FALL: usize = 11;
102
103pub const INTERVAL_Z: f64 = 1.644_853_626_951_472_2;
106
107pub const GATE_SIGMAS: f64 = 5.0;
110
111pub const GATE_RESET_S: f64 = 1.0;
114
115pub const FAST_NOISE_M: f64 = 0.5;
118
119pub const FAST_NOISE_TAU_S: f64 = 0.028_853_900_817_779_27;
121
122pub const SLOW_NOISE_M: f64 = 0.29;
125
126pub const SLOW_NOISE_TAU_S: f64 = 1.0;
128
129pub const PORT_PRIOR_MEAN: f64 = -0.01;
133
134pub const PORT_PRIOR_SD: f64 = 0.02;
141
142pub const TRANSONIC_FROM_MACH: f64 = 0.8;
147
148pub const TRANSONIC_FULL_MACH: f64 = 0.9;
150
151pub const TRANSONIC_PORT_SHIFT: f64 = 0.2;
154
155pub const DEWEIGHT_TAU_S: f64 = 1.0;
158
159fn 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
167pub const CHARGE_WINDOW_S: f64 = 0.5;
170
171pub const APOGEE_CHARGE_SPAN_M: f64 = 5.0;
177
178pub const CHARGE_SD_M: f64 = 32.0;
180
181pub const CHARGE_WORDS: [&str; 7] = [
183 "drogue", "main", "charge", "deploy", "pyro", "eject", "apogee",
184];
185
186pub 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
203pub const WIND_LIMIT_M_S: f64 = 8.9408;
206
207pub const TILT_FLOOR_M_S2: f64 = 0.5;
210
211pub const TILT_TAU_S: f64 = 2.0;
213
214pub const JERK_PAD: f64 = 1.0;
216
217pub const JERK_POWERED: f64 = 1.0e5;
220
221pub const JERK_COAST: f64 = 10.0;
223
224pub const JERK_DESCENT: f64 = 100.0;
226
227pub const POWERED_HOLD_S: f64 = 1.0;
229
230pub const APOGEE_SPEED_M_S: f64 = 10.0;
232
233pub const PAD_SD: f64 = 0.01;
235
236pub const GNSS_AGREE_WINDOW_S: f64 = 2.0;
239
240pub const GNSS_AGREE_M_S: f64 = 3.0;
242
243pub const GNSS_GAP_S: f64 = 2.0;
245
246pub const GNSS_SETTLE_S: f64 = 5.0;
248
249pub const GNSS_SETTLE_FACTOR: f64 = 4.0;
251
252pub const GNSS_SD_DEFAULT_M: f64 = 10.0;
255
256pub const GNSS_SD_FLOOR_M: f64 = 2.0;
258
259pub const GNSS_OFFSET_SD_M: f64 = 1000.0;
261
262pub const GNSS_OFFSET_WALK: f64 = 4.0;
264
265pub const UNKNOWN_PAD_MSL_M: f64 = 3000.0;
268
269pub const PAD_MIN_SAMPLES: usize = 5;
272
273pub const ASSUMED_HEIGHT_SD_M: f64 = 100.0;
276
277pub const ASSUMED_SPEED_SD_M_S: f64 = 100.0;
282
283pub const ASSUMED_ACCELERATION_SD_M_S2: f64 = 300.0;
286
287pub const ASSUMED_SETTLE_S: f64 = 1.0;
293
294pub const ASSUMED_SETTLE_SAMPLES: usize = 10;
296
297pub const MAX_STEPS: usize = 2_000_000;
302
303pub const DIVERGED_SPEED_M_S: f64 = 11_186.0;
324
325#[derive(Debug, Clone, Copy, PartialEq, Eq, Default, Serialize, Deserialize)]
327#[serde(rename_all = "snake_case")]
328#[non_exhaustive]
329pub enum GnssTrust {
330 #[default]
332 Agreement,
333 FromLiftoff,
336}
337
338#[derive(Debug, Clone, Copy, PartialEq, Eq, Default, Serialize, Deserialize)]
340pub struct Options {
341 pub gnss_trust: GnssTrust,
343}
344
345#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize, JsonSchema)]
347#[serde(rename_all = "snake_case")]
348#[non_exhaustive]
349pub enum Reason {
350 NoBarometer,
352 NoLiftoff,
354 TooLong,
356 Diverged,
360}
361
362#[derive(Debug, Clone, PartialEq, Serialize, Deserialize, JsonSchema)]
364pub struct Withheld {
365 pub reason: Reason,
367 pub detail: String,
369}
370
371#[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 Barometer,
380 Accelerometer,
382 Gnss,
384}
385
386#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize, JsonSchema)]
388#[serde(rename_all = "snake_case")]
389pub enum LiftoffFrom {
390 Accelerometer,
392 Barometer,
394}
395
396#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize, JsonSchema)]
398pub struct Liftoff {
399 pub time_s: f64,
401 pub bound_s: f64,
404 pub from: LiftoffFrom,
406}
407
408#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize, JsonSchema)]
410#[serde(rename_all = "snake_case")]
411pub enum Deweighted {
412 Transonic,
414 Charge,
416}
417
418#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize, JsonSchema)]
420pub struct Span {
421 pub start_s: f64,
423 pub end_s: f64,
425 pub why: Deweighted,
427}
428
429#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize, JsonSchema)]
431pub struct Rejected {
432 pub sensor: Sensor,
434 pub time_s: f64,
436 pub ratio: f64,
439}
440
441#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize, JsonSchema)]
443pub struct Largest {
444 pub ratio: f64,
446 pub time_s: f64,
448 pub mach: f64,
451}
452
453#[derive(Debug, Clone, PartialEq, Serialize, Deserialize, JsonSchema)]
455pub struct SensorHealth {
456 pub sensor: Sensor,
458 pub samples: usize,
460 pub used: usize,
462 pub rejected: usize,
464 pub unweighed: usize,
467 pub resets: usize,
469 pub inside_gate_share: Option<f64>,
471 pub largest: Option<Largest>,
473 pub mean_nis: Option<f64>,
477}
478
479#[derive(Debug, Clone, PartialEq, Serialize, Deserialize, JsonSchema)]
482pub struct Health {
483 pub sensors: Vec<SensorHealth>,
485 pub deweighted: Vec<Span>,
487 pub rejected: Vec<Rejected>,
489 pub gnss_trusted_s: Option<f64>,
492 pub baro_minus_inertial_at_apogee_m: Option<f64>,
495 pub unsmoothed_steps: usize,
497 pub assumed_pad: bool,
502 pub accelerometer_set_aside: bool,
505 pub apogee_less_reading_m: Option<f64>,
507 pub apogee_past_reading_bound: bool,
512}
513
514impl Sensor {
515 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 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 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
622fn 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#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)]
651pub struct Smoothed {
652 pub time_s: Vec<f64>,
654 pub height_m: Vec<f64>,
656 pub height_sd_m: Vec<f64>,
658 pub speed_m_s: Vec<f64>,
660 pub speed_sd_m_s: Vec<f64>,
662 pub liftoff: Liftoff,
664 pub apogee_time_s: f64,
667 pub apogee_is_floor: bool,
671 pub health: Health,
673}
674
675#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
677pub struct State {
678 pub height_m: f64,
680 pub height_sd_m: f64,
682 pub speed_m_s: f64,
684 pub speed_sd_m_s: f64,
686}
687
688impl Smoothed {
689 pub fn at(&self, time_s: f64) -> Option<State> {
692 let n = self.time_s.len();
693 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
731pub fn smooth(log: &FlightLog) -> Result<Smoothed, Withheld> {
733 smooth_with(log, &readings::read(log), Options::default())
734}
735
736pub fn smooth_with(
744 log: &FlightLog,
745 readings: &Readings,
746 options: Options,
747) -> Result<Smoothed, Withheld> {
748 let inputs = Inputs::gather(log, readings)?;
749 let (mut smoothed, choices) = run(&inputs, options, PORT_PRIOR_MEAN, None, true);
751 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#[derive(Debug, Clone, Copy)]
817struct Baro {
818 time_s: f64,
819 height_m: f64,
821 smoothed_m: f64,
823 rate_m_s: f64,
826 scatter_var_m2: f64,
828 unweighed: bool,
830}
831
832#[derive(Debug, Clone, Copy)]
834struct Accel {
835 time_s: f64,
836 value_m_s2: f64,
838 smoothed_m_s2: f64,
841 clipped: bool,
842}
843
844#[derive(Debug, Clone)]
846struct Inputs {
847 baro: Vec<Baro>,
848 baro_white_var: f64,
850 pad_msl_m: Option<f64>,
853 assumed_pad: bool,
855 accelerometer_set_aside: bool,
857 zero_sd_m: f64,
860 accel: Vec<Accel>,
861 accel_sd: f64,
863 accel_pad: f64,
865 gnss: Vec<(f64, f64)>,
866 gnss_sd: f64,
867 liftoff: Liftoff,
868 pad_end_s: f64,
870 charges: Vec<(f64, f64)>,
872 landing_s: Option<f64>,
874 descent_from_s: f64,
877}
878
879fn 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 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
929fn 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
940fn 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
948fn 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 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 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, ×)) {
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 let descent_from_s = apogee_span.map_or(f64::INFINITY, |(_, end)| end);
1095
1096 let step = step_of(&track.height_m);
1097 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 let pad_end_s = if assumed_pad {
1110 f64::NEG_INFINITY
1111 } else {
1112 found_pad_end_s
1113 };
1114 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 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
1183fn 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, ×);
1188 let rates = slopes(&medians, ×, 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, ×))
1194 {
1195 b.smoothed_m = m;
1196 b.rate_m_s = rate;
1197 b.scatter_var_m2 = var;
1198 }
1199}
1200
1201fn 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
1227fn 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
1253fn 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
1264fn 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
1285pub const REGRESSOR_HALF_S: f64 = 0.05;
1292
1293pub const REGRESSOR_MAX_HALF: usize = 250;
1297
1298pub const LANDED_SPAN_M: f64 = 5.0;
1300
1301pub const LANDED_S: f64 = 3.0;
1303
1304pub const LANDING_LEAD_S: f64 = 0.1;
1307
1308pub const RATE_HALF_S: f64 = 0.25;
1313
1314pub const SWING_TAU_S: f64 = 2.0;
1322
1323pub const SCATTER_HALF_S: f64 = 3.0;
1330
1331pub const SCATTER_MIN_SAMPLES: usize = 5;
1333
1334pub const SCATTER_MAX_SAMPLES: usize = 201;
1337
1338fn 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 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 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
1387fn 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 0.5 * (lower.iter().copied().fold(f64::NEG_INFINITY, f64::max) + upper)
1397 }
1398}
1399
1400fn quadratic(values: &[f64], times: &[f64], center: f64) -> Option<[f64; 3]> {
1404 if values.len() < 4 {
1405 return None;
1406 }
1407 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
1447fn 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 ¢er 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
1483fn 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
1499fn 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
1520fn 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
1560fn 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 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
1575fn 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
1582fn sound(temperature_k: f64) -> f64 {
1584 (1.4 * 287.052_87 * temperature_k).sqrt()
1585}
1586
1587fn 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#[derive(Debug, Clone, Copy, Default)]
1599struct Transonic {
1600 open: bool,
1601 clear_since_s: Option<f64>,
1603}
1604
1605impl Transonic {
1606 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 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
1635pub const RATE_DROP_SHARE: f64 = 0.1;
1639
1640pub const TRANSONIC_CLEAR_S: f64 = 1.0;
1643
1644fn 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#[derive(Debug, Clone, Copy, PartialEq)]
1653enum Gated {
1654 Used,
1655 Rejected,
1656}
1657
1658#[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
1697struct Filter {
1699 x: Vector,
1700 p: Matrix,
1701 jump: Matrix,
1705 held: [bool; N],
1709 rejected: Vec<Rejected>,
1710 pad_msl_for_sound_m: f64,
1711 gates: Vec<Gate>,
1713 replay: Option<(Vec<Gate>, usize)>,
1715}
1716
1717#[derive(Debug, Clone, Copy, PartialEq)]
1719enum Gate {
1720 Used,
1721 Rejected,
1722 TakenBack,
1724}
1725
1726#[derive(Debug, Clone, Default, PartialEq)]
1732struct Choices {
1733 steps: Vec<(Phase, bool)>,
1735 transonic: Vec<(f64, f64)>,
1737 gnss: Vec<(bool, bool, bool)>,
1739 gates: Vec<Gate>,
1741}
1742
1743#[derive(Debug, Clone, Copy)]
1745struct Variance {
1746 fused: f64,
1749 gate: f64,
1752}
1753
1754impl Variance {
1755 fn white(variance: f64) -> Self {
1757 Self {
1758 fused: variance,
1759 gate: variance,
1760 }
1761 }
1762}
1763
1764impl Filter {
1765 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 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 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 tally.nis_sum += nis;
1835 Gated::Used
1836 }
1837}
1838
1839impl Filter {
1840 fn widen(&mut self, state: usize, variance: f64) {
1842 self.p[state][state] += variance;
1843 self.jump[state][state] += variance;
1844 }
1845}
1846
1847fn 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
1859fn 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#[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord)]
1880enum Measured {
1881 Accel(usize),
1882 Baro(usize),
1883 Gnss(usize),
1884}
1885
1886fn 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#[derive(Debug, Clone, Copy, PartialEq)]
1923enum Phase {
1924 Pad,
1925 Powered,
1926 Coast,
1927 Descent,
1928}
1929
1930#[derive(Debug, Clone)]
1932struct Trust {
1933 mode: GnssTrust,
1934 trusted: bool,
1935 since_s: Option<f64>,
1937 first_s: Option<f64>,
1939 agree_from_s: Option<f64>,
1941 recent: Vec<(f64, f64)>,
1943 misses: usize,
1944}
1945
1946impl Trust {
1947 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
2008fn 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 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 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 p[AIR][AIR] = air_sd * air_sd;
2063 if inputs.assumed_pad {
2064 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 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 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 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 _ 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 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 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 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 let held = correlated_factor(dt_baro, DEWEIGHT_TAU_S);
2294 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 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 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 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 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 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 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 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 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 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
2576fn 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
2592fn 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 const BOOST_M_S2: f64 = 5.0 * G0;
2638 const DESCENT_M_S: f64 = 15.0;
2639
2640 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 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 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 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 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 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 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 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, ×), ×);
2832 let mut sorted = var.clone();
2833 let middle = median(&mut sorted);
2834 assert!((0.7..1.05).contains(&(middle / 0.25)), "{middle}");
2835 let clean: Vec<f64> = times.iter().map(|&t| 80.0 * t - 0.5 * G0 * t * t).collect();
2840 let medians = super::smoothed(&clean, ×);
2841 let var = scatter(&clean, &medians, ×);
2842 let end = times.last().copied().expect("samples") - SCATTER_HALF_S;
2843 let largest = var
2844 .iter()
2845 .zip(×)
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 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 for t in [30.0, 40.0, 50.0] {
2875 holds_at(&smoothed, t, 30.0, 6.0);
2876 }
2877 }
2878
2879 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 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 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 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 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 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 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 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 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 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 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 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 let landing = landing(&baro, LIFTOFF_S).expect("it lands");
3118 assert!((56.65..=56.75).contains(&landing), "{landing}");
3119 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 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, ×, RATE_HALF_S);
3134 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 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 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 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 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 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 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 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 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 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 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 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 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 #[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 #[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 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 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 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 #[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 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 #[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 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 #[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 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 #[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 #[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, ×);
3603 assert_eq!(medians.len(), heights.len());
3604 let spread = scatter(&heights, &medians, ×);
3605 assert_eq!(spread.len(), heights.len());
3606 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 #[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 #[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 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 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 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 #[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 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 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 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 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}