dsmc: Digitalservo Motion Control Library

A Rust library for designing and analyzing motion control systems: transfer functions and state space models, discretization, frequency analysis, trajectory generation, feedforward control (perfect tracking control), signal processing and system identification.

Features

Installation

cargo add dsmc

or in Cargo.toml:

[dependencies]
dsmc = "0.1"

Examples

Transfer functions

use dsmc::{tf, TransferFunction};

// From a string: the variable (`s` or `z`) selects continuous / discrete time at compile time.
let wn = 100.0;
let plant = tf!("{} / (s^2 + {} s + {})", wn * wn, 0.2 * wn, wn * wn);
let controller = tf!("(10 s + 100) / s");

// Arithmetic (common poles / zeros are cancelled)
let open_loop = &controller * &plant;
println!("{open_loop}");

// Closed loop under unity negative feedback: L / (1 + L), same as `&open_loop / &(1.0 + &open_loop)`
let closed_loop = open_loop.unity_feedback();

// From coefficients (descending order), or parsed at runtime with error handling
let g = TransferFunction::continuous(&[1.0], &[1.0, 3.0, 2.0]);
let h: TransferFunction<f64> = "1 / (s + 1)^2".parse().unwrap();

// Poles, zeros, partial fractions and the inverse Laplace transform
let poles = g.pz_map().poles;
println!("{:.3}", g.partial_fraction().time_domain()); // x(t) = exp(-1.000t) - exp(-2.000t)
let x = h.inverse_laplace();
assert!((x(1.0) - (-1.0f64).exp()).abs() < 1e-9);

Discretization and simulation

use dsmc::{tf, DiscreteSystem, StateSpace};
use dsmc::discretize::{BackwardDifference, DiscretizeMethod, MatchedZ, Tustin, Zoh, ZerosAtInfinity};

let g = tf!("100 / (s + 100)");
let ts = 1e-3;

let g_zoh = g.discretize(Zoh, ts).unwrap();
let g_tustin = g.discretize(Tustin, ts).unwrap();
let g_backward = g.discretize(BackwardDifference, ts).unwrap();
let g_matched = g.discretize(MatchedZ(ZerosAtInfinity::MinusOne), ts).unwrap();
let ss_tustin = StateSpace::try_from(&g).unwrap().discretize(Tustin, ts).unwrap();

// The method chosen at run time
let method = DiscretizeMethod::MatchedZ(ZerosAtInfinity::KeepOneDelay);
let g_z = g.discretize(method, ts).unwrap();

// Step response, one sample at a time: a DiscreteSystem runs a discrete-time transfer
// function (its difference equation; single input and output: scalar in, scalar out)...
let mut system = DiscreteSystem::try_from(&g_zoh).unwrap();
let y: Vec<f64> = (0..100).map(|_| system.update(1.0)).collect();
// ...or a discrete-time state-space model (any number of inputs / outputs)
let mut system = DiscreteSystem::from(StateSpace::try_from(&g).unwrap().discretize(Zoh, ts).unwrap());
let y: Vec<f64> = (0..100).map(|_| system.update(&[1.0]).unwrap()[0]).collect();

Bode diagram and Nyquist plot

use dsmc::{tf, BodeDiagramPlotter, FrequencyTransferFunction, NyquistPlotter, RadPerSec};
use num_complex::Complex;

let plant = tf!("1 / (s (0.01 s + 1))");
let controller = tf!("(100 s + 1000) / s");

// Any element can be put in series, e.g. a dead time of 1 ms
let delay = FrequencyTransferFunction::new(|w: f64| Complex::new(0.0, -w * 1e-3).exp());
let open_loop = &(&controller * &plant).frequency_transfer_function() * &delay;

// Gain [dB] and phase [rad] from 0.1 Hz to 1 kHz
let bode = BodeDiagramPlotter::<f64>::new(0.1, 1000.0, 0.1, true).plot(&open_loop);
// The same in rad/s
let bode_rad = BodeDiagramPlotter::<f64, RadPerSec>::new(1.0, 6000.0, 1.0, true).plot(&open_loop);
let nyquist = NyquistPlotter::<f64>::new(0.1, 1000.0, 0.1).plot(&open_loop);

// Single points
let c = open_loop.characteristics::<RadPerSec>(100.0, true);
println!("{:.1} dB, {:.1} deg", c.gain, c.phase.to_degrees());

Results can be written to CSV directly (one row per point: frequency, gain, phase or omega, re, im):

use dsmc::{tf, BodeDiagramPlotter};
use dsmc::logger::DataStorage;

let bode = BodeDiagramPlotter::<f64>::new(0.1, 1000.0, 0.1, true).plot(&tf!("1 / (s + 1)"));

let mut csv = DataStorage::new("./out/bode.csv").unwrap().set_header(["frequency", "gain", "phase"]);
for point in &bode {
    csv.add(point).unwrap();
}
csv.close().unwrap();

Trajectory and perfect tracking control

use dsmc::{tf, StateSpace};
use dsmc::feedforward::ptc::LiftedDiscretizedSystem;
use dsmc::trajectory::{self, ModifiedSine, Trajectory};

// Normalized motion profiles: position, velocity and acceleration over x = 0..1
let profile = ModifiedSine.generate(1.0_f64, 1001);

// Feedforward input that makes the output of a nonminimum-phase plant follow a smooth move
// exactly at every frame (with pre-actuation before the move starts)
let ts = 1e-4;
let plant = tf!("(1 - 0.001 s) / (s (0.005 s + 1) (0.001 s + 1))");
let y_d = trajectory::smoothstep::piecewise(1.0, 0.05, 0.02, 4); // distance, duration, start, smoothness

let model = StateSpace::normalized_controllable_canonical(&plant).unwrap();
let lifted = LiftedDiscretizedSystem::new(&model, ts).unwrap();
let u = lifted.calculate_ptc_input_for_reference_output(&y_d, 900).unwrap();

System identification

use dsmc::tf;
use dsmc::DiscreteSystem;
use dsmc::discretize::Zoh;
use dsmc::system_identification::lsm;

let ts = 1e-3;
let mut plant = DiscreteSystem::try_from(&tf!("1000 / (s^2 + 20 s + 1000)").discretize(Zoh, ts).unwrap()).unwrap();

// ARX model y[k] = a1 y[k-1] + a2 y[k-2] + b0 u[k-1] + b1 u[k-2]
let mut arx = lsm::arx::LeastSquares::<f64>::new(2, 1).with_input_delay(1);
let mut y_prev = 0.0;
for k in 0..1000 {
    let t = k as f64 * ts;
    let u = (1..50).map(|i| (i as f64 * 10.0 * t).sin()).sum::<f64>();
    let y = plant.update(u);
    arx.add(u, y_prev, y);
    y_prev = y;
}
let g_z = arx.identify().unwrap();
println!("{g_z}");

With measurement noise on the output, least squares is biased. The instrumental variable (IV) method removes the bias, either for the discrete-time ARX model or, with SRIVC (simplified refined instrumental variable method for continuous-time systems), directly for G(s) from sampled data:

use dsmc::tf;
use dsmc::DiscreteSystem;
use dsmc::discretize::Zoh;
use dsmc::system_identification::{arx::Arx, iv};
use dsmc::system_identification::iv::srivc::{Initialization, SrivcOptions};

let ts = 1e-3;
let mut plant = DiscreteSystem::try_from(&tf!("1000 / (s^2 + 20 s + 1000)").discretize(Zoh, ts).unwrap()).unwrap();
let u: Vec<f64> = (0..5000)
    .map(|k| (1..50).map(|i| (i as f64 * 10.0 * k as f64 * ts).sin()).sum::<f64>())
    .collect();
let y: Vec<f64> = u.iter().map(|&uk| plant.update(uk)).collect(); // + noise

// ARX model by least squares, then 3 IV iterations (each with the previous estimate as the
// auxiliary model generating the instruments)
let model = iv::arx::identify(&u, &y, &Arx::new(2, 1).with_input_delay(1), 3).unwrap();
let g_z = model.transfer_function();

// Continuous-time B(s) / A(s) with deg A = 2, deg B = 0 (orders: denominator first), started
// from a state-variable filter 1 / (s + 30)^2
let result = iv::srivc::identify(
    &u, &y, ts, 2, 0, &Initialization::StateVariableFilter(30.0), &SrivcOptions::default(),
).unwrap();
let g_s = result.model;

An identified model is validated on another experiment (system_identification::validation) by its residual ε = y - ŷ, with ŷ the output of the model simulated with the measured input (output error: Validation::continuous, Validation::discrete) or, for an ARX model with its noise model, the one-step prediction (Validation::one_step_prediction):

use dsmc::tf;
use dsmc::DiscreteSystem;
use dsmc::discretize::Zoh;
use dsmc::signal::excitation::chirp;
use dsmc::system_identification::validation::Validation;

let ts = 1e-3;
let u: Vec<f64> = chirp(20.0, ts, 0.5, 50.0).unwrap(); // 0.5 .. 50 Hz over 20 s (non-periodic)
let g_s = tf!("1000 / (s^2 + 20 s + 1000)");
let mut plant = DiscreteSystem::try_from(&g_s.discretize(Zoh, ts).unwrap()).unwrap();
let y: Vec<f64> = u.iter().map(|&uk| plant.update(uk)).collect(); // + noise

let validation = Validation::continuous(&g_s, ts, &u, &y).unwrap();
let (bic, aic) = (validation.bic(3), validation.aic(3)); // p = n + m + 1
let correlation = validation.cross_correlation(50, 2.58); // r(τ) within ±bound at 99 % per lag
let coherence = validation.coherence_test(1.0, 0.99).unwrap().excited(1e-2); // 1 s segments: 1 Hz bins
let response = validation.frequency_response(1.0).unwrap().excited(1e-2);
println!(
    "BIC {bic}, AIC {aic}, lags outside {}, bins outside {}, max gain error {} dB",
    correlation.fraction_outside(),
    coherence.fraction_outside(),
    response.gain_error_db().iter().fold(0.0f64, |m, e| m.max(e.abs())),
);

For an ARX model, the one-step prediction error must be white and independent of the input:

use dsmc::system_identification::{lsm, validation::Validation};

let noise = |k: usize| {
    // splitmix64: uniform in [-0.5, 0.5)
    let mut z = (k as u64 + 1).wrapping_mul(0x9E37_79B9_7F4A_7C15);
    z = (z ^ (z >> 30)).wrapping_mul(0xBF58_476D_1CE4_E5B9);
    z = (z ^ (z >> 27)).wrapping_mul(0x94D0_49BB_1331_11EB);
    ((z ^ (z >> 31)) >> 11) as f64 / (1u64 << 53) as f64 - 0.5
};
let u: Vec<f64> = (0..2000).map(|k| noise(k + 10_000)).collect();
let mut y = vec![0.0; u.len()];
for k in 2..u.len() {
    let e = 0.01 * noise(k); // equation error
    y[k] = 1.5 * y[k - 1] - 0.7 * y[k - 2] + u[k - 1] + 0.5 * u[k - 2] + e;
}

let mut arx = lsm::arx::LeastSquares::<f64>::new(2, 1).with_input_delay(1);
for k in 0..u.len() {
    arx.add(u[k], if k > 0 { y[k - 1] } else { 0.0 }, y[k]);
}
arx.identify().unwrap();

let ts = 1e-3; // sampling period of the data
let validation = Validation::one_step_prediction(&arx.arx, ts, &u, &y).unwrap();
let whiteness = validation.autocorrelation(20, 2.58);
let white = whiteness.ljung_box <= whiteness.ljung_box_bound(2.33); // χ²(20) at 99 %
let independent = validation.cross_correlation(50, 2.58).fraction_outside() < 0.05;
let bic = validation.bic(arx.arx.parameter_len());
assert!(white && independent);

The tests can be run together with one confidence level by Validation::check, which also decides pass / fail (whiteness by Ljung-Box, cross-correlation with a Bonferroni bound over the lags, coherence and lines by the number outside against a binomial quantile) and prints a report:

use dsmc::tf;
use dsmc::system_identification::validation::{Check, CoherenceCheck, Validation};

let ts = 1e-3;
let g_s = tf!("1000 / (s^2 + 20 s + 1000)");
let noise = |k: usize| {
    // splitmix64: uniform in [-0.5, 0.5)
    let mut z = (k as u64 + 1).wrapping_mul(0x9E37_79B9_7F4A_7C15);
    z = (z ^ (z >> 30)).wrapping_mul(0xBF58_476D_1CE4_E5B9);
    z = (z ^ (z >> 27)).wrapping_mul(0x94D0_49BB_1331_11EB);
    ((z ^ (z >> 31)) >> 11) as f64 / (1u64 << 53) as f64 - 0.5
};
let u: Vec<f64> = (0..10000).map(|k| noise(k + 100_000)).collect();
let y0 = Validation::continuous(&g_s, ts, &u, &u).unwrap().simulated; // plant output
let y: Vec<f64> = y0.iter().enumerate().map(|(k, v)| v + 0.01 * noise(k)).collect();

let report = Validation::continuous(&g_s, ts, &u, &y)
    .unwrap()
    .check(
        &[
            Check::InformationCriteria { parameters: 3 },
            Check::Whiteness { max_lag: 20 },
            Check::CrossCorrelation { max_lag: 50 },
            Check::Coherence(CoherenceCheck::new(1.0, 1e-2)),
        ],
        0.99,
    )
    .unwrap();
println!("{report}"); // one line per check, then "overall: passed"
assert!(report.passed());

A model that leaves out modes on purpose (e.g. a rigid-body model of a plant whose resonances are above the control bandwidth) fails the tests on the whole band, rightly: the residual keeps the error of those modes. CoherenceCheck::new(segment_s, excited).set_band((low, high)) tests the coherence only in [low, high] [Hz] (CoherenceTest::band, FrequencyResponseComparison::band for a closer look), with a margin to the lowest mode left out; run it without the whiteness and cross-correlation tests, which see the whole band.

Durations (evaluated_from, the segments of coherence_test and frequency_response) are in seconds and the fundamental frequency of line_test in Hz, whole numbers of the sampling period (the period 1 / f0 for the fundamental); the lags of autocorrelation and cross_correlation are in samples.

With a periodic input (e.g. a multisine of fundamental frequency f0 [Hz], excited at the harmonics lines), validation.evaluated_from(1.0 / f0)?.line_test(f0, &lines, 0.99) compares the model error at every excited line with the noise level estimated from the period-to-period variation (Check::Lines { fundamental_frequency: f0, lines } in check).

The structure of a continuous-time model (orders n, m, integrators q and input delay nk, searched together: missing poles are made up for by a longer delay) is chosen by srivc::search: every candidate is identified, validated on another experiment by check, and the lowest BIC among the candidates passing every test is selected:

use dsmc::{tf, TransferFunctionWithDelay};
use dsmc::signal::excitation::multisine;
use dsmc::system_identification::iv::srivc::{self, Initialization, SearchOptions, Structure};
use dsmc::system_identification::validation::Check;

let ts = 1e-3;
let f0 = 1.0; // multisine of fundamental 1 Hz (period 1 s) over 1 .. 40 Hz
let lines: Vec<usize> = (1..=40).collect();
let plant = TransferFunctionWithDelay::new(tf!("1000 / (s^2 + 20 s + 1000)"), 3.0 * ts);
let noise = |k: u64| {
    // splitmix64: uniform in [-0.5, 0.5)
    let mut z = k.wrapping_add(1).wrapping_mul(0x9E37_79B9_7F4A_7C15);
    z = (z ^ (z >> 30)).wrapping_mul(0xBF58_476D_1CE4_E5B9);
    z = (z ^ (z >> 27)).wrapping_mul(0x94D0_49BB_1331_11EB);
    ((z ^ (z >> 31)) >> 11) as f64 / (1u64 << 53) as f64 - 0.5
};
let experiment = |periods: f64, seed: u64| {
    let u: Vec<f64> = multisine(periods / f0, ts, f0, &lines, |_| 1.0, seed).unwrap();
    let y = plant.simulate(ts, &u).unwrap(); // plant output, plus measurement noise:
    let y: Vec<f64> = y.iter().enumerate().map(|(k, v)| v + 0.02 * noise(k as u64 + (seed << 32))).collect();
    (u, y)
};
let (u, y) = experiment(3.0, 1);
let (u_val, y_val) = experiment(5.0, 2);

// n = 1 ..= 3, m = 0 ..= 1, q = 0 (no pole at the origin), nk = 2 ..= 4
let structures = Structure::grid(1..=3, 0..=1, 0..=0, 2..=4);
let options = SearchOptions {
    evaluated_from: 1000, // samples: leave out the transient of the first period
    ..SearchOptions::new(
        Initialization::StateVariableFilter(30.0),
        vec![Check::CrossCorrelation { max_lag: 50 }, Check::Lines { fundamental_frequency: f0, lines: lines.clone() }],
        0.99,
    )
};
let search = srivc::search((&u, &y), (&u_val, &y_val), ts, &structures, &options).unwrap();
println!("{search}"); // one row per candidate: BIC, tests, iterations
assert_eq!(search.selected().unwrap().structure, Structure::new(2, 0, 0, 3));

A plant with a rigid-body mode (poles at, or very close to, the origin) is identified as R(s) / s^q by srivc::identify_with_prefilter with a Prefilter::new(q, omega_c): the input is pseudo-integrated, s / (s + ω_c)^(q+1), and the output high-passed, s^(q+1) / (s + ω_c)^(q+1), so that no integrator is applied to the data. With SearchOptions::prefilter = Some(omega_c), srivc::search searches q with the other parameters of the structure. R may be a constant (n = 0, only with q >= 1): the pure integrator b_0 / s^q, e.g. a single inertia 1 / (J s^2) (torque -> position) is (n, m, q) = (0, 0, 2) with b_0 = 1 / J, or the rigid-body mode alone of a plant whose resonances are above the band of interest.

Data with a drift (e.g. an integrating plant driven by an unknown input offset) are high-passed before the identification by preprocessing::high_pass(&u, &y, cutoff, ts, order), which filters the input and the output alike from rest, so that they are still related by the same G(s).

From measured frequency responses (e.g. dsmc::fft::welch), a continuous-time model can be fitted:

use dsmc::{tf, FrequencyResponse, TransferFunction};
use dsmc::system_identification::frequency_response::vector_fitting::{identify, VectorFittingOptions};

let g = tf!("1000 / (s^2 + 20 s + 1000)").frequency_transfer_function();
let samples: Vec<FrequencyResponse<f64>> = (1..=500)
    .map(|k| {
        let omega = 0.5 * k as f64;
        FrequencyResponse { omega, value: g.response(omega) }
    })
    .collect();

let fitted = identify(&samples, 2, &VectorFittingOptions::default()).unwrap();
let model: TransferFunction<f64> = fitted.into();

The model is kept real (residues of real poles real, of conjugate pairs conjugate) also on noisy measurements. identify_weighted(&samples, &weights, n, &options) weights the error per sample, e.g. by 1 / |G| for the relative error of a response spanning decades.

More examples are in tests/ and in the API documentation.

License

This project is licensed under the MIT License. You are free to use, copy, modify, merge, publish, distribute, sublicense and sell copies of the software, provided that the copyright notice and the permission notice are included in all copies or substantial portions of the software. The software is provided "as is", without warranty of any kind.

Contribution

Unless you explicitly state otherwise, any contribution intentionally submitted for inclusion in this project by you shall be licensed under the MIT License, without any additional terms or conditions.