1use hpr_atmos::{AtmosError, Barometer};
35use hpr_core::gravity::STANDARD_GRAVITY_MPS2;
36use hpr_flightdata::log::{Channel, FlightLog, LogFormat, Quantity, Stated};
37use hpr_flightdata::readings::{self, Reading, Readings, Source, Withheld};
38use hpr_flightdata::smoother;
39use hpr_flightdata::timeline::{self, Cell, Flew, Offset, What};
40use hpr_sim::{Environment, FlightEvent, FlightStep, Observer, SimError};
41use serde::{Deserialize, Serialize};
42use thiserror::Error;
43
44use crate::synthetic::truth::{Grid, TRUTH_STEP_S, TruthSample, truth_sample};
45
46const PAD_S: f64 = 10.0;
49const LANDED_S: f64 = 3.0 * readings::LANDED_FOR_S;
52const COARSEST_INTERVAL_S: f64 = readings::MAX_INTERVAL_S;
54const RATE_STEP_M: f64 = 1.0;
56
57#[derive(Debug, Error)]
59#[non_exhaustive]
60pub enum ResidualsError {
61 #[error("the log's liftoff can't be read: {0}")]
63 LogLiftoff(String),
64 #[error("the simulated flight's liftoff can't be read: {0}")]
66 SimulationLiftoff(String),
67 #[error("the simulation kept no samples")]
69 NoSamples,
70 #[error(transparent)]
72 Atmosphere(#[from] AtmosError),
73 #[error(transparent)]
75 Sim(#[from] SimError),
76}
77
78#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)]
80#[serde(rename_all = "snake_case")]
81pub enum HeightKind {
82 Barometric,
84 Geometric,
86}
87
88impl HeightKind {
89 #[must_use]
91 pub fn of(source: Source) -> Self {
92 match source {
93 Source::Gnss | Source::Accelerometer => Self::Geometric,
94 _ => Self::Barometric,
96 }
97 }
98}
99
100#[derive(Debug, Default)]
103pub struct Track {
104 grid: Grid,
105}
106
107impl Track {
108 #[must_use]
110 pub fn new() -> Self {
111 Self::default()
112 }
113
114 pub fn simulated(self, environment: &Environment) -> Result<Simulated, ResidualsError> {
120 let mut samples = self.grid.samples;
121 samples.sort_by(|a, b| a.time_s.total_cmp(&b.time_s));
122 samples.dedup_by(|b, a| a.time_s == b.time_s);
123 let samples = samples
124 .iter()
125 .map(|sample| truth_sample(environment, sample))
126 .collect::<Result<Vec<_>, _>>()?;
127 Simulated::new(samples, environment)
128 }
129}
130
131impl Observer for Track {
132 fn step(&mut self, step: &dyn FlightStep) -> Result<(), SimError> {
133 self.grid.step(step)
134 }
135
136 fn event(&mut self, event: &FlightEvent) {
137 self.grid.event(event);
138 }
139}
140
141#[derive(Debug, Clone, Copy, PartialEq)]
143struct Moment {
144 time_s: f64,
145 height_m: f64,
147 reading_m: f64,
149 speed_m_s: f64,
151 reading_speed_m_s: f64,
153 axial_specific_force_m_s2: f64,
155 east_m: f64,
157 north_m: f64,
159 mach: f64,
161 port_m: f64,
164}
165
166#[derive(Debug, Clone, PartialEq)]
168pub struct Simulated {
169 moments: Vec<Moment>,
170}
171
172impl Simulated {
173 pub fn new(
179 samples: Vec<TruthSample>,
180 environment: &Environment,
181 ) -> Result<Self, ResidualsError> {
182 let first = samples.first().ok_or(ResidualsError::NoSamples)?;
183 let pad_m = first.height_m;
184 let (start_east_m, start_north_m) = (first.position_enu_m[0], first.position_enu_m[1]);
185 let site_msl_m = environment.site().height_m - environment.geoid_undulation_m;
186 let day = environment.atmosphere.as_ref();
187 let barometer = Barometer::on_pad(day, site_msl_m, pad_m)?;
188 let moments = samples
189 .iter()
190 .map(|sample| {
191 let reading_m = barometer.reading_m(sample.height_m)?;
192 let above = barometer.reading_m(sample.height_m + RATE_STEP_M)?;
195 let below = barometer.reading_m(sample.height_m - RATE_STEP_M)?;
196 let slope = (above - below) / (2.0 * RATE_STEP_M);
197 let density = day.air(site_msl_m + sample.height_m)?.air.density_kg_m3;
198 let port_m = sample.dynamic_pressure_pa / (density / slope * STANDARD_GRAVITY_MPS2);
199 Ok(Moment {
200 time_s: sample.time_s,
201 height_m: sample.height_m - pad_m,
202 reading_m,
203 speed_m_s: sample.vertical_speed_m_s,
204 reading_speed_m_s: sample.vertical_speed_m_s * slope,
205 axial_specific_force_m_s2: sample.axial_specific_force_m_s2,
206 east_m: sample.position_enu_m[0] - start_east_m,
207 north_m: sample.position_enu_m[1] - start_north_m,
208 mach: sample.mach,
209 port_m,
210 })
211 })
212 .collect::<Result<Vec<_>, AtmosError>>()?;
213 Ok(Self { moments })
214 }
215
216 fn at(&self, t: f64) -> Option<Moment> {
219 let (first, last) = (self.moments.first()?, self.moments.last()?);
220 if t > last.time_s {
221 return None;
222 }
223 if t <= first.time_s {
224 return Some(*first);
225 }
226 let after = self.moments.partition_point(|moment| moment.time_s < t);
227 let b = self.moments[after];
228 if b.time_s == t {
229 return Some(b);
230 }
231 let a = self.moments[after - 1];
232 let w = (t - a.time_s) / (b.time_s - a.time_s);
233 let mix = |x: f64, y: f64| x + w * (y - x);
234 Some(Moment {
235 time_s: t,
236 height_m: mix(a.height_m, b.height_m),
237 reading_m: mix(a.reading_m, b.reading_m),
238 speed_m_s: mix(a.speed_m_s, b.speed_m_s),
239 reading_speed_m_s: mix(a.reading_speed_m_s, b.reading_speed_m_s),
240 axial_specific_force_m_s2: mix(
241 a.axial_specific_force_m_s2,
242 b.axial_specific_force_m_s2,
243 ),
244 east_m: mix(a.east_m, b.east_m),
245 north_m: mix(a.north_m, b.north_m),
246 mach: mix(a.mach, b.mach),
247 port_m: mix(a.port_m, b.port_m),
248 })
249 }
250
251 fn held_at(&self, t: f64) -> Option<Moment> {
253 self.at(t).or_else(|| self.moments.last().copied())
254 }
255
256 pub(crate) fn top(&self) -> Option<(f64, f64)> {
260 let (at, top) = self
261 .moments
262 .iter()
263 .enumerate()
264 .filter(|(_, moment)| moment.height_m.is_finite())
265 .max_by(|(_, a), (_, b)| a.height_m.total_cmp(&b.height_m))?;
266 (at > 0 && at + 1 < self.moments.len()).then_some((top.time_s, top.height_m))
267 }
268
269 #[must_use]
274 pub fn top_reading_m(&self, kind: HeightKind) -> Option<f64> {
275 let (at, _) = self.top()?;
276 Some(self.read_height_m(at, kind)).filter(|reading| reading.is_finite())
277 }
278
279 pub(crate) fn read_height_m(&self, t: f64, kind: HeightKind) -> f64 {
281 self.held_at(t).map_or(f64::NAN, |moment| match kind {
282 HeightKind::Barometric => moment.reading_m,
283 HeightKind::Geometric => moment.height_m,
284 })
285 }
286
287 pub(crate) fn port_m(&self, t: f64) -> f64 {
289 self.held_at(t).map_or(f64::NAN, |moment| moment.port_m)
290 }
291
292 pub(crate) fn mach(&self, t: f64) -> f64 {
294 self.held_at(t).map_or(f64::NAN, |moment| moment.mach)
295 }
296
297 pub(crate) fn lagged_drift_m(
308 &self,
309 omega_rad_s: f64,
310 zeta: f64,
311 times_s: &[f64],
312 ) -> Vec<(f64, f64)> {
313 let mut out = Vec::with_capacity(times_s.len());
314 let Some(first) = self.moments.first() else {
315 return vec![(f64::NAN, f64::NAN); times_s.len()];
316 };
317 let (w, damping) = (omega_rad_s, 2.0 * zeta * omega_rad_s);
318 let mut error = [0.0_f64; 2];
319 let mut error_rate = [0.0_f64; 2];
320 let mut speed: [Option<f64>; 2] = [Some(0.0), Some(0.0)];
321 let mut previous = (
322 first.time_s,
323 [first.east_m, first.north_m],
324 [first.east_m, first.north_m],
325 );
326 let mut wanted = times_s.iter().copied().peekable();
327 while let Some(&t) = wanted.peek() {
328 if t > first.time_s {
329 break;
330 }
331 out.push((first.east_m, first.north_m));
332 wanted.next();
333 }
334 for moment in self.moments.iter().skip(1) {
335 if wanted.peek().is_none() {
336 break;
337 }
338 let dt = moment.time_s - previous.0;
339 if dt.is_nan() || dt <= 0.0 {
340 continue;
341 }
342 let p = [moment.east_m, moment.north_m];
343 let mut output = [0.0; 2];
344 for axis in 0..2 {
345 let rate = (p[axis] - previous.1[axis]) / dt;
346 let accel = speed[axis].map_or(0.0, |before| (rate - before) / dt);
347 speed[axis] = Some(rate);
348 error_rate[axis] += dt * (accel - w * w * error[axis] - damping * error_rate[axis]);
349 error[axis] += dt * error_rate[axis];
350 output[axis] = p[axis] - error[axis];
351 }
352 while let Some(&t) = wanted.peek() {
353 if t > moment.time_s {
354 break;
355 }
356 let share = (t - previous.0) / dt;
357 out.push((
358 previous.2[0] + share * (output[0] - previous.2[0]),
359 previous.2[1] + share * (output[1] - previous.2[1]),
360 ));
361 wanted.next();
362 }
363 previous = (moment.time_s, p, output);
364 }
365 for _ in wanted {
366 out.push((previous.2[0], previous.2[1]));
367 }
368 out
369 }
370
371 pub(crate) fn drift_m(&self, t: f64) -> (f64, f64) {
373 self.held_at(t).map_or((f64::NAN, f64::NAN), |moment| {
374 (moment.east_m, moment.north_m)
375 })
376 }
377
378 fn as_log(&self, log: &FlightLog, kind: HeightKind) -> Result<FlightLog, ResidualsError> {
382 let first = self.moments.first().ok_or(ResidualsError::NoSamples)?;
383 let last = self.moments.last().ok_or(ResidualsError::NoSamples)?;
384 let (track_s, _) = readings::height_track(log).unwrap_or_default();
385 let interval = median_interval_s(&track_s)
386 .unwrap_or(COARSEST_INTERVAL_S)
387 .clamp(TRUTH_STEP_S, COARSEST_INTERVAL_S);
388 let end_s = PAD_S + last.time_s + LANDED_S;
389 let rows = (end_s / interval).ceil() as usize;
390 let (mut time_s, mut altitude_m, mut force) = (
391 Vec::with_capacity(rows),
392 Vec::with_capacity(rows),
393 Vec::with_capacity(rows),
394 );
395 let height = |moment: &Moment| match kind {
396 HeightKind::Barometric => moment.reading_m,
397 HeightKind::Geometric => moment.height_m,
398 };
399 for row in 0..rows {
400 let clock_s = row as f64 * interval;
401 let t = clock_s - PAD_S;
402 let moment = if t < first.time_s {
403 first
404 } else if t > last.time_s {
405 last
406 } else {
407 &self.at(t).unwrap_or(*last)
408 };
409 time_s.push(clock_s);
410 altitude_m.push(height(moment));
411 force.push(moment.axial_specific_force_m_s2);
412 }
413 let channels = if log.channels_of(Quantity::Acceleration).next().is_some() {
414 vec![Channel::new(
415 "simulated axial specific force",
416 Quantity::Acceleration,
417 time_s.clone(),
418 force,
419 )]
420 } else {
421 Vec::new()
422 };
423 Ok(FlightLog {
424 format: LogFormat::GenericTable,
425 logger: "HPR Sim's simulated flight".to_owned(),
426 serial_number: None,
427 firmware: None,
428 flight_number: None,
429 stated: Stated::default(),
430 time_s,
431 altitude_m,
432 vertical_speed_m_s: None,
433 temperature_k: None,
434 battery_v: None,
435 channels,
436 events: Vec::new(),
437 notes: Vec::new(),
438 })
439 }
440}
441
442fn median_interval_s(times: &[f64]) -> Option<f64> {
444 let mut steps: Vec<f64> = times
445 .windows(2)
446 .map(|pair| pair[1] - pair[0])
447 .filter(|step| step.is_finite() && *step > 0.0)
448 .collect();
449 steps.sort_by(f64::total_cmp);
450 steps.get(steps.len() / 2).copied()
451}
452
453#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)]
455#[serde(rename_all = "snake_case")]
456pub enum Phase {
457 Boost,
459 Coast,
461 Ascent,
463 Descent,
465 Flight,
467}
468
469#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
471pub struct Summary {
472 pub phase: Phase,
474 pub start_s: f64,
476 pub end_s: f64,
478 pub samples: usize,
480 pub mean: f64,
482 pub rms: f64,
484 pub largest: f64,
486 pub largest_at_s: f64,
488}
489
490#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
492pub struct Point {
493 pub time_after_liftoff_s: f64,
495 pub logged: f64,
497 pub logged_sd: Option<f64>,
499 pub simulated: f64,
501 pub residual: f64,
503}
504
505#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)]
507pub struct Residuals {
508 pub points: Vec<Point>,
510 pub phases: Vec<Summary>,
512 pub past_simulation: usize,
514}
515
516#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)]
518#[serde(tag = "status", rename_all = "snake_case")]
519pub enum Speed {
520 Read(Residuals),
522 Withheld {
524 detail: String,
526 },
527}
528
529#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)]
531pub struct Row {
532 pub what: What,
534 pub logged: Cell,
536 pub simulated: Cell,
539 pub logged_minus_simulated: Option<f64>,
541}
542
543#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)]
545pub struct Comparison {
546 pub method: String,
548 pub height_read_as: HeightKind,
550 pub log_liftoff_s: f64,
552 pub simulated_liftoff_s: f64,
554 pub clock: Offset,
557 pub simulated_interval_s: f64,
559 pub reaches_landing: bool,
561 pub height_m: Residuals,
563 pub speed_m_s: Speed,
565 pub rows: Vec<Row>,
567}
568
569pub fn compare(
578 log: &FlightLog,
579 read: &Readings,
580 simulated: &Simulated,
581 height_read_as: Option<HeightKind>,
582) -> Result<Comparison, ResidualsError> {
583 let liftoff = match &read.liftoff {
584 Reading::Read(liftoff) => liftoff,
585 Reading::Withheld(withheld) => {
586 return Err(ResidualsError::LogLiftoff(withheld.detail.clone()));
587 }
588 };
589 let kind = height_read_as.unwrap_or_else(|| HeightKind::of(liftoff.source));
590 let simulated_log = simulated.as_log(log, kind)?;
591 let simulated_read = readings::read(&simulated_log);
592 let simulated_liftoff = match &simulated_read.liftoff {
593 Reading::Read(liftoff) => liftoff,
594 Reading::Withheld(withheld) => {
595 return Err(ResidualsError::SimulationLiftoff(withheld.detail.clone()));
596 }
597 };
598 let simulated_liftoff_s = simulated_liftoff.time_s - PAD_S;
599 let side = timeline::side_by_side_read(
600 &[(log, read), (&simulated_log, &simulated_read)],
601 Flew::OneAirframe,
602 );
603 let clock = match side.offsets.first() {
604 Some(Reading::Read(offset)) => *offset,
605 Some(Reading::Withheld(Withheld { detail, .. })) => {
607 return Err(ResidualsError::LogLiftoff(detail.clone()));
608 }
609 None => return Err(ResidualsError::NoSamples),
610 };
611 let rows = side
612 .rows
613 .into_iter()
614 .filter(|row| row.what != What::SmoothedApogee)
616 .filter_map(|row| {
617 let mut cells = row.cells.into_iter();
618 let (logged, simulated) = (cells.next()?, cells.next()?);
619 let simulated = unmeasured(row.what, simulated);
620 let logged_minus_simulated = difference(&logged, &simulated);
621 Some(Row {
622 what: row.what,
623 logged,
624 simulated,
625 logged_minus_simulated,
626 })
627 })
628 .collect();
629
630 let window = Window::of(read, liftoff.time_s, log);
631 let pad_m = read.pad_altitude_m.unwrap_or(0.0);
632 let (track_s, track_m) = readings::height_track(log).unwrap_or_default();
633 let height_samples = track_s
634 .iter()
635 .zip(&track_m)
636 .map(|(&t, &altitude)| (t, altitude - pad_m, None));
637 let height_m =
638 window.residuals(
639 height_samples,
640 simulated,
641 simulated_liftoff_s,
642 |moment| match kind {
643 HeightKind::Barometric => moment.reading_m,
644 HeightKind::Geometric => moment.height_m,
645 },
646 );
647 let smoothed = match kind {
650 HeightKind::Barometric => smoother::smooth(&without_gnss(log)),
651 HeightKind::Geometric => smoother::smooth(log),
652 };
653 let speed_m_s = match smoothed {
654 Ok(smoothed) => {
655 let samples = smoothed
656 .time_s
657 .iter()
658 .zip(&smoothed.speed_m_s)
659 .zip(&smoothed.speed_sd_m_s)
660 .map(|((&t, &speed), &sd)| (t, speed, Some(sd)));
661 Speed::Read(
662 window.residuals(
663 samples,
664 simulated,
665 simulated_liftoff_s,
666 |moment| match kind {
667 HeightKind::Barometric => moment.reading_speed_m_s,
668 HeightKind::Geometric => moment.speed_m_s,
669 },
670 ),
671 )
672 }
673 Err(withheld) => Speed::Withheld {
674 detail: withheld.detail,
675 },
676 };
677 Ok(Comparison {
678 method: side.method,
679 height_read_as: kind,
680 log_liftoff_s: liftoff.time_s,
681 simulated_liftoff_s,
682 clock,
683 simulated_interval_s: median_interval_s(&simulated_log.time_s).unwrap_or(0.0),
684 reaches_landing: read.landing.value().is_some(),
685 height_m,
686 speed_m_s,
687 rows,
688 })
689}
690
691fn without_gnss(log: &FlightLog) -> FlightLog {
693 let mut log = log.clone();
694 log.channels
695 .retain(|channel| channel.quantity != Quantity::GnssAltitude);
696 log
697}
698
699fn unmeasured(what: What, cell: Cell) -> Cell {
703 match cell {
704 Cell::Read(given) => Cell::Read(timeline::Given {
705 source: None,
706 provenance: None,
707 interval: None,
708 is_floor: given.is_floor && what != What::TopAcceleration,
709 ..given
710 }),
711 withheld @ Cell::Withheld { .. } => withheld,
712 }
713}
714
715fn difference(logged: &Cell, simulated: &Cell) -> Option<f64> {
717 match (logged, simulated) {
718 (Cell::Read(logged), Cell::Read(simulated)) if !logged.is_floor && !simulated.is_floor => {
719 Some(logged.value - simulated.value)
720 }
721 _ => None,
722 }
723}
724
725#[derive(Debug, Clone)]
727struct Window {
728 liftoff_s: f64,
729 end_s: f64,
731 phases: Vec<(Phase, f64)>,
733}
734
735impl Window {
736 fn of(read: &Readings, liftoff_s: f64, log: &FlightLog) -> Self {
737 let (track_s, _) = readings::height_track(log).unwrap_or_default();
738 let last = track_s.last().copied().unwrap_or(liftoff_s);
739 let end_s = read
740 .landing
741 .value()
742 .map_or(last, |landing| landing.time_s)
743 .max(liftoff_s)
744 - liftoff_s;
745 let apogee = read
746 .apogee
747 .value()
748 .filter(|apogee| !apogee.is_floor)
749 .map(|apogee| apogee.time_s - liftoff_s)
750 .filter(|&time_s| time_s > 0.0 && time_s <= end_s);
751 let burnout = read
752 .burnout
753 .value()
754 .filter(|burnout| !burnout.is_floor)
755 .map(|burnout| burnout.time_s - liftoff_s);
756 let phases = match (apogee, burnout) {
757 (Some(apogee), Some(burnout)) if burnout > 0.0 && burnout < apogee => vec![
758 (Phase::Boost, burnout),
759 (Phase::Coast, apogee),
760 (Phase::Descent, end_s),
761 ],
762 (Some(apogee), _) => vec![(Phase::Ascent, apogee), (Phase::Descent, end_s)],
763 (None, _) => vec![(Phase::Flight, end_s)],
764 };
765 Self {
766 liftoff_s,
767 end_s,
768 phases,
769 }
770 }
771
772 fn residuals(
775 &self,
776 samples: impl Iterator<Item = (f64, f64, Option<f64>)>,
777 simulated: &Simulated,
778 simulated_liftoff_s: f64,
779 value_of: impl Fn(&Moment) -> f64,
780 ) -> Residuals {
781 let mut points = Vec::new();
782 let mut past_simulation = 0;
783 for (t, logged, logged_sd) in samples {
784 let after = t - self.liftoff_s;
785 if !(0.0..=self.end_s).contains(&after) || !logged.is_finite() {
787 continue;
788 }
789 let Some(moment) = simulated.at(simulated_liftoff_s + after) else {
790 past_simulation += 1;
791 continue;
792 };
793 let simulated = value_of(&moment);
794 points.push(Point {
795 time_after_liftoff_s: after,
796 logged,
797 logged_sd,
798 simulated,
799 residual: logged - simulated,
800 });
801 }
802 let mut phases = Vec::with_capacity(self.phases.len());
803 let mut start_s = 0.0;
804 for &(phase, end_s) in &self.phases {
805 let last = phase == Phase::Descent || phase == Phase::Flight;
806 let inside: Vec<&Point> = points
807 .iter()
808 .filter(|point| {
809 let t = point.time_after_liftoff_s;
810 t >= start_s && (t < end_s || (last && t <= end_s))
811 })
812 .collect();
813 if let Some(summary) = summarize(phase, start_s, end_s, &inside) {
814 phases.push(summary);
815 }
816 start_s = end_s;
817 }
818 Residuals {
819 points,
820 phases,
821 past_simulation,
822 }
823 }
824}
825
826fn summarize(phase: Phase, start_s: f64, end_s: f64, points: &[&Point]) -> Option<Summary> {
828 let extreme = points
829 .iter()
830 .max_by(|a, b| a.residual.abs().total_cmp(&b.residual.abs()))?;
831 let count = points.len() as f64;
832 let sum = points.iter().fold(0.0, |sum, point| sum + point.residual);
833 let squares = points
834 .iter()
835 .fold(0.0, |sum, point| sum + point.residual * point.residual);
836 Some(Summary {
837 phase,
838 start_s,
839 end_s,
840 samples: points.len(),
841 mean: sum / count,
842 rms: (squares / count).sqrt(),
843 largest: extreme.residual,
844 largest_at_s: extreme.time_after_liftoff_s,
845 })
846}
847
848#[cfg(test)]
850pub(crate) mod tests_support {
851 use super::{Moment, Simulated};
852
853 pub(crate) fn simulated_of(until_s: f64, flight: impl Fn(f64) -> (f64, f64, f64)) -> Simulated {
856 simulated_at_mach(until_s, 0.0, flight)
857 }
858
859 pub(crate) fn simulated_at_mach(
861 until_s: f64,
862 mach: f64,
863 flight: impl Fn(f64) -> (f64, f64, f64),
864 ) -> Simulated {
865 let steps = (until_s / 0.01).ceil() as u32;
866 let moments = (0..=steps)
867 .map(|k| {
868 let time_s = 0.01 * f64::from(k);
869 let (height_m, east_m, north_m) = flight(time_s);
870 Moment {
871 time_s,
872 height_m,
873 reading_m: height_m,
874 speed_m_s: 0.0,
875 reading_speed_m_s: 0.0,
876 axial_specific_force_m_s2: 0.0,
877 east_m,
878 north_m,
879 mach,
880 port_m: 0.0,
881 }
882 })
883 .collect();
884 Simulated { moments }
885 }
886}
887
888#[cfg(test)]
889#[allow(
890 clippy::unwrap_used,
891 clippy::expect_used,
892 reason = "a test stops at the failure (clippy.toml)"
893)]
894mod tests {
895 use std::sync::Arc;
896
897 use hpr_atmos::{AtmosphereModel, Ussa76};
898 use hpr_core::geodesy::Geodetic;
899
900 use super::*;
901
902 fn flown(t: f64) -> (f64, f64, f64) {
905 const G: f64 = 9.8;
906 const BOOST: f64 = 60.0;
907 const BURN_S: f64 = 3.0;
908 const DESCENT: f64 = 8.0;
909 if t <= BURN_S {
910 return (0.5 * BOOST * t * t, BOOST * t, BOOST + G);
911 }
912 let burnout_m = 0.5 * BOOST * BURN_S * BURN_S;
913 let burnout_speed = BOOST * BURN_S;
914 let coast_s = burnout_speed / G;
915 let top_m = burnout_m + burnout_speed * burnout_speed / (2.0 * G);
916 if t <= BURN_S + coast_s {
917 let after = t - BURN_S;
918 let height = burnout_m + burnout_speed * after - 0.5 * G * after * after;
919 return (height, burnout_speed - G * after, 0.0);
920 }
921 let after = t - BURN_S - coast_s;
922 ((top_m - DESCENT * after).max(0.0), -DESCENT, G)
923 }
924
925 fn landed_s() -> f64 {
927 let mut t = 0.0;
928 while flown(t + 0.01).0 > 0.0 {
929 t += 0.01;
930 }
931 t
932 }
933
934 fn environment(atmosphere: Option<AtmosphereModel>) -> Environment {
935 let site = Geodetic::from_degrees(35.0, -106.0, 1000.0).unwrap();
936 let mut environment = Environment::standard(site).unwrap();
937 if let Some(atmosphere) = atmosphere {
938 environment.atmosphere = Arc::new(atmosphere);
939 }
940 environment
941 }
942
943 fn simulated(environment: &Environment, height: impl Fn(f64) -> (f64, f64, f64)) -> Simulated {
944 let end = landed_s() + 0.01;
945 let samples = (0..=(end / 0.01) as usize)
946 .map(|i| {
947 let time_s = i as f64 * 0.01;
948 let (height_m, speed, force) = height(time_s);
949 TruthSample {
950 time_s,
951 height_m,
952 vertical_speed_m_s: speed,
953 axial_specific_force_m_s2: force,
954 mach: 0.0,
955 dynamic_pressure_pa: 0.0,
956 position_enu_m: [0.0, 0.0, height_m],
957 velocity_enu_m_s: [0.0, 0.0, speed],
958 }
959 })
960 .collect();
961 Simulated::new(samples, environment).unwrap()
962 }
963
964 fn logged(pad_s: f64, height: impl Fn(f64) -> f64) -> FlightLog {
966 logged_every(0.05, pad_s, height)
967 }
968
969 fn logged_every(interval_s: f64, pad_s: f64, height: impl Fn(f64) -> f64) -> FlightLog {
971 let rows = ((landed_s() + pad_s + 20.0) / interval_s) as usize;
972 let time_s: Vec<f64> = (0..rows).map(|i| i as f64 * interval_s).collect();
973 let altitude_m = time_s
974 .iter()
975 .map(|&clock| height((clock - pad_s).max(0.0).min(landed_s())))
976 .collect();
977 FlightLog {
978 format: LogFormat::GenericTable,
979 logger: "a test's".to_owned(),
980 serial_number: None,
981 firmware: None,
982 flight_number: None,
983 stated: Stated::default(),
984 time_s,
985 altitude_m,
986 vertical_speed_m_s: None,
987 temperature_k: None,
988 battery_v: None,
989 channels: Vec::new(),
990 events: Vec::new(),
991 notes: Vec::new(),
992 }
993 }
994
995 fn rms_of(comparison: &Comparison, phase: Phase) -> Summary {
996 *comparison
997 .height_m
998 .phases
999 .iter()
1000 .find(|summary| summary.phase == phase)
1001 .unwrap_or_else(|| panic!("no {phase:?} in {:?}", comparison.height_m.phases))
1002 }
1003
1004 fn row(comparison: &Comparison, what: What) -> &Row {
1005 comparison.rows.iter().find(|row| row.what == what).unwrap()
1006 }
1007
1008 #[test]
1011 fn the_same_flight_leaves_almost_nothing() {
1012 let environment = environment(None);
1013 let simulation = simulated(&environment, flown);
1014 for pad_s in [20.0, 37.3] {
1015 let log = logged(pad_s, |t| flown(t).0 + 12.0);
1016 let comparison = compare(
1017 &log,
1018 &readings::read(&log),
1019 &simulation,
1020 Some(HeightKind::Geometric),
1021 )
1022 .unwrap();
1023 for summary in &comparison.height_m.phases {
1024 assert!(summary.rms < 1.0, "{pad_s}: {summary:?}");
1026 }
1027 assert_eq!(comparison.height_m.past_simulation, 0);
1028 assert!(comparison.reaches_landing);
1029 let apogee = row(&comparison, What::ApogeeHeight);
1030 assert!(
1031 apogee.logged_minus_simulated.unwrap().abs() < 1.0,
1032 "{apogee:?}"
1033 );
1034 }
1035 }
1036
1037 #[test]
1040 fn ignition_between_samples_leaves_the_clocks_offset() {
1041 let environment = environment(None);
1042 let simulation = simulated(&environment, flown);
1043 let log = logged_every(0.2, 37.3, |t| flown(t).0 + 12.0);
1044 let comparison = compare(
1045 &log,
1046 &readings::read(&log),
1047 &simulation,
1048 Some(HeightKind::Geometric),
1049 )
1050 .unwrap();
1051 let ascent = comparison
1052 .height_m
1053 .phases
1054 .iter()
1055 .find(|summary| matches!(summary.phase, Phase::Ascent | Phase::Boost))
1056 .unwrap();
1057 assert!((9.0..12.0).contains(&ascent.rms), "{ascent:?}");
1058 }
1059
1060 #[test]
1062 fn a_residual_is_logged_less_simulated() {
1063 let environment = environment(None);
1064 let simulation = simulated(&environment, flown);
1065 let log = logged(20.0, |t| 1.1 * flown(t).0);
1066 let comparison = compare(
1067 &log,
1068 &readings::read(&log),
1069 &simulation,
1070 Some(HeightKind::Geometric),
1071 )
1072 .unwrap();
1073 let top_m = flown(3.0 + 180.0 / 9.8).0;
1074 let apogee = row(&comparison, What::ApogeeHeight);
1075 let difference = apogee.logged_minus_simulated.unwrap();
1076 assert!(
1077 (difference - 0.1 * top_m).abs() < 2.0,
1078 "{difference} of {top_m}"
1079 );
1080 assert!(rms_of(&comparison, Phase::Descent).mean > 0.0);
1081 let Speed::Read(speed) = &comparison.speed_m_s else {
1083 panic!("{:?}", comparison.speed_m_s);
1084 };
1085 let climb = speed
1086 .phases
1087 .iter()
1088 .find(|summary| summary.phase != Phase::Descent)
1089 .unwrap();
1090 assert!(climb.mean > 1.0, "{climb:?}");
1091 let point = comparison.height_m.points.last().unwrap();
1093 assert_eq!(point.residual, point.logged - point.simulated);
1094 let largest = comparison
1095 .height_m
1096 .points
1097 .iter()
1098 .map(|point| point.residual)
1099 .fold(f64::MIN, f64::max);
1100 assert!((largest - 0.1 * top_m).abs() < 5.0, "{largest} of {top_m}");
1101 }
1102
1103 #[test]
1106 fn a_barometric_log_is_compared_with_a_barometers_reading() {
1107 let warm = AtmosphereModel::Standard(Ussa76::with_offset(20.0, 101_325.0).unwrap());
1108 let environment = environment(Some(warm));
1109 let simulation = simulated(&environment, flown);
1110 let barometer = Barometer::on_pad(environment.atmosphere.as_ref(), 1000.0, 0.0).unwrap();
1111 let log = logged(20.0, |t| barometer.reading_m(flown(t).0).unwrap());
1112 let read = readings::read(&log);
1113 let by_barometer = compare(&log, &read, &simulation, Some(HeightKind::Barometric)).unwrap();
1114 let by_height = compare(&log, &read, &simulation, Some(HeightKind::Geometric)).unwrap();
1115 let apogee = |comparison: &Comparison| {
1116 row(comparison, What::ApogeeHeight)
1117 .logged_minus_simulated
1118 .unwrap()
1119 };
1120 assert!(apogee(&by_barometer).abs() < 1.0, "{by_barometer:?}");
1121 let top_m = flown(3.0 + 180.0 / 9.8).0;
1123 let low = -apogee(&by_height) / top_m;
1124 assert!((0.05..0.09).contains(&low), "{low}");
1125 let largest = |points: &[Point]| {
1127 points
1128 .iter()
1129 .fold(0.0_f64, |most, point| most.max(point.residual.abs()))
1130 };
1131 assert!(
1132 largest(&by_barometer.height_m.points) < 0.01 * top_m,
1133 "{}",
1134 largest(&by_barometer.height_m.points)
1135 );
1136 assert!(largest(&by_height.height_m.points) > 0.05 * top_m);
1137 let coast_rms = |comparison: &Comparison| {
1138 let Speed::Read(speed) = &comparison.speed_m_s else {
1139 panic!("{:?}", comparison.speed_m_s);
1140 };
1141 speed
1142 .phases
1143 .iter()
1144 .find(|summary| summary.phase != Phase::Descent)
1145 .unwrap()
1146 .rms
1147 };
1148 assert!(
1149 coast_rms(&by_barometer) < coast_rms(&by_height),
1150 "{} against {}",
1151 coast_rms(&by_barometer),
1152 coast_rms(&by_height)
1153 );
1154 }
1155
1156 #[test]
1159 fn a_barometers_rate_is_the_speed_times_its_slope() {
1160 let warm = AtmosphereModel::Standard(Ussa76::with_offset(20.0, 101_325.0).unwrap());
1161 let environment = environment(Some(warm));
1162 let simulation = simulated(&environment, flown);
1163 let moment = simulation.at(10.0).unwrap();
1164 let barometer = Barometer::on_pad(environment.atmosphere.as_ref(), 1000.0, 0.0).unwrap();
1165 let (height_m, speed, _) = flown(10.0);
1166 let slope = (barometer.reading_m(height_m + 0.5).unwrap()
1167 - barometer.reading_m(height_m - 0.5).unwrap())
1168 / 1.0;
1169 assert!(slope < 0.97, "{slope}");
1170 assert!((moment.reading_speed_m_s - speed * slope).abs() < 1e-3 * speed);
1171 assert!((moment.speed_m_s - speed).abs() < 1e-9);
1172 }
1173
1174 #[test]
1177 fn a_gnss_channel_leaves_the_speed_on_the_barometers_scale() {
1178 let warm = AtmosphereModel::Standard(Ussa76::with_offset(20.0, 101_325.0).unwrap());
1179 let environment = environment(Some(warm));
1180 let simulation = simulated(&environment, flown);
1181 let barometer = Barometer::on_pad(environment.atmosphere.as_ref(), 1000.0, 0.0).unwrap();
1182 let plain = logged(20.0, |t| barometer.reading_m(flown(t).0).unwrap());
1183 let mut with_gnss = plain.clone();
1184 with_gnss.channels.push(Channel::new(
1185 "GPS Altitude",
1186 Quantity::GnssAltitude,
1187 plain.time_s.clone(),
1188 plain
1189 .time_s
1190 .iter()
1191 .map(|&clock| flown((clock - 20.0).clamp(0.0, landed_s())).0)
1192 .collect(),
1193 ));
1194 let speed = |log: &FlightLog| {
1195 let comparison = compare(log, &readings::read(log), &simulation, None).unwrap();
1196 let Speed::Read(speed) = comparison.speed_m_s else {
1197 panic!("{:?}", comparison.speed_m_s);
1198 };
1199 speed.phases[0].mean
1200 };
1201 assert_eq!(speed(&plain), speed(&with_gnss));
1202 }
1203
1204 #[test]
1206 fn a_tracks_source_says_what_its_height_stands_for() {
1207 for source in [Source::Barometer, Source::Pressure, Source::AltitudeChannel] {
1208 assert_eq!(HeightKind::of(source), HeightKind::Barometric);
1209 }
1210 for source in [Source::Gnss, Source::Accelerometer] {
1211 assert_eq!(HeightKind::of(source), HeightKind::Geometric);
1212 }
1213 }
1214
1215 #[test]
1219 fn a_slow_logs_simulation_is_read_at_its_interval() {
1220 let environment = environment(None);
1221 let simulation = simulated(&environment, flown);
1222 for interval_s in [0.33, 0.7, 0.9, 1.5] {
1223 let log = logged_every(interval_s, 20.0, |t| flown(t).0);
1224 let comparison = compare(
1225 &log,
1226 &readings::read(&log),
1227 &simulation,
1228 Some(HeightKind::Geometric),
1229 )
1230 .unwrap();
1231 let read_at = comparison.simulated_interval_s;
1232 assert!(
1233 (read_at - interval_s).abs() < 1e-9,
1234 "{interval_s}: {read_at}"
1235 );
1236 assert!(comparison.height_m.past_simulation <= 2, "{interval_s}");
1239 let first = comparison.height_m.points[0].time_after_liftoff_s;
1240 assert_eq!(first, 0.0, "{interval_s}");
1241 let apogee = row(&comparison, What::ApogeeHeight);
1242 let difference = apogee.logged_minus_simulated.unwrap_or(0.0);
1243 assert!(difference.abs() < 30.0, "{interval_s}: {apogee:?}");
1244 }
1245 }
1246
1247 #[test]
1249 fn samples_past_the_simulation_are_counted_not_compared() {
1250 let environment = environment(None);
1251 let simulation = {
1253 let end = 3.0 + 180.0 / 9.8 + 20.0;
1254 let samples: Vec<TruthSample> = (0..=(end / 0.01) as usize)
1255 .map(|i| {
1256 let t = i as f64 * 0.01;
1257 let (height_m, speed, force) = flown(t);
1258 TruthSample {
1259 time_s: t,
1260 height_m,
1261 vertical_speed_m_s: speed,
1262 axial_specific_force_m_s2: force,
1263 mach: 0.0,
1264 dynamic_pressure_pa: 0.0,
1265 position_enu_m: [0.0; 3],
1266 velocity_enu_m_s: [0.0; 3],
1267 }
1268 })
1269 .collect();
1270 Simulated::new(samples, &environment).unwrap()
1271 };
1272 let log = logged(20.0, |t| flown(t).0);
1273 let comparison = compare(
1274 &log,
1275 &readings::read(&log),
1276 &simulation,
1277 Some(HeightKind::Geometric),
1278 )
1279 .unwrap();
1280 assert!(comparison.height_m.past_simulation > 100);
1281 let last = comparison.height_m.points.last().unwrap();
1282 assert!(
1283 last.time_after_liftoff_s < 3.0 + 180.0 / 9.8 + 20.0 + 0.1,
1284 "{last:?}"
1285 );
1286 }
1287
1288 #[test]
1290 fn a_reading_keeps_its_provenance_beside_a_simulated_number() {
1291 let environment = environment(None);
1292 let simulation = simulated(&environment, flown);
1293 let log = logged(20.0, |t| flown(t).0);
1294 let read = readings::read(&log);
1295 let comparison = compare(&log, &read, &simulation, Some(HeightKind::Geometric)).unwrap();
1296 let apogee = read.apogee.value().unwrap();
1297 let top = row(&comparison, What::ApogeeHeight);
1298 let Cell::Read(logged) = &top.logged else {
1299 panic!("{top:?}");
1300 };
1301 assert_eq!(logged.provenance, Some(apogee.provenance));
1302 assert_eq!(logged.source, Some(apogee.source));
1303 assert_eq!(logged.value, apogee.height_m);
1304 let Cell::Read(simulated) = &top.simulated else {
1305 panic!("{top:?}");
1306 };
1307 assert_eq!((simulated.provenance, simulated.source), (None, None));
1308 assert_eq!(simulated.interval, None);
1309 let speed = read.max_speed.value().unwrap();
1310 let Cell::Read(logged) = &row(&comparison, What::TopSpeed).logged else {
1311 panic!("no top speed");
1312 };
1313 assert_eq!(logged.provenance, Some(speed.provenance));
1314 }
1315
1316 #[test]
1319 fn a_height_in_a_channel_is_the_logs_height() {
1320 let environment = environment(None);
1321 let simulation = simulated(&environment, flown);
1322 let mut log = logged(20.0, |t| flown(t).0);
1323 let own = compare(
1324 &log,
1325 &readings::read(&log),
1326 &simulation,
1327 Some(HeightKind::Geometric),
1328 )
1329 .unwrap();
1330 let time_s = std::mem::take(&mut log.time_s);
1331 let altitude_m = std::mem::take(&mut log.altitude_m);
1332 log.channels.push(Channel::new(
1333 "Altitude",
1334 Quantity::AltitudeAboveGround,
1335 time_s,
1336 altitude_m,
1337 ));
1338 let channel = compare(
1339 &log,
1340 &readings::read(&log),
1341 &simulation,
1342 Some(HeightKind::Geometric),
1343 )
1344 .unwrap();
1345 assert_eq!(channel.height_m.points.len(), own.height_m.points.len());
1346 assert!(!channel.height_m.points.is_empty());
1347 assert_eq!(channel.simulated_interval_s, own.simulated_interval_s);
1348 }
1349
1350 #[test]
1353 fn a_log_with_no_liftoff_is_refused() {
1354 let environment = environment(None);
1355 let simulation = simulated(&environment, flown);
1356 let mut log = logged(20.0, |_| 0.0);
1357 log.altitude_m.iter_mut().for_each(|h| *h = 0.0);
1358 let error = compare(&log, &readings::read(&log), &simulation, None).unwrap_err();
1359 assert!(matches!(error, ResidualsError::LogLiftoff(_)), "{error}");
1360 }
1361
1362 #[test]
1365 fn a_logged_accelerometer_gets_a_simulated_one() {
1366 let environment = environment(None);
1367 let shaped = |t: f64| {
1369 let (height_m, speed, force) = flown(t);
1370 let burning = t <= 3.0;
1371 let swell = if burning {
1372 30.0 * (t / 3.0 * std::f64::consts::PI).sin()
1373 } else {
1374 0.0
1375 };
1376 (height_m, speed, force + swell)
1377 };
1378 let simulation = simulated(&environment, shaped);
1379 let mut log = logged(20.0, |t| flown(t).0);
1380 let forces: Vec<f64> = log
1381 .time_s
1382 .iter()
1383 .map(|&clock| match clock - 20.0 {
1384 t if (0.0..=landed_s()).contains(&t) => shaped(t).2,
1385 _ => 9.8,
1386 })
1387 .collect();
1388 log.channels.push(Channel::new(
1389 "axial force",
1390 Quantity::Acceleration,
1391 log.time_s.clone(),
1392 forces,
1393 ));
1394 let comparison = compare(
1395 &log,
1396 &readings::read(&log),
1397 &simulation,
1398 Some(HeightKind::Geometric),
1399 )
1400 .unwrap();
1401 let top = row(&comparison, What::TopAcceleration);
1402 let difference = top.logged_minus_simulated;
1403 assert!(difference.is_some_and(|d| d.abs() < 1.0), "{top:?}");
1404 }
1405
1406 #[test]
1408 fn phases_follow_the_logs_readings() {
1409 let environment = environment(None);
1410 let simulation = simulated(&environment, flown);
1411 let top_s = 3.0 + 180.0 / 9.8;
1412 let log = logged(20.0, |t| flown(t).0);
1414 let read = readings::read(&log);
1415 let comparison = compare(&log, &read, &simulation, Some(HeightKind::Geometric)).unwrap();
1416 let phases = &comparison.height_m.phases;
1417 let names: Vec<Phase> = phases.iter().map(|s| s.phase).collect();
1418 assert_eq!(names, [Phase::Ascent, Phase::Descent]);
1419 assert!((phases[0].end_s - top_s).abs() < 1.0, "{phases:?}");
1420 assert_eq!(phases[1].start_s, phases[0].end_s);
1421 let count: usize = phases.iter().map(|s| s.samples).sum();
1422 assert_eq!(count, comparison.height_m.points.len(), "{names:?}");
1423 let mut read = readings::read(&log);
1425 let Reading::Read(burnout) = &mut read.burnout else {
1426 panic!("{:?}", read.burnout);
1427 };
1428 assert!(burnout.is_floor, "{burnout:?}");
1429 burnout.is_floor = false;
1430 let comparison = compare(&log, &read, &simulation, Some(HeightKind::Geometric)).unwrap();
1431 let phases = &comparison.height_m.phases;
1432 let names: Vec<Phase> = phases.iter().map(|s| s.phase).collect();
1433 assert_eq!(names, [Phase::Boost, Phase::Coast, Phase::Descent]);
1434 assert!((phases[0].end_s - 3.0).abs() < 0.5, "{phases:?}");
1435 assert!((phases[1].end_s - top_s).abs() < 1.0, "{phases:?}");
1436 }
1437
1438 #[test]
1441 fn a_receivers_filter_lags_a_constant_acceleration_by_a_over_omega_squared() {
1442 let (a, v, omega) = (2.0, 3.0, 0.5);
1443 let flight = tests_support::simulated_of(40.0, |t| (0.0, 0.5 * a * t * t, v * t));
1444 let times = [30.0, 35.0, 39.0];
1445 let lagged = flight.lagged_drift_m(omega, std::f64::consts::FRAC_1_SQRT_2, ×);
1446 for (&t, &(east, north)) in times.iter().zip(&lagged) {
1447 let lag = 0.5 * a * t * t - east;
1448 assert!((lag - a / (omega * omega)).abs() < 0.05, "{t} s: {lag}");
1449 assert!((v * t - north).abs() < 0.01, "{t} s: {}", v * t - north);
1450 }
1451 let fast = flight.lagged_drift_m(50.0, std::f64::consts::FRAC_1_SQRT_2, ×);
1452 for (&t, &(east, _)) in times.iter().zip(&fast) {
1453 assert!((0.5 * a * t * t - east).abs() < 0.01, "{t} s");
1454 }
1455 let before = flight.lagged_drift_m(omega, std::f64::consts::FRAC_1_SQRT_2, &[-1.0]);
1457 assert_eq!(before, vec![(0.0, 0.0)]);
1458 }
1459
1460 #[test]
1463 fn a_receivers_filter_matches_the_synthetic_receivers_loop() {
1464 let east = |t: f64| {
1466 if t < 2.0 {
1467 10.0 * t * t
1468 } else if t < 8.0 {
1469 40.0 + 40.0 * (t - 2.0) - 1.5 * (t - 2.0).powi(2)
1470 } else {
1471 40.0 + 240.0 - 54.0 + 22.0 * (t - 8.0) + 0.2 * (t - 8.0).powi(3)
1472 }
1473 };
1474 let flight = tests_support::simulated_of(20.0, |t| (0.0, east(t), 0.0));
1475 for omega in [0.25, 0.6, 1.0] {
1476 let mut lag = crate::synthetic::noise::Lag::new(omega, 0.0);
1477 lag.step(0.0, 0.0, 0.0);
1478 let mut expected = Vec::new();
1479 let times: Vec<f64> = (1..=19).map(f64::from).collect();
1480 let mut now = 0.0_f64;
1481 for &t in × {
1482 while now < t - 1e-9 {
1483 let next = (now + 0.01).min(t);
1484 let rate = (east(next) - east(now)) / (next - now);
1485 lag.step(next - now, east(next), rate);
1486 now = next;
1487 }
1488 expected.push(lag.output());
1489 }
1490 let got = flight.lagged_drift_m(omega, std::f64::consts::FRAC_1_SQRT_2, ×);
1491 for ((t, want), (have, _)) in times.iter().zip(&expected).zip(&got) {
1492 assert!(
1493 (want - have).abs() < 1e-3,
1494 "ωn {omega}, {t} s: {want} and {have}"
1495 );
1496 }
1497 }
1498 }
1499}