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