88 lines
2.5 KiB
Rust
88 lines
2.5 KiB
Rust
pub trait Controller<const D: usize> {
|
|
fn determine_step(&mut self, h: f64, err: f64) -> (bool, f64);
|
|
}
|
|
|
|
#[derive(Debug, Clone, Copy)]
|
|
pub struct PIController {
|
|
pub alpha: f64,
|
|
pub beta: f64,
|
|
pub factor_c1: f64,
|
|
pub factor_c2: f64,
|
|
pub factor_old: f64,
|
|
pub h_max: f64,
|
|
pub safety_factor: f64,
|
|
pub old_h: f64,
|
|
}
|
|
|
|
impl<const D: usize> Controller<D> for PIController {
|
|
/// Determines if the previously run step size and error were valid or not. Either way, it also
|
|
/// returns what the next step size should be
|
|
fn determine_step(&mut self, h: f64, err: f64) -> (bool, f64) {
|
|
let factor_11 = err.powf(self.alpha);
|
|
let factor = self.factor_c2.max(
|
|
self.factor_c1
|
|
.min(factor_11 * self.factor_old.powf(-self.beta) / self.safety_factor),
|
|
);
|
|
let mut h_new = h / factor;
|
|
if err <= 1.0 {
|
|
// Accept the stepsize
|
|
self.factor_old = err.max(1.0e-4);
|
|
if h_new.abs() > self.h_max {
|
|
// If the step is too big
|
|
h_new = self.h_max.copysign(h_new);
|
|
}
|
|
(true, h_new)
|
|
// (true, h_new)
|
|
} else {
|
|
// Reject the stepsize and propose a smaller one
|
|
h_new = h / (self.factor_c1.min(factor_11 / self.safety_factor));
|
|
(false, h_new)
|
|
}
|
|
}
|
|
}
|
|
|
|
impl PIController {
|
|
pub fn new(
|
|
alpha: f64,
|
|
beta: f64,
|
|
max_factor: f64,
|
|
min_factor: f64,
|
|
h_max: f64,
|
|
safety_factor: f64,
|
|
initial_h: f64,
|
|
) -> Self {
|
|
Self {
|
|
alpha: alpha,
|
|
beta: beta,
|
|
factor_c1: 1.0 / min_factor,
|
|
factor_c2: 1.0 / max_factor,
|
|
factor_old: 1.0e-4,
|
|
h_max: h_max.abs(),
|
|
safety_factor: safety_factor,
|
|
old_h: initial_h,
|
|
}
|
|
}
|
|
pub fn default() -> Self {
|
|
Self::new(0.17, 0.04, 10.0, 0.2, 100000.0, 0.9, 1e-4)
|
|
}
|
|
}
|
|
|
|
#[cfg(test)]
|
|
mod tests {
|
|
use super::*;
|
|
|
|
#[test]
|
|
fn test_controller_creation() {
|
|
let controller = PIController::new(0.17, 0.04, 10.0, 0.2, 10.0, 0.9, 1e-4);
|
|
|
|
assert!(controller.alpha == 0.17);
|
|
assert!(controller.beta == 0.04);
|
|
assert!(controller.factor_c1 == 1.0 / 0.2);
|
|
assert!(controller.factor_c2 == 1.0 / 10.0);
|
|
assert!(controller.factor_old == 1.0e-4);
|
|
assert!(controller.h_max == 10.0);
|
|
assert!(controller.safety_factor == 0.9);
|
|
assert!(controller.old_h == 1e-4);
|
|
}
|
|
}
|