Skip to main content

continuous_blocks/
transfer_function.rs

1use crate::state_space::StateSpace;
2
3/// A single-input single-output transfer function `N(s) / D(s)`, coefficients highest-degree
4/// first (e.g. `s + 1` is `[1.0, 1.0]`). `D` must be nonzero-leading (degree = `den.len() - 1`)
5/// and `deg(N) <= deg(D)` — a proper transfer function; this is what every realizable
6/// continuous-time physical system is.
7#[derive(Debug, Clone, PartialEq)]
8pub struct TransferFunction {
9    pub num: Vec<f64>,
10    pub den: Vec<f64>,
11}
12
13#[derive(Debug, Clone, Copy, PartialEq, Eq)]
14pub enum TransferFunctionError {
15    EmptyDenominator,
16    ZeroLeadingDenominatorCoefficient,
17    ImproperTransferFunction,
18}
19
20impl TransferFunction {
21    pub fn new(num: Vec<f64>, den: Vec<f64>) -> Result<Self, TransferFunctionError> {
22        if den.is_empty() {
23            return Err(TransferFunctionError::EmptyDenominator);
24        }
25        if den[0] == 0.0 {
26            return Err(TransferFunctionError::ZeroLeadingDenominatorCoefficient);
27        }
28        if num.len() > den.len() {
29            return Err(TransferFunctionError::ImproperTransferFunction);
30        }
31        Ok(TransferFunction { num, den })
32    }
33
34    /// Realizes this transfer function in controllable canonical form.
35    ///
36    /// `D(s) = s^n + a_{n-1} s^{n-1} + ... + a_0` (monic, after normalizing by the original
37    /// leading coefficient). If `deg(N) == n`, one step of polynomial long division extracts
38    /// the direct feedthrough `d = N[0]` (after normalization) and leaves a strictly proper
39    /// remainder `R(s) = b_{n-1} s^{n-1} + ... + b_0`; otherwise `d = 0` and `R = N`.
40    ///
41    /// ```text
42    /// A = [ 0      1      0     ...  0    ]      B = [0]
43    ///     [ 0      0      1     ...  0    ]          [0]
44    ///     [ ...                             ]         [...]
45    ///     [ 0      0      0     ...  1    ]          [0]
46    ///     [-a_0   -a_1   -a_2   ... -a_{n-1}]         [1]
47    ///
48    /// C = [b_0, b_1, ..., b_{n-1}]      D = [d]
49    /// ```
50    ///
51    /// Verified against a first-order case by hand (`docs`/tests): for `1/(s+1)`, this gives
52    /// exactly `A=[[-1]], B=[[1]], C=[[1]], D=[[0]]`, the textbook single-state realization.
53    pub fn to_state_space(&self) -> StateSpace {
54        let leading = self.den[0];
55        let n = self.den.len() - 1;
56
57        // Monic denominator coefficients a_0..a_{n-1} (a_i is the coefficient of s^i).
58        let mut a_coeffs = vec![0.0; n];
59        for (power, coeff) in self.den[1..].iter().rev().enumerate() {
60            a_coeffs[power] = coeff / leading;
61        }
62
63        // Numerator, normalized by the same leading coefficient, padded to length n+1
64        // (highest-degree first) so direct feedthrough extraction is a uniform subtraction.
65        let mut num_norm = vec![0.0; n + 1 - self.num.len()];
66        num_norm.extend(self.num.iter().map(|c| c / leading));
67
68        let d_feedthrough = num_norm[0];
69        // Remainder = N(s) - d * D_monic(s), both already highest-degree first, length n+1;
70        // dropping the (now-zero) leading term leaves the n remaining coefficients.
71        let mut den_monic = vec![1.0];
72        den_monic.extend(a_coeffs.iter().rev());
73        let remainder: Vec<f64> = num_norm
74            .iter()
75            .zip(den_monic.iter())
76            .map(|(ni, di)| ni - d_feedthrough * di)
77            .collect();
78        // b_i is the coefficient of s^i; remainder[1..] is highest-degree-first length n.
79        let b_coeffs: Vec<f64> = remainder[1..].iter().rev().copied().collect();
80
81        let a: Vec<Vec<f64>> = (0..n)
82            .map(|row| {
83                if row < n - 1 {
84                    (0..n)
85                        .map(|col| if col == row + 1 { 1.0 } else { 0.0 })
86                        .collect()
87                } else {
88                    (0..n).map(|col| -a_coeffs[col]).collect()
89                }
90            })
91            .collect();
92        let b: Vec<Vec<f64>> = (0..n)
93            .map(|row| vec![if row == n - 1 { 1.0 } else { 0.0 }])
94            .collect();
95        let c: Vec<Vec<f64>> = vec![b_coeffs];
96        let d: Vec<Vec<f64>> = vec![vec![d_feedthrough]];
97
98        // e: None always passes StateSpace::new's own validation trivially.
99        StateSpace::new(a, b, c, d, None).expect("e=None is always a valid descriptor matrix")
100    }
101}
102
103#[cfg(test)]
104mod tests {
105    use super::*;
106
107    /// 1/(s+1): a_0=1, b_0=1, d=0 -- matches the textbook single-state realization exactly,
108    /// derived by hand before writing this test (see `docs/architecture.md`'s Milestone 5
109    /// note).
110    #[test]
111    fn first_order_lowpass_matches_hand_derived_canonical_form() {
112        let tf = TransferFunction::new(vec![1.0], vec![1.0, 1.0]).unwrap();
113        let ss = tf.to_state_space();
114        assert_eq!(ss.a, vec![vec![-1.0]]);
115        assert_eq!(ss.b, vec![vec![1.0]]);
116        assert_eq!(ss.c, vec![vec![1.0]]);
117        assert_eq!(ss.d, vec![vec![0.0]]);
118    }
119
120    #[test]
121    fn rejects_improper_transfer_function() {
122        let err = TransferFunction::new(vec![1.0, 0.0, 0.0], vec![1.0, 1.0]).unwrap_err();
123        assert_eq!(err, TransferFunctionError::ImproperTransferFunction);
124    }
125}