1use hpr_core::random::SeededRng;
23
24pub fn between(rng: &mut SeededRng, low: f64, high: f64) -> f64 {
26 low + (high - low) * rng.uniform()
27}
28
29pub fn ornstein_uhlenbeck(
36 rng: &mut SeededRng,
37 times_s: &[f64],
38 sigma: f64,
39 tau_s: f64,
40) -> Vec<f64> {
41 let mut out = Vec::with_capacity(times_s.len());
42 let mut previous: Option<(f64, f64)> = None;
43 for &t in times_s {
44 let x = match previous {
45 None => sigma * rng.standard_normal(),
46 Some((t0, x0)) => {
47 let phi = (-(t - t0) / tau_s).exp();
48 phi * x0 + sigma * (1.0 - phi * phi).sqrt() * rng.standard_normal()
49 }
50 };
51 out.push(x);
52 previous = Some((t, x));
53 }
54 out
55}
56
57pub fn tau_for(phi: f64, rate_hz: f64) -> f64 {
59 -1.0 / (rate_hz * phi.ln())
60}
61
62pub fn white(rng: &mut SeededRng, count: usize, sigma: f64) -> Vec<f64> {
64 (0..count).map(|_| sigma * rng.standard_normal()).collect()
65}
66
67pub fn quantize(value: f64, step: f64) -> f64 {
69 if step > 0.0 {
70 (value / step).round() * step
71 } else {
72 value
73 }
74}
75
76const PORT_CALIBERS: [f64; 6] = [0.0, 1.0, 2.0, 3.0, 4.0, 6.0];
79const PORT_MACH: [f64; 5] = [0.7, 0.9, 1.0, 1.1, 1.2];
81const PORT_CP: [[f64; 5]; 6] = [
85 [-0.07, -0.08, -0.20, -0.14, -0.11],
86 [-0.02, -0.02, -0.08, -0.07, -0.04],
87 [-0.01, -0.01, -0.06, -0.05, -0.01],
88 [-0.01, -0.01, 0.03, -0.02, -0.01],
89 [-0.01, -0.01, 0.02, f64::NAN, -0.02],
90 [0.00, -0.01, 0.00, f64::NAN, -0.01],
91];
92
93fn bracket(grid: &[f64], x: f64) -> (usize, f64) {
96 let last = grid.len() - 1;
97 if x <= grid[0] {
98 return (0, 0.0);
99 }
100 if x >= grid[last] {
101 return (last - 1, 1.0);
102 }
103 let i = grid
104 .iter()
105 .rposition(|&g| g <= x)
106 .unwrap_or(0)
107 .min(last - 1);
108 (i, (x - grid[i]) / (grid[i + 1] - grid[i]))
109}
110
111pub fn port_pressure_coefficient(calibers: f64, mach: f64) -> f64 {
115 let (row, fr) = bracket(&PORT_CALIBERS, calibers);
116 let (col, fc) = bracket(&PORT_MACH, mach);
117 let at = |r: usize, c: usize| -> f64 {
118 let cp = PORT_CP[r][c];
119 if cp.is_nan() {
120 0.5 * (PORT_CP[r][c - 1] + PORT_CP[r][c + 1])
122 } else {
123 cp
124 }
125 };
126 let lower = at(row, col) + fc * (at(row, col + 1) - at(row, col));
127 let upper = at(row + 1, col) + fc * (at(row + 1, col + 1) - at(row + 1, col));
128 lower + fr * (upper - lower)
129}
130
131pub fn port_error_pa(calibers: f64, mach: f64, dynamic_pressure_pa: f64) -> f64 {
133 port_pressure_coefficient(calibers, mach) * dynamic_pressure_pa
134}
135
136pub const PULSE_M: [f64; 7] = [-8.0, 1.0, 16.0, 15.0, 2.0, -3.0, -3.0];
139pub const PULSE_STEP_S: f64 = 0.05;
141
142pub fn pulse_m(since_s: f64, scale: f64) -> f64 {
145 if !(0.0..=PULSE_STEP_S * (PULSE_M.len() - 1) as f64).contains(&since_s) {
146 return 0.0;
147 }
148 let x = since_s / PULSE_STEP_S;
149 let i = (x.floor() as usize).min(PULSE_M.len() - 2);
150 let f = x - i as f64;
151 scale * (PULSE_M[i] + f * (PULSE_M[i + 1] - PULSE_M[i]))
152}
153
154#[derive(Debug, Clone, Copy)]
163pub struct Lag {
164 omega_rad_s: f64,
165 zeta: f64,
166 error: f64,
167 error_rate: f64,
168 output: f64,
169 rate: Option<f64>,
170}
171
172pub const LAG_DAMPING: f64 = std::f64::consts::FRAC_1_SQRT_2;
174
175impl Lag {
176 pub fn new(omega_rad_s: f64, p: f64) -> Self {
178 Self {
179 omega_rad_s,
180 zeta: LAG_DAMPING,
181 error: 0.0,
182 error_rate: 0.0,
183 output: p,
184 rate: None,
185 }
186 }
187
188 pub fn output(&self) -> f64 {
190 self.output
191 }
192
193 pub fn step(&mut self, dt_s: f64, p: f64, p_rate: f64) -> f64 {
197 if dt_s > 0.0 {
198 let accel = self.rate.map_or(0.0, |previous| (p_rate - previous) / dt_s);
199 let w = self.omega_rad_s;
200 let e_dot_dot = accel - w * w * self.error - 2.0 * self.zeta * w * self.error_rate;
201 self.error_rate += e_dot_dot * dt_s;
202 self.error += self.error_rate * dt_s;
203 }
204 self.rate = Some(p_rate);
205 self.output = p - self.error;
206 self.output
207 }
208}
209
210#[cfg(test)]
211mod tests {
212 use super::*;
213
214 const SEED: u64 = 7_202;
215
216 fn mean(x: &[f64]) -> f64 {
217 x.iter().sum::<f64>() / x.len() as f64
218 }
219
220 fn sd(x: &[f64]) -> f64 {
221 let m = mean(x);
222 (x.iter().map(|v| (v - m) * (v - m)).sum::<f64>() / (x.len() - 1) as f64).sqrt()
223 }
224
225 fn lag_one(x: &[f64]) -> f64 {
226 let m = mean(x);
227 let num: f64 = x.windows(2).map(|w| (w[0] - m) * (w[1] - m)).sum();
228 let den: f64 = x.iter().map(|v| (v - m) * (v - m)).sum();
229 num / den
230 }
231
232 fn holds_ar1(x: &[f64], sigma: f64, phi: f64) -> bool {
235 let n = x.len() as f64;
236 let sd_error = sigma * ((1.0 + phi * phi) / (2.0 * n * (1.0 - phi * phi))).sqrt();
239 let phi_error = ((1.0 - phi * phi) / n).sqrt();
240 (sd(x) - sigma).abs() < 5.0 * sd_error && (lag_one(x) - phi).abs() < 5.0 * phi_error
241 }
242
243 #[test]
244 fn the_correlated_noise_holds_its_spread_and_correlation_and_white_noise_fails() {
245 let rate = 50.0;
246 let times: Vec<f64> = (0..200_000).map(|k| k as f64 / rate).collect();
247 let tau = tau_for(0.5, rate);
248 let mut rng = SeededRng::for_stream(SEED, &[1]);
249 let x = ornstein_uhlenbeck(&mut rng, ×, 0.5, tau);
250 assert!(holds_ar1(&x, 0.5, 0.5), "sd {} phi {}", sd(&x), lag_one(&x));
251 let mut rng = SeededRng::for_stream(SEED, &[1]);
254 let w = white(&mut rng, times.len(), 0.5);
255 assert!(!holds_ar1(&w, 0.5, 0.5));
256 let wide: Vec<f64> = x.iter().map(|v| 1.2 * v).collect();
257 assert!(!holds_ar1(&wide, 0.5, 0.5));
258 }
259
260 #[test]
261 fn the_correlation_follows_the_spacing_when_the_rate_changes() {
262 let tau = tau_for(0.5, 50.0);
265 let fast: Vec<f64> = (0..100_000).map(|k| k as f64 / 100.0).collect();
266 let slow: Vec<f64> = (0..100_000).map(|k| 1_000.0 + k as f64 / 10.0).collect();
267 let times: Vec<f64> = fast.iter().chain(&slow).copied().collect();
268 let mut rng = SeededRng::for_stream(SEED, &[2]);
269 let x = ornstein_uhlenbeck(&mut rng, ×, 1.0, tau);
270 assert!(holds_ar1(&x[..100_000], 1.0, (-0.01 / tau).exp()));
271 assert!(holds_ar1(&x[100_000..], 1.0, (-0.1 / tau).exp()));
272 }
273
274 #[test]
275 fn quantizing_rounds_to_the_step_and_keeps_a_zero_step() {
276 assert_eq!(quantize(101_325.4, 1.0), 101_325.0);
277 assert_eq!(quantize(101_326.0, 10.0), 101_330.0);
278 assert_eq!(quantize(0.3048 * 2.6, 0.3048), 0.3048 * 3.0);
279 assert_eq!(quantize(1.234, 0.0), 1.234);
280 }
281
282 #[test]
283 fn the_port_reads_the_tunnels_pressure_coefficients_at_its_measured_points() {
284 for (r, &cal) in PORT_CALIBERS.iter().enumerate() {
285 for (c, &mach) in PORT_MACH.iter().enumerate() {
286 let cp = PORT_CP[r][c];
287 let read = port_pressure_coefficient(cal, mach);
288 if cp.is_nan() {
289 let across = 0.5 * (PORT_CP[r][c - 1] + PORT_CP[r][c + 1]);
290 assert!((read - across).abs() < 1e-15);
291 } else {
292 assert!((read - cp).abs() < 1e-15, "{cal} cal, Mach {mach}");
293 }
294 }
295 }
296 let mid = port_pressure_coefficient(1.5, 0.95);
298 let expect = 0.25 * (-0.02 - 0.08 - 0.01 - 0.06);
299 assert!((mid - expect).abs() < 1e-15);
300 assert_eq!(port_pressure_coefficient(1.0, 0.3), -0.02);
301 assert_eq!(port_pressure_coefficient(1.0, 2.0), -0.04);
302 let error = port_error_pa(1.0, 1.0, 40_000.0);
305 assert!((error + 3_200.0).abs() < 1e-9);
306 assert!(error < 0.0);
307 }
308
309 #[test]
310 fn the_pulse_has_the_pnut_shape_scaled_and_nothing_outside_it() {
311 for (k, &h) in PULSE_M.iter().enumerate() {
312 let t = k as f64 * PULSE_STEP_S;
313 assert!((pulse_m(t, 1.0) - h).abs() < 1e-12);
314 assert!((pulse_m(t, 1.5) - 1.5 * h).abs() < 1e-12);
315 }
316 assert_eq!(pulse_m(-0.01, 1.0), 0.0);
317 assert_eq!(pulse_m(0.31, 1.0), 0.0);
318 assert!((pulse_m(0.125, 1.0) - 15.5).abs() < 1e-12);
319 }
320
321 #[test]
322 fn the_gnss_loop_settles_a_over_omega_squared_behind_a_constant_acceleration() {
323 let a = 100.0;
324 let dt = 0.01;
325 for omega in [0.25, 0.5, 1.0] {
326 let mut lag = Lag::new(omega, 0.0);
327 lag.step(0.0, 0.0, 0.0);
328 let mut out = 0.0;
329 let mut p = 0.0;
330 for k in 1..=(60.0 / omega / dt) as usize {
331 let t = k as f64 * dt;
332 p = 0.5 * a * t * t;
333 out = lag.step(dt, p, a * t);
334 }
335 let behind = p - out;
336 assert!(
337 (behind - a / (omega * omega)).abs() < 1e-6 * a / (omega * omega),
338 "ωn {omega}: {behind}"
339 );
340 }
341 let mut fast = Lag::new(0.5, 0.0);
345 fast.step(0.0, 0.0, 0.0);
346 let (mut p, mut out) = (0.0, 0.0);
347 for k in 1..=12_000 {
348 let t = k as f64 * dt;
349 p = 0.5 * a * t * t;
350 out = fast.step(dt, p, a * t);
351 }
352 assert!((p - out - a / 0.25).abs() < 1e-6 * a / 0.25);
353 assert!((p - out - a / 0.0625).abs() > 0.5 * a / 0.0625);
354 }
355
356 #[test]
357 fn a_draw_between_bounds_stays_within_and_fills_them() {
358 let mut rng = SeededRng::for_stream(SEED, &[3]);
359 let x: Vec<f64> = (0..10_000)
360 .map(|_| between(&mut rng, -100.0, 100.0))
361 .collect();
362 assert!(x.iter().all(|v| (-100.0..100.0).contains(v)));
363 let lo = x.iter().copied().fold(f64::INFINITY, f64::min);
364 let hi = x.iter().copied().fold(f64::NEG_INFINITY, f64::max);
365 assert!(lo < -99.0 && hi > 99.0);
366 assert!((sd(&x) - 200.0 / 12_f64.sqrt()).abs() < 2.0);
368 }
369}