Skip to main content

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}