continuous_blocks/state_space.rs
1//! A descriptor state-space system `E * dx/dt = A*x + B*u`, `y = C*x + D*u` — the same
2//! `A x + K dx/dt = B u` shape `general-mna` uses for circuits, with `K = E` (a standard
3//! "Descriptor State-Space" block, `E dx/dt = Ax + Bu`, is textually identical to this
4//! convention — see `docs/architecture.md`). Plain `E = I` is the ordinary (non-descriptor)
5//! state-space case.
6//!
7//! Matrices are plain row-major `Vec<Vec<f64>>` here rather than `general_mna::Matrix`: block
8//! parameters (gains, pole/zero locations, PID coefficients) are ordinary known numbers at
9//! model-build time, not symbolic netlist parameters, so there is no `Expression` layer to
10//! carry — wiring a block's *evaluated* `(A, K, B)` into a circuit's global descriptor system
11//! is `dae-runtime`'s job in a later milestone.
12
13#[derive(Debug, Clone, PartialEq)]
14pub struct StateSpace {
15 /// `n x n` dynamics matrix.
16 pub a: Vec<Vec<f64>>,
17 /// `n x m` input matrix (`m` = number of inputs).
18 pub b: Vec<Vec<f64>>,
19 /// `p x n` output matrix (`p` = number of outputs).
20 pub c: Vec<Vec<f64>>,
21 /// `p x m` feedthrough matrix.
22 pub d: Vec<Vec<f64>>,
23 /// `n x n` descriptor matrix. `None` means the identity (ordinary state-space); an explicit
24 /// non-identity `e` is the "Descriptor State-Space" case.
25 pub e: Option<Vec<Vec<f64>>>,
26}
27
28#[derive(Debug, Clone, Copy, PartialEq, Eq)]
29pub enum StateSpaceError {
30 /// The descriptor matrix `e` is singular — `E * dx/dt = A*x + B*u` has no unique `dx/dt`
31 /// for a generic `(A, x, B, u)`, so [`StateSpace::derivative`] could never solve it.
32 /// Checked once here, at construction, rather than on every single step's own
33 /// [`StateSpace::derivative`] call (which stays infallible as a result) — the same
34 /// validate-once-at-construction convention [`crate::Vco`]/[`crate::Pmsm`]/
35 /// [`crate::Hysteresis`]/[`crate::Pid`] all use for their own invariants.
36 NonInvertibleDescriptorMatrix,
37}
38
39impl StateSpace {
40 /// Validates `e` (if given) is invertible, then returns the assembled system. Every
41 /// [`StateSpace::derivative`] call downstream relies on this having already been checked —
42 /// see [`StateSpaceError::NonInvertibleDescriptorMatrix`]'s own doc comment.
43 pub fn new(
44 a: Vec<Vec<f64>>,
45 b: Vec<Vec<f64>>,
46 c: Vec<Vec<f64>>,
47 d: Vec<Vec<f64>>,
48 e: Option<Vec<Vec<f64>>>,
49 ) -> Result<Self, StateSpaceError> {
50 if let Some(e) = &e {
51 let n = e.len();
52 if dense_solve(e, &vec![0.0; n]).is_err() {
53 return Err(StateSpaceError::NonInvertibleDescriptorMatrix);
54 }
55 }
56 Ok(StateSpace { a, b, c, d, e })
57 }
58
59 pub fn states(&self) -> usize {
60 self.a.len()
61 }
62
63 pub fn inputs(&self) -> usize {
64 self.b.first().map_or(0, |row| row.len())
65 }
66
67 pub fn outputs(&self) -> usize {
68 self.c.len()
69 }
70
71 /// `y = C*x + D*u`.
72 pub fn output(&self, x: &[f64], u: &[f64]) -> Vec<f64> {
73 (0..self.outputs())
74 .map(|row| {
75 let cx: f64 = (0..self.states())
76 .map(|col| self.c[row][col] * x[col])
77 .sum();
78 let du: f64 = (0..self.inputs())
79 .map(|col| self.d[row][col] * u[col])
80 .sum();
81 cx + du
82 })
83 .collect()
84 }
85
86 /// `dx/dt` at the given `(x, u)`, solving `E * dx/dt = A*x + B*u` for `dx/dt` when a
87 /// non-identity descriptor matrix `e` is present (via the same dense Gauss-Jordan solve
88 /// `dae-runtime` uses — small, hand-checkable, `faer` deferred until it's actually needed;
89 /// see that crate's `linsolve` module doc for the same reasoning applied here).
90 pub fn derivative(&self, x: &[f64], u: &[f64]) -> Vec<f64> {
91 let rhs: Vec<f64> = (0..self.states())
92 .map(|row| {
93 let ax: f64 = (0..self.states())
94 .map(|col| self.a[row][col] * x[col])
95 .sum();
96 let bu: f64 = (0..self.inputs())
97 .map(|col| self.b[row][col] * u[col])
98 .sum();
99 ax + bu
100 })
101 .collect();
102 match &self.e {
103 None => rhs,
104 // Safe: StateSpace::new already rejected a singular `e` at construction time.
105 Some(e) => dense_solve(e, &rhs).expect("StateSpace::new guarantees e is invertible"),
106 }
107 }
108
109 /// Advances one RK4 step of size `dt`, holding `u` constant over the step (zero-order
110 /// hold — matches how a discrete-timestep circuit/controller simulation would drive this
111 /// block in `dae-runtime`'s eventual timestep loop). Returns the new state; call
112 /// [`StateSpace::output`] separately for `y` at the new state.
113 pub fn rk4_step(&self, x: &[f64], u: &[f64], dt: f64) -> Vec<f64> {
114 let add = |a: &[f64], b: &[f64], scale: f64| -> Vec<f64> {
115 a.iter().zip(b).map(|(ai, bi)| ai + scale * bi).collect()
116 };
117 let k1 = self.derivative(x, u);
118 let k2 = self.derivative(&add(x, &k1, dt / 2.0), u);
119 let k3 = self.derivative(&add(x, &k2, dt / 2.0), u);
120 let k4 = self.derivative(&add(x, &k3, dt), u);
121 (0..x.len())
122 .map(|i| x[i] + (dt / 6.0) * (k1[i] + 2.0 * k2[i] + 2.0 * k3[i] + k4[i]))
123 .collect()
124 }
125
126 /// One step of a genuinely **discrete-time** system: `x[i+1] = A*x[i] + B*u[i]`, plain
127 /// linear recursion, no RK4, no `dt`, no numerical integration at all — the discrete-domain
128 /// counterpart to [`Self::rk4_step`]. `A`/`B` here are already discrete-domain matrices,
129 /// given directly by the caller (this method does no continuous-to-discrete conversion of
130 /// its own) — see `book/dev-guide/src/discrete-time-blocks.md`. `dae-runtime` calls this
131 /// once per declared sample period instead of once per circuit step, the same zero-order-
132 /// hold accumulator `cscript`/`pyblock` already use. Ignores `self.e` entirely — a
133 /// descriptor matrix has no meaning for a plain discrete recursion (there is no `dx/dt` to
134 /// solve `E` against here).
135 pub fn discrete_step(&self, x: &[f64], u: &[f64]) -> Vec<f64> {
136 (0..self.states())
137 .map(|row| {
138 let ax: f64 = (0..self.states())
139 .map(|col| self.a[row][col] * x[col])
140 .sum();
141 let bu: f64 = (0..self.inputs())
142 .map(|col| self.b[row][col] * u[col])
143 .sum();
144 ax + bu
145 })
146 .collect()
147 }
148}
149
150#[derive(Debug, Clone, Copy, PartialEq, Eq)]
151pub struct SingularMatrix;
152
153fn dense_solve(a: &[Vec<f64>], b: &[f64]) -> Result<Vec<f64>, SingularMatrix> {
154 let n = a.len();
155 let mut augmented: Vec<Vec<f64>> = a
156 .iter()
157 .zip(b)
158 .map(|(row, &rhs)| {
159 let mut r = row.clone();
160 r.push(rhs);
161 r
162 })
163 .collect();
164
165 for pivot_col in 0..n {
166 let pivot_row = (pivot_col..n)
167 .max_by(|&l, &r| {
168 augmented[l][pivot_col]
169 .abs()
170 .total_cmp(&augmented[r][pivot_col].abs())
171 })
172 .expect("range is nonempty");
173 if augmented[pivot_row][pivot_col].abs() <= 1e-12 {
174 return Err(SingularMatrix);
175 }
176 augmented.swap(pivot_row, pivot_col);
177 let pivot = augmented[pivot_col][pivot_col];
178 // `skip(pivot_col)` is exactly `pivot_col..=n`: each augmented row is the matrix row
179 // plus the RHS, so it holds `n + 1` entries.
180 for v in augmented[pivot_col].iter_mut().skip(pivot_col) {
181 *v /= pivot;
182 }
183 for row in 0..n {
184 if row == pivot_col {
185 continue;
186 }
187 let factor = augmented[row][pivot_col];
188 if factor == 0.0 {
189 continue;
190 }
191 // Indexed deliberately, against `clippy::needless_range_loop`: this reads one row
192 // while writing another, so an iterator over `augmented` would need the rows split
193 // apart or the pivot row copied. `augmented[row][col] -= factor *
194 // augmented[pivot_col][col]` is the textbook spelling of a Gauss-Jordan row
195 // operation, and in numerical code matching the reference form is worth more than
196 // satisfying a style lint. Every index is in `pivot_col..=n`, inside both rows.
197 #[allow(clippy::needless_range_loop)]
198 for col in pivot_col..=n {
199 augmented[row][col] -= factor * augmented[pivot_col][col];
200 }
201 }
202 }
203 Ok((0..n).map(|row| augmented[row][n]).collect())
204}
205
206#[cfg(test)]
207mod tests {
208 use super::*;
209
210 #[test]
211 fn e_none_is_always_valid() {
212 let ss = StateSpace::new(
213 vec![vec![-1.0]],
214 vec![vec![1.0]],
215 vec![vec![1.0]],
216 vec![vec![0.0]],
217 None,
218 );
219 assert!(ss.is_ok());
220 }
221
222 #[test]
223 fn a_singular_descriptor_matrix_is_rejected_not_a_panic() {
224 // A 2x2 all-zero e: E*dx/dt = A*x+B*u has no unique dx/dt for a generic RHS.
225 let e = Some(vec![vec![0.0, 0.0], vec![0.0, 0.0]]);
226 let ss = StateSpace::new(
227 vec![vec![-1.0, 0.0], vec![0.0, -1.0]],
228 vec![vec![1.0], vec![1.0]],
229 vec![vec![1.0, 0.0]],
230 vec![vec![0.0]],
231 e,
232 );
233 assert_eq!(ss, Err(StateSpaceError::NonInvertibleDescriptorMatrix));
234 }
235
236 #[test]
237 fn an_invertible_descriptor_matrix_is_accepted() {
238 let e = Some(vec![vec![2.0, 0.0], vec![0.0, 3.0]]);
239 let ss = StateSpace::new(
240 vec![vec![-1.0, 0.0], vec![0.0, -1.0]],
241 vec![vec![1.0], vec![1.0]],
242 vec![vec![1.0, 0.0]],
243 vec![vec![0.0]],
244 e,
245 );
246 assert!(ss.is_ok());
247 }
248
249 #[test]
250 fn discrete_step_is_a_plain_linear_recursion_no_dt_involved() {
251 // x[i+1] = 0.5*x[i] + 2*u[i] -- a first-order decaying discrete system, hand-computed
252 // for a few steps starting from x=1, u=1 held constant.
253 let ss = StateSpace::new(
254 vec![vec![0.5]],
255 vec![vec![2.0]],
256 vec![vec![1.0]],
257 vec![vec![0.0]],
258 None,
259 )
260 .unwrap();
261 let mut x = vec![1.0];
262 x = ss.discrete_step(&x, &[1.0]);
263 assert_eq!(x, vec![0.5 * 1.0 + 2.0 * 1.0]); // 2.5
264 x = ss.discrete_step(&x, &[1.0]);
265 assert_eq!(x, vec![0.5 * 2.5 + 2.0 * 1.0]); // 3.25
266 // output y = C*x + D*u = x here (C=[1], D=[0]).
267 assert_eq!(ss.output(&x, &[1.0]), vec![3.25]);
268 }
269}