1use serde::Serialize;
2
3use crate::scenario::ScenarioDefinition;
4use crate::trust::{trust_from_envelope, update_envelope};
5
6#[derive(Debug, Clone)]
7pub struct ObserverSpec {
8 pub level: usize,
9 pub name: String,
10 pub gain: f64,
11 pub trust_ceiling: f64,
12 pub trust_beta: f64,
13 pub envelope_decay: f64,
14 pub drift_scale: f64,
15 pub measurement_bias_scale: f64,
16 pub availability_penalty: f64,
17}
18
19#[derive(Debug, Clone, Serialize)]
20pub struct ObserverSeries {
21 pub level: usize,
22 pub name: String,
23 pub prediction: Vec<f64>,
24 pub estimate: Vec<f64>,
25 pub measurement: Vec<Option<f64>>,
26 pub innovation: Vec<f64>,
27 pub residual: Vec<f64>,
28 pub trust: Vec<f64>,
29 pub envelope: Vec<f64>,
30 pub available: Vec<bool>,
31}
32
33impl ObserverSeries {
34 pub fn correction_driver(&self, step: usize) -> f64 {
35 self.innovation[step] + 0.35 * self.residual[step]
36 }
37
38 pub fn recompute_after_estimate_update(&mut self, truth: &[f64], spec: &ObserverSpec) {
39 let mut previous_envelope = 0.0;
40 for step in 0..self.estimate.len() {
41 self.residual[step] = truth[step] - self.estimate[step];
42 let penalty = if self.available[step] {
43 0.0
44 } else {
45 spec.availability_penalty
46 };
47 let next_envelope = update_envelope(
48 previous_envelope,
49 self.residual[step],
50 penalty,
51 spec.envelope_decay,
52 );
53 self.envelope[step] = next_envelope;
54 let mut trust = trust_from_envelope(next_envelope, spec.trust_beta, spec.trust_ceiling);
55 if spec.level == 3 && !self.available[step] {
56 trust = trust.max(spec.trust_ceiling * 0.72);
57 }
58 self.trust[step] = trust;
59 previous_envelope = next_envelope;
60 }
61 }
62}
63
64pub fn build_specs(definition: &ScenarioDefinition) -> Vec<ObserverSpec> {
65 (0..3)
66 .map(|index| ObserverSpec {
67 level: index + 1,
68 name: ScenarioDefinition::level_name(index + 1).to_string(),
69 gain: definition.level_gains[index],
70 trust_ceiling: definition.trust_ceilings[index],
71 trust_beta: definition.trust_betas[index],
72 envelope_decay: definition.envelope_decay[index],
73 drift_scale: definition.drift_scales[index],
74 measurement_bias_scale: definition.measurement_bias_scales[index],
75 availability_penalty: definition.availability_penalties[index],
76 })
77 .collect()
78}
79
80pub fn simulate_observers(
81 definition: &ScenarioDefinition,
82 truth: &[f64],
83) -> (Vec<ObserverSpec>, Vec<ObserverSeries>) {
84 let specs = build_specs(definition);
85 let series = specs
86 .iter()
87 .map(|spec| simulate_single_observer(spec, definition, truth))
88 .collect();
89 (specs, series)
90}
91
92fn simulate_single_observer(
93 spec: &ObserverSpec,
94 definition: &ScenarioDefinition,
95 truth: &[f64],
96) -> ObserverSeries {
97 let n = truth.len();
98 let mut prediction = vec![0.0; n];
99 let mut estimate = vec![0.0; n];
100 let mut measurement = vec![None; n];
101 let mut innovation = vec![0.0; n];
102 let mut residual = vec![0.0; n];
103 let mut trust = vec![0.0; n];
104 let mut envelope = vec![0.0; n];
105 let mut available = vec![false; n];
106 let mut previous_envelope = 0.0;
107
108 for step in 0..n {
109 let u = step as f64;
110 let degrade = definition.degradation_factor(step);
111 let is_available = if spec.level == 3 {
112 definition.l3_available(step)
113 } else {
114 true
115 };
116 available[step] = is_available;
117
118 let base_measurement_bias = spec.measurement_bias_scale
119 * ((0.031 * u + spec.level as f64 * 0.6).sin()
120 + 0.45 * (0.089 * u + spec.level as f64).cos());
121 let degrade_bias = match spec.level {
122 1 => degrade * (0.28 + spec.drift_scale * 2.6 + 0.06 * (0.15 * u).sin()),
123 2 => degrade * (0.12 + spec.drift_scale * 1.8 + 0.04 * (0.11 * u).cos()),
124 _ => 0.0,
125 };
126 let measured_value = truth[step] + base_measurement_bias + degrade_bias;
127 if is_available {
128 measurement[step] = Some(measured_value);
129 }
130
131 prediction[step] = if step == 0 {
132 truth[0] + spec.level as f64 * 0.04
133 } else {
134 let previous_velocity = if step > 1 {
135 estimate[step - 1] - estimate[step - 2]
136 } else {
137 0.0
138 };
139 let drift_term = spec.drift_scale
140 * (0.014 + 0.005 * (0.071 * u + spec.level as f64).sin())
141 * (1.0 + 1.9 * degrade);
142 estimate[step - 1] + 0.89 * previous_velocity + drift_term
143 };
144
145 innovation[step] = if is_available {
146 measured_value - prediction[step]
147 } else {
148 0.0
149 };
150 estimate[step] = if is_available {
151 prediction[step] + spec.gain * innovation[step]
152 } else {
153 prediction[step]
154 };
155 residual[step] = truth[step] - estimate[step];
156
157 let penalty = if is_available {
158 0.0
159 } else {
160 spec.availability_penalty
161 };
162 let next_envelope = update_envelope(
163 previous_envelope,
164 residual[step],
165 penalty,
166 spec.envelope_decay,
167 );
168 envelope[step] = next_envelope;
169 let mut trust_value =
170 trust_from_envelope(next_envelope, spec.trust_beta, spec.trust_ceiling);
171 if spec.level == 3 && !is_available {
172 trust_value = trust_value.max(spec.trust_ceiling * 0.72);
173 }
174 trust[step] = trust_value;
175 previous_envelope = next_envelope;
176 }
177
178 ObserverSeries {
179 level: spec.level,
180 name: spec.name.clone(),
181 prediction,
182 estimate,
183 measurement,
184 innovation,
185 residual,
186 trust,
187 envelope,
188 available,
189 }
190}