cal_model/calibrate.rs
1//! Calibration fitting: monotone piecewise-linear curves and bounded
2//! working-point optimisation.
3//!
4//! These are the two computations a calibration engineer reaches for when
5//! the ECU will not tell them the answer — a measured set of points that
6//! must become a curve the ECU can interpolate, and a set of adjustable
7//! parameters whose best combination has to be found inside their limits.
8//!
9//! # Curve nodes are parameter *intervals*
10//!
11//! A [`CalParameter`] is one node of a curve under construction: `value` is
12//! the ordinate (the calibrated output at that node) and `lower`/`upper`
13//! are the abscissa bounds of the operating condition it was measured at —
14//! "`idle` between 700 and 900 rpm". The node's abscissa is the interval's
15//! midpoint, which is what makes the fit reproducible from the measurement
16//! record rather than from an implicit index order. Nodes are sorted by
17//! abscissa before the fit, so the caller may pass them in any order.
18//!
19//! # Monotonicity is enforced, not assumed
20//!
21//! A calibration curve whose ordinates change direction makes the ECU's
22//! interpolation physically wrong at the turn, so [`calibrate_curve`]
23//! rejects it with [`CalError::NonMonotone`] rather than fitting a curve
24//! that lies. Both directions are accepted — a gain that falls with
25//! temperature is as legitimate as one that rises with load.
26//!
27//! # The optimiser is deterministic
28//!
29//! [`optimize_working_point`] is bounded coordinate descent from a fixed
30//! set of starting points (each parameter's midpoint and the box corners),
31//! halving the step on every sweep without improvement. Same inputs, same
32//! answer, every run — which is the only acceptable behaviour for something
33//! that ends up in an ECU's flash. A non-finite objective is reported as
34//! [`CalError::Convergence`] carrying the iteration count, never silently
35//! returning a number from a diverged search.
36
37use crate::error::CalError;
38use std::cmp::Ordering;
39use std::fmt;
40
41/// Sweep budget for coordinate descent before the optimiser gives up.
42const MAX_SWEEPS: u32 = 400;
43
44/// Relative step below which a sweep counts as converged.
45const STEP_TOLERANCE: f64 = 1e-9;
46
47/// One node of a curve under construction, or one adjustable of an
48/// optimisation.
49#[derive(Debug, Clone, PartialEq)]
50pub struct CalParameter {
51 /// What this node is — the operating condition or the parameter name.
52 pub name: String,
53 /// For a curve node, the ordinate (the calibrated value). For an
54 /// adjustable, the starting point.
55 pub value: f64,
56 /// Lower bound: the abscissa interval's start (a curve node) or the
57 /// parameter's calibration limit.
58 pub lower: f64,
59 /// Upper bound: the abscissa interval's end (a curve node) or the
60 /// parameter's calibration limit.
61 pub upper: f64,
62}
63
64impl CalParameter {
65 /// A parameter with a name, a value, and bounds.
66 #[must_use]
67 pub fn new(name: impl Into<String>, value: f64, lower: f64, upper: f64) -> Self {
68 Self {
69 name: name.into(),
70 value,
71 lower,
72 upper,
73 }
74 }
75
76 /// The abscissa of a curve node: the midpoint of its interval.
77 #[must_use]
78 pub fn midpoint(&self) -> f64 {
79 // f64::midpoint rather than (lower + upper) / 2.0: it cannot
80 // overflow on two finite bounds, which (a + b) cannot promise.
81 self.lower.midpoint(self.upper)
82 }
83
84 /// Clamp `x` into this parameter's bounds.
85 #[must_use]
86 pub fn clamp(&self, x: f64) -> f64 {
87 if x < self.lower {
88 self.lower
89 } else if x > self.upper {
90 self.upper
91 } else {
92 x
93 }
94 }
95}
96
97impl fmt::Display for CalParameter {
98 fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
99 write!(
100 f,
101 "{} = {:.6} in [{:.6}, {:.6}]",
102 self.name, self.value, self.lower, self.upper
103 )
104 }
105}
106
107/// A monotone piecewise-linear calibration curve.
108#[derive(Debug, Clone, PartialEq)]
109pub struct Curve {
110 points: Vec<(f64, f64)>,
111}
112
113impl Curve {
114 /// Build a curve from `(abscissa, ordinate)` points.
115 ///
116 /// # Errors
117 ///
118 /// [`CalError::Unsupported`] when fewer than two points are given, when
119 /// the abscissae are not strictly ascending, or when any point is
120 /// non-finite.
121 pub fn new(points: Vec<(f64, f64)>) -> Result<Self, CalError> {
122 if points.len() < 2 {
123 return Err(CalError::Unsupported {
124 subject: "calibration curve".to_string(),
125 reason: format!("{} point(s) given, at least 2 are required", points.len()),
126 });
127 }
128 if points
129 .iter()
130 .any(|&(x, y)| !x.is_finite() || !y.is_finite())
131 {
132 return Err(CalError::Unsupported {
133 subject: "calibration curve".to_string(),
134 reason: "points must be finite".to_string(),
135 });
136 }
137 // Ascending and strictly increasing: `partial_cmp` rather than a
138 // negated `<`, so a NaN abscissa is visibly incomparable rather than
139 // silently "not ascending".
140 if points.windows(2).any(|pair| match pair {
141 [first, second] => !matches!(first.0.partial_cmp(&second.0), Some(Ordering::Less)),
142 _ => true,
143 }) {
144 return Err(CalError::Unsupported {
145 subject: "calibration curve".to_string(),
146 reason: "abscissae must be strictly ascending".to_string(),
147 });
148 }
149 Ok(Self { points })
150 }
151
152 /// The fitted points, ascending by abscissa.
153 #[must_use]
154 pub fn points(&self) -> &[(f64, f64)] {
155 &self.points
156 }
157
158 /// Number of nodes.
159 #[must_use]
160 pub fn len(&self) -> usize {
161 self.points.len()
162 }
163
164 /// `true` when the curve carries no nodes (never true for a curve built
165 /// through [`Curve::new`]).
166 #[must_use]
167 pub fn is_empty(&self) -> bool {
168 self.points.is_empty()
169 }
170
171 /// `true` when the ordinates never change direction.
172 #[must_use]
173 pub fn is_monotone(&self) -> bool {
174 let mut rising = false;
175 let mut falling = false;
176 for pair in self.points.windows(2) {
177 let [first, second] = pair else { continue };
178 if second.1 > first.1 {
179 rising = true;
180 } else if second.1 < first.1 {
181 falling = true;
182 }
183 }
184 !(rising && falling)
185 }
186
187 /// Evaluate the curve at `x`: linear interpolation between the
188 /// bracketing nodes, clamped to the end nodes outside the domain (the
189 /// ASAP2 `TAB_INTP` convention).
190 #[must_use]
191 pub fn eval(&self, x: f64) -> f64 {
192 let Some(&(first_x, first_y)) = self.points.first() else {
193 return f64::NAN;
194 };
195 if x <= first_x {
196 return first_y;
197 }
198 let Some(&(last_x, last_y)) = self.points.last() else {
199 return f64::NAN;
200 };
201 if x >= last_x {
202 return last_y;
203 }
204 for pair in self.points.windows(2) {
205 let [first, second] = pair else { continue };
206 let (x0, y0) = *first;
207 let (x1, y1) = *second;
208 if x >= x0 && x <= x1 {
209 let span = x1 - x0;
210 if span == 0.0 {
211 return y0;
212 }
213 return y0 + (y1 - y0) * (x - x0) / span;
214 }
215 }
216 last_y
217 }
218}
219
220impl fmt::Display for Curve {
221 fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
222 writeln!(f, "calibration curve ({} points):", self.points.len())?;
223 for &(x, y) in &self.points {
224 writeln!(f, " x = {x:>12.4} y = {y:>12.6}")?;
225 }
226 Ok(())
227 }
228}
229
230/// Fit a monotone piecewise-linear curve through the given nodes.
231///
232/// Nodes are ordered by their abscissa (the midpoint of `lower..upper`) and
233/// the ordinates must not change direction. The fit reproduces every node
234/// exactly and interpolates linearly between them — which is precisely what
235/// an ECU's `TAB_INTP` conversion does, so what this returns is what the ECU
236/// will compute.
237///
238/// # Errors
239///
240/// [`CalError::Unsupported`] for fewer than two nodes, non-finite bounds, or
241/// two nodes sharing an abscissa; [`CalError::OutOfBounds`] when a node's
242/// `lower` exceeds its `upper`; [`CalError::NonMonotone`] when the ordinates
243/// change direction.
244pub fn calibrate_curve(params: &[CalParameter]) -> Result<Curve, CalError> {
245 let mut nodes: Vec<(f64, &CalParameter)> = Vec::with_capacity(params.len());
246 for param in params {
247 validate_bounds(param)?;
248 nodes.push((param.midpoint(), param));
249 }
250 nodes.sort_by(|a, b| {
251 a.0.partial_cmp(&b.0)
252 .unwrap_or(std::cmp::Ordering::Equal)
253 .then_with(|| a.1.name.cmp(&b.1.name))
254 });
255 for pair in nodes.windows(2) {
256 let [first, second] = pair else { continue };
257 if first.0 == second.0 {
258 return Err(CalError::Unsupported {
259 subject: "calibration curve".to_string(),
260 reason: format!(
261 "nodes `{}` and `{}` share the abscissa {}",
262 first.1.name, second.1.name, first.0
263 ),
264 });
265 }
266 }
267 let mut points: Vec<(f64, f64)> = Vec::with_capacity(nodes.len());
268 let mut direction = 0_i8;
269 for (abscissa, param) in &nodes {
270 if let Some(&(_, previous)) = points.last() {
271 if param.value > previous && direction < 0 {
272 return Err(CalError::NonMonotone {
273 name: param.name.clone(),
274 value: param.value,
275 previous,
276 });
277 }
278 if param.value < previous && direction > 0 {
279 return Err(CalError::NonMonotone {
280 name: param.name.clone(),
281 value: param.value,
282 previous,
283 });
284 }
285 if param.value > previous {
286 direction = 1;
287 } else if param.value < previous {
288 direction = -1;
289 }
290 }
291 points.push((*abscissa, param.value));
292 }
293 Curve::new(points)
294}
295
296/// Check a parameter's bounds are usable.
297fn validate_bounds(param: &CalParameter) -> Result<(), CalError> {
298 if !param.lower.is_finite() || !param.upper.is_finite() || !param.value.is_finite() {
299 return Err(CalError::Unsupported {
300 subject: format!("parameter `{}`", param.name),
301 reason: "value and bounds must be finite".to_string(),
302 });
303 }
304 if param.lower > param.upper {
305 return Err(CalError::OutOfBounds {
306 name: param.name.clone(),
307 value: param.lower,
308 lower: param.lower,
309 upper: param.upper,
310 });
311 }
312 Ok(())
313}
314
315/// Maximise `objective` over the box defined by `params`, returning the
316/// point and its value.
317///
318/// Coordinate descent from each parameter's midpoint and from the box
319/// corners, halving the step after every sweep without improvement, until
320/// the step falls below [`STEP_TOLERANCE`] or [`MAX_SWEEPS`] sweeps pass.
321/// The returned point is always inside the box.
322///
323/// # Errors
324///
325/// [`CalError::OutOfBounds`] for an empty parameter set or a degenerate
326/// bound, [`CalError::Unsupported`] for non-finite bounds,
327/// [`CalError::Convergence`] when the objective never yields a finite value.
328pub fn optimal_point(
329 params: &[CalParameter],
330 objective: &dyn Fn(&[f64]) -> f64,
331) -> Result<(Vec<f64>, f64), CalError> {
332 if params.is_empty() {
333 return Err(CalError::Unsupported {
334 subject: "optimiser".to_string(),
335 reason: "no parameters to optimise".to_string(),
336 });
337 }
338 for param in params {
339 validate_bounds(param)?;
340 }
341 let n = params.len();
342 let widths: Vec<f64> = params.iter().map(|p| p.upper - p.lower).collect();
343
344 // Deterministic multi-start: the midpoint, then every corner of the box
345 // for small problems, so a descent that would otherwise stall in a
346 // corner's basin starts from that corner too.
347 let mut starts: Vec<Vec<f64>> = vec![params.iter().map(CalParameter::midpoint).collect()];
348 if n <= 8 {
349 for corner in 0..n {
350 for high_corner in [true, false] {
351 let mut point: Vec<f64> = params
352 .iter()
353 .map(|p| if high_corner { p.lower } else { p.upper })
354 .collect();
355 let Some(param) = params.get(corner) else {
356 continue;
357 };
358 let opposite = if high_corner {
359 param.upper
360 } else {
361 param.lower
362 };
363 if let Some(slot) = point.get_mut(corner) {
364 *slot = opposite;
365 }
366 starts.push(point);
367 }
368 }
369 }
370
371 let mut best: Option<(Vec<f64>, f64)> = None;
372 let mut worst_sweeps = 0_u32;
373 for start in starts {
374 match descend(params, &widths, objective, start) {
375 Ok(found) => {
376 let replace = best.as_ref().is_none_or(|(_, value)| found.1 > *value);
377 if replace {
378 best = Some(found);
379 }
380 }
381 Err(sweeps) => worst_sweeps = worst_sweeps.max(sweeps),
382 }
383 }
384 best.ok_or(CalError::Convergence {
385 iterations: worst_sweeps.max(1),
386 })
387}
388
389/// One coordinate-descent run from `start`.
390///
391/// `Ok` carries the point and its value; `Err` carries the number of sweeps
392/// the run consumed before giving up.
393fn descend(
394 params: &[CalParameter],
395 widths: &[f64],
396 objective: &dyn Fn(&[f64]) -> f64,
397 mut point: Vec<f64>,
398) -> Result<(Vec<f64>, f64), u32> {
399 for (index, param) in params.iter().enumerate() {
400 if let Some(slot) = point.get_mut(index) {
401 *slot = param.clamp(*slot);
402 }
403 }
404 // A non-finite start is still worth walking away from (the box corners
405 // may well be finite), but a run that never sees a finite value reports
406 // failure rather than returning -inf as an answer.
407 let initial = objective(&point);
408 let mut value = if initial.is_finite() {
409 initial
410 } else {
411 f64::NEG_INFINITY
412 };
413 let mut steps: Vec<f64> = widths.iter().map(|w| 0.5 * w).collect();
414 let mut sweep = 0_u32;
415 loop {
416 if sweep >= MAX_SWEEPS {
417 return Err(MAX_SWEEPS);
418 }
419 sweep += 1;
420 let mut improved = false;
421 let mut attempted = false;
422 let mut finite_trial = false;
423 for index in 0..point.len() {
424 for direction in [1.0_f64, -1.0_f64] {
425 let step = steps.get(index).copied().unwrap_or(0.0);
426 let current = point.get(index).copied().unwrap_or(0.0);
427 let Some(param) = params.get(index) else {
428 continue;
429 };
430 let trial = param.clamp(current + direction * step);
431 if trial == current {
432 continue;
433 }
434 attempted = true;
435 let mut candidate = point.clone();
436 if let Some(slot) = candidate.get_mut(index) {
437 *slot = trial;
438 }
439 let score = objective(&candidate);
440 if !score.is_finite() {
441 continue;
442 }
443 finite_trial = true;
444 if score > value {
445 value = score;
446 point = candidate;
447 improved = true;
448 break;
449 }
450 }
451 }
452 if improved {
453 continue;
454 }
455 if !finite_trial {
456 // No trial moved the needle. Either every trial was rejected as
457 // non-finite (a diverged objective — report it), or the box has
458 // collapsed to a point (there is nowhere left to search, which is
459 // a converged answer, not a failure).
460 return if attempted {
461 Err(sweep)
462 } else {
463 Ok((point, value))
464 };
465 }
466 let mut moving = false;
467 for step in &mut steps {
468 *step *= 0.5;
469 if *step > STEP_TOLERANCE {
470 moving = true;
471 }
472 }
473 if !moving {
474 return Ok((point, value));
475 }
476 }
477}
478
479/// Maximise `objective` over the box defined by `params` and return the best
480/// value found.
481///
482/// See [`optimal_point`] for the search itself and its guarantees; this is
483/// the value-only form calibration tools usually want.
484///
485/// # Errors
486///
487/// As [`optimal_point`].
488pub fn optimize_working_point(
489 params: &[CalParameter],
490 objective: &dyn Fn(&[f64]) -> f64,
491) -> Result<f64, CalError> {
492 optimal_point(params, objective).map(|(_, value)| value)
493}