Skip to main content

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}