1mod first_scenario;
2mod full_bucket;
3mod percentile_bucket;
4mod sampled_bucket;
5mod variance_triggered;
6
7use std::time::Instant;
8
9use nalgebra::{Matrix2, Matrix4, Vector2, Vector4};
10use rand::{Rng, SeedableRng};
11use rand_distr::{Distribution, Normal};
12use rust_robotics_core::{
13 annotate_against_reference as annotate_reference_reports, average_coverage_ratio,
14 read_source_metrics, ExperimentObservation, ExperimentSamplingPlan, ExperimentVariantReport,
15 ExtensibilityMetrics, VariantDescriptor,
16};
17
18use crate::{
19 cubature_kalman_filter::{CKFConfig, CKFLocalizer},
20 unscented_kalman_filter::{UKFConfig, UKFLocalizer, UKFParams},
21};
22
23pub use first_scenario::FirstScenarioAccuracyAggregation;
24pub use full_bucket::FullBucketAccuracyAggregation;
25pub use percentile_bucket::PercentileBucketAccuracyAggregation;
26pub use sampled_bucket::SampledBucketAccuracyAggregation;
27pub use variance_triggered::VarianceTriggeredAccuracyAggregation;
28
29const DT: f64 = 0.1;
30
31pub type AccuracySamplingPlan = ExperimentSamplingPlan;
32
33pub trait AccuracyAggregationVariant {
34 fn descriptor(&self) -> VariantDescriptor;
35 fn selected_slots(&self, total_scenarios: usize) -> Vec<usize>;
36
37 fn sampling_plan(&self, total_scenarios: usize) -> AccuracySamplingPlan {
38 AccuracySamplingPlan::static_slots(self.selected_slots(total_scenarios))
39 }
40}
41
42#[derive(Debug, Clone, Copy)]
43struct MotionProfile {
44 velocity: f64,
45 yaw_rate: f64,
46 true_velocity_scale: f64,
47 true_velocity_wave: f64,
48 true_yaw_rate_scale: f64,
49 true_yaw_wave_deg: f64,
50 command_velocity_wave: f64,
51 command_yaw_wave_deg: f64,
52 control_latency_steps: usize,
53 actuator_velocity_limit: f64,
54 actuator_yaw_limit_deg: f64,
55 process_noise_longitudinal: f64,
56 process_noise_lateral: f64,
57 process_noise_yaw_deg: f64,
58 control_noise_v: f64,
59 control_noise_yaw_deg: f64,
60 control_bias_v: f64,
61 control_bias_yaw_deg: f64,
62 obs_noise_x: f64,
63 obs_noise_y: f64,
64 observation_refresh_interval: usize,
65 observation_hold_probability: f64,
66 observation_outlier_probability: f64,
67 observation_outlier_scale: f64,
68 observation_outlier_burst_len: usize,
69 observation_bias_burst_interval: usize,
70}
71
72#[derive(Debug, Clone, Copy)]
73pub struct AccuracyExperimentCase {
74 pub family_name: &'static str,
75 seed_offset: u64,
76 profile: MotionProfile,
77 pub buckets: &'static [u32],
78}
79
80#[derive(Debug, Clone)]
81pub struct AccuracyObservation {
82 pub family_name: &'static str,
83 pub bucket: u32,
84 pub total_scenarios: usize,
85 pub initial_slots: Vec<usize>,
86 pub selected_slots: Vec<usize>,
87 pub escalated: bool,
88 pub ukf_bucket_median_rmse: f64,
89 pub ckf_bucket_median_rmse: f64,
90 pub ukf_min_rmse: f64,
91 pub ukf_max_rmse: f64,
92 pub ckf_min_rmse: f64,
93 pub ckf_max_rmse: f64,
94 pub ckf_wins: usize,
95}
96
97impl AccuracyObservation {
98 pub fn ukf_over_ckf(&self) -> f64 {
99 self.ukf_bucket_median_rmse / self.ckf_bucket_median_rmse.max(1e-9)
100 }
101
102 pub fn winner(&self) -> &'static str {
103 if self.ukf_over_ckf() > 1.0 {
104 "CKF"
105 } else {
106 "UKF"
107 }
108 }
109
110 pub fn coverage_ratio(&self) -> f64 {
111 self.selected_slots.len() as f64 / self.total_scenarios as f64
112 }
113}
114
115impl ExperimentObservation for AccuracyObservation {
116 type Key = (&'static str, u32);
117
118 fn comparison_key(&self) -> Self::Key {
119 (self.family_name, self.bucket)
120 }
121
122 fn winner_label(&self) -> &'static str {
123 AccuracyObservation::winner(self)
124 }
125
126 fn ratio_value(&self) -> f64 {
127 AccuracyObservation::ukf_over_ckf(self)
128 }
129
130 fn coverage_ratio(&self) -> f64 {
131 AccuracyObservation::coverage_ratio(self)
132 }
133}
134
135pub type AccuracyVariantReport = ExperimentVariantReport<AccuracyObservation>;
136
137#[derive(Debug, Clone, Copy)]
138pub struct AccuracyEvaluationConfig {
139 pub scenarios_per_bucket: usize,
140}
141
142impl Default for AccuracyEvaluationConfig {
143 fn default() -> Self {
144 Self {
145 scenarios_per_bucket: 10,
146 }
147 }
148}
149
150pub fn localization_noise_process_problem() -> Vec<AccuracyExperimentCase> {
151 vec![
152 AccuracyExperimentCase {
153 family_name: "balanced-circle",
154 seed_offset: 10_000,
155 profile: MotionProfile {
156 velocity: 1.0,
157 yaw_rate: 0.10,
158 true_velocity_scale: 1.0,
159 true_velocity_wave: 0.0,
160 true_yaw_rate_scale: 1.0,
161 true_yaw_wave_deg: 0.0,
162 command_velocity_wave: 0.0,
163 command_yaw_wave_deg: 0.0,
164 control_latency_steps: 0,
165 actuator_velocity_limit: 0.0,
166 actuator_yaw_limit_deg: 0.0,
167 process_noise_longitudinal: 0.0,
168 process_noise_lateral: 0.0,
169 process_noise_yaw_deg: 0.0,
170 control_noise_v: 0.25,
171 control_noise_yaw_deg: 4.0,
172 control_bias_v: 0.0,
173 control_bias_yaw_deg: 0.0,
174 obs_noise_x: 0.35,
175 obs_noise_y: 0.35,
176 observation_refresh_interval: 1,
177 observation_hold_probability: 0.0,
178 observation_outlier_probability: 0.0,
179 observation_outlier_scale: 0.0,
180 observation_outlier_burst_len: 0,
181 observation_bias_burst_interval: 0,
182 },
183 buckets: &[60, 100, 140],
184 },
185 AccuracyExperimentCase {
186 family_name: "fast-turn",
187 seed_offset: 20_000,
188 profile: MotionProfile {
189 velocity: 1.3,
190 yaw_rate: 0.22,
191 true_velocity_scale: 1.0,
192 true_velocity_wave: 0.0,
193 true_yaw_rate_scale: 1.0,
194 true_yaw_wave_deg: 0.0,
195 command_velocity_wave: 0.0,
196 command_yaw_wave_deg: 0.0,
197 control_latency_steps: 0,
198 actuator_velocity_limit: 0.0,
199 actuator_yaw_limit_deg: 0.0,
200 process_noise_longitudinal: 0.0,
201 process_noise_lateral: 0.0,
202 process_noise_yaw_deg: 0.0,
203 control_noise_v: 0.30,
204 control_noise_yaw_deg: 6.0,
205 control_bias_v: 0.0,
206 control_bias_yaw_deg: 0.0,
207 obs_noise_x: 0.40,
208 obs_noise_y: 0.40,
209 observation_refresh_interval: 1,
210 observation_hold_probability: 0.0,
211 observation_outlier_probability: 0.0,
212 observation_outlier_scale: 0.0,
213 observation_outlier_burst_len: 0,
214 observation_bias_burst_interval: 0,
215 },
216 buckets: &[80, 120, 160],
217 },
218 AccuracyExperimentCase {
219 family_name: "gps-stress",
220 seed_offset: 30_000,
221 profile: MotionProfile {
222 velocity: 0.9,
223 yaw_rate: 0.14,
224 true_velocity_scale: 1.0,
225 true_velocity_wave: 0.0,
226 true_yaw_rate_scale: 1.0,
227 true_yaw_wave_deg: 0.0,
228 command_velocity_wave: 0.0,
229 command_yaw_wave_deg: 0.0,
230 control_latency_steps: 0,
231 actuator_velocity_limit: 0.0,
232 actuator_yaw_limit_deg: 0.0,
233 process_noise_longitudinal: 0.0,
234 process_noise_lateral: 0.0,
235 process_noise_yaw_deg: 0.0,
236 control_noise_v: 0.20,
237 control_noise_yaw_deg: 5.0,
238 control_bias_v: 0.0,
239 control_bias_yaw_deg: 0.0,
240 obs_noise_x: 0.30,
241 obs_noise_y: 0.75,
242 observation_refresh_interval: 1,
243 observation_hold_probability: 0.0,
244 observation_outlier_probability: 0.0,
245 observation_outlier_scale: 0.0,
246 observation_outlier_burst_len: 0,
247 observation_bias_burst_interval: 0,
248 },
249 buckets: &[80, 120, 160],
250 },
251 ]
252}
253
254pub fn localization_long_horizon_process_problem() -> Vec<AccuracyExperimentCase> {
255 vec![
256 AccuracyExperimentCase {
257 family_name: "long-horizon-turn",
258 seed_offset: 40_000,
259 profile: MotionProfile {
260 velocity: 1.1,
261 yaw_rate: 0.18,
262 true_velocity_scale: 1.0,
263 true_velocity_wave: 0.0,
264 true_yaw_rate_scale: 1.0,
265 true_yaw_wave_deg: 0.0,
266 command_velocity_wave: 0.0,
267 command_yaw_wave_deg: 0.0,
268 control_latency_steps: 0,
269 actuator_velocity_limit: 0.0,
270 actuator_yaw_limit_deg: 0.0,
271 process_noise_longitudinal: 0.0,
272 process_noise_lateral: 0.0,
273 process_noise_yaw_deg: 0.0,
274 control_noise_v: 0.35,
275 control_noise_yaw_deg: 8.0,
276 control_bias_v: 0.0,
277 control_bias_yaw_deg: 0.0,
278 obs_noise_x: 0.45,
279 obs_noise_y: 0.45,
280 observation_refresh_interval: 1,
281 observation_hold_probability: 0.0,
282 observation_outlier_probability: 0.0,
283 observation_outlier_scale: 0.0,
284 observation_outlier_burst_len: 0,
285 observation_bias_burst_interval: 0,
286 },
287 buckets: &[180, 240, 320],
288 },
289 AccuracyExperimentCase {
290 family_name: "high-curvature",
291 seed_offset: 50_000,
292 profile: MotionProfile {
293 velocity: 0.8,
294 yaw_rate: 0.32,
295 true_velocity_scale: 1.0,
296 true_velocity_wave: 0.0,
297 true_yaw_rate_scale: 1.0,
298 true_yaw_wave_deg: 0.0,
299 command_velocity_wave: 0.0,
300 command_yaw_wave_deg: 0.0,
301 control_latency_steps: 0,
302 actuator_velocity_limit: 0.0,
303 actuator_yaw_limit_deg: 0.0,
304 process_noise_longitudinal: 0.0,
305 process_noise_lateral: 0.0,
306 process_noise_yaw_deg: 0.0,
307 control_noise_v: 0.28,
308 control_noise_yaw_deg: 9.0,
309 control_bias_v: 0.0,
310 control_bias_yaw_deg: 0.0,
311 obs_noise_x: 0.40,
312 obs_noise_y: 0.40,
313 observation_refresh_interval: 1,
314 observation_hold_probability: 0.0,
315 observation_outlier_probability: 0.0,
316 observation_outlier_scale: 0.0,
317 observation_outlier_burst_len: 0,
318 observation_bias_burst_interval: 0,
319 },
320 buckets: &[140, 220, 300],
321 },
322 AccuracyExperimentCase {
323 family_name: "anisotropic-gps",
324 seed_offset: 60_000,
325 profile: MotionProfile {
326 velocity: 1.0,
327 yaw_rate: 0.12,
328 true_velocity_scale: 1.0,
329 true_velocity_wave: 0.0,
330 true_yaw_rate_scale: 1.0,
331 true_yaw_wave_deg: 0.0,
332 command_velocity_wave: 0.0,
333 command_yaw_wave_deg: 0.0,
334 control_latency_steps: 0,
335 actuator_velocity_limit: 0.0,
336 actuator_yaw_limit_deg: 0.0,
337 process_noise_longitudinal: 0.0,
338 process_noise_lateral: 0.0,
339 process_noise_yaw_deg: 0.0,
340 control_noise_v: 0.22,
341 control_noise_yaw_deg: 5.5,
342 control_bias_v: 0.0,
343 control_bias_yaw_deg: 0.0,
344 obs_noise_x: 0.25,
345 obs_noise_y: 1.10,
346 observation_refresh_interval: 1,
347 observation_hold_probability: 0.0,
348 observation_outlier_probability: 0.0,
349 observation_outlier_scale: 0.0,
350 observation_outlier_burst_len: 0,
351 observation_bias_burst_interval: 0,
352 },
353 buckets: &[160, 240, 320],
354 },
355 ]
356}
357
358pub fn localization_dropout_bias_process_problem() -> Vec<AccuracyExperimentCase> {
359 vec![
360 AccuracyExperimentCase {
361 family_name: "dropout-lag",
362 seed_offset: 70_000,
363 profile: MotionProfile {
364 velocity: 1.0,
365 yaw_rate: 0.16,
366 true_velocity_scale: 1.0,
367 true_velocity_wave: 0.0,
368 true_yaw_rate_scale: 1.0,
369 true_yaw_wave_deg: 0.0,
370 command_velocity_wave: 0.0,
371 command_yaw_wave_deg: 0.0,
372 control_latency_steps: 0,
373 actuator_velocity_limit: 0.0,
374 actuator_yaw_limit_deg: 0.0,
375 process_noise_longitudinal: 0.0,
376 process_noise_lateral: 0.0,
377 process_noise_yaw_deg: 0.0,
378 control_noise_v: 0.24,
379 control_noise_yaw_deg: 5.0,
380 control_bias_v: 0.0,
381 control_bias_yaw_deg: 0.0,
382 obs_noise_x: 0.35,
383 obs_noise_y: 0.35,
384 observation_refresh_interval: 1,
385 observation_hold_probability: 0.18,
386 observation_outlier_probability: 0.0,
387 observation_outlier_scale: 0.0,
388 observation_outlier_burst_len: 0,
389 observation_bias_burst_interval: 0,
390 },
391 buckets: &[100, 180, 260],
392 },
393 AccuracyExperimentCase {
394 family_name: "biased-odometry",
395 seed_offset: 80_000,
396 profile: MotionProfile {
397 velocity: 1.1,
398 yaw_rate: 0.14,
399 true_velocity_scale: 1.0,
400 true_velocity_wave: 0.0,
401 true_yaw_rate_scale: 1.0,
402 true_yaw_wave_deg: 0.0,
403 command_velocity_wave: 0.0,
404 command_yaw_wave_deg: 0.0,
405 control_latency_steps: 0,
406 actuator_velocity_limit: 0.0,
407 actuator_yaw_limit_deg: 0.0,
408 process_noise_longitudinal: 0.0,
409 process_noise_lateral: 0.0,
410 process_noise_yaw_deg: 0.0,
411 control_noise_v: 0.20,
412 control_noise_yaw_deg: 4.5,
413 control_bias_v: 0.08,
414 control_bias_yaw_deg: 1.8,
415 obs_noise_x: 0.32,
416 obs_noise_y: 0.32,
417 observation_refresh_interval: 1,
418 observation_hold_probability: 0.0,
419 observation_outlier_probability: 0.0,
420 observation_outlier_scale: 0.0,
421 observation_outlier_burst_len: 0,
422 observation_bias_burst_interval: 0,
423 },
424 buckets: &[120, 200, 280],
425 },
426 AccuracyExperimentCase {
427 family_name: "dropout-bias-mix",
428 seed_offset: 90_000,
429 profile: MotionProfile {
430 velocity: 0.95,
431 yaw_rate: 0.24,
432 true_velocity_scale: 1.0,
433 true_velocity_wave: 0.0,
434 true_yaw_rate_scale: 1.0,
435 true_yaw_wave_deg: 0.0,
436 command_velocity_wave: 0.0,
437 command_yaw_wave_deg: 0.0,
438 control_latency_steps: 0,
439 actuator_velocity_limit: 0.0,
440 actuator_yaw_limit_deg: 0.0,
441 process_noise_longitudinal: 0.0,
442 process_noise_lateral: 0.0,
443 process_noise_yaw_deg: 0.0,
444 control_noise_v: 0.26,
445 control_noise_yaw_deg: 6.5,
446 control_bias_v: -0.05,
447 control_bias_yaw_deg: 2.4,
448 obs_noise_x: 0.30,
449 obs_noise_y: 0.85,
450 observation_refresh_interval: 1,
451 observation_hold_probability: 0.22,
452 observation_outlier_probability: 0.0,
453 observation_outlier_scale: 0.0,
454 observation_outlier_burst_len: 0,
455 observation_bias_burst_interval: 0,
456 },
457 buckets: &[120, 200, 280],
458 },
459 ]
460}
461
462pub fn localization_outlier_burst_process_problem() -> Vec<AccuracyExperimentCase> {
463 vec![
464 AccuracyExperimentCase {
465 family_name: "sparse-outlier-burst",
466 seed_offset: 100_000,
467 profile: MotionProfile {
468 velocity: 1.0,
469 yaw_rate: 0.15,
470 true_velocity_scale: 1.0,
471 true_velocity_wave: 0.0,
472 true_yaw_rate_scale: 1.0,
473 true_yaw_wave_deg: 0.0,
474 command_velocity_wave: 0.0,
475 command_yaw_wave_deg: 0.0,
476 control_latency_steps: 0,
477 actuator_velocity_limit: 0.0,
478 actuator_yaw_limit_deg: 0.0,
479 process_noise_longitudinal: 0.0,
480 process_noise_lateral: 0.0,
481 process_noise_yaw_deg: 0.0,
482 control_noise_v: 0.20,
483 control_noise_yaw_deg: 4.5,
484 control_bias_v: 0.0,
485 control_bias_yaw_deg: 0.0,
486 obs_noise_x: 0.30,
487 obs_noise_y: 0.30,
488 observation_refresh_interval: 1,
489 observation_hold_probability: 0.0,
490 observation_outlier_probability: 0.05,
491 observation_outlier_scale: 6.0,
492 observation_outlier_burst_len: 3,
493 observation_bias_burst_interval: 0,
494 },
495 buckets: &[120, 200, 280],
496 },
497 AccuracyExperimentCase {
498 family_name: "anisotropic-outlier-burst",
499 seed_offset: 110_000,
500 profile: MotionProfile {
501 velocity: 0.95,
502 yaw_rate: 0.20,
503 true_velocity_scale: 1.0,
504 true_velocity_wave: 0.0,
505 true_yaw_rate_scale: 1.0,
506 true_yaw_wave_deg: 0.0,
507 command_velocity_wave: 0.0,
508 command_yaw_wave_deg: 0.0,
509 control_latency_steps: 0,
510 actuator_velocity_limit: 0.0,
511 actuator_yaw_limit_deg: 0.0,
512 process_noise_longitudinal: 0.0,
513 process_noise_lateral: 0.0,
514 process_noise_yaw_deg: 0.0,
515 control_noise_v: 0.24,
516 control_noise_yaw_deg: 5.5,
517 control_bias_v: 0.0,
518 control_bias_yaw_deg: 0.0,
519 obs_noise_x: 0.25,
520 obs_noise_y: 0.95,
521 observation_refresh_interval: 1,
522 observation_hold_probability: 0.0,
523 observation_outlier_probability: 0.06,
524 observation_outlier_scale: 7.5,
525 observation_outlier_burst_len: 4,
526 observation_bias_burst_interval: 0,
527 },
528 buckets: &[120, 220, 320],
529 },
530 AccuracyExperimentCase {
531 family_name: "burst-plus-bias",
532 seed_offset: 120_000,
533 profile: MotionProfile {
534 velocity: 1.1,
535 yaw_rate: 0.11,
536 true_velocity_scale: 1.0,
537 true_velocity_wave: 0.0,
538 true_yaw_rate_scale: 1.0,
539 true_yaw_wave_deg: 0.0,
540 command_velocity_wave: 0.0,
541 command_yaw_wave_deg: 0.0,
542 control_latency_steps: 0,
543 actuator_velocity_limit: 0.0,
544 actuator_yaw_limit_deg: 0.0,
545 process_noise_longitudinal: 0.0,
546 process_noise_lateral: 0.0,
547 process_noise_yaw_deg: 0.0,
548 control_noise_v: 0.22,
549 control_noise_yaw_deg: 4.0,
550 control_bias_v: 0.06,
551 control_bias_yaw_deg: 1.4,
552 obs_noise_x: 0.28,
553 obs_noise_y: 0.28,
554 observation_refresh_interval: 1,
555 observation_hold_probability: 0.0,
556 observation_outlier_probability: 0.04,
557 observation_outlier_scale: 5.5,
558 observation_outlier_burst_len: 3,
559 observation_bias_burst_interval: 0,
560 },
561 buckets: &[140, 220, 300],
562 },
563 ]
564}
565
566pub fn localization_process_mismatch_process_problem() -> Vec<AccuracyExperimentCase> {
567 vec![
568 AccuracyExperimentCase {
569 family_name: "slip-understeer",
570 seed_offset: 130_000,
571 profile: MotionProfile {
572 velocity: 1.1,
573 yaw_rate: 0.15,
574 true_velocity_scale: 0.88,
575 true_velocity_wave: 0.10,
576 true_yaw_rate_scale: 0.78,
577 true_yaw_wave_deg: 4.0,
578 command_velocity_wave: 0.0,
579 command_yaw_wave_deg: 0.0,
580 control_latency_steps: 0,
581 actuator_velocity_limit: 0.0,
582 actuator_yaw_limit_deg: 0.0,
583 process_noise_longitudinal: 0.0,
584 process_noise_lateral: 0.0,
585 process_noise_yaw_deg: 0.0,
586 control_noise_v: 0.22,
587 control_noise_yaw_deg: 4.5,
588 control_bias_v: 0.0,
589 control_bias_yaw_deg: 0.0,
590 obs_noise_x: 0.30,
591 obs_noise_y: 0.30,
592 observation_refresh_interval: 1,
593 observation_hold_probability: 0.0,
594 observation_outlier_probability: 0.0,
595 observation_outlier_scale: 0.0,
596 observation_outlier_burst_len: 0,
597 observation_bias_burst_interval: 0,
598 },
599 buckets: &[120, 200, 280],
600 },
601 AccuracyExperimentCase {
602 family_name: "actuator-scale-drift",
603 seed_offset: 140_000,
604 profile: MotionProfile {
605 velocity: 0.95,
606 yaw_rate: 0.22,
607 true_velocity_scale: 1.12,
608 true_velocity_wave: 0.14,
609 true_yaw_rate_scale: 1.22,
610 true_yaw_wave_deg: 6.5,
611 command_velocity_wave: 0.0,
612 command_yaw_wave_deg: 0.0,
613 control_latency_steps: 0,
614 actuator_velocity_limit: 0.0,
615 actuator_yaw_limit_deg: 0.0,
616 process_noise_longitudinal: 0.0,
617 process_noise_lateral: 0.0,
618 process_noise_yaw_deg: 0.0,
619 control_noise_v: 0.24,
620 control_noise_yaw_deg: 5.5,
621 control_bias_v: 0.0,
622 control_bias_yaw_deg: 0.0,
623 obs_noise_x: 0.28,
624 obs_noise_y: 0.55,
625 observation_refresh_interval: 1,
626 observation_hold_probability: 0.0,
627 observation_outlier_probability: 0.0,
628 observation_outlier_scale: 0.0,
629 observation_outlier_burst_len: 0,
630 observation_bias_burst_interval: 0,
631 },
632 buckets: &[140, 220, 320],
633 },
634 AccuracyExperimentCase {
635 family_name: "oscillatory-wheel-slip",
636 seed_offset: 150_000,
637 profile: MotionProfile {
638 velocity: 1.05,
639 yaw_rate: 0.18,
640 true_velocity_scale: 0.94,
641 true_velocity_wave: 0.20,
642 true_yaw_rate_scale: 1.08,
643 true_yaw_wave_deg: 8.0,
644 command_velocity_wave: 0.0,
645 command_yaw_wave_deg: 0.0,
646 control_latency_steps: 0,
647 actuator_velocity_limit: 0.0,
648 actuator_yaw_limit_deg: 0.0,
649 process_noise_longitudinal: 0.0,
650 process_noise_lateral: 0.0,
651 process_noise_yaw_deg: 0.0,
652 control_noise_v: 0.26,
653 control_noise_yaw_deg: 6.0,
654 control_bias_v: 0.0,
655 control_bias_yaw_deg: 0.0,
656 obs_noise_x: 0.32,
657 obs_noise_y: 0.32,
658 observation_refresh_interval: 1,
659 observation_hold_probability: 0.0,
660 observation_outlier_probability: 0.0,
661 observation_outlier_scale: 0.0,
662 observation_outlier_burst_len: 0,
663 observation_bias_burst_interval: 0,
664 },
665 buckets: &[140, 240, 340],
666 },
667 ]
668}
669
670pub fn localization_sensor_rate_mismatch_process_problem() -> Vec<AccuracyExperimentCase> {
671 vec![
672 AccuracyExperimentCase {
673 family_name: "slow-gps-3x",
674 seed_offset: 160_000,
675 profile: MotionProfile {
676 velocity: 1.0,
677 yaw_rate: 0.16,
678 true_velocity_scale: 1.0,
679 true_velocity_wave: 0.0,
680 true_yaw_rate_scale: 1.0,
681 true_yaw_wave_deg: 0.0,
682 command_velocity_wave: 0.14,
683 command_yaw_wave_deg: 4.0,
684 control_latency_steps: 2,
685 actuator_velocity_limit: 0.0,
686 actuator_yaw_limit_deg: 0.0,
687 process_noise_longitudinal: 0.0,
688 process_noise_lateral: 0.0,
689 process_noise_yaw_deg: 0.0,
690 control_noise_v: 0.22,
691 control_noise_yaw_deg: 4.5,
692 control_bias_v: 0.0,
693 control_bias_yaw_deg: 0.0,
694 obs_noise_x: 0.28,
695 obs_noise_y: 0.28,
696 observation_refresh_interval: 3,
697 observation_hold_probability: 0.0,
698 observation_outlier_probability: 0.0,
699 observation_outlier_scale: 0.0,
700 observation_outlier_burst_len: 0,
701 observation_bias_burst_interval: 0,
702 },
703 buckets: &[120, 200, 280],
704 },
705 AccuracyExperimentCase {
706 family_name: "anisotropic-gps-5x",
707 seed_offset: 170_000,
708 profile: MotionProfile {
709 velocity: 0.95,
710 yaw_rate: 0.20,
711 true_velocity_scale: 1.0,
712 true_velocity_wave: 0.0,
713 true_yaw_rate_scale: 1.0,
714 true_yaw_wave_deg: 0.0,
715 command_velocity_wave: 0.10,
716 command_yaw_wave_deg: 5.5,
717 control_latency_steps: 4,
718 actuator_velocity_limit: 0.0,
719 actuator_yaw_limit_deg: 0.0,
720 process_noise_longitudinal: 0.0,
721 process_noise_lateral: 0.0,
722 process_noise_yaw_deg: 0.0,
723 control_noise_v: 0.24,
724 control_noise_yaw_deg: 5.5,
725 control_bias_v: 0.0,
726 control_bias_yaw_deg: 0.0,
727 obs_noise_x: 0.24,
728 obs_noise_y: 0.95,
729 observation_refresh_interval: 5,
730 observation_hold_probability: 0.0,
731 observation_outlier_probability: 0.0,
732 observation_outlier_scale: 0.0,
733 observation_outlier_burst_len: 0,
734 observation_bias_burst_interval: 0,
735 },
736 buckets: &[140, 220, 320],
737 },
738 AccuracyExperimentCase {
739 family_name: "turning-camera-4x",
740 seed_offset: 180_000,
741 profile: MotionProfile {
742 velocity: 1.1,
743 yaw_rate: 0.26,
744 true_velocity_scale: 1.0,
745 true_velocity_wave: 0.0,
746 true_yaw_rate_scale: 1.0,
747 true_yaw_wave_deg: 0.0,
748 command_velocity_wave: 0.16,
749 command_yaw_wave_deg: 7.0,
750 control_latency_steps: 3,
751 actuator_velocity_limit: 0.0,
752 actuator_yaw_limit_deg: 0.0,
753 process_noise_longitudinal: 0.0,
754 process_noise_lateral: 0.0,
755 process_noise_yaw_deg: 0.0,
756 control_noise_v: 0.26,
757 control_noise_yaw_deg: 6.5,
758 control_bias_v: 0.0,
759 control_bias_yaw_deg: 0.0,
760 obs_noise_x: 0.35,
761 obs_noise_y: 0.35,
762 observation_refresh_interval: 4,
763 observation_hold_probability: 0.0,
764 observation_outlier_probability: 0.0,
765 observation_outlier_scale: 0.0,
766 observation_outlier_burst_len: 0,
767 observation_bias_burst_interval: 0,
768 },
769 buckets: &[140, 240, 340],
770 },
771 ]
772}
773
774pub fn localization_control_latency_process_problem() -> Vec<AccuracyExperimentCase> {
775 vec![
776 AccuracyExperimentCase {
777 family_name: "steering-lag-2",
778 seed_offset: 190_000,
779 profile: MotionProfile {
780 velocity: 1.0,
781 yaw_rate: 0.18,
782 true_velocity_scale: 1.0,
783 true_velocity_wave: 0.0,
784 true_yaw_rate_scale: 1.0,
785 true_yaw_wave_deg: 0.0,
786 command_velocity_wave: 0.12,
787 command_yaw_wave_deg: 5.0,
788 control_latency_steps: 2,
789 actuator_velocity_limit: 0.0,
790 actuator_yaw_limit_deg: 0.0,
791 process_noise_longitudinal: 0.0,
792 process_noise_lateral: 0.0,
793 process_noise_yaw_deg: 0.0,
794 control_noise_v: 0.22,
795 control_noise_yaw_deg: 4.5,
796 control_bias_v: 0.0,
797 control_bias_yaw_deg: 0.0,
798 obs_noise_x: 0.30,
799 obs_noise_y: 0.30,
800 observation_refresh_interval: 1,
801 observation_hold_probability: 0.0,
802 observation_outlier_probability: 0.0,
803 observation_outlier_scale: 0.0,
804 observation_outlier_burst_len: 0,
805 observation_bias_burst_interval: 0,
806 },
807 buckets: &[120, 200, 280],
808 },
809 AccuracyExperimentCase {
810 family_name: "drive-lag-4",
811 seed_offset: 200_000,
812 profile: MotionProfile {
813 velocity: 0.95,
814 yaw_rate: 0.22,
815 true_velocity_scale: 1.0,
816 true_velocity_wave: 0.0,
817 true_yaw_rate_scale: 1.0,
818 true_yaw_wave_deg: 0.0,
819 command_velocity_wave: 0.18,
820 command_yaw_wave_deg: 6.0,
821 control_latency_steps: 4,
822 actuator_velocity_limit: 0.0,
823 actuator_yaw_limit_deg: 0.0,
824 process_noise_longitudinal: 0.0,
825 process_noise_lateral: 0.0,
826 process_noise_yaw_deg: 0.0,
827 control_noise_v: 0.24,
828 control_noise_yaw_deg: 5.5,
829 control_bias_v: 0.0,
830 control_bias_yaw_deg: 0.0,
831 obs_noise_x: 0.28,
832 obs_noise_y: 0.55,
833 observation_refresh_interval: 1,
834 observation_hold_probability: 0.0,
835 observation_outlier_probability: 0.0,
836 observation_outlier_scale: 0.0,
837 observation_outlier_burst_len: 0,
838 observation_bias_burst_interval: 0,
839 },
840 buckets: &[140, 220, 320],
841 },
842 AccuracyExperimentCase {
843 family_name: "oscillatory-command-lag-3",
844 seed_offset: 210_000,
845 profile: MotionProfile {
846 velocity: 1.1,
847 yaw_rate: 0.26,
848 true_velocity_scale: 1.0,
849 true_velocity_wave: 0.0,
850 true_yaw_rate_scale: 1.0,
851 true_yaw_wave_deg: 0.0,
852 command_velocity_wave: 0.14,
853 command_yaw_wave_deg: 8.0,
854 control_latency_steps: 3,
855 actuator_velocity_limit: 0.0,
856 actuator_yaw_limit_deg: 0.0,
857 process_noise_longitudinal: 0.0,
858 process_noise_lateral: 0.0,
859 process_noise_yaw_deg: 0.0,
860 control_noise_v: 0.26,
861 control_noise_yaw_deg: 6.5,
862 control_bias_v: 0.0,
863 control_bias_yaw_deg: 0.0,
864 obs_noise_x: 0.32,
865 obs_noise_y: 0.32,
866 observation_refresh_interval: 1,
867 observation_hold_probability: 0.0,
868 observation_outlier_probability: 0.0,
869 observation_outlier_scale: 0.0,
870 observation_outlier_burst_len: 0,
871 observation_bias_burst_interval: 0,
872 },
873 buckets: &[140, 240, 340],
874 },
875 ]
876}
877
878pub fn localization_process_noise_anisotropy_process_problem() -> Vec<AccuracyExperimentCase> {
879 vec![
880 AccuracyExperimentCase {
881 family_name: "crosswind-corridor",
882 seed_offset: 220_000,
883 profile: MotionProfile {
884 velocity: 1.0,
885 yaw_rate: 0.16,
886 true_velocity_scale: 1.0,
887 true_velocity_wave: 0.0,
888 true_yaw_rate_scale: 1.0,
889 true_yaw_wave_deg: 0.0,
890 command_velocity_wave: 0.0,
891 command_yaw_wave_deg: 0.0,
892 control_latency_steps: 0,
893 actuator_velocity_limit: 0.0,
894 actuator_yaw_limit_deg: 0.0,
895 process_noise_longitudinal: 0.01,
896 process_noise_lateral: 0.18,
897 process_noise_yaw_deg: 1.2,
898 control_noise_v: 0.22,
899 control_noise_yaw_deg: 4.5,
900 control_bias_v: 0.0,
901 control_bias_yaw_deg: 0.0,
902 obs_noise_x: 0.28,
903 obs_noise_y: 0.28,
904 observation_refresh_interval: 1,
905 observation_hold_probability: 0.0,
906 observation_outlier_probability: 0.0,
907 observation_outlier_scale: 0.0,
908 observation_outlier_burst_len: 0,
909 observation_bias_burst_interval: 0,
910 },
911 buckets: &[120, 200, 280],
912 },
913 AccuracyExperimentCase {
914 family_name: "traction-slip-turn",
915 seed_offset: 230_000,
916 profile: MotionProfile {
917 velocity: 0.95,
918 yaw_rate: 0.24,
919 true_velocity_scale: 1.0,
920 true_velocity_wave: 0.0,
921 true_yaw_rate_scale: 1.0,
922 true_yaw_wave_deg: 0.0,
923 command_velocity_wave: 0.0,
924 command_yaw_wave_deg: 0.0,
925 control_latency_steps: 0,
926 actuator_velocity_limit: 0.0,
927 actuator_yaw_limit_deg: 0.0,
928 process_noise_longitudinal: 0.04,
929 process_noise_lateral: 0.12,
930 process_noise_yaw_deg: 3.5,
931 control_noise_v: 0.24,
932 control_noise_yaw_deg: 5.5,
933 control_bias_v: 0.0,
934 control_bias_yaw_deg: 0.0,
935 obs_noise_x: 0.30,
936 obs_noise_y: 0.55,
937 observation_refresh_interval: 1,
938 observation_hold_probability: 0.0,
939 observation_outlier_probability: 0.0,
940 observation_outlier_scale: 0.0,
941 observation_outlier_burst_len: 0,
942 observation_bias_burst_interval: 0,
943 },
944 buckets: &[140, 220, 320],
945 },
946 AccuracyExperimentCase {
947 family_name: "yaw-dominant-drift",
948 seed_offset: 240_000,
949 profile: MotionProfile {
950 velocity: 1.1,
951 yaw_rate: 0.28,
952 true_velocity_scale: 1.0,
953 true_velocity_wave: 0.0,
954 true_yaw_rate_scale: 1.0,
955 true_yaw_wave_deg: 0.0,
956 command_velocity_wave: 0.0,
957 command_yaw_wave_deg: 0.0,
958 control_latency_steps: 0,
959 actuator_velocity_limit: 0.0,
960 actuator_yaw_limit_deg: 0.0,
961 process_noise_longitudinal: 0.02,
962 process_noise_lateral: 0.10,
963 process_noise_yaw_deg: 5.0,
964 control_noise_v: 0.26,
965 control_noise_yaw_deg: 6.0,
966 control_bias_v: 0.0,
967 control_bias_yaw_deg: 0.0,
968 obs_noise_x: 0.32,
969 obs_noise_y: 0.32,
970 observation_refresh_interval: 1,
971 observation_hold_probability: 0.0,
972 observation_outlier_probability: 0.0,
973 observation_outlier_scale: 0.0,
974 observation_outlier_burst_len: 0,
975 observation_bias_burst_interval: 0,
976 },
977 buckets: &[140, 240, 340],
978 },
979 ]
980}
981
982pub fn localization_sensor_bias_burst_process_problem() -> Vec<AccuracyExperimentCase> {
983 vec![
984 AccuracyExperimentCase {
985 family_name: "gps-bias-burst",
986 seed_offset: 250_000,
987 profile: MotionProfile {
988 velocity: 1.0,
989 yaw_rate: 0.16,
990 true_velocity_scale: 1.0,
991 true_velocity_wave: 0.0,
992 true_yaw_rate_scale: 1.0,
993 true_yaw_wave_deg: 0.0,
994 command_velocity_wave: 0.0,
995 command_yaw_wave_deg: 0.0,
996 control_latency_steps: 0,
997 actuator_velocity_limit: 0.0,
998 actuator_yaw_limit_deg: 0.0,
999 process_noise_longitudinal: 0.0,
1000 process_noise_lateral: 0.0,
1001 process_noise_yaw_deg: 0.0,
1002 control_noise_v: 0.22,
1003 control_noise_yaw_deg: 4.5,
1004 control_bias_v: 0.0,
1005 control_bias_yaw_deg: 0.0,
1006 obs_noise_x: 0.28,
1007 obs_noise_y: 0.28,
1008 observation_refresh_interval: 1,
1009 observation_hold_probability: 0.0,
1010 observation_outlier_probability: 0.0,
1011 observation_outlier_scale: 0.35,
1012 observation_outlier_burst_len: 6,
1013 observation_bias_burst_interval: 45,
1014 },
1015 buckets: &[120, 200, 280],
1016 },
1017 AccuracyExperimentCase {
1018 family_name: "camera-bias-burst",
1019 seed_offset: 260_000,
1020 profile: MotionProfile {
1021 velocity: 0.95,
1022 yaw_rate: 0.24,
1023 true_velocity_scale: 1.0,
1024 true_velocity_wave: 0.0,
1025 true_yaw_rate_scale: 1.0,
1026 true_yaw_wave_deg: 0.0,
1027 command_velocity_wave: 0.0,
1028 command_yaw_wave_deg: 0.0,
1029 control_latency_steps: 0,
1030 actuator_velocity_limit: 0.0,
1031 actuator_yaw_limit_deg: 0.0,
1032 process_noise_longitudinal: 0.0,
1033 process_noise_lateral: 0.0,
1034 process_noise_yaw_deg: 0.0,
1035 control_noise_v: 0.24,
1036 control_noise_yaw_deg: 5.0,
1037 control_bias_v: 0.0,
1038 control_bias_yaw_deg: 0.0,
1039 obs_noise_x: 0.30,
1040 obs_noise_y: 0.55,
1041 observation_refresh_interval: 1,
1042 observation_hold_probability: 0.0,
1043 observation_outlier_probability: 0.0,
1044 observation_outlier_scale: 0.45,
1045 observation_outlier_burst_len: 5,
1046 observation_bias_burst_interval: 36,
1047 },
1048 buckets: &[140, 220, 320],
1049 },
1050 AccuracyExperimentCase {
1051 family_name: "anisotropic-bias-burst",
1052 seed_offset: 270_000,
1053 profile: MotionProfile {
1054 velocity: 1.1,
1055 yaw_rate: 0.28,
1056 true_velocity_scale: 1.0,
1057 true_velocity_wave: 0.0,
1058 true_yaw_rate_scale: 1.0,
1059 true_yaw_wave_deg: 0.0,
1060 command_velocity_wave: 0.0,
1061 command_yaw_wave_deg: 0.0,
1062 control_latency_steps: 0,
1063 actuator_velocity_limit: 0.0,
1064 actuator_yaw_limit_deg: 0.0,
1065 process_noise_longitudinal: 0.0,
1066 process_noise_lateral: 0.0,
1067 process_noise_yaw_deg: 0.0,
1068 control_noise_v: 0.26,
1069 control_noise_yaw_deg: 6.0,
1070 control_bias_v: 0.0,
1071 control_bias_yaw_deg: 0.0,
1072 obs_noise_x: 0.32,
1073 obs_noise_y: 0.32,
1074 observation_refresh_interval: 1,
1075 observation_hold_probability: 0.0,
1076 observation_outlier_probability: 0.0,
1077 observation_outlier_scale: 0.55,
1078 observation_outlier_burst_len: 4,
1079 observation_bias_burst_interval: 30,
1080 },
1081 buckets: &[140, 240, 340],
1082 },
1083 ]
1084}
1085
1086pub fn localization_actuator_saturation_process_problem() -> Vec<AccuracyExperimentCase> {
1087 vec![
1088 AccuracyExperimentCase {
1089 family_name: "velocity-clipping",
1090 seed_offset: 280_000,
1091 profile: MotionProfile {
1092 velocity: 1.0,
1093 yaw_rate: 0.16,
1094 true_velocity_scale: 1.0,
1095 true_velocity_wave: 0.0,
1096 true_yaw_rate_scale: 1.0,
1097 true_yaw_wave_deg: 0.0,
1098 command_velocity_wave: 0.42,
1099 command_yaw_wave_deg: 4.0,
1100 control_latency_steps: 0,
1101 actuator_velocity_limit: 1.05,
1102 actuator_yaw_limit_deg: 0.0,
1103 process_noise_longitudinal: 0.0,
1104 process_noise_lateral: 0.0,
1105 process_noise_yaw_deg: 0.0,
1106 control_noise_v: 0.22,
1107 control_noise_yaw_deg: 4.5,
1108 control_bias_v: 0.0,
1109 control_bias_yaw_deg: 0.0,
1110 obs_noise_x: 0.28,
1111 obs_noise_y: 0.28,
1112 observation_refresh_interval: 1,
1113 observation_hold_probability: 0.0,
1114 observation_outlier_probability: 0.0,
1115 observation_outlier_scale: 0.0,
1116 observation_outlier_burst_len: 0,
1117 observation_bias_burst_interval: 0,
1118 },
1119 buckets: &[120, 200, 280],
1120 },
1121 AccuracyExperimentCase {
1122 family_name: "steering-clipping",
1123 seed_offset: 290_000,
1124 profile: MotionProfile {
1125 velocity: 0.95,
1126 yaw_rate: 0.26,
1127 true_velocity_scale: 1.0,
1128 true_velocity_wave: 0.0,
1129 true_yaw_rate_scale: 1.0,
1130 true_yaw_wave_deg: 0.0,
1131 command_velocity_wave: 0.18,
1132 command_yaw_wave_deg: 15.0,
1133 control_latency_steps: 0,
1134 actuator_velocity_limit: 0.0,
1135 actuator_yaw_limit_deg: 10.0,
1136 process_noise_longitudinal: 0.0,
1137 process_noise_lateral: 0.0,
1138 process_noise_yaw_deg: 0.0,
1139 control_noise_v: 0.24,
1140 control_noise_yaw_deg: 5.5,
1141 control_bias_v: 0.0,
1142 control_bias_yaw_deg: 0.0,
1143 obs_noise_x: 0.30,
1144 obs_noise_y: 0.55,
1145 observation_refresh_interval: 1,
1146 observation_hold_probability: 0.0,
1147 observation_outlier_probability: 0.0,
1148 observation_outlier_scale: 0.0,
1149 observation_outlier_burst_len: 0,
1150 observation_bias_burst_interval: 0,
1151 },
1152 buckets: &[140, 220, 320],
1153 },
1154 AccuracyExperimentCase {
1155 family_name: "coupled-saturation",
1156 seed_offset: 300_000,
1157 profile: MotionProfile {
1158 velocity: 1.1,
1159 yaw_rate: 0.22,
1160 true_velocity_scale: 1.0,
1161 true_velocity_wave: 0.0,
1162 true_yaw_rate_scale: 1.0,
1163 true_yaw_wave_deg: 0.0,
1164 command_velocity_wave: 0.30,
1165 command_yaw_wave_deg: 12.0,
1166 control_latency_steps: 0,
1167 actuator_velocity_limit: 1.18,
1168 actuator_yaw_limit_deg: 11.0,
1169 process_noise_longitudinal: 0.0,
1170 process_noise_lateral: 0.0,
1171 process_noise_yaw_deg: 0.0,
1172 control_noise_v: 0.26,
1173 control_noise_yaw_deg: 6.0,
1174 control_bias_v: 0.0,
1175 control_bias_yaw_deg: 0.0,
1176 obs_noise_x: 0.32,
1177 obs_noise_y: 0.32,
1178 observation_refresh_interval: 1,
1179 observation_hold_probability: 0.0,
1180 observation_outlier_probability: 0.0,
1181 observation_outlier_scale: 0.0,
1182 observation_outlier_burst_len: 0,
1183 observation_bias_burst_interval: 0,
1184 },
1185 buckets: &[140, 240, 340],
1186 },
1187 ]
1188}
1189
1190pub fn default_accuracy_variants() -> Vec<Box<dyn AccuracyAggregationVariant>> {
1191 vec![
1192 Box::new(FirstScenarioAccuracyAggregation::new()),
1193 Box::new(SampledBucketAccuracyAggregation::new(vec![0, 4, 9])),
1194 Box::new(PercentileBucketAccuracyAggregation::new(vec![
1195 0.0, 0.25, 0.5, 0.75, 1.0,
1196 ])),
1197 Box::new(VarianceTriggeredAccuracyAggregation::new(
1198 vec![0, 4, 9],
1199 0.10,
1200 )),
1201 Box::new(FullBucketAccuracyAggregation::new()),
1202 ]
1203}
1204
1205pub fn run_variant_suite(
1206 variants: &[Box<dyn AccuracyAggregationVariant>],
1207 cases: &[AccuracyExperimentCase],
1208 config: AccuracyEvaluationConfig,
1209) -> Vec<AccuracyVariantReport> {
1210 let mut reports = Vec::with_capacity(variants.len());
1211
1212 for variant in variants {
1213 let started = Instant::now();
1214 let mut observations = Vec::new();
1215 for case in cases {
1216 for &bucket in case.buckets {
1217 observations.push(measure_bucket_observation(
1218 &**variant,
1219 case,
1220 bucket,
1221 config.scenarios_per_bucket,
1222 ));
1223 }
1224 }
1225 let descriptor = variant.descriptor();
1226 let source_metrics = read_source_metrics(std::path::Path::new(descriptor.source_path))
1227 .expect("experiment source metrics should be readable");
1228 let average_coverage_ratio = average_coverage_ratio(&observations);
1229 let extensibility_metrics = ExtensibilityMetrics {
1230 average_coverage_ratio,
1231 knob_count: descriptor.knob_count,
1232 reports_dispersion: descriptor.reports_dispersion,
1233 };
1234 reports.push(AccuracyVariantReport {
1235 descriptor,
1236 evaluation_runtime_ms: started.elapsed().as_secs_f64() * 1000.0,
1237 observations,
1238 source_metrics,
1239 extensibility_metrics,
1240 agreement_vs_reference: None,
1241 mean_ratio_error_vs_reference: None,
1242 });
1243 }
1244
1245 annotate_reference_reports(&mut reports, "full-bucket");
1246 reports
1247}
1248
1249fn measure_bucket_observation(
1250 variant: &dyn AccuracyAggregationVariant,
1251 case: &AccuracyExperimentCase,
1252 bucket: u32,
1253 total_scenarios: usize,
1254) -> AccuracyObservation {
1255 let plan = variant.sampling_plan(total_scenarios);
1256 let initial_slots = normalize_slots(total_scenarios, &plan.initial_slots);
1257 assert!(
1258 !initial_slots.is_empty(),
1259 "{} bucket {} should select at least one scenario",
1260 case.family_name,
1261 bucket
1262 );
1263
1264 let mut slot_samples = measure_slot_samples(&initial_slots, case, bucket);
1265 let mut selected_slots = initial_slots.clone();
1266 let mut escalated = false;
1267 let escalation_slots = normalize_slots(total_scenarios, &plan.escalation_slots);
1268
1269 if should_escalate(&slot_samples, &plan) {
1270 let additional_slots: Vec<usize> = escalation_slots
1271 .into_iter()
1272 .filter(|slot| !selected_slots.contains(slot))
1273 .collect();
1274 if !additional_slots.is_empty() {
1275 slot_samples.extend(measure_slot_samples(&additional_slots, case, bucket));
1276 selected_slots.extend(additional_slots);
1277 selected_slots.sort_unstable();
1278 escalated = true;
1279 }
1280 }
1281
1282 let ukf_samples: Vec<f64> = slot_samples.iter().map(|sample| sample.ukf_rmse).collect();
1283 let ckf_samples: Vec<f64> = slot_samples.iter().map(|sample| sample.ckf_rmse).collect();
1284 let ckf_wins = slot_samples
1285 .iter()
1286 .filter(|sample| sample.ckf_rmse < sample.ukf_rmse)
1287 .count();
1288
1289 AccuracyObservation {
1290 family_name: case.family_name,
1291 bucket,
1292 total_scenarios,
1293 initial_slots,
1294 selected_slots,
1295 escalated,
1296 ukf_bucket_median_rmse: median_value(&ukf_samples),
1297 ckf_bucket_median_rmse: median_value(&ckf_samples),
1298 ukf_min_rmse: min_value(&ukf_samples),
1299 ukf_max_rmse: max_value(&ukf_samples),
1300 ckf_min_rmse: min_value(&ckf_samples),
1301 ckf_max_rmse: max_value(&ckf_samples),
1302 ckf_wins,
1303 }
1304}
1305
1306#[derive(Debug, Clone, Copy)]
1307struct SlotAccuracySample {
1308 ukf_rmse: f64,
1309 ckf_rmse: f64,
1310}
1311
1312fn measure_slot_samples(
1313 slots: &[usize],
1314 case: &AccuracyExperimentCase,
1315 bucket: u32,
1316) -> Vec<SlotAccuracySample> {
1317 let mut samples = Vec::with_capacity(slots.len());
1318 for slot in slots {
1319 let data = generate_sim_data(case, bucket, *slot);
1320 let ukf_rmse = run_ukf_rmse(&data);
1321 let ckf_rmse = run_ckf_rmse(&data);
1322 assert!(ukf_rmse.is_finite(), "UKF RMSE should stay finite");
1323 assert!(ckf_rmse.is_finite(), "CKF RMSE should stay finite");
1324 samples.push(SlotAccuracySample { ukf_rmse, ckf_rmse });
1325 }
1326 samples
1327}
1328
1329fn should_escalate(slot_samples: &[SlotAccuracySample], plan: &AccuracySamplingPlan) -> bool {
1330 if slot_samples.is_empty() || plan.escalation_slots.is_empty() {
1331 return false;
1332 }
1333
1334 let vote_split = {
1335 let ckf_wins = slot_samples
1336 .iter()
1337 .filter(|sample| sample.ckf_rmse < sample.ukf_rmse)
1338 .count();
1339 ckf_wins > 0 && ckf_wins < slot_samples.len()
1340 };
1341 let ratio_close = plan
1342 .escalate_if_ratio_margin_below
1343 .map(|threshold| {
1344 let ukf_samples: Vec<f64> = slot_samples.iter().map(|sample| sample.ukf_rmse).collect();
1345 let ckf_samples: Vec<f64> = slot_samples.iter().map(|sample| sample.ckf_rmse).collect();
1346 (median_value(&ukf_samples) / median_value(&ckf_samples).max(1e-9) - 1.0).abs()
1347 < threshold
1348 })
1349 .unwrap_or(false);
1350
1351 (plan.escalate_if_vote_split && vote_split) || ratio_close
1352}
1353
1354fn normalize_slots(total_scenarios: usize, slots: &[usize]) -> Vec<usize> {
1355 let mut normalized = slots
1356 .iter()
1357 .copied()
1358 .filter(|slot| *slot < total_scenarios)
1359 .collect::<Vec<_>>();
1360 normalized.sort_unstable();
1361 normalized.dedup();
1362 normalized
1363}
1364
1365#[derive(Debug)]
1366struct SimData {
1367 ground_truth: Vec<Vector4<f64>>,
1368 noisy_controls: Vec<Vector2<f64>>,
1369 noisy_observations: Vec<Vector2<f64>>,
1370}
1371
1372fn generate_sim_data(case: &AccuracyExperimentCase, bucket: u32, slot: usize) -> SimData {
1373 let steps = bucket as usize;
1374 let scale = bucket as f64 / 100.0;
1375 let profile = case.profile;
1376 let mut rng =
1377 rand::rngs::StdRng::seed_from_u64(case.seed_offset + bucket as u64 * 100 + slot as u64);
1378
1379 let noise_v = Normal::new(0.0, profile.control_noise_v * scale).unwrap();
1380 let noise_yaw = Normal::new(0.0, profile.control_noise_yaw_deg.to_radians() * scale).unwrap();
1381 let noise_obs_x = Normal::new(0.0, profile.obs_noise_x * scale).unwrap();
1382 let noise_obs_y = Normal::new(0.0, profile.obs_noise_y * scale).unwrap();
1383
1384 let initial_true_control = true_control_for_step(profile, scale, 0, slot);
1385 let mut state = Vector4::new(0.0, 0.0, 0.0, initial_true_control[0]);
1386 let mut last_observation = None;
1387 let mut outlier_burst_steps_remaining = 0usize;
1388 let mut outlier_offset = Vector2::zeros();
1389
1390 let mut ground_truth = Vec::with_capacity(steps);
1391 let mut noisy_controls = Vec::with_capacity(steps);
1392 let mut noisy_observations = Vec::with_capacity(steps);
1393 let observation_refresh_interval = profile.observation_refresh_interval.max(1);
1394 let refresh_phase = slot % observation_refresh_interval;
1395
1396 for step in 0..steps {
1397 let commanded_control = commanded_control_for_step(profile, scale, step, slot);
1398 let true_control = true_control_for_step(profile, scale, step, slot);
1399 state = motion_model(&state, &true_control, DT);
1400 state = apply_process_disturbance(state, profile, scale, &mut rng);
1401
1402 let noisy_control = Vector2::new(
1403 commanded_control[0] + profile.control_bias_v + noise_v.sample(&mut rng),
1404 commanded_control[1]
1405 + profile.control_bias_yaw_deg.to_radians()
1406 + noise_yaw.sample(&mut rng),
1407 );
1408 let fresh_observation = Vector2::new(
1409 state[0] + noise_obs_x.sample(&mut rng),
1410 state[1] + noise_obs_y.sample(&mut rng),
1411 );
1412 if outlier_burst_steps_remaining == 0
1413 && profile.observation_outlier_probability > 0.0
1414 && rng.random::<f64>() < profile.observation_outlier_probability
1415 {
1416 outlier_burst_steps_remaining = profile.observation_outlier_burst_len.max(1);
1417 outlier_offset = Vector2::new(
1418 Normal::new(0.0, profile.observation_outlier_scale * scale)
1419 .unwrap()
1420 .sample(&mut rng),
1421 Normal::new(0.0, profile.observation_outlier_scale * scale)
1422 .unwrap()
1423 .sample(&mut rng),
1424 );
1425 }
1426 let burst_adjusted_observation = if outlier_burst_steps_remaining > 0 {
1427 outlier_burst_steps_remaining -= 1;
1428 fresh_observation + outlier_offset
1429 } else {
1430 fresh_observation
1431 };
1432 let bias_adjusted_observation =
1433 burst_adjusted_observation + sensor_bias_offset_for_step(profile, scale, step, slot);
1434 let reused_by_cadence =
1435 last_observation.is_some() && step % observation_refresh_interval != refresh_phase;
1436 let cadence_observation = if reused_by_cadence {
1437 last_observation.expect("stale observation should exist")
1438 } else {
1439 bias_adjusted_observation
1440 };
1441 let noisy_observation = match (reused_by_cadence, last_observation) {
1442 (true, _) | (_, None) => cadence_observation,
1443 (false, Some(prev)) if rng.random::<f64>() < profile.observation_hold_probability => {
1444 prev
1445 }
1446 _ => bias_adjusted_observation,
1447 };
1448
1449 ground_truth.push(state);
1450 noisy_controls.push(noisy_control);
1451 noisy_observations.push(noisy_observation);
1452 last_observation = Some(noisy_observation);
1453 }
1454
1455 SimData {
1456 ground_truth,
1457 noisy_controls,
1458 noisy_observations,
1459 }
1460}
1461
1462fn true_control_for_step(
1463 profile: MotionProfile,
1464 scale: f64,
1465 step: usize,
1466 slot: usize,
1467) -> Vector2<f64> {
1468 let delayed_step = step.saturating_sub(profile.control_latency_steps);
1469 let commanded_control = apply_actuator_saturation(
1470 profile,
1471 commanded_control_for_step(profile, scale, delayed_step, slot),
1472 );
1473 let velocity_phase = step as f64 * 0.07 + slot as f64 * 0.31;
1474 let yaw_phase = step as f64 * 0.05 + slot as f64 * 0.17 + 0.6;
1475 let true_velocity = (commanded_control[0] * profile.true_velocity_scale
1476 + profile.true_velocity_wave * scale * velocity_phase.sin())
1477 .max(0.05);
1478 let true_yaw_rate = commanded_control[1] * profile.true_yaw_rate_scale
1479 + profile.true_yaw_wave_deg.to_radians() * scale * yaw_phase.cos();
1480
1481 Vector2::new(true_velocity, true_yaw_rate)
1482}
1483
1484fn commanded_control_for_step(
1485 profile: MotionProfile,
1486 scale: f64,
1487 step: usize,
1488 slot: usize,
1489) -> Vector2<f64> {
1490 let velocity_phase = step as f64 * 0.09 + slot as f64 * 0.21 + 0.3;
1491 let yaw_phase = step as f64 * 0.06 + slot as f64 * 0.27 + 0.8;
1492 let commanded_velocity =
1493 (profile.velocity + profile.command_velocity_wave * scale * velocity_phase.sin()).max(0.05);
1494 let commanded_yaw_rate =
1495 profile.yaw_rate + profile.command_yaw_wave_deg.to_radians() * scale * yaw_phase.cos();
1496
1497 Vector2::new(commanded_velocity, commanded_yaw_rate)
1498}
1499
1500fn apply_actuator_saturation(profile: MotionProfile, control: Vector2<f64>) -> Vector2<f64> {
1501 let velocity_limit = profile.actuator_velocity_limit;
1502 let yaw_limit = profile.actuator_yaw_limit_deg.to_radians();
1503 let saturated_velocity = if velocity_limit > 0.0 {
1504 control[0].clamp(-velocity_limit, velocity_limit)
1505 } else {
1506 control[0]
1507 };
1508 let saturated_yaw = if yaw_limit > 0.0 {
1509 control[1].clamp(-yaw_limit, yaw_limit)
1510 } else {
1511 control[1]
1512 };
1513
1514 Vector2::new(saturated_velocity, saturated_yaw)
1515}
1516
1517fn apply_process_disturbance(
1518 mut state: Vector4<f64>,
1519 profile: MotionProfile,
1520 scale: f64,
1521 rng: &mut rand::rngs::StdRng,
1522) -> Vector4<f64> {
1523 let longitudinal = sample_zero_mean(rng, profile.process_noise_longitudinal * scale);
1524 let lateral = sample_zero_mean(rng, profile.process_noise_lateral * scale);
1525 let yaw_noise = sample_zero_mean(rng, profile.process_noise_yaw_deg.to_radians() * scale);
1526
1527 if longitudinal == 0.0 && lateral == 0.0 && yaw_noise == 0.0 {
1528 return state;
1529 }
1530
1531 let yaw = state[2];
1532 state[0] += longitudinal * yaw.cos() - lateral * yaw.sin();
1533 state[1] += longitudinal * yaw.sin() + lateral * yaw.cos();
1534 state[2] += yaw_noise;
1535 state
1536}
1537
1538fn sensor_bias_offset_for_step(
1539 profile: MotionProfile,
1540 scale: f64,
1541 step: usize,
1542 slot: usize,
1543) -> Vector2<f64> {
1544 let interval = profile.observation_bias_burst_interval;
1545 if interval == 0 {
1546 return Vector2::zeros();
1547 }
1548
1549 let burst_len = profile.observation_outlier_burst_len.max(1);
1550 let phase = slot % interval;
1551 if (step + phase) % interval >= burst_len {
1552 return Vector2::zeros();
1553 }
1554
1555 let magnitude = profile.observation_outlier_scale * scale;
1556 let direction = if slot % 2 == 0 { 1.0 } else { -1.0 };
1557 Vector2::new(magnitude, direction * 0.6 * magnitude)
1558}
1559
1560fn sample_zero_mean(rng: &mut rand::rngs::StdRng, sigma: f64) -> f64 {
1561 if sigma <= 0.0 {
1562 0.0
1563 } else {
1564 Normal::new(0.0, sigma).unwrap().sample(rng)
1565 }
1566}
1567
1568fn motion_model(x: &Vector4<f64>, u: &Vector2<f64>, dt: f64) -> Vector4<f64> {
1569 let yaw = x[2];
1570 Vector4::new(
1571 x[0] + dt * u[0] * yaw.cos(),
1572 x[1] + dt * u[0] * yaw.sin(),
1573 x[2] + dt * u[1],
1574 u[0],
1575 )
1576}
1577
1578fn run_ukf_rmse(data: &SimData) -> f64 {
1579 let first_observation = data
1580 .noisy_observations
1581 .first()
1582 .copied()
1583 .unwrap_or_else(Vector2::zeros);
1584 let observation_var_x = variance(data.noisy_observations.iter().map(|z| z[0]));
1585 let observation_var_y = variance(data.noisy_observations.iter().map(|z| z[1]));
1586 let control_var_v = variance(data.noisy_controls.iter().map(|u| u[0]));
1587 let control_var_yaw = variance(data.noisy_controls.iter().map(|u| u[1]));
1588
1589 let config = UKFConfig {
1590 params: UKFParams::default(),
1591 process_noise: Vector4::new(
1592 control_var_v.max(1e-4),
1593 control_var_v.max(1e-4),
1594 control_var_yaw.max(1e-5),
1595 control_var_v.max(1e-4),
1596 ),
1597 observation_noise: Vector2::new(observation_var_x.max(1e-4), observation_var_y.max(1e-4)),
1598 dt: DT,
1599 };
1600 let mut ukf = UKFLocalizer::with_initial_state(
1601 Vector4::new(first_observation[0], first_observation[1], 0.0, 0.0),
1602 config,
1603 );
1604
1605 let mut positions = Vec::with_capacity(data.ground_truth.len());
1606 for (control, observation) in data
1607 .noisy_controls
1608 .iter()
1609 .zip(data.noisy_observations.iter())
1610 {
1611 ukf.step(control, observation);
1612 let state = ukf.estimate();
1613 positions.push((state[0], state[1]));
1614 }
1615 compute_rmse(&positions, &data.ground_truth)
1616}
1617
1618fn run_ckf_rmse(data: &SimData) -> f64 {
1619 let first_observation = data
1620 .noisy_observations
1621 .first()
1622 .copied()
1623 .unwrap_or_else(Vector2::zeros);
1624 let observation_var_x = variance(data.noisy_observations.iter().map(|z| z[0]));
1625 let observation_var_y = variance(data.noisy_observations.iter().map(|z| z[1]));
1626 let control_var_v = variance(data.noisy_controls.iter().map(|u| u[0]));
1627 let control_var_yaw = variance(data.noisy_controls.iter().map(|u| u[1]));
1628
1629 let mut r = Matrix2::<f64>::identity();
1630 r[(0, 0)] = observation_var_x.max(1e-4);
1631 r[(1, 1)] = observation_var_y.max(1e-4);
1632 let q = Matrix4::from_diagonal(&Vector4::new(
1633 control_var_v.max(1e-4),
1634 control_var_v.max(1e-4),
1635 control_var_yaw.max(1e-5),
1636 control_var_v.max(1e-4),
1637 ));
1638
1639 let mut ckf = CKFLocalizer::with_initial_state(
1640 Vector4::new(first_observation[0], first_observation[1], 0.0, 0.0),
1641 CKFConfig { q, r },
1642 );
1643
1644 let mut positions = Vec::with_capacity(data.ground_truth.len());
1645 for (control, observation) in data
1646 .noisy_controls
1647 .iter()
1648 .zip(data.noisy_observations.iter())
1649 {
1650 ckf.step(control, observation, DT);
1651 let state = ckf.estimate();
1652 positions.push((state[0], state[1]));
1653 }
1654 compute_rmse(&positions, &data.ground_truth)
1655}
1656
1657fn compute_rmse(positions: &[(f64, f64)], ground_truth: &[Vector4<f64>]) -> f64 {
1658 let n = positions.len().min(ground_truth.len());
1659 if n == 0 {
1660 return f64::NAN;
1661 }
1662 let sum_sq: f64 = positions
1663 .iter()
1664 .zip(ground_truth.iter())
1665 .map(|((x, y), gt)| (x - gt[0]).powi(2) + (y - gt[1]).powi(2))
1666 .sum();
1667 (sum_sq / n as f64).sqrt()
1668}
1669
1670fn variance<I>(values: I) -> f64
1671where
1672 I: Iterator<Item = f64>,
1673{
1674 let samples: Vec<f64> = values.collect();
1675 if samples.len() <= 1 {
1676 return 0.0;
1677 }
1678 let mean = samples.iter().sum::<f64>() / samples.len() as f64;
1679 samples
1680 .iter()
1681 .map(|value| (value - mean).powi(2))
1682 .sum::<f64>()
1683 / samples.len() as f64
1684}
1685
1686fn median_value(samples: &[f64]) -> f64 {
1687 assert!(
1688 !samples.is_empty(),
1689 "median_value requires at least one sample"
1690 );
1691 let mut sorted = samples.to_vec();
1692 sorted.sort_by(|a, b| a.partial_cmp(b).unwrap());
1693 sorted[sorted.len() / 2]
1694}
1695
1696fn min_value(samples: &[f64]) -> f64 {
1697 samples.iter().copied().fold(f64::INFINITY, f64::min)
1698}
1699
1700fn max_value(samples: &[f64]) -> f64 {
1701 samples.iter().copied().fold(f64::NEG_INFINITY, f64::max)
1702}
1703
1704#[cfg(test)]
1705mod tests {
1706 use super::*;
1707
1708 #[test]
1709 fn first_variant_selects_one_slot() {
1710 let variant = FirstScenarioAccuracyAggregation::new();
1711 assert_eq!(variant.selected_slots(10), vec![0]);
1712 }
1713
1714 #[test]
1715 fn percentile_variant_maps_percentiles_to_unique_slots() {
1716 let variant = PercentileBucketAccuracyAggregation::new(vec![0.0, 0.25, 0.5, 0.75, 1.0]);
1717 assert_eq!(variant.selected_slots(10), vec![0, 2, 5, 7, 9]);
1718 assert_eq!(variant.selected_slots(1), vec![0]);
1719 }
1720
1721 #[test]
1722 fn variance_variant_builds_adaptive_sampling_plan() {
1723 let variant = VarianceTriggeredAccuracyAggregation::new(vec![0, 4, 9], 0.10);
1724 let plan = variant.sampling_plan(10);
1725 assert_eq!(plan.initial_slots, vec![0, 4, 9]);
1726 assert_eq!(plan.escalation_slots, (0..10).collect::<Vec<_>>());
1727 assert!(plan.escalate_if_vote_split);
1728 assert_eq!(plan.escalate_if_ratio_margin_below, Some(0.10));
1729 }
1730
1731 #[test]
1732 fn suite_runs_on_small_noise_problem() {
1733 let variants = default_accuracy_variants();
1734 let cases = vec![AccuracyExperimentCase {
1735 family_name: "test-case",
1736 seed_offset: 99_000,
1737 profile: MotionProfile {
1738 velocity: 1.0,
1739 yaw_rate: 0.10,
1740 true_velocity_scale: 1.0,
1741 true_velocity_wave: 0.0,
1742 true_yaw_rate_scale: 1.0,
1743 true_yaw_wave_deg: 0.0,
1744 command_velocity_wave: 0.0,
1745 command_yaw_wave_deg: 0.0,
1746 control_latency_steps: 0,
1747 actuator_velocity_limit: 0.0,
1748 actuator_yaw_limit_deg: 0.0,
1749 process_noise_longitudinal: 0.0,
1750 process_noise_lateral: 0.0,
1751 process_noise_yaw_deg: 0.0,
1752 control_noise_v: 0.20,
1753 control_noise_yaw_deg: 4.0,
1754 control_bias_v: 0.0,
1755 control_bias_yaw_deg: 0.0,
1756 obs_noise_x: 0.30,
1757 obs_noise_y: 0.30,
1758 observation_refresh_interval: 1,
1759 observation_hold_probability: 0.0,
1760 observation_outlier_probability: 0.0,
1761 observation_outlier_scale: 0.0,
1762 observation_outlier_burst_len: 0,
1763 observation_bias_burst_interval: 0,
1764 },
1765 buckets: &[60, 100],
1766 }];
1767 let reports = run_variant_suite(
1768 &variants,
1769 &cases,
1770 AccuracyEvaluationConfig {
1771 scenarios_per_bucket: 3,
1772 },
1773 );
1774 assert_eq!(reports.len(), 5);
1775 assert!(reports.iter().all(|report| report.observations.len() == 2));
1776 }
1777
1778 #[test]
1779 fn sensor_rate_mismatch_reuses_observations_between_refreshes() {
1780 let case = AccuracyExperimentCase {
1781 family_name: "sensor-rate-test",
1782 seed_offset: 123_000,
1783 profile: MotionProfile {
1784 velocity: 1.0,
1785 yaw_rate: 0.12,
1786 true_velocity_scale: 1.0,
1787 true_velocity_wave: 0.0,
1788 true_yaw_rate_scale: 1.0,
1789 true_yaw_wave_deg: 0.0,
1790 command_velocity_wave: 0.0,
1791 command_yaw_wave_deg: 0.0,
1792 control_latency_steps: 0,
1793 actuator_velocity_limit: 0.0,
1794 actuator_yaw_limit_deg: 0.0,
1795 process_noise_longitudinal: 0.0,
1796 process_noise_lateral: 0.0,
1797 process_noise_yaw_deg: 0.0,
1798 control_noise_v: 0.0,
1799 control_noise_yaw_deg: 0.0,
1800 control_bias_v: 0.0,
1801 control_bias_yaw_deg: 0.0,
1802 obs_noise_x: 0.0,
1803 obs_noise_y: 0.0,
1804 observation_refresh_interval: 3,
1805 observation_hold_probability: 0.0,
1806 observation_outlier_probability: 0.0,
1807 observation_outlier_scale: 0.0,
1808 observation_outlier_burst_len: 0,
1809 observation_bias_burst_interval: 0,
1810 },
1811 buckets: &[6],
1812 };
1813
1814 let data = generate_sim_data(&case, 6, 0);
1815 assert_eq!(data.noisy_observations[0], data.noisy_observations[1]);
1816 assert_eq!(data.noisy_observations[1], data.noisy_observations[2]);
1817 assert_ne!(data.noisy_observations[2], data.noisy_observations[3]);
1818 assert_eq!(data.noisy_observations[3], data.noisy_observations[4]);
1819 assert_eq!(data.noisy_observations[4], data.noisy_observations[5]);
1820 }
1821
1822 #[test]
1823 fn control_latency_uses_delayed_commanded_control_for_truth() {
1824 let profile = MotionProfile {
1825 velocity: 1.0,
1826 yaw_rate: 0.20,
1827 true_velocity_scale: 1.0,
1828 true_velocity_wave: 0.0,
1829 true_yaw_rate_scale: 1.0,
1830 true_yaw_wave_deg: 0.0,
1831 command_velocity_wave: 0.20,
1832 command_yaw_wave_deg: 6.0,
1833 control_latency_steps: 2,
1834 actuator_velocity_limit: 0.0,
1835 actuator_yaw_limit_deg: 0.0,
1836 process_noise_longitudinal: 0.0,
1837 process_noise_lateral: 0.0,
1838 process_noise_yaw_deg: 0.0,
1839 control_noise_v: 0.0,
1840 control_noise_yaw_deg: 0.0,
1841 control_bias_v: 0.0,
1842 control_bias_yaw_deg: 0.0,
1843 obs_noise_x: 0.0,
1844 obs_noise_y: 0.0,
1845 observation_refresh_interval: 1,
1846 observation_hold_probability: 0.0,
1847 observation_outlier_probability: 0.0,
1848 observation_outlier_scale: 0.0,
1849 observation_outlier_burst_len: 0,
1850 observation_bias_burst_interval: 0,
1851 };
1852
1853 let delayed_command = commanded_control_for_step(profile, 1.0, 2, 0);
1854 let truth_control = true_control_for_step(profile, 1.0, 4, 0);
1855
1856 assert!((truth_control[0] - delayed_command[0]).abs() < 1e-9);
1857 assert!((truth_control[1] - delayed_command[1]).abs() < 1e-9);
1858 }
1859
1860 #[test]
1861 fn process_noise_anisotropy_changes_ground_truth_trajectory() {
1862 let base_case = AccuracyExperimentCase {
1863 family_name: "process-noise-test",
1864 seed_offset: 321_000,
1865 profile: MotionProfile {
1866 velocity: 1.0,
1867 yaw_rate: 0.18,
1868 true_velocity_scale: 1.0,
1869 true_velocity_wave: 0.0,
1870 true_yaw_rate_scale: 1.0,
1871 true_yaw_wave_deg: 0.0,
1872 command_velocity_wave: 0.0,
1873 command_yaw_wave_deg: 0.0,
1874 control_latency_steps: 0,
1875 actuator_velocity_limit: 0.0,
1876 actuator_yaw_limit_deg: 0.0,
1877 process_noise_longitudinal: 0.0,
1878 process_noise_lateral: 0.0,
1879 process_noise_yaw_deg: 0.0,
1880 control_noise_v: 0.0,
1881 control_noise_yaw_deg: 0.0,
1882 control_bias_v: 0.0,
1883 control_bias_yaw_deg: 0.0,
1884 obs_noise_x: 0.0,
1885 obs_noise_y: 0.0,
1886 observation_refresh_interval: 1,
1887 observation_hold_probability: 0.0,
1888 observation_outlier_probability: 0.0,
1889 observation_outlier_scale: 0.0,
1890 observation_outlier_burst_len: 0,
1891 observation_bias_burst_interval: 0,
1892 },
1893 buckets: &[8],
1894 };
1895 let noisy_case = AccuracyExperimentCase {
1896 profile: MotionProfile {
1897 process_noise_longitudinal: 0.02,
1898 process_noise_lateral: 0.18,
1899 process_noise_yaw_deg: 4.0,
1900 ..base_case.profile
1901 },
1902 ..base_case
1903 };
1904
1905 let clean = generate_sim_data(&base_case, 8, 0);
1906 let noisy = generate_sim_data(&noisy_case, 8, 0);
1907
1908 assert_ne!(clean.ground_truth, noisy.ground_truth);
1909 }
1910
1911 #[test]
1912 fn sensor_bias_burst_adds_deterministic_observation_offset() {
1913 let case = AccuracyExperimentCase {
1914 family_name: "sensor-bias-test",
1915 seed_offset: 456_000,
1916 profile: MotionProfile {
1917 velocity: 1.0,
1918 yaw_rate: 0.14,
1919 true_velocity_scale: 1.0,
1920 true_velocity_wave: 0.0,
1921 true_yaw_rate_scale: 1.0,
1922 true_yaw_wave_deg: 0.0,
1923 command_velocity_wave: 0.0,
1924 command_yaw_wave_deg: 0.0,
1925 control_latency_steps: 0,
1926 actuator_velocity_limit: 0.0,
1927 actuator_yaw_limit_deg: 0.0,
1928 process_noise_longitudinal: 0.0,
1929 process_noise_lateral: 0.0,
1930 process_noise_yaw_deg: 0.0,
1931 control_noise_v: 0.0,
1932 control_noise_yaw_deg: 0.0,
1933 control_bias_v: 0.0,
1934 control_bias_yaw_deg: 0.0,
1935 obs_noise_x: 0.0,
1936 obs_noise_y: 0.0,
1937 observation_refresh_interval: 1,
1938 observation_hold_probability: 0.0,
1939 observation_outlier_probability: 0.0,
1940 observation_outlier_scale: 1.0,
1941 observation_outlier_burst_len: 2,
1942 observation_bias_burst_interval: 4,
1943 },
1944 buckets: &[4],
1945 };
1946
1947 let data = generate_sim_data(&case, 4, 0);
1948 let offset0 = data.noisy_observations[0]
1949 - Vector2::new(data.ground_truth[0][0], data.ground_truth[0][1]);
1950 let offset1 = data.noisy_observations[1]
1951 - Vector2::new(data.ground_truth[1][0], data.ground_truth[1][1]);
1952 let offset2 = data.noisy_observations[2]
1953 - Vector2::new(data.ground_truth[2][0], data.ground_truth[2][1]);
1954 assert!((offset0[0] - 0.04).abs() < 1e-9 && (offset0[1] - 0.024).abs() < 1e-9);
1955 assert!((offset1[0] - 0.04).abs() < 1e-9 && (offset1[1] - 0.024).abs() < 1e-9);
1956 assert!(offset2.norm() < 1e-12);
1957 }
1958
1959 #[test]
1960 fn actuator_saturation_caps_truth_control() {
1961 let profile = MotionProfile {
1962 velocity: 1.0,
1963 yaw_rate: 0.24,
1964 true_velocity_scale: 1.0,
1965 true_velocity_wave: 0.0,
1966 true_yaw_rate_scale: 1.0,
1967 true_yaw_wave_deg: 0.0,
1968 command_velocity_wave: 0.50,
1969 command_yaw_wave_deg: 14.0,
1970 control_latency_steps: 0,
1971 actuator_velocity_limit: 1.05,
1972 actuator_yaw_limit_deg: 9.0,
1973 process_noise_longitudinal: 0.0,
1974 process_noise_lateral: 0.0,
1975 process_noise_yaw_deg: 0.0,
1976 control_noise_v: 0.0,
1977 control_noise_yaw_deg: 0.0,
1978 control_bias_v: 0.0,
1979 control_bias_yaw_deg: 0.0,
1980 obs_noise_x: 0.0,
1981 obs_noise_y: 0.0,
1982 observation_refresh_interval: 1,
1983 observation_hold_probability: 0.0,
1984 observation_outlier_probability: 0.0,
1985 observation_outlier_scale: 0.0,
1986 observation_outlier_burst_len: 0,
1987 observation_bias_burst_interval: 0,
1988 };
1989
1990 let unsaturated = commanded_control_for_step(profile, 1.0, 7, 0);
1991 let truth = true_control_for_step(profile, 1.0, 7, 0);
1992
1993 assert!(unsaturated[0] > profile.actuator_velocity_limit);
1994 assert!(unsaturated[1].abs() > profile.actuator_yaw_limit_deg.to_radians());
1995 assert!((truth[0] - profile.actuator_velocity_limit).abs() < 1e-9);
1996 assert!((truth[1].abs() - profile.actuator_yaw_limit_deg.to_radians()).abs() < 1e-9);
1997 }
1998}