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
20/// Why [`TransferFunction::settled_state`]/[`TransferFunction::settled_state_discrete`] could
21/// not produce a state for the requested output.
22#[derive(Debug, Clone, Copy, PartialEq, Eq)]
23pub enum SettledStateError {
24    /// `deg(D) = 0` — a pure gain has no state to initialize at all.
25    NoStates,
26    /// The numerator vanishes at DC ($N(0) = 0$ in continuous time, $N(1) = 0$ in discrete
27    /// time), so the settled output is identically zero and no state holds a nonzero one.
28    ZeroDcNumerator,
29}
30
31impl TransferFunction {
32    pub fn new(num: Vec<f64>, den: Vec<f64>) -> Result<Self, TransferFunctionError> {
33        if den.is_empty() {
34            return Err(TransferFunctionError::EmptyDenominator);
35        }
36        if den[0] == 0.0 {
37            return Err(TransferFunctionError::ZeroLeadingDenominatorCoefficient);
38        }
39        if num.len() > den.len() {
40            return Err(TransferFunctionError::ImproperTransferFunction);
41        }
42        Ok(TransferFunction { num, den })
43    }
44
45    /// Realizes this transfer function in controllable canonical form.
46    ///
47    /// `D(s) = s^n + a_{n-1} s^{n-1} + ... + a_0` (monic, after normalizing by the original
48    /// leading coefficient). If `deg(N) == n`, one step of polynomial long division extracts
49    /// the direct feedthrough `d = N[0]` (after normalization) and leaves a strictly proper
50    /// remainder `R(s) = b_{n-1} s^{n-1} + ... + b_0`; otherwise `d = 0` and `R = N`.
51    ///
52    /// ```text
53    /// A = [ 0      1      0     ...  0    ]      B = [0]
54    ///     [ 0      0      1     ...  0    ]          [0]
55    ///     [ ...                             ]         [...]
56    ///     [ 0      0      0     ...  1    ]          [0]
57    ///     [-a_0   -a_1   -a_2   ... -a_{n-1}]         [1]
58    ///
59    /// C = [b_0, b_1, ..., b_{n-1}]      D = [d]
60    /// ```
61    ///
62    /// Verified against a first-order case by hand (`docs`/tests): for `1/(s+1)`, this gives
63    /// exactly `A=[[-1]], B=[[1]], C=[[1]], D=[[0]]`, the textbook single-state realization.
64    pub fn to_state_space(&self) -> StateSpace {
65        let leading = self.den[0];
66        let n = self.den.len() - 1;
67
68        // Monic denominator coefficients a_0..a_{n-1} (a_i is the coefficient of s^i).
69        let mut a_coeffs = vec![0.0; n];
70        for (power, coeff) in self.den[1..].iter().rev().enumerate() {
71            a_coeffs[power] = coeff / leading;
72        }
73
74        // Numerator, normalized by the same leading coefficient, padded to length n+1
75        // (highest-degree first) so direct feedthrough extraction is a uniform subtraction.
76        let mut num_norm = vec![0.0; n + 1 - self.num.len()];
77        num_norm.extend(self.num.iter().map(|c| c / leading));
78
79        let d_feedthrough = num_norm[0];
80        // Remainder = N(s) - d * D_monic(s), both already highest-degree first, length n+1;
81        // dropping the (now-zero) leading term leaves the n remaining coefficients.
82        let mut den_monic = vec![1.0];
83        den_monic.extend(a_coeffs.iter().rev());
84        let remainder: Vec<f64> = num_norm
85            .iter()
86            .zip(den_monic.iter())
87            .map(|(ni, di)| ni - d_feedthrough * di)
88            .collect();
89        // b_i is the coefficient of s^i; remainder[1..] is highest-degree-first length n.
90        let b_coeffs: Vec<f64> = remainder[1..].iter().rev().copied().collect();
91
92        let a: Vec<Vec<f64>> = (0..n)
93            .map(|row| {
94                if row < n - 1 {
95                    (0..n)
96                        .map(|col| if col == row + 1 { 1.0 } else { 0.0 })
97                        .collect()
98                } else {
99                    (0..n).map(|col| -a_coeffs[col]).collect()
100                }
101            })
102            .collect();
103        let b: Vec<Vec<f64>> = (0..n)
104            .map(|row| vec![if row == n - 1 { 1.0 } else { 0.0 }])
105            .collect();
106        let c: Vec<Vec<f64>> = vec![b_coeffs];
107        let d: Vec<Vec<f64>> = vec![vec![d_feedthrough]];
108
109        // e: None always passes StateSpace::new's own validation trivially.
110        StateSpace::new(a, b, c, d, None).expect("e=None is always a valid descriptor matrix")
111    }
112
113    /// The state of [`Self::to_state_space`]'s realization that a *continuous-time* filter
114    /// already settled at output $y_0$ holds — what a netlist's `y0=` (and `kind=pid`'s own
115    /// `ic=`) means.
116    ///
117    /// In controllable canonical form every state is the derivative of the one before it, so an
118    /// equilibrium is $x = (x_1, 0, \dots, 0)$, and the last row, $-a_0 x_1 + u_0 = 0$, fixes
119    /// the constant input holding it there at $u_0 = a_0 x_1$. The output is then
120    /// $y_0 = b_0 x_1 + d\,u_0 = (b_0 + d\,a_0)\,x_1$, and $b_0 + d\,a_0$ is exactly the
121    /// normalized numerator's constant term $N(0)$, hence
122    ///
123    /// $$x_1 = \frac{y_0}{b_0 + d\,a_0}.$$
124    ///
125    /// Nothing here needs $a_0 \ne 0$: for a transfer function with a pole at the origin (an
126    /// integrator, a PID) the holding input is simply $u_0 = 0$, which is the "integrator
127    /// pre-loaded, zero error" start a controller wants.
128    pub fn settled_state(&self, y0: f64) -> Result<Vec<f64>, SettledStateError> {
129        let ss = self.to_state_space();
130        let n = ss.states();
131        if n == 0 {
132            return Err(SettledStateError::NoStates);
133        }
134        let a0 = -ss.a[n - 1][0];
135        let dc_numerator = ss.c[0][0] + ss.d[0][0] * a0;
136        if dc_numerator == 0.0 {
137            return Err(SettledStateError::ZeroDcNumerator);
138        }
139        let mut x = vec![0.0; n];
140        x[0] = y0 / dc_numerator;
141        Ok(x)
142    }
143
144    /// The *discrete-time* counterpart of [`Self::settled_state`], for the same realization
145    /// stepped as $x[k+1] = A x[k] + B u[k]$.
146    ///
147    /// Here every state is the one before it delayed by a sample, so an equilibrium has all $n$
148    /// states equal, $x_i = \bar{x}$; the last row gives the holding input
149    /// $u_0 = (1 + \sum_i a_i)\,\bar{x}$, and the output is
150    /// $y_0 = \big(\sum_i b_i + d\,(1 + \sum_i a_i)\big)\,\bar{x}$ — the normalized
151    /// numerator evaluated at $z = 1$ — hence
152    ///
153    /// $$\bar{x} = \frac{y_0}{\sum_i b_i + d\,\big(1 + \sum_i a_i\big)}.$$
154    pub fn settled_state_discrete(&self, y0: f64) -> Result<Vec<f64>, SettledStateError> {
155        let ss = self.to_state_space();
156        let n = ss.states();
157        if n == 0 {
158            return Err(SettledStateError::NoStates);
159        }
160        let den_at_one = 1.0 - ss.a[n - 1].iter().sum::<f64>();
161        let dc_numerator = ss.c[0].iter().sum::<f64>() + ss.d[0][0] * den_at_one;
162        if dc_numerator == 0.0 {
163            return Err(SettledStateError::ZeroDcNumerator);
164        }
165        Ok(vec![y0 / dc_numerator; n])
166    }
167}
168
169#[cfg(test)]
170mod tests {
171    use super::*;
172
173    /// 1/(s+1): a_0=1, b_0=1, d=0 -- matches the textbook single-state realization exactly,
174    /// derived by hand before writing this test (see `docs/architecture.md`'s Milestone 5
175    /// note).
176    #[test]
177    fn first_order_lowpass_matches_hand_derived_canonical_form() {
178        let tf = TransferFunction::new(vec![1.0], vec![1.0, 1.0]).unwrap();
179        let ss = tf.to_state_space();
180        assert_eq!(ss.a, vec![vec![-1.0]]);
181        assert_eq!(ss.b, vec![vec![1.0]]);
182        assert_eq!(ss.c, vec![vec![1.0]]);
183        assert_eq!(ss.d, vec![vec![0.0]]);
184    }
185
186    /// $G(s) = (s+3)/(s^2+3s+2)$: $a_0 = 2$, $b_0 = 3$, $d = 0$, so $y_0 = 6$ needs
187    /// $x_1 = 6/3 = 2$ held by $u_0 = a_0 x_1 = 4$ -- and indeed $G(0)\,u_0 = 1.5 \cdot 4 = 6$.
188    /// Checked as a real equilibrium, not just as a formula: the derivative there is zero.
189    #[test]
190    fn settled_state_is_an_equilibrium_at_the_requested_output() {
191        let tf = TransferFunction::new(vec![1.0, 3.0], vec![1.0, 3.0, 2.0]).unwrap();
192        let x = tf.settled_state(6.0).unwrap();
193        assert_eq!(x, vec![2.0, 0.0]);
194        let ss = tf.to_state_space();
195        assert_eq!(ss.derivative(&x, &[4.0]), vec![0.0, 0.0]);
196        assert_eq!(ss.output(&x, &[4.0]), vec![6.0]);
197    }
198
199    /// A pole at the origin: $G(s) = 5/s$, $b_0 = 5$, $a_0 = 0$ -- the holding input is zero and
200    /// $x_1 = y_0 / 5$.
201    #[test]
202    fn settled_state_of_an_integrator_is_held_by_zero_input() {
203        let tf = TransferFunction::new(vec![5.0], vec![1.0, 0.0]).unwrap();
204        let x = tf.settled_state(10.0).unwrap();
205        assert_eq!(x, vec![2.0]);
206        let ss = tf.to_state_space();
207        assert_eq!(ss.derivative(&x, &[0.0]), vec![0.0]);
208        assert_eq!(ss.output(&x, &[0.0]), vec![10.0]);
209    }
210
211    /// Biproper, so the feedthrough term matters: $G(s) = (2s+6)/(s+1)$ has $d = 2$,
212    /// $b_0 = 6 - 2 = 4$, $a_0 = 1$, $N(0) = 4 + 2 = 6$; $y_0 = 3$ gives $x_1 = 0.5$, $u_0 = 0.5$,
213    /// and $y = 4(0.5) + 2(0.5) = 3$.
214    #[test]
215    fn settled_state_accounts_for_direct_feedthrough() {
216        let tf = TransferFunction::new(vec![2.0, 6.0], vec![1.0, 1.0]).unwrap();
217        let x = tf.settled_state(3.0).unwrap();
218        assert_eq!(x, vec![0.5]);
219        assert_eq!(tf.to_state_space().output(&x, &[0.5]), vec![3.0]);
220    }
221
222    /// $G(s) = s/(s+1)$ blocks DC entirely; a pure gain has no state.
223    #[test]
224    fn settled_state_rejects_what_cannot_hold_an_output() {
225        let highpass = TransferFunction::new(vec![1.0, 0.0], vec![1.0, 1.0]).unwrap();
226        assert_eq!(
227            highpass.settled_state(1.0),
228            Err(SettledStateError::ZeroDcNumerator)
229        );
230        let gain = TransferFunction::new(vec![2.0], vec![1.0]).unwrap();
231        assert_eq!(gain.settled_state(1.0), Err(SettledStateError::NoStates));
232        assert_eq!(
233            gain.settled_state_discrete(1.0),
234            Err(SettledStateError::NoStates)
235        );
236    }
237
238    /// $H(z) = (0.5z + 0.25)/(z^2 - 0.5z)$: $H(1) = 0.75/0.5 = 1.5$. With $a = (0, -0.5)$,
239    /// $b = (0.25, 0.5)$, $d = 0$: $\bar{x} = 3/0.75 = 4$, held by $u_0 = (1 - 0.5) \cdot 4 = 2$,
240    /// and $H(1)\,u_0 = 3$. Checked as a fixed point of the discrete update itself.
241    #[test]
242    fn discrete_settled_state_is_a_fixed_point_at_the_requested_output() {
243        let tf = TransferFunction::new(vec![0.5, 0.25], vec![1.0, -0.5, 0.0]).unwrap();
244        let x = tf.settled_state_discrete(3.0).unwrap();
245        assert_eq!(x, vec![4.0, 4.0]);
246        let ss = tf.to_state_space();
247        assert_eq!(ss.discrete_step(&x, &[2.0]), x);
248        assert_eq!(ss.output(&x, &[2.0]), vec![3.0]);
249    }
250
251    #[test]
252    fn rejects_improper_transfer_function() {
253        let err = TransferFunction::new(vec![1.0, 0.0, 0.0], vec![1.0, 1.0]).unwrap_err();
254        assert_eq!(err, TransferFunctionError::ImproperTransferFunction);
255    }
256}