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}