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}