1use std::cell::{Cell, RefCell};
59
60use hpr_flightdata::log::{FlightLog, Quantity};
61use hpr_flightdata::readings::interval::Z_90;
62use hpr_flightdata::readings::{self, Reading, Readings};
63use serde::{Deserialize, Serialize};
64
65use crate::posterior::{self, Posterior};
66use crate::residuals::{HeightKind, Simulated};
67
68pub const BIN_S: f64 = 0.25;
70pub const FLOOR_M: f64 = readings::NOISE_M;
73pub const FLOOR_HORIZONTAL_M: f64 = 10.0;
76const MAX_ITERATIONS: usize = 60;
78const CONVERGED_CHI2: f64 = 1e-3;
80const OFFSET_SEARCH_S: f64 = 3.0;
82const OFFSET_STEP_S: f64 = 0.05;
84const EARTH_RADIUS_M: f64 = 6_371_008.8;
86
87pub const FLOWN: usize = 6;
89const UNKNOWNS: usize = FLOWN + 5;
92const WIND_EAST: usize = 3;
94const WIND_NORTH: usize = 4;
95
96#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
98pub struct Parameters {
99 pub drag_scale: f64,
101 pub impulse_scale: f64,
103 pub burn_time_scale: f64,
106 pub wind_east_m_s: f64,
108 pub wind_north_m_s: f64,
110 pub mass_scale: f64,
112}
113
114impl Parameters {
115 #[must_use]
117 pub fn to_array(self) -> [f64; FLOWN] {
118 [
119 self.drag_scale,
120 self.impulse_scale,
121 self.burn_time_scale,
122 self.wind_east_m_s,
123 self.wind_north_m_s,
124 self.mass_scale,
125 ]
126 }
127
128 #[must_use]
130 pub fn from_array(x: [f64; FLOWN]) -> Self {
131 Self {
132 drag_scale: x[0],
133 impulse_scale: x[1],
134 burn_time_scale: x[2],
135 wind_east_m_s: x[3],
136 wind_north_m_s: x[4],
137 mass_scale: x[5],
138 }
139 }
140}
141
142#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
144pub struct Prior {
145 pub mean: f64,
147 pub sd: f64,
149}
150
151pub type Stated = [Prior; FLOWN];
153
154#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
156pub enum Noise {
157 FromResiduals,
159 Given {
162 height_m: f64,
164 horizontal_m: f64,
166 },
167 Known {
170 height_m: f64,
172 horizontal_m: f64,
174 },
175}
176
177#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)]
179pub struct Observations {
180 pub kind: HeightKind,
182 pub heights: Vec<(f64, f64)>,
184 pub horizontal: Vec<(f64, f64, f64)>,
186 pub liftoff_s: f64,
188 pub flown_s: f64,
190 pub reads_transonic: bool,
193}
194
195fn median(values: &mut Vec<f64>) -> Option<f64> {
197 values.retain(|v| v.is_finite());
198 values.sort_by(f64::total_cmp);
199 let n = values.len();
200 match n {
201 0 => None,
202 _ if n % 2 == 1 => Some(values[n / 2]),
203 _ => Some(0.5 * (values[n / 2 - 1] + values[n / 2])),
204 }
205}
206
207const MAX_WINDOW_S: f64 = 1_000.0;
210
211pub const MISFIT_SHARE: f64 = 0.1;
219
220fn binned(samples: impl Iterator<Item = (f64, f64)>, from_s: f64, to_s: f64) -> Vec<(f64, f64)> {
222 #[expect(
223 clippy::cast_possible_truncation,
224 clippy::cast_sign_loss,
225 reason = "a window of a flight's seconds in bins of a quarter second, checked to be positive"
226 )]
227 let bins = ((to_s - from_s) / BIN_S).ceil().max(0.0) as usize;
228 let mut held: Vec<Vec<f64>> = vec![Vec::new(); bins];
229 for (t, h) in samples {
230 if t.is_finite() && t >= from_s && t < to_s {
231 #[expect(
232 clippy::cast_possible_truncation,
233 clippy::cast_sign_loss,
234 reason = "inside the window, so positive and below the bins' count"
235 )]
236 let bin = ((t - from_s) / BIN_S) as usize;
237 if let Some(values) = held.get_mut(bin) {
238 values.push(h);
239 }
240 }
241 }
242 held.iter_mut()
243 .enumerate()
244 .filter_map(|(bin, values)| {
245 #[expect(
246 clippy::cast_precision_loss,
247 reason = "a bin's number is far below 2^53"
248 )]
249 let t = from_s + (bin as f64 + 0.5) * BIN_S;
250 median(values).map(|h| (t, h))
251 })
252 .collect()
253}
254
255#[must_use]
259pub fn observations(log: &FlightLog, read: &Readings) -> Option<Observations> {
260 let Reading::Read(apogee) = &read.apogee else {
261 return None;
262 };
263 observations_until(
264 log,
265 read,
266 apogee.time_s,
267 HeightKind::of(apogee.source),
268 false,
269 )
270}
271
272pub(crate) fn observations_until(
280 log: &FlightLog,
281 read: &Readings,
282 to_s: f64,
283 kind: HeightKind,
284 whole_bins: bool,
285) -> Option<Observations> {
286 let pad_m = read.pad_altitude_m?;
287 let (times, heights) = readings::height_track(log)?;
288 let apogee_s = to_s;
289 let liftoff_s = match &read.liftoff {
290 Reading::Read(liftoff) => liftoff.time_s,
291 Reading::Withheld(_) => times.iter().copied().find(|t| t.is_finite())?,
293 };
294 if apogee_s <= liftoff_s || apogee_s - liftoff_s > MAX_WINDOW_S {
295 return None;
296 }
297 let from_s = liftoff_s - 1.0;
298 let apogee_s = if whole_bins {
299 from_s + ((apogee_s - from_s) / BIN_S).floor() * BIN_S
300 } else {
301 apogee_s
302 };
303 if apogee_s <= liftoff_s {
304 return None;
305 }
306 let track = times.iter().copied().zip(heights.iter().map(|h| h - pad_m));
307 let bins = binned(track, from_s, apogee_s);
308 if bins.len() <= UNKNOWNS {
309 return None;
310 }
311 Some(Observations {
312 kind,
313 reads_transonic: kind == HeightKind::Geometric,
314 heights: bins,
315 horizontal: horizontal(log, from_s, apogee_s, liftoff_s),
316 liftoff_s,
317 flown_s: apogee_s - liftoff_s,
318 })
319}
320
321fn horizontal(log: &FlightLog, from_s: f64, to_s: f64, liftoff_s: f64) -> Vec<(f64, f64, f64)> {
324 let rockets = |quantity| {
326 log.channels_of(quantity)
327 .find(|channel| !channel.column.starts_with("gs_"))
328 };
329 let (Some(latitude), Some(longitude)) =
330 (rockets(Quantity::Latitude), rockets(Quantity::Longitude))
331 else {
332 return Vec::new();
333 };
334 if !latitude
336 .time_s
337 .iter()
338 .map(|t| t.to_bits())
339 .eq(longitude.time_s.iter().map(|t| t.to_bits()))
340 {
341 return Vec::new();
342 }
343 let before = |values: &[f64]| {
344 let mut pad: Vec<f64> = latitude
345 .time_s
346 .iter()
347 .zip(values)
348 .filter(|(t, _)| **t < liftoff_s)
349 .map(|(_, v)| *v)
350 .collect();
351 median(&mut pad).or_else(|| values.iter().copied().find(|v| v.is_finite()))
352 };
353 let (Some(pad_latitude), Some(pad_longitude)) =
354 (before(&latitude.values), before(&longitude.values))
355 else {
356 return Vec::new();
357 };
358 let cos_latitude = pad_latitude.to_radians().cos();
359 let times = || latitude.time_s.iter().copied();
360 let north = binned(
361 times().zip(
362 latitude
363 .values
364 .iter()
365 .map(|v| (v - pad_latitude).to_radians() * EARTH_RADIUS_M),
366 ),
367 from_s,
368 to_s,
369 );
370 let east = binned(
371 times().zip(
372 longitude
373 .values
374 .iter()
375 .map(|v| (v - pad_longitude).to_radians() * EARTH_RADIUS_M * cos_latitude),
376 ),
377 from_s,
378 to_s,
379 );
380 east.iter()
381 .filter_map(|&(t, e)| {
382 north
384 .binary_search_by(|(u, _)| u.total_cmp(&t))
385 .ok()
386 .map(|at| (t, e, north[at].1))
387 })
388 .collect()
389}
390
391pub trait Forward {
393 fn fly(&mut self, parameters: &Parameters, until_s: f64) -> Result<Simulated, String>;
399
400 fn fly_sheared(
407 &mut self,
408 parameters: &Parameters,
409 until_s: f64,
410 exponent: f64,
411 ) -> Result<Simulated, String>;
412}
413
414pub const SHEAR_EXPONENT: f64 = 0.2;
418
419#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)]
421pub struct Fit {
422 pub parameters: Parameters,
424 pub standard_errors: [f64; FLOWN],
426 pub clock_offset_s: f64,
428 pub height_offset_m: f64,
430 pub rms_m: Option<f64>,
432 pub height_noise_m: f64,
434 pub horizontal_noise_m: f64,
436 pub fitted: bool,
438 pub converged: bool,
441 pub misfit_share: Option<f64>,
444 pub read: Vec<bool>,
448 pub flown: u64,
452}
453
454impl Fit {
455 #[must_use]
457 pub fn of_nothing(stated: &Stated) -> Self {
458 Self {
459 parameters: Parameters::from_array(stated.map(|p| p.mean)),
460 standard_errors: stated.map(|p| p.sd),
461 clock_offset_s: 0.0,
462 height_offset_m: 0.0,
463 rms_m: None,
464 height_noise_m: FLOOR_M,
465 horizontal_noise_m: FLOOR_HORIZONTAL_M,
466 fitted: false,
467 converged: false,
468 misfit_share: None,
469 read: Vec::new(),
470 flown: 0,
471 }
472 }
473}
474
475const STEPS: [f64; FLOWN] = [0.02, 0.01, 0.02, 1.0, 1.0, 0.01];
477const HEIGHT_OFFSET_SD_M: f64 = 5.0;
479const CLOCK_OFFSET_SD_S: f64 = 2.0;
481const GAIN_SD: f64 = 0.0025 / 1.732_050_807_568_877_2;
484const DRIFT_SD_M_S: f64 = 0.05;
486const PORT_MEAN: f64 = 0.5 * readings::PORT_PRESSURE_COEFFICIENT;
490const PORT_SD: f64 = readings::PORT_PRESSURE_COEFFICIENT / 3.464_101_615_137_754_6;
492const KNOWN_SD: f64 = 1e-4;
495
496#[derive(Clone)]
499struct Problem<'a> {
500 observed: &'a Observations,
501 stated: &'a Stated,
502 clock_mean_s: f64,
503 clock_sd_s: f64,
504 height_offset_sd_m: f64,
505 gain_sd: f64,
506 drift_sd_m_s: f64,
507 port_mean: f64,
508 port_sd: f64,
509 height_sd_m: f64,
510 horizontal_sd_m: f64,
511 receiver_rad_s: Option<f64>,
515 track_unknowns: bool,
519 read: Vec<bool>,
522}
523
524impl Problem<'_> {
525 fn height_m(&self, x: &[f64; UNKNOWNS], flown: &Simulated, t: f64) -> f64 {
527 let (tau, h0, gain, drift, port) = (
528 x[FLOWN],
529 x[FLOWN + 1],
530 x[FLOWN + 2],
531 x[FLOWN + 3],
532 x[FLOWN + 4],
533 );
534 let at = t - tau;
535 (1.0 + gain) * flown.read_height_m(at, self.observed.kind)
536 + h0
537 + drift * (t - self.observed.liftoff_s)
538 + port * flown.port_m(at)
539 }
540
541 fn residuals(&self, x: &[f64; UNKNOWNS], flown: &Simulated) -> Vec<f64> {
542 let (tau, h0) = (x[FLOWN], x[FLOWN + 1]);
543 let mut r = Vec::with_capacity(
544 self.observed.heights.len() + 2 * self.observed.horizontal.len() + UNKNOWNS,
545 );
546 for (&(t, y), read) in self.observed.heights.iter().zip(&self.read) {
547 r.push(if *read {
548 (y - self.height_m(x, flown, t)) / self.height_sd_m
549 } else {
550 0.0
551 });
552 }
553 for (east, north) in self.track_misfit_m(tau, flown) {
554 r.push(east / self.horizontal_sd_m);
555 r.push(north / self.horizontal_sd_m);
556 }
557 for (value, prior) in x.iter().zip(self.stated) {
558 r.push((value - prior.mean) / prior.sd);
559 }
560 r.push((tau - self.clock_mean_s) / self.clock_sd_s);
561 r.push(h0 / self.height_offset_sd_m);
562 r.push(x[FLOWN + 2] / self.gain_sd);
563 r.push(x[FLOWN + 3] / self.drift_sd_m_s);
564 r.push((x[FLOWN + 4] - self.port_mean) / self.port_sd);
565 r
566 }
567
568 fn sensor_columns(&self, x: &[f64; UNKNOWNS], flown: &Simulated) -> [Vec<f64>; 5] {
570 let base = self.residuals(x, flown);
571 [0.02, 1.0, 1e-3, 0.01, 1e-3]
572 .into_iter()
573 .enumerate()
574 .map(|(k, delta)| {
575 let mut ahead = *x;
576 ahead[FLOWN + k] += delta;
577 let mut column = self.residuals(&ahead, flown);
578 for (c, b) in column.iter_mut().zip(&base) {
579 *c = (*c - b) / delta;
580 }
581 column
582 })
583 .collect::<Vec<_>>()
584 .try_into()
585 .unwrap_or_else(|_| unreachable!("five deltas make five columns"))
586 }
587}
588
589fn chi2(r: &[f64]) -> f64 {
590 r.iter().map(|v| v * v).sum()
591}
592
593pub(crate) fn cholesky<const N: usize>(a: &[[f64; N]; N]) -> Option<[[f64; N]; N]> {
595 let mut l = [[0.0; N]; N];
596 for i in 0..N {
597 for j in 0..=i {
598 let sum: f64 = (0..j).map(|k| l[i][k] * l[j][k]).sum();
599 if i == j {
600 let d = a[i][i] - sum;
601 if d <= 0.0 || !d.is_finite() {
602 return None;
603 }
604 l[i][i] = d.sqrt();
605 } else {
606 l[i][j] = (a[i][j] - sum) / l[j][j];
607 }
608 }
609 }
610 Some(l)
611}
612
613fn solve<const N: usize>(l: &[[f64; N]; N], b: &[f64; N]) -> [f64; N] {
615 let mut x = *b;
616 for i in 0..N {
617 let sum: f64 = (0..i).map(|k| l[i][k] * x[k]).sum();
618 x[i] = (x[i] - sum) / l[i][i];
619 }
620 for i in (0..N).rev() {
621 let sum: f64 = (i + 1..N).map(|k| l[k][i] * x[k]).sum();
622 x[i] = (x[i] - sum) / l[i][i];
623 }
624 x
625}
626
627fn best_offset_s(observed: &Observations, flown: &Simulated, first_s: f64) -> f64 {
629 let steps = (OFFSET_SEARCH_S / OFFSET_STEP_S).round();
630 let mut best = (f64::INFINITY, first_s);
631 let mut k = -steps;
632 while k <= steps {
633 let tau = first_s + k * OFFSET_STEP_S;
634 let cost: f64 = observed
635 .heights
636 .iter()
637 .map(|&(t, y)| (y - flown.read_height_m(t - tau, observed.kind)).powi(2))
638 .sum();
639 if cost < best.0 {
640 best = (cost, tau);
641 }
642 k += 1.0;
643 }
644 best.1
645}
646
647pub fn fit(
650 observed: Option<&Observations>,
651 stated: &Stated,
652 noise: Noise,
653 forward: &mut dyn Forward,
654) -> Fit {
655 fit_and_sample(observed, stated, noise, forward, None).0
656}
657
658#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)]
660pub struct Sampling {
661 pub pilot: usize,
664 pub draws: usize,
666 pub seed: u64,
668}
669
670#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)]
673pub struct Bayesian {
674 pub least_squares: Fit,
676 pub posterior: Option<Posterior<FLOWN>>,
680}
681
682pub fn bayesian(
704 observed: Option<&Observations>,
705 stated: &Stated,
706 noise: Noise,
707 forward: &mut dyn Forward,
708 sampling: Sampling,
709) -> Bayesian {
710 let (least_squares, posterior) =
711 fit_and_sample(observed, stated, noise, forward, Some(sampling));
712 Bayesian {
713 least_squares,
714 posterior,
715 }
716}
717
718fn fit_and_sample(
720 observed: Option<&Observations>,
721 stated: &Stated,
722 noise: Noise,
723 forward: &mut dyn Forward,
724 sampling: Option<Sampling>,
725) -> (Fit, Option<Posterior<FLOWN>>) {
726 let Some(observed) = observed else {
727 return (Fit::of_nothing(stated), None);
728 };
729 let until_s = 1.6 * observed.flown_s + 5.0;
731 let flown = Cell::new(0_u64);
732 let mut fly = |x: &[f64; UNKNOWNS]| {
733 flown.set(flown.get() + 1);
734 let mut parameters = [0.0; FLOWN];
735 parameters.copy_from_slice(&x[..FLOWN]);
736 forward
737 .fly(&Parameters::from_array(parameters), until_s)
738 .ok()
739 };
740 let mut x = [0.0; UNKNOWNS];
741 for (value, prior) in x.iter_mut().zip(stated) {
742 *value = prior.mean;
743 }
744 let nothing = || Fit {
745 flown: flown.get(),
746 ..Fit::of_nothing(stated)
747 };
748 let Some(mut at_x) = fly(&x) else {
749 return (nothing(), None);
750 };
751 let open_sensors = |height_sd_m: f64, horizontal_sd_m: f64| Problem {
752 observed,
753 stated,
754 clock_mean_s: best_offset_s(observed, &at_x, observed.liftoff_s),
757 clock_sd_s: CLOCK_OFFSET_SD_S,
758 height_offset_sd_m: HEIGHT_OFFSET_SD_M,
759 gain_sd: if observed.kind == HeightKind::Barometric {
760 GAIN_SD
761 } else {
762 KNOWN_SD
763 },
764 drift_sd_m_s: DRIFT_SD_M_S,
765 port_mean: if observed.kind == HeightKind::Barometric
766 && matches!(noise, Noise::FromResiduals)
767 {
768 PORT_MEAN
769 } else {
770 0.0
771 },
772 port_sd: if observed.kind == HeightKind::Barometric {
773 PORT_SD
774 } else {
775 KNOWN_SD
776 },
777 height_sd_m,
778 horizontal_sd_m,
779 receiver_rad_s: None,
780 track_unknowns: matches!(noise, Noise::FromResiduals),
781 read: Vec::new(),
782 };
783 let mut problem = match noise {
784 Noise::FromResiduals => open_sensors(2.0 * FLOOR_M, 2.0 * FLOOR_HORIZONTAL_M),
785 Noise::Given {
786 height_m,
787 horizontal_m,
788 } => open_sensors(height_m, horizontal_m),
789 Noise::Known {
790 height_m,
791 horizontal_m,
792 } => Problem {
793 observed,
794 stated,
795 clock_mean_s: 0.0,
796 clock_sd_s: KNOWN_SD,
797 height_offset_sd_m: KNOWN_SD,
798 gain_sd: KNOWN_SD,
799 drift_sd_m_s: KNOWN_SD,
800 port_mean: 0.0,
801 port_sd: KNOWN_SD,
802 height_sd_m: height_m,
803 horizontal_sd_m: horizontal_m,
804 receiver_rad_s: None,
805 track_unknowns: false,
806 read: Vec::new(),
807 },
808 };
809 x[FLOWN] = problem.clock_mean_s;
810 x[FLOWN + 4] = problem.port_mean;
811 let mut information = [[0.0; UNKNOWNS]; UNKNOWNS];
812 let mut converged = false;
813 let passes = if matches!(noise, Noise::Known { .. }) {
817 1
818 } else {
819 2
820 };
821 for pass in 0..passes {
822 problem.read = observed
823 .heights
824 .iter()
825 .map(|&(t, _)| {
826 observed.reads_transonic || at_x.mach(t - x[FLOWN]) < readings::TRANSONIC_MACH
827 })
828 .collect();
829 if problem.read.iter().filter(|read| **read).count() <= UNKNOWNS {
830 return (nothing(), None);
831 }
832 converged = false;
833 let mut lambda = 1e-3;
834 let mut r = problem.residuals(&x, &at_x);
835 let mut cost = chi2(&r);
836 for _ in 0..MAX_ITERATIONS {
837 let mut columns: Vec<Vec<f64>> = Vec::with_capacity(UNKNOWNS);
839 let mut failed = false;
840 for (j, step) in STEPS.iter().enumerate() {
841 let mut ahead = x;
842 ahead[j] += step;
843 match fly(&ahead) {
844 Some(flight) => {
845 let mut column = problem.residuals(&ahead, &flight);
846 for (c, b) in column.iter_mut().zip(&r) {
847 *c = (*c - b) / step;
848 }
849 columns.push(column);
850 }
851 None => {
852 failed = true;
853 break;
854 }
855 }
856 }
857 if failed {
858 break;
859 }
860 columns.extend(problem.sensor_columns(&x, &at_x));
861 let mut gradient = [0.0; UNKNOWNS];
862 for i in 0..UNKNOWNS {
863 gradient[i] = columns[i].iter().zip(&r).map(|(a, b)| a * b).sum();
864 for j in 0..=i {
865 let value: f64 = columns[i].iter().zip(&columns[j]).map(|(a, b)| a * b).sum();
866 information[i][j] = value;
867 information[j][i] = value;
868 }
869 }
870 let mut improved = None;
871 while lambda < 1e8 {
872 let mut damped = information;
873 for (i, row) in damped.iter_mut().enumerate() {
874 row[i] *= 1.0 + lambda;
875 }
876 let Some(l) = cholesky(&damped) else {
877 lambda *= 10.0;
878 continue;
879 };
880 let step = solve(&l, &gradient.map(|g| -g));
881 let mut next = x;
882 for (value, delta) in next.iter_mut().zip(step) {
883 *value += delta;
884 }
885 let tried = fly(&next).and_then(|flight| {
886 let tried = problem.residuals(&next, &flight);
887 let tried_cost = chi2(&tried);
888 (tried_cost < cost).then_some((flight, tried, tried_cost))
889 });
890 if let Some(better) = tried {
891 x = next;
892 improved = Some(better);
893 lambda = (lambda * 0.1).max(1e-9);
894 break;
895 }
896 lambda *= 10.0;
897 }
898 let Some((flight, tried, tried_cost)) = improved else {
899 converged = true;
901 break;
902 };
903 let drop = cost - tried_cost;
904 at_x = flight;
905 r = tried;
906 cost = tried_cost;
907 if drop < CONVERGED_CHI2 {
908 converged = true;
909 break;
910 }
911 }
912 if pass + 1 < passes && matches!(noise, Noise::FromResiduals) {
913 let (height, horizontal) = residual_noise(&problem, &x, &at_x);
914 problem.height_sd_m = height.max(FLOOR_M);
915 problem.horizontal_sd_m = horizontal.max(FLOOR_HORIZONTAL_M);
916 }
917 }
918 let (height_noise_m, horizontal_noise_m) = (problem.height_sd_m, problem.horizontal_sd_m);
921 if matches!(noise, Noise::FromResiduals) {
922 let (height, horizontal) = residual_correlation(&problem, &x, &at_x);
923 problem.height_sd_m *= inflation(height).sqrt();
924 problem.horizontal_sd_m *= inflation(horizontal).sqrt();
925 }
926 let r = problem.residuals(&x, &at_x);
928 let mut columns: Vec<Vec<f64>> = Vec::with_capacity(UNKNOWNS);
929 for (j, step) in STEPS.iter().enumerate() {
930 let mut ahead = x;
931 ahead[j] += step;
932 let Some(flight) = fly(&ahead) else {
933 return (nothing(), None);
934 };
935 let mut column = problem.residuals(&ahead, &flight);
936 for (c, b) in column.iter_mut().zip(&r) {
937 *c = (*c - b) / step;
938 }
939 columns.push(column);
940 }
941 columns.extend(problem.sensor_columns(&x, &at_x));
942 for i in 0..UNKNOWNS {
943 for j in 0..=i {
944 let value: f64 = columns[i].iter().zip(&columns[j]).map(|(a, b)| a * b).sum();
945 information[i][j] = value;
946 information[j][i] = value;
947 }
948 }
949 let Some(l) = cholesky(&information) else {
950 return (nothing(), None);
951 };
952 let mut parameters = [0.0; FLOWN];
953 parameters.copy_from_slice(&x[..FLOWN]);
954 let mut windy = [0.0; UNKNOWNS];
961 if matches!(noise, Noise::FromResiduals) && observed.horizontal.is_empty() {
962 let (east, north) = (&stated[3], &stated[4]);
963 let toward = east.mean.atan2(north.mean);
965 let top = east.mean.hypot(north.mean) + Z_90 * east.sd.max(north.sd);
966 let with_wind = |speed: f64| {
967 let mut at = parameters;
968 at[3] = speed * toward.sin();
969 at[4] = speed * toward.cos();
970 Parameters::from_array(at)
971 };
972 flown.set(flown.get() + 2);
973 let (Ok(calm), Ok(strong)) = (
974 forward.fly(&with_wind(0.0), until_s),
975 forward.fly_sheared(&with_wind(top), until_s, SHEAR_EXPONENT),
976 ) else {
977 return (nothing(), None);
978 };
979 let moved = |flight: &Simulated| {
980 let to = problem.residuals(&x, flight);
981 let mut pull = [0.0; UNKNOWNS];
982 for (p, column) in pull.iter_mut().zip(&columns) {
983 *p = column
984 .iter()
985 .zip(to.iter().zip(&r))
986 .map(|(c, (m, b))| c * (m - b))
987 .sum();
988 }
989 solve(&l, &pull)
990 };
991 let (to_calm, to_strong) = (moved(&calm), moved(&strong));
992 for (w, (a, b)) in windy.iter_mut().zip(to_calm.iter().zip(&to_strong)) {
993 *w = a.abs().max(b.abs());
994 }
995 }
996 let mut standard_errors = [0.0; FLOWN];
997 for (j, se) in standard_errors.iter_mut().enumerate() {
998 let mut unit = [0.0; UNKNOWNS];
999 unit[j] = 1.0;
1000 *se = (solve(&l, &unit)[j] + windy[j].powi(2) / 3.0).sqrt();
1001 }
1002 let (rms_m, _) = residual_noise(&problem, &x, &at_x);
1003 let misfit_share = rms_m / problem.read_spread_m();
1004 if misfit_share.is_nan() || misfit_share > MISFIT_SHARE {
1005 return (
1006 Fit {
1007 misfit_share: Some(misfit_share),
1008 ..nothing()
1009 },
1010 None,
1011 );
1012 }
1013 let posterior = sampling.and_then(|sampling| {
1014 let mut posterior_problem = problem.clone();
1021 let mut posterior_columns = columns.clone();
1022 let mut posterior_l = l;
1023 let mut center = x;
1024 if problem.track_unknowns && !observed.horizontal.is_empty() {
1025 let as_parameters = |x: &[f64; UNKNOWNS]| {
1026 let mut parameters = [0.0; FLOWN];
1027 parameters.copy_from_slice(&x[..FLOWN]);
1028 Parameters::from_array(parameters)
1029 };
1030 for _ in 0..PEAK_STEPS {
1031 flown.set(flown.get() + 1);
1032 let at_center = forward.fly(&as_parameters(¢er), until_s).ok()?;
1033 let omega = posterior_problem
1034 .receiver_logs(center[FLOWN], &at_center)
1035 .into_iter()
1036 .max_by(|a, b| a.1.total_cmp(&b.1))
1037 .map(|(omega, _)| omega)?;
1038 posterior_problem.receiver_rad_s = Some(omega);
1039 let (_, rms_m) = residual_noise(&posterior_problem, ¢er, &at_center);
1040 let (_, rho) = residual_correlation(&posterior_problem, ¢er, &at_center);
1041 posterior_problem.horizontal_sd_m =
1042 rms_m.max(PROPOSAL_HORIZONTAL_FLOOR_M) * inflation(rho).sqrt();
1043 let r = posterior_problem.residuals(¢er, &at_center);
1044 let mut step_columns: Vec<Vec<f64>> = Vec::with_capacity(UNKNOWNS);
1045 for (j, step) in STEPS.iter().enumerate() {
1046 let mut ahead = center;
1047 ahead[j] += step;
1048 flown.set(flown.get() + 1);
1049 let flight = forward.fly(&as_parameters(&ahead), until_s).ok()?;
1050 let mut column = posterior_problem.residuals(&ahead, &flight);
1051 for (c, b) in column.iter_mut().zip(&r) {
1052 *c = (*c - b) / step;
1053 }
1054 step_columns.push(column);
1055 }
1056 step_columns.extend(posterior_problem.sensor_columns(¢er, &at_center));
1057 let mut information = [[0.0; UNKNOWNS]; UNKNOWNS];
1058 for i in 0..UNKNOWNS {
1059 for j in 0..=i {
1060 let value: f64 = step_columns[i]
1061 .iter()
1062 .zip(&step_columns[j])
1063 .map(|(a, b)| a * b)
1064 .sum();
1065 information[i][j] = value;
1066 information[j][i] = value;
1067 }
1068 }
1069 let factor = cholesky(&information)?;
1070 let pull: [f64; UNKNOWNS] = std::array::from_fn(|j| {
1071 -step_columns[j]
1072 .iter()
1073 .zip(&r)
1074 .map(|(c, v)| c * v)
1075 .sum::<f64>()
1076 });
1077 let step = solve(&factor, &pull);
1078 posterior_columns = step_columns;
1079 posterior_l = factor;
1080 if !step.iter().all(|v| v.is_finite()) {
1081 break;
1082 }
1083 for (c, d) in center.iter_mut().zip(step) {
1084 *c += d;
1085 }
1086 }
1087 }
1088 let mut covariance = [[0.0; UNKNOWNS]; UNKNOWNS];
1089 for j in 0..UNKNOWNS {
1090 let mut unit = [0.0; UNKNOWNS];
1091 unit[j] = 1.0;
1092 for (i, value) in solve(&posterior_l, &unit).into_iter().enumerate() {
1093 covariance[i][j] = value;
1094 }
1095 }
1096 let laplace = Laplace {
1097 x: ¢er,
1098 covariance: &covariance,
1099 columns: &posterior_columns,
1100 problem: &posterior_problem,
1101 until_s,
1102 };
1103 if matches!(noise, Noise::FromResiduals) && observed.horizontal.is_empty() {
1106 draw_posterior::<{ DRAWN + 1 }>(&laplace, forward, sampling)
1107 } else {
1108 draw_posterior::<DRAWN>(&laplace, forward, sampling)
1109 }
1110 });
1111 let fit = Fit {
1112 parameters: Parameters::from_array(parameters),
1113 standard_errors,
1114 clock_offset_s: x[FLOWN],
1115 height_offset_m: x[FLOWN + 1],
1116 rms_m: Some(rms_m),
1117 height_noise_m,
1118 horizontal_noise_m,
1119 fitted: true,
1120 converged,
1121 misfit_share: Some(misfit_share),
1122 read: problem.read,
1123 flown: flown.get(),
1124 };
1125 (fit, posterior)
1126}
1127
1128struct Laplace<'a> {
1131 x: &'a [f64; UNKNOWNS],
1132 covariance: &'a [[f64; UNKNOWNS]; UNKNOWNS],
1133 columns: &'a [Vec<f64>],
1134 problem: &'a Problem<'a>,
1135 until_s: f64,
1136}
1137
1138const SHEAR_STATED: [f64; 2] = [0.0, SHEAR_EXPONENT];
1141
1142const DRAWN: usize = FLOWN + 1;
1145
1146const LINEAR: usize = UNKNOWNS - DRAWN;
1149
1150struct Integrated {
1153 chi2_min: f64,
1155 log_det: f64,
1157 #[cfg_attr(
1159 not(test),
1160 expect(dead_code, reason = "read by the tests' check of the integral")
1161 )]
1162 best: [f64; LINEAR],
1163 at: Vec<f64>,
1165}
1166
1167impl Integrated {
1168 fn minus_two_log(&self) -> f64 {
1170 self.chi2_min + self.log_det
1171 }
1172}
1173
1174impl Problem<'_> {
1175 fn integrated(&self, theta: &[f64; DRAWN], flown: &Simulated) -> Option<Integrated> {
1181 let mut base = [0.0; UNKNOWNS];
1182 base[..DRAWN].copy_from_slice(theta);
1183 let r0 = self.residuals(&base, flown);
1184 let columns: [Vec<f64>; LINEAR] = std::array::from_fn(|k| {
1185 let mut unit = base;
1186 unit[DRAWN + k] = 1.0;
1187 let mut column = self.residuals(&unit, flown);
1188 for (c, b) in column.iter_mut().zip(&r0) {
1189 *c -= b;
1190 }
1191 column
1192 });
1193 let mut h = [[0.0; LINEAR]; LINEAR];
1194 let mut pull = [0.0; LINEAR];
1195 for i in 0..LINEAR {
1196 pull[i] = -columns[i].iter().zip(&r0).map(|(a, b)| a * b).sum::<f64>();
1197 for j in 0..=i {
1198 let value: f64 = columns[i].iter().zip(&columns[j]).map(|(a, b)| a * b).sum();
1199 h[i][j] = value;
1200 h[j][i] = value;
1201 }
1202 }
1203 let l = cholesky(&h)?;
1204 let best = solve(&l, &pull);
1205 let mut at = r0;
1206 for (k, column) in columns.iter().enumerate() {
1207 for (r, c) in at.iter_mut().zip(column) {
1208 *r += best[k] * c;
1209 }
1210 }
1211 Some(Integrated {
1212 chi2_min: chi2(&at),
1213 log_det: (0..LINEAR).map(|i| 2.0 * l[i][i].ln()).sum(),
1214 best,
1215 at,
1216 })
1217 }
1218}
1219
1220fn draw_posterior<const M: usize>(
1227 laplace: &Laplace<'_>,
1228 forward: &mut dyn Forward,
1229 sampling: Sampling,
1230) -> Option<Posterior<FLOWN>> {
1231 let sheared = M > DRAWN;
1232 let (x, problem) = (laplace.x, laplace.problem);
1233 let [shear_low, shear_high] = SHEAR_STATED;
1234 let mut peak = [0.0; M];
1235 peak[..DRAWN].copy_from_slice(&x[..DRAWN]);
1236 let mut covariance = [[0.0; M]; M];
1238 for (row, from) in covariance.iter_mut().zip(laplace.covariance).take(DRAWN) {
1239 row[..DRAWN].copy_from_slice(&from[..DRAWN]);
1240 }
1241 let mut outer = [false; M];
1242 outer[WIND_EAST] = true;
1243 outer[WIND_NORTH] = true;
1244 if sheared {
1245 peak[DRAWN] = 0.5 * (shear_low + shear_high);
1247 covariance[DRAWN][DRAWN] = (shear_high - shear_low).powi(2) / 12.0;
1248 outer[DRAWN] = true;
1249 }
1250 let inner: Vec<usize> = (0..DRAWN).filter(|&i| !outer[i]).collect();
1251 let mut block = [[0.0; DRAWN]; DRAWN];
1254 for (row, from) in block.iter_mut().zip(laplace.covariance) {
1255 row.copy_from_slice(&from[..DRAWN]);
1256 }
1257 let block = cholesky(&block)?;
1258 let mut held = [[0.0; DRAWN - 2]; DRAWN - 2];
1259 let mut held_full = [[0.0; DRAWN]; DRAWN];
1260 for j in 0..DRAWN {
1261 let mut unit = [0.0; DRAWN];
1262 unit[j] = 1.0;
1263 for (i, value) in solve(&block, &unit).into_iter().enumerate() {
1264 held_full[i][j] = value;
1265 }
1266 }
1267 for (a, &i) in inner.iter().enumerate() {
1268 for (b, &j) in inner.iter().enumerate() {
1269 held[a][b] = held_full[i][j];
1270 }
1271 }
1272 let held = cholesky(&held)?;
1273 let forward = RefCell::new(forward);
1274 let fly_at = |point: &[f64; M]| {
1275 let mut parameters = [0.0; FLOWN];
1276 parameters.copy_from_slice(&point[..FLOWN]);
1277 let parameters = Parameters::from_array(parameters);
1278 let mut forward = forward.borrow_mut();
1279 if sheared {
1280 forward.fly_sheared(¶meters, laplace.until_s, point[DRAWN])
1281 } else {
1282 forward.fly(¶meters, laplace.until_s)
1283 }
1284 .ok()
1285 };
1286 let theta = |point: &[f64; M]| -> [f64; DRAWN] { std::array::from_fn(|i| point[i]) };
1287 let outside = |point: &[f64; M]| sheared && !(shear_low..=shear_high).contains(&point[DRAWN]);
1288 let drawn = posterior::sample_in_two(
1289 &peak,
1290 &covariance,
1291 &outer,
1292 sampling.pilot,
1293 sampling.draws,
1294 sampling.seed,
1295 |point| {
1296 if outside(point) {
1297 return Some(*point);
1299 }
1300 let flight = fly_at(point)?;
1301 let at = problem.integrated(&theta(point), &flight)?.at;
1302 let pull: [f64; DRAWN - 2] = std::array::from_fn(|a| {
1303 laplace.columns[inner[a]]
1304 .iter()
1305 .zip(&at)
1306 .map(|(c, v)| c * v)
1307 .sum()
1308 });
1309 let step = solve(&held, &pull);
1310 let mut middle = *point;
1311 for (a, &i) in inner.iter().enumerate() {
1312 middle[i] = x[i] - step[a];
1313 }
1314 Some(middle)
1315 },
1316 |point| {
1317 if outside(point) {
1318 return Some(f64::NEG_INFINITY);
1319 }
1320 let flight = fly_at(point)?;
1321 let theta = theta(point);
1322 let integrated = problem.integrated(&theta, &flight)?;
1323 Some(
1324 -0.5 * (integrated.minus_two_log()
1325 + problem.track_noise_integrated(theta[FLOWN], &flight, &integrated.at)),
1326 )
1327 },
1328 )?;
1329 Some(Posterior {
1330 quantiles: std::array::from_fn(|j| drawn.quantiles[j]),
1331 effective_draws: drawn.effective_draws,
1332 draws: drawn.draws,
1333 failed: drawn.failed,
1334 enough: drawn.enough,
1335 })
1336}
1337
1338impl Problem<'_> {
1339 fn read_spread_m(&self) -> f64 {
1341 let read: Vec<f64> = self
1342 .observed
1343 .heights
1344 .iter()
1345 .zip(&self.read)
1346 .filter(|(_, read)| **read)
1347 .map(|(&(_, h), _)| h)
1348 .collect();
1349 #[expect(
1350 clippy::cast_precision_loss,
1351 reason = "a count of bins is far below 2^53"
1352 )]
1353 let n = read.len().max(1) as f64;
1354 let mean = read.iter().sum::<f64>() / n;
1355 (read.iter().map(|h| (h - mean).powi(2)).sum::<f64>() / n).sqrt()
1356 }
1357}
1358
1359impl Problem<'_> {
1360 fn track_noise_integrated(&self, clock_s: f64, flown: &Simulated, at: &[f64]) -> f64 {
1365 if !self.track_unknowns || self.observed.horizontal.is_empty() {
1366 return 0.0;
1367 }
1368 let start = self.observed.heights.len();
1370 let rows = &at[start..start + 2 * self.observed.horizontal.len()];
1371 let logs: Vec<f64> = self
1372 .receiver_logs(clock_s, flown)
1373 .into_iter()
1374 .map(|(_, log)| log)
1375 .collect();
1376 let top = logs.iter().copied().fold(f64::NEG_INFINITY, f64::max);
1377 let sum: f64 = logs.iter().map(|l| (l - top).exp()).sum();
1378 let nodes = f64::from(RECEIVER_LOOP_NODES);
1379 -2.0 * (top + (sum / nodes).ln()) - chi2(rows)
1380 }
1381
1382 fn receiver_logs(&self, clock_s: f64, flown: &Simulated) -> Vec<(f64, f64)> {
1386 let times: Vec<f64> = self
1387 .observed
1388 .horizontal
1389 .iter()
1390 .map(|&(t, _, _)| t - clock_s)
1391 .collect();
1392 let nodes = f64::from(RECEIVER_LOOP_NODES);
1393 let [low, high] = RECEIVER_LOOP_STATED_RAD_S;
1394 (0..RECEIVER_LOOP_NODES)
1395 .map(|i| {
1396 let omega = low * (high / low).powf((f64::from(i) + 0.5) / nodes);
1397 let lagged = flown.lagged_drift_m(omega, RECEIVER_LOOP_DAMPING, ×);
1398 let (east, north): (Vec<f64>, Vec<f64>) = self
1399 .observed
1400 .horizontal
1401 .iter()
1402 .zip(&lagged)
1403 .map(|(&(_, e, n), &(east, north))| (e - east, n - north))
1404 .unzip();
1405 (omega, -0.5 * track_minus_two_log([&east, &north]))
1406 })
1407 .collect()
1408 }
1409
1410 fn track_misfit_m(&self, clock_s: f64, flown: &Simulated) -> Vec<(f64, f64)> {
1415 let observed = &self.observed.horizontal;
1416 match self.receiver_rad_s {
1417 None => observed
1418 .iter()
1419 .map(|&(t, e, n)| {
1420 let (east, north) = flown.drift_m(t - clock_s);
1421 (e - east, n - north)
1422 })
1423 .collect(),
1424 Some(omega) => {
1425 let times: Vec<f64> = observed.iter().map(|&(t, _, _)| t - clock_s).collect();
1426 let misfit: Vec<(f64, f64)> = observed
1427 .iter()
1428 .zip(flown.lagged_drift_m(omega, RECEIVER_LOOP_DAMPING, ×))
1429 .map(|(&(_, e, n), (east, north))| (e - east, n - north))
1430 .collect();
1431 #[expect(
1433 clippy::cast_precision_loss,
1434 reason = "a count of bins is far below 2^53"
1435 )]
1436 let count = misfit.len().max(1) as f64;
1437 let east = misfit.iter().map(|m| m.0).sum::<f64>() / count;
1438 let north = misfit.iter().map(|m| m.1).sum::<f64>() / count;
1439 misfit.iter().map(|m| (m.0 - east, m.1 - north)).collect()
1440 }
1441 }
1442 }
1443}
1444
1445const RECEIVER_LOOP_STATED_RAD_S: [f64; 2] = [0.2, 5.0];
1452
1453const RECEIVER_LOOP_DAMPING: f64 = std::f64::consts::FRAC_1_SQRT_2;
1455
1456const RECEIVER_LOOP_NODES: u32 = 96;
1458
1459const TRACK_CORRELATION_STATED: f64 = 0.99;
1464
1465const TRACK_CORRELATION_NODES: u32 = 198;
1468
1469const PEAK_STEPS: usize = 3;
1472
1473const PROPOSAL_HORIZONTAL_FLOOR_M: f64 = 0.5;
1476
1477fn track_minus_two_log(series: [&[f64]; 2]) -> f64 {
1500 let axes: Vec<[f64; 7]> = series
1503 .iter()
1504 .filter(|e| e.len() >= 2)
1505 .map(|e| {
1506 let m = e.len();
1507 #[expect(
1508 clippy::cast_precision_loss,
1509 reason = "a count of residuals is far below 2^53"
1510 )]
1511 let count = m as f64;
1512 [
1513 count,
1514 e[0],
1515 e.iter().map(|v| v * v).sum(),
1516 e.windows(2).map(|w| w[0] * w[1]).sum(),
1517 e[1..m - 1].iter().map(|v| v * v).sum(),
1518 e[1..].iter().sum(),
1519 e[..m - 1].iter().sum(),
1520 ]
1521 })
1522 .collect();
1523 if axes.is_empty() {
1524 return 0.0;
1525 }
1526 let total: f64 = axes.iter().map(|a| a[0]).sum();
1527 #[expect(clippy::cast_precision_loss, reason = "two axes at most")]
1528 let offsets = axes.len() as f64;
1529 let nodes = f64::from(TRACK_CORRELATION_NODES);
1530 let logs: Vec<f64> = (0..TRACK_CORRELATION_NODES)
1531 .map(|i| {
1532 let phi = (f64::from(i) + 0.5) * TRACK_CORRELATION_STATED / nodes;
1533 let keep = 1.0 - phi * phi;
1534 let (mut q, mut log_det) = (0.0, 0.0);
1535 for &[count, first, all, along, inner, later, earlier] in &axes {
1536 let ww = all - 2.0 * phi * along + phi * phi * inner;
1539 let wx = keep * first + (1.0 - phi) * (later - phi * earlier);
1540 let xx = keep + (count - 1.0) * (1.0 - phi).powi(2);
1541 q += ww - wx * wx / xx;
1542 log_det += xx.ln();
1543 }
1544 if q.is_nan() {
1546 return f64::NAN;
1547 }
1548 0.5 * offsets * keep.ln()
1549 - 0.5 * log_det
1550 - 0.5 * (total - offsets) * q.max(f64::MIN_POSITIVE).ln()
1551 })
1552 .collect();
1553 let top = logs.iter().copied().fold(f64::NEG_INFINITY, f64::max);
1554 let sum: f64 = logs.iter().map(|l| (l - top).exp()).sum();
1555 -2.0 * (top + (sum / nodes).ln())
1556}
1557
1558fn residual_noise(problem: &Problem<'_>, x: &[f64; UNKNOWNS], flown: &Simulated) -> (f64, f64) {
1560 let tau = x[FLOWN];
1561 let observed = problem.observed;
1562 let rms = |sum: f64, n: usize| {
1563 if n == 0 {
1564 0.0
1565 } else {
1566 #[expect(
1567 clippy::cast_precision_loss,
1568 reason = "a count of bins is far below 2^53"
1569 )]
1570 let n = n as f64;
1571 (sum / n).sqrt()
1572 }
1573 };
1574 let (mut heights, mut read) = (0.0, 0);
1575 for (&(t, y), used) in observed.heights.iter().zip(&problem.read) {
1576 if *used {
1577 heights += (y - problem.height_m(x, flown, t)).powi(2);
1578 read += 1;
1579 }
1580 }
1581 let horizontal: f64 = problem
1582 .track_misfit_m(tau, flown)
1583 .iter()
1584 .map(|(east, north)| east.powi(2) + north.powi(2))
1585 .sum();
1586 (
1587 rms(heights, read),
1588 rms(horizontal, 2 * observed.horizontal.len()),
1589 )
1590}
1591
1592const MAX_CORRELATION: f64 = 0.9;
1595
1596fn inflation(rho: f64) -> f64 {
1601 let rho = rho.clamp(0.0, MAX_CORRELATION);
1602 (1.0 + rho) / (1.0 - rho)
1603}
1604
1605fn residual_correlation(
1608 problem: &Problem<'_>,
1609 x: &[f64; UNKNOWNS],
1610 flown: &Simulated,
1611) -> (f64, f64) {
1612 let observed = problem.observed;
1613 let heights: Vec<f64> = observed
1614 .heights
1615 .iter()
1616 .zip(&problem.read)
1617 .filter(|(_, used)| **used)
1618 .map(|(&(t, y), _)| y - problem.height_m(x, flown, t))
1619 .collect();
1620 let (east, north): (Vec<f64>, Vec<f64>) =
1621 problem.track_misfit_m(x[FLOWN], flown).into_iter().unzip();
1622 (lag_one(&[&heights]), lag_one(&[&east, &north]))
1623}
1624
1625fn lag_one(series: &[&[f64]]) -> f64 {
1627 let (mut along, mut whole) = (0.0, 0.0);
1628 for values in series {
1629 if values.len() < 3 {
1630 continue;
1631 }
1632 #[expect(
1633 clippy::cast_precision_loss,
1634 reason = "a count of bins is far below 2^53"
1635 )]
1636 let mean = values.iter().sum::<f64>() / values.len() as f64;
1637 along += values
1638 .windows(2)
1639 .map(|w| (w[0] - mean) * (w[1] - mean))
1640 .sum::<f64>();
1641 whole += values.iter().map(|v| (v - mean).powi(2)).sum::<f64>();
1642 }
1643 if whole > 0.0 { along / whole } else { 0.0 }
1644}
1645
1646#[cfg(test)]
1647mod tests {
1648 use super::*;
1649
1650 use crate::residuals::tests_support::simulated_of;
1651
1652 struct Line;
1655
1656 impl Forward for Line {
1657 fn fly(&mut self, p: &Parameters, until_s: f64) -> Result<Simulated, String> {
1658 if p.drag_scale <= 0.0 {
1659 return Err("no drag".to_owned());
1660 }
1661 Ok(simulated_of(until_s, |t| {
1662 (
1663 100.0 * p.drag_scale * t + 5.0 * p.impulse_scale * t * t,
1664 p.wind_east_m_s * t,
1665 p.wind_north_m_s * t,
1666 )
1667 }))
1668 }
1669
1670 fn fly_sheared(
1672 &mut self,
1673 p: &Parameters,
1674 until_s: f64,
1675 exponent: f64,
1676 ) -> Result<Simulated, String> {
1677 if p.drag_scale <= 0.0 {
1678 return Err("no drag".to_owned());
1679 }
1680 Ok(simulated_of(until_s, |t| {
1681 (
1682 (100.0 * p.drag_scale - 10.0 * exponent) * t + 5.0 * p.impulse_scale * t * t,
1683 p.wind_east_m_s * t,
1684 p.wind_north_m_s * t,
1685 )
1686 }))
1687 }
1688 }
1689
1690 fn wide_held() -> Stated {
1692 let mut stated = wide();
1693 stated[0].sd = 1e-6;
1694 stated[1].sd = 1e-6;
1695 stated
1696 }
1697
1698 fn wide() -> Stated {
1699 [Prior {
1700 mean: 1.0,
1701 sd: 100.0,
1702 }; FLOWN]
1703 }
1704
1705 fn observed(drag: f64, impulse: f64, east: f64, north: f64) -> Observations {
1706 let times = (0..80).map(|k| 0.25 * f64::from(k));
1707 Observations {
1708 kind: HeightKind::Geometric,
1709 heights: times
1710 .clone()
1711 .map(|t| (t, 100.0 * drag * t + 5.0 * impulse * t * t))
1712 .collect(),
1713 horizontal: times.map(|t| (t, east * t, north * t)).collect(),
1714 liftoff_s: 0.0,
1715 flown_s: 20.0,
1716 reads_transonic: true,
1717 }
1718 }
1719
1720 const MANY: Sampling = Sampling {
1722 pilot: 500,
1723 draws: 4_000,
1724 seed: 9,
1725 };
1726
1727 #[test]
1728 fn on_a_line_the_bayesian_points_are_the_closed_forms() {
1729 let seen = observed(1.1, 0.9, 3.0, -2.0);
1732 let mut seen_noisy = seen.clone();
1733 let mut rng = hpr_core::random::SeededRng::seed_from_u64(3);
1734 for (_, h) in &mut seen_noisy.heights {
1735 *h += rng.standard_normal();
1736 }
1737 let mut stated = wide();
1738 stated[0].sd = 0.2;
1739 let got = bayesian(
1740 Some(&seen_noisy),
1741 &stated,
1742 Noise::Known {
1743 height_m: 1.0,
1744 horizontal_m: 1.0,
1745 },
1746 &mut Line,
1747 MANY,
1748 );
1749 let fit = &got.least_squares;
1750 let posterior = got.posterior.expect("a posterior");
1751 assert!(posterior.enough, "{}", posterior.effective_draws);
1752 let values = fit.parameters.to_array();
1753 for j in [0, 1, 3, 4] {
1754 let se = fit.standard_errors[j];
1755 let [low, mid, high] = posterior.quantiles[j];
1756 for (got, want) in [low, mid, high].into_iter().zip([
1757 values[j] - Z_90 * se,
1758 values[j],
1759 values[j] + Z_90 * se,
1760 ]) {
1761 assert!(
1762 ((got - want) / se).abs() < 0.1,
1763 "{j}: {got} against {want}, se {se}"
1764 );
1765 }
1766 }
1767 }
1768
1769 #[test]
1770 fn on_heights_alone_the_wind_shear_is_drawn_as_stated() {
1771 let mut tracked_seen = observed(1.0, 1.0, 0.0, 0.0);
1781 let mut rng = hpr_core::random::SeededRng::seed_from_u64(5);
1782 for (_, h) in &mut tracked_seen.heights {
1783 *h += rng.standard_normal();
1784 }
1785 for (_, east, north) in &mut tracked_seen.horizontal {
1786 *east += 2.0 * rng.standard_normal();
1787 *north += 2.0 * rng.standard_normal();
1788 }
1789 let tracked = bayesian(
1790 Some(&tracked_seen),
1791 &wide(),
1792 Noise::FromResiduals,
1793 &mut Line,
1794 MANY,
1795 );
1796 let with_track = tracked.posterior.expect("a posterior").quantiles[0];
1797 let mut seen = tracked_seen;
1798 seen.horizontal.clear();
1799 let alone = bayesian(Some(&seen), &wide(), Noise::FromResiduals, &mut Line, MANY);
1800 assert!(alone.least_squares.fitted);
1801 let alone = alone.posterior.expect("a posterior").quantiles[0];
1802 let shift: Vec<f64> = alone.iter().zip(&with_track).map(|(a, t)| a - t).collect();
1803 assert!(shift.iter().all(|d| (0.0..0.02).contains(d)), "{shift:?}");
1804 assert!((shift[1] - 0.01).abs() < 0.001, "{shift:?}");
1805 assert!(shift[0] < 0.009 && shift[2] > 0.011, "{shift:?}");
1806 }
1807
1808 #[test]
1809 fn the_linear_sensor_unknowns_integrate_out_exactly() {
1810 let mut seen = observed(1.05, 0.95, 0.0, 0.0);
1813 seen.kind = HeightKind::Barometric;
1814 seen.horizontal.clear();
1815 for (k, (t, h)) in seen.heights.iter_mut().enumerate() {
1816 *h = 1.003 * *h + 2.0 + 0.02 * *t + if k % 2 == 0 { 0.4 } else { -0.4 };
1817 }
1818 let stated = wide();
1819 let problem = Problem {
1820 observed: &seen,
1821 stated: &stated,
1822 clock_mean_s: 0.0,
1823 clock_sd_s: CLOCK_OFFSET_SD_S,
1824 height_offset_sd_m: HEIGHT_OFFSET_SD_M,
1825 gain_sd: GAIN_SD,
1826 drift_sd_m_s: DRIFT_SD_M_S,
1827 port_mean: PORT_MEAN,
1828 port_sd: PORT_SD,
1829 height_sd_m: 0.5,
1830 horizontal_sd_m: FLOOR_HORIZONTAL_M,
1831 receiver_rad_s: None,
1832 track_unknowns: false,
1833 read: vec![true; seen.heights.len()],
1834 };
1835 let theta = [1.04, 0.96, 1.0, 0.0, 0.0, 1.0, 0.01];
1836 let mut parameters = [0.0; FLOWN];
1837 parameters.copy_from_slice(&theta[..FLOWN]);
1838 let flown = Line.fly(&Parameters::from_array(parameters), 30.0).unwrap();
1839 let got = problem.integrated(&theta, &flown).unwrap();
1840 let mut at_best = [0.0; UNKNOWNS];
1841 at_best[..DRAWN].copy_from_slice(&theta);
1842 at_best[DRAWN..].copy_from_slice(&got.best);
1843 assert!((chi2(&problem.residuals(&at_best, &flown)) - got.chi2_min).abs() < 1e-6);
1844 let mut rng = hpr_core::random::SeededRng::seed_from_u64(5);
1845 for _ in 0..5 {
1846 let offset: [f64; LINEAR] = [
1847 rng.standard_normal(),
1848 1e-3 * rng.standard_normal(),
1849 0.05 * rng.standard_normal(),
1850 0.01 * rng.standard_normal(),
1851 ];
1852 let mut x = at_best;
1853 for (v, o) in x[DRAWN..].iter_mut().zip(offset) {
1854 *v += o;
1855 }
1856 let rise = chi2(&problem.residuals(&x, &flown)) - got.chi2_min;
1857 let r0 = problem.residuals(&at_best, &flown);
1859 let quadratic: f64 = {
1860 let mut total = 0.0;
1861 let columns: Vec<Vec<f64>> = (0..LINEAR)
1862 .map(|k| {
1863 let mut unit = at_best;
1864 unit[DRAWN + k] += 1.0;
1865 problem
1866 .residuals(&unit, &flown)
1867 .iter()
1868 .zip(&r0)
1869 .map(|(a, b)| a - b)
1870 .collect()
1871 })
1872 .collect();
1873 for i in 0..LINEAR {
1874 for j in 0..LINEAR {
1875 let hij: f64 = columns[i].iter().zip(&columns[j]).map(|(a, b)| a * b).sum();
1876 total += offset[i] * hij * offset[j];
1877 }
1878 }
1879 total
1880 };
1881 assert!(
1882 (rise - quadratic).abs() < 1e-6 * quadratic.max(1.0),
1883 "{rise} against {quadratic}"
1884 );
1885 }
1886 }
1887
1888 #[test]
1889 fn a_noise_free_line_comes_back_with_the_errors_its_design_matrix_gives() {
1890 let seen = observed(1.1, 0.9, 3.0, -2.0);
1891 let got = fit(
1892 Some(&seen),
1893 &wide(),
1894 Noise::Known {
1895 height_m: 1.0,
1896 horizontal_m: 1.0,
1897 },
1898 &mut Line,
1899 );
1900 assert!(got.fitted);
1901 assert!((got.parameters.drag_scale - 1.1).abs() < 1e-3, "{got:?}");
1902 assert!((got.parameters.impulse_scale - 0.9).abs() < 1e-3, "{got:?}");
1903 assert!((got.parameters.wind_east_m_s - 3.0).abs() < 1e-3, "{got:?}");
1904 assert!(
1905 (got.parameters.wind_north_m_s + 2.0).abs() < 1e-3,
1906 "{got:?}"
1907 );
1908 let sum_t2: f64 = (0..80).map(|k| (0.25 * f64::from(k)).powi(2)).sum();
1911 let expected = sum_t2.recip().sqrt();
1912 assert!(
1913 (got.standard_errors[3] / expected - 1.0).abs() < 1e-2,
1914 "{} against {expected}",
1915 got.standard_errors[3]
1916 );
1917 assert!((got.parameters.mass_scale - 1.0).abs() < 1e-6);
1919 assert!((got.standard_errors[5] - 100.0).abs() < 1e-3);
1920 }
1921
1922 #[test]
1923 fn what_is_stated_pulls_as_much_as_it_says() {
1924 let mut stated = wide();
1926 stated[3] = Prior { mean: 0.0, sd: 0.1 };
1927 let seen = observed(1.0, 1.0, 3.0, 0.0);
1928 let got = fit(
1929 Some(&seen),
1930 &stated,
1931 Noise::Known {
1932 height_m: 1.0,
1933 horizontal_m: 1.0,
1934 },
1935 &mut Line,
1936 );
1937 let sum_t2: f64 = (0..80).map(|k| (0.25 * f64::from(k)).powi(2)).sum();
1938 let expected = 3.0 * sum_t2 / (sum_t2 + 100.0);
1939 assert!(
1940 (got.parameters.wind_east_m_s - expected).abs() < 1e-3,
1941 "{} against {expected}",
1942 got.parameters.wind_east_m_s
1943 );
1944 }
1945
1946 #[test]
1947 fn a_log_that_says_nothing_or_a_flight_that_wont_fly_gives_what_was_stated() {
1948 let stated = wide();
1949 let nothing = fit(None, &stated, Noise::FromResiduals, &mut Line);
1950 assert!(!nothing.fitted);
1951 assert_eq!(nothing.parameters.drag_scale, 1.0);
1952 assert_eq!(nothing.standard_errors[0], 100.0);
1953 let mut grounded = stated;
1954 grounded[0].mean = -1.0;
1955 let seen = observed(1.0, 1.0, 0.0, 0.0);
1956 let refused = fit(Some(&seen), &grounded, Noise::FromResiduals, &mut Line);
1957 assert!(!refused.fitted);
1958 assert_eq!(refused.parameters.drag_scale, -1.0);
1959 }
1960
1961 #[test]
1964 fn a_barometers_gain_goes_to_the_sensor_not_the_rocket() {
1965 let held = {
1966 let mut stated = wide();
1967 stated[0].sd = 1e-6;
1968 stated[1].sd = 1e-6;
1969 stated
1970 };
1971 let mut seen = observed(1.0, 1.0, 0.0, 0.0);
1972 seen.horizontal.clear();
1973 for sample in &mut seen.heights {
1974 sample.1 *= 1.004;
1975 }
1976 let height_only = fit(Some(&seen), &held, Noise::FromResiduals, &mut Line);
1977 seen.kind = HeightKind::Barometric;
1978 let barometer = fit(Some(&seen), &held, Noise::FromResiduals, &mut Line);
1979 let (rms_height, rms_barometer) = (height_only.rms_m.unwrap(), barometer.rms_m.unwrap());
1980 assert!(
1981 rms_barometer < 0.7 * rms_height,
1982 "{rms_barometer} {rms_height}"
1983 );
1984 assert!((barometer.parameters.drag_scale - 1.0).abs() < 1e-4);
1985 }
1986
1987 fn jumping_log(jump_s: f64) -> FlightLog {
1990 let pad_s = 2.0;
1991 let height = |tau: f64| {
1992 if tau <= 0.0 {
1993 0.0
1994 } else if tau < 2.0 {
1995 25.0 * tau * tau
1996 } else if tau < 12.2 {
1997 100.0 + 100.0 * (tau - 2.0) - 4.9 * (tau - 2.0).powi(2)
1998 } else {
1999 (610.2 - 20.0 * (tau - 12.2)).max(0.0)
2000 }
2001 };
2002 let time_s: Vec<f64> = (0..1_400).map(|i| f64::from(i) * 0.05).collect();
2003 let altitude_m = time_s.iter().map(|&t| height(t - pad_s)).collect();
2004 let time_s = time_s
2005 .into_iter()
2006 .map(|t| if t - pad_s > 9.2 { t + jump_s } else { t })
2007 .collect();
2008 FlightLog {
2009 format: hpr_flightdata::log::LogFormat::GenericTable,
2010 logger: "a test's".to_owned(),
2011 serial_number: None,
2012 firmware: None,
2013 flight_number: None,
2014 stated: hpr_flightdata::log::Stated::default(),
2015 time_s,
2016 altitude_m,
2017 vertical_speed_m_s: None,
2018 temperature_k: None,
2019 battery_v: None,
2020 channels: Vec::new(),
2021 events: Vec::new(),
2022 notes: Vec::new(),
2023 }
2024 }
2025
2026 #[test]
2029 fn a_clock_that_jumps_past_any_flight_says_nothing() {
2030 let plain = jumping_log(0.0);
2031 assert!(observations(&plain, &readings::read(&plain)).is_some());
2032 let jumped = jumping_log(5_000.0);
2033 let read = readings::read(&jumped);
2034 assert!(matches!(read.apogee, Reading::Read(_)), "{:?}", read.apogee);
2035 assert!(observations(&jumped, &read).is_none());
2036 }
2037
2038 #[test]
2040 fn a_window_past_any_flight_says_nothing() {
2041 const { assert!(MAX_WINDOW_S < 1e4) };
2042 let held = binned([(0.0, 1.0), (999.0, 2.0)].into_iter(), 0.0, MAX_WINDOW_S);
2043 assert_eq!(held.len(), 2);
2044 }
2045
2046 #[test]
2049 fn a_log_far_from_any_flight_says_nothing() {
2050 let mut seen = observed(1.0, 1.0, 0.0, 0.0);
2051 seen.horizontal.clear();
2052 for sample in &mut seen.heights {
2053 sample.1 = 300.0 * (3.0 * sample.0).sin();
2054 }
2055 let got = fit(Some(&seen), &wide_held(), Noise::FromResiduals, &mut Line);
2056 assert!(!got.fitted);
2057 let share = got.misfit_share.unwrap();
2058 assert!(share > MISFIT_SHARE && share < 2.0, "{share}");
2059 assert!(got.flown > 0);
2060 assert_eq!(got.rms_m, None);
2061 }
2062
2063 #[test]
2065 fn a_barometer_with_too_few_bins_below_mach_0_8_says_nothing() {
2066 struct Fast;
2067 impl Forward for Fast {
2068 fn fly(&mut self, p: &Parameters, until_s: f64) -> Result<Simulated, String> {
2069 let _ = p;
2070 Ok(crate::residuals::tests_support::simulated_at_mach(
2071 until_s,
2072 2.3,
2073 |t| (800.0 * t, 0.0, 0.0),
2074 ))
2075 }
2076
2077 fn fly_sheared(
2078 &mut self,
2079 p: &Parameters,
2080 until_s: f64,
2081 _: f64,
2082 ) -> Result<Simulated, String> {
2083 self.fly(p, until_s)
2084 }
2085 }
2086 let mut seen = observed(1.0, 1.0, 0.0, 0.0);
2087 seen.kind = HeightKind::Barometric;
2088 seen.reads_transonic = false;
2089 seen.horizontal.clear();
2090 let got = fit(Some(&seen), &wide(), Noise::FromResiduals, &mut Fast);
2091 assert!(!got.fitted);
2092 assert_eq!(got.flown, 1);
2093 }
2094
2095 #[test]
2099 fn a_wind_heights_alone_cant_tell_widens_the_errors() {
2100 struct Unsheared(Line);
2101 impl Forward for Unsheared {
2102 fn fly(&mut self, p: &Parameters, until_s: f64) -> Result<Simulated, String> {
2103 self.0.fly(p, until_s)
2104 }
2105
2106 fn fly_sheared(
2107 &mut self,
2108 p: &Parameters,
2109 until_s: f64,
2110 _: f64,
2111 ) -> Result<Simulated, String> {
2112 self.0.fly(p, until_s)
2113 }
2114 }
2115 let mut seen = observed(1.0, 1.0, 0.0, 0.0);
2116 seen.horizontal.clear();
2117 let sheared = fit(Some(&seen), &wide(), Noise::FromResiduals, &mut Line);
2118 let flat = fit(
2119 Some(&seen),
2120 &wide(),
2121 Noise::FromResiduals,
2122 &mut Unsheared(Line),
2123 );
2124 assert!(sheared.fitted && flat.fitted);
2125 assert_eq!(sheared.parameters, flat.parameters);
2126 let moved = 0.1 * SHEAR_EXPONENT;
2127 let added = sheared.standard_errors[0].powi(2) - flat.standard_errors[0].powi(2);
2128 assert!(
2129 (added - moved * moved / 3.0).abs() < 1e-9,
2130 "{added} against {}",
2131 moved * moved / 3.0
2132 );
2133 let tracked = observed(1.0, 1.0, 0.0, 0.0);
2134 let with_track = fit(Some(&tracked), &wide(), Noise::FromResiduals, &mut Line);
2135 let without = fit(
2136 Some(&tracked),
2137 &wide(),
2138 Noise::FromResiduals,
2139 &mut Unsheared(Line),
2140 );
2141 assert_eq!(with_track.standard_errors, without.standard_errors);
2142 }
2143
2144 #[test]
2147 fn a_residual_that_runs_on_tells_less() {
2148 let alternating = [1.0, -1.0, 1.0, -1.0, 1.0, -1.0];
2149 assert!(lag_one(&[&alternating]) < 0.0);
2150 assert_eq!(inflation(lag_one(&[&alternating])), 1.0);
2151 let slow: Vec<f64> = (0..200).map(|k| (f64::from(k) * 0.01).sin()).collect();
2152 assert!(lag_one(&[&slow]) > MAX_CORRELATION);
2153 assert!((inflation(1.0) - 19.0).abs() < 1e-12);
2154 assert!((inflation(0.5) - 3.0).abs() < 1e-12);
2155 assert_eq!(lag_one(&[&[1.0, 2.0]]), 0.0);
2156 }
2157
2158 #[test]
2159 fn a_clock_offset_is_found_and_the_noise_comes_from_the_residuals() {
2160 let mut seen = observed(1.0, 1.0, 0.0, 0.0);
2161 let mut state = 5_u64;
2162 for sample in &mut seen.heights {
2163 sample.0 += 1.5;
2164 state = state
2165 .wrapping_mul(6_364_136_223_846_793_005)
2166 .wrapping_add(1_442_695_040_888_963_407);
2167 #[expect(
2168 clippy::cast_precision_loss,
2169 reason = "noise for a test, any rounding will do"
2170 )]
2171 let u = (state >> 11) as f64 / (1_u64 << 53) as f64 - 0.5;
2172 sample.1 += 40.0 * u;
2173 }
2174 seen.horizontal.clear();
2175 seen.liftoff_s = 1.0;
2176 let got = fit(Some(&seen), &wide(), Noise::FromResiduals, &mut Line);
2177 assert!(
2178 (got.clock_offset_s - 1.5).abs() < 0.3,
2179 "{}",
2180 got.clock_offset_s
2181 );
2182 let rms = got.rms_m.unwrap();
2184 assert!((9.0..13.0).contains(&rms), "{rms}");
2185 }
2186
2187 #[test]
2188 fn bins_take_a_median_so_one_garbled_sample_doesnt_reach_them() {
2189 let samples = [(0.01, 1.0), (0.1, 900.0), (0.2, 2.0), (0.3, 7.0)];
2190 let bins = binned(samples.into_iter(), 0.0, 0.5);
2191 assert_eq!(bins, vec![(0.125, 2.0), (0.375, 7.0)]);
2192 }
2193
2194 #[test]
2195 fn cholesky_solves_and_refuses_what_isnt_positive() {
2196 let a = [[4.0, 2.0], [2.0, 3.0]];
2197 let l = cholesky(&a).unwrap();
2198 let x = solve(&l, &[6.0, 5.0]);
2199 assert!((x[0] - 1.0).abs() < 1e-12 && (x[1] - 1.0).abs() < 1e-12);
2200 assert!(cholesky(&[[1.0, 2.0], [2.0, 1.0]]).is_none());
2201 }
2202
2203 #[expect(
2208 clippy::needless_range_loop,
2209 clippy::cast_possible_wrap,
2210 clippy::cast_possible_truncation,
2211 clippy::cast_precision_loss,
2212 reason = "a dense matrix written out by its indices, of a few rows"
2213 )]
2214 fn dense_track_minus_two_log(series: [&[f64]; 2]) -> f64 {
2215 let nodes = f64::from(TRACK_CORRELATION_NODES);
2216 let total: usize = series.iter().map(|e| e.len()).sum();
2217 let logs: Vec<f64> = (0..TRACK_CORRELATION_NODES)
2218 .map(|i| {
2219 let phi = (f64::from(i) + 0.5) * TRACK_CORRELATION_STATED / nodes;
2220 let (mut q, mut log_terms) = (0.0, 0.0);
2221 for e in series {
2222 let m = e.len();
2223 let mut a = vec![vec![0.0; 2 * m]; m];
2225 for r in 0..m {
2226 for c in 0..m {
2227 a[r][c] = phi.powi((r as i32 - c as i32).abs()) / (1.0 - phi * phi);
2228 }
2229 a[r][m + r] = 1.0;
2230 }
2231 let mut log_det = 0.0;
2232 for c in 0..m {
2233 let pivot = a[c][c];
2234 log_det += pivot.ln();
2235 for v in a[c].iter_mut() {
2236 *v /= pivot;
2237 }
2238 for r in 0..m {
2239 if r != c {
2240 let f = a[r][c];
2241 let row = a[c].clone();
2242 for (v, w) in a[r].iter_mut().zip(&row) {
2243 *v -= f * w;
2244 }
2245 }
2246 }
2247 }
2248 let inv = |r: usize, c: usize| a[r][m + c];
2249 let (mut ee, mut oe, mut oo) = (0.0, 0.0, 0.0);
2250 for r in 0..m {
2251 for c in 0..m {
2252 ee += e[r] * inv(r, c) * e[c];
2253 oe += inv(r, c) * e[c];
2254 oo += inv(r, c);
2255 }
2256 }
2257 q += ee - oe * oe / oo;
2258 log_terms += -0.5 * log_det - 0.5 * oo.ln();
2259 }
2260 log_terms - 0.5 * (total as f64 - 2.0) * q.ln()
2261 })
2262 .collect();
2263 let top = logs.iter().copied().fold(f64::NEG_INFINITY, f64::max);
2264 let sum: f64 = logs.iter().map(|l| (l - top).exp()).sum();
2265 -2.0 * (top + (sum / nodes).ln())
2266 }
2267
2268 #[test]
2272 fn the_tracks_noise_and_pad_integrate_out_as_the_dense_computation_does() {
2273 let east = [1.3, -0.4, 2.2, 0.9, -1.7, 0.5, 1.1];
2274 let north = [-0.6, 0.2, 0.8, 1.9, 1.4, -0.3];
2275 let got = track_minus_two_log([&east, &north]);
2276 let dense = dense_track_minus_two_log([&east, &north]);
2277 assert!(
2278 (got - dense).abs() < 1e-9 * dense.abs().max(1.0),
2279 "{got} and {dense}"
2280 );
2281 let moved: Vec<f64> = east.iter().map(|v| v + 40.0).collect();
2282 let shifted = track_minus_two_log([&moved, &north]);
2283 assert!((shifted - got).abs() < 1e-9, "{shifted} and {got}");
2284 let k: f64 = 3.0;
2285 let (big_east, big_north): (Vec<f64>, Vec<f64>) = (
2286 east.iter().map(|v| k * v).collect(),
2287 north.iter().map(|v| k * v).collect(),
2288 );
2289 let scaled = track_minus_two_log([&big_east, &big_north]);
2290 let expected = got + (13.0 - 2.0) * (k * k).ln();
2291 assert!((scaled - expected).abs() < 1e-9, "{scaled} and {expected}");
2292 let run = [0.0, 0.5, 1.0, 1.5, 2.0, 2.5, 3.0];
2295 let mut scattered = run;
2296 scattered.swap(1, 5);
2297 scattered.swap(2, 6);
2298 assert!(track_minus_two_log([&run, &[]]) < track_minus_two_log([&scattered, &[]]));
2299 }
2300
2301 #[test]
2304 fn a_nan_residual_leaves_the_tracks_likelihood_nan() {
2305 let east = [1.3, -0.4, f64::NAN, 0.9, -1.7];
2306 let north = [-0.6, 0.2, 0.8, 1.9];
2307 assert!(track_minus_two_log([&east, &north]).is_nan());
2308 assert!(track_minus_two_log([&north, &east]).is_nan());
2309 }
2310}