dsfb_robotics/
uncertainty.rs1use crate::math;
14
15#[derive(Debug, Clone, Copy, PartialEq)]
17#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
18pub struct UncertaintyComponent {
19 pub name: &'static str,
21 pub ty: GumType,
23 pub standard_uncertainty: f64,
27}
28
29#[derive(Debug, Clone, Copy, PartialEq, Eq)]
31#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
32pub enum GumType {
33 A,
35 B,
38}
39
40impl GumType {
41 #[inline]
43 #[must_use]
44 pub const fn label(self) -> &'static str {
45 match self {
46 Self::A => "TypeA",
47 Self::B => "TypeB",
48 }
49 }
50}
51
52#[must_use]
58pub fn combined_standard_uncertainty(components: &[UncertaintyComponent]) -> Option<f64> {
59 if components.is_empty() {
60 return None;
61 }
62 let mut ssq = 0.0_f64;
63 for c in components {
64 let u = c.standard_uncertainty;
65 if !u.is_finite() || u < 0.0 {
66 return None;
67 }
68 ssq += u * u;
69 }
70 math::sqrt_f64(ssq)
71}
72
73#[must_use]
79pub fn expanded_uncertainty(
80 components: &[UncertaintyComponent],
81 coverage_factor: f64,
82) -> Option<f64> {
83 if !coverage_factor.is_finite() || coverage_factor < 0.0 {
84 return None;
85 }
86 let u_c = combined_standard_uncertainty(components)?;
87 Some(coverage_factor * u_c)
88}
89
90#[cfg(test)]
91mod tests {
92 use super::*;
93
94 fn mk(u: f64, ty: GumType) -> UncertaintyComponent {
95 UncertaintyComponent { name: "test", ty, standard_uncertainty: u }
96 }
97
98 #[test]
99 fn empty_components_is_none() {
100 assert!(combined_standard_uncertainty(&[]).is_none());
101 }
102
103 #[test]
104 fn single_component_passthrough() {
105 let c = [mk(0.5, GumType::A)];
106 let u = combined_standard_uncertainty(&c).expect("finite");
107 assert!((u - 0.5).abs() < 1e-12);
108 }
109
110 #[test]
111 fn quadrature_sum() {
112 let c = [mk(3.0, GumType::A), mk(4.0, GumType::B)];
114 let u = combined_standard_uncertainty(&c).expect("finite");
115 assert!((u - 5.0).abs() < 1e-12);
116 }
117
118 #[test]
119 fn rejects_negative_or_non_finite() {
120 assert!(combined_standard_uncertainty(&[mk(-0.1, GumType::A)]).is_none());
121 assert!(combined_standard_uncertainty(&[mk(f64::NAN, GumType::A)]).is_none());
122 }
123
124 #[test]
125 fn expanded_is_k_times_combined() {
126 let c = [mk(3.0, GumType::A), mk(4.0, GumType::B)];
127 let u95 = expanded_uncertainty(&c, 2.0).expect("finite");
128 assert!((u95 - 10.0).abs() < 1e-12);
129 }
130
131 #[test]
132 fn labels_are_stable() {
133 assert_eq!(GumType::A.label(), "TypeA");
134 assert_eq!(GumType::B.label(), "TypeB");
135 }
136}