1use std::fmt;
2
3use crate::numeric::StateSpaceError;
4use crate::{Expression, Matrix, MnaSystem};
5
6#[derive(Debug, Clone, PartialEq)]
15pub struct SymbolicStateSpace {
16 pub a: Matrix<Expression>,
18 pub b: Matrix<Expression>,
20 pub states: Vec<String>,
22 pub inputs: Vec<String>,
24 pub mna_state_indices: Vec<usize>,
26}
27
28impl MnaSystem {
29 pub fn to_symbolic_state_space(&self) -> Result<SymbolicStateSpace, StateSpaceError> {
45 let order = self.unknowns.len();
46 if self.a.rows() != order
47 || self.a.cols() != order
48 || self.k.rows() != order
49 || self.k.cols() != order
50 || self.b.rows() != order
51 {
52 return Err(StateSpaceError::InconsistentDimensions);
53 }
54
55 let state_indices: Vec<usize> = (0..order)
56 .filter(|row| (0..order).any(|col| !self.k[(*row, col)].is_zero()))
57 .collect();
58 if state_indices.is_empty() {
59 return Err(StateSpaceError::NoDynamicVariables);
60 }
61 let algebraic_indices: Vec<usize> = (0..order)
62 .filter(|index| !state_indices.contains(index))
63 .collect();
64
65 let a_ss = extract(&self.a, &state_indices, &state_indices);
66 let k_ss = extract(&self.k, &state_indices, &state_indices);
67 let b_s = extract_rows(&self.b, &state_indices);
68
69 let (a_reduced, b_reduced) = if algebraic_indices.is_empty() {
70 (a_ss, b_s)
71 } else {
72 let a_sa = extract(&self.a, &state_indices, &algebraic_indices);
73 let a_as = extract(&self.a, &algebraic_indices, &state_indices);
74 let a_aa = extract(&self.a, &algebraic_indices, &algebraic_indices);
75 let b_a = extract_rows(&self.b, &algebraic_indices);
76 let solved_as = solve(&a_aa, &a_as).map_err(|pivot| {
77 StateSpaceError::singular_algebraic(&self.unknowns, &algebraic_indices, pivot)
78 })?;
79 let solved_b = solve(&a_aa, &b_a).map_err(|pivot| {
80 StateSpaceError::singular_algebraic(&self.unknowns, &algebraic_indices, pivot)
81 })?;
82 (
83 subtract(&a_ss, &multiply(&a_sa, &solved_as)),
84 subtract(&b_s, &multiply(&a_sa, &solved_b)),
85 )
86 };
87
88 let negative_a = a_reduced.map(|value| -value.clone());
89 let state_a = solve(&k_ss, &negative_a).map_err(|pivot| {
90 StateSpaceError::singular_storage(&self.unknowns, &state_indices, pivot)
91 })?;
92 let state_b = solve(&k_ss, &b_reduced).map_err(|pivot| {
93 StateSpaceError::singular_storage(&self.unknowns, &state_indices, pivot)
94 })?;
95
96 Ok(SymbolicStateSpace {
97 a: state_a,
98 b: state_b,
99 states: state_indices
100 .iter()
101 .map(|index| self.unknowns[*index].clone())
102 .collect(),
103 inputs: self.inputs.clone(),
104 mna_state_indices: state_indices,
105 })
106 }
107}
108
109#[derive(Debug, Clone, PartialEq)]
116pub struct SymbolicTransferFunction {
117 pub numerator: Vec<Expression>,
121 pub denominator: Vec<Expression>,
125 pub output: String,
127 pub input: String,
129}
130
131#[derive(Debug, Clone, Copy, PartialEq, Eq)]
134pub enum TransferFunctionError {
135 InvalidOutputIndex(usize),
137 InvalidInputIndex(usize),
139}
140
141impl fmt::Display for TransferFunctionError {
142 fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
143 match self {
144 Self::InvalidOutputIndex(index) => {
145 write!(f, "state index {index} is out of range")
146 }
147 Self::InvalidInputIndex(index) => {
148 write!(f, "input index {index} is out of range")
149 }
150 }
151 }
152}
153
154impl std::error::Error for TransferFunctionError {}
155
156impl SymbolicStateSpace {
157 pub fn transfer_function(
163 &self,
164 output_index: usize,
165 input_index: usize,
166 ) -> Result<SymbolicTransferFunction, TransferFunctionError> {
167 let n = self.states.len();
168 if output_index >= n {
169 return Err(TransferFunctionError::InvalidOutputIndex(output_index));
170 }
171 if input_index >= self.inputs.len() {
172 return Err(TransferFunctionError::InvalidInputIndex(input_index));
173 }
174
175 let (denominator, adjugate_terms) = faddeev_leverrier(&self.a);
176 let numerator = adjugate_terms
177 .iter()
178 .map(|term| {
179 (0..n).fold(Expression::zero(), |sum, column| {
180 let value = term[(output_index, column)].clone()
181 * self.b[(column, input_index)].clone();
182 if value.is_zero() {
183 sum
184 } else {
185 sum + value
186 }
187 })
188 })
189 .collect();
190
191 Ok(SymbolicTransferFunction {
192 numerator,
193 denominator,
194 output: self.states[output_index].clone(),
195 input: self.inputs[input_index].clone(),
196 })
197 }
198}
199
200pub fn faddeev_leverrier(a: &Matrix<Expression>) -> (Vec<Expression>, Vec<Matrix<Expression>>) {
216 let n = a.rows();
217 assert_eq!(a.cols(), n, "faddeev_leverrier requires a square matrix");
218
219 let mut m = zeros(n);
220 let mut c = Expression::one();
221 let mut coefficients = vec![c.clone()];
222 let mut adjugate_terms = Vec::with_capacity(n);
223
224 for k in 1..=n {
225 let scaled_identity = scalar_multiply(&identity(n), c);
226 m = add(&multiply(a, &m), &scaled_identity);
227 let trace_term = trace(&multiply(a, &m));
228 c = trace_term * Expression::Constant(-1.0 / k as f64);
229 coefficients.push(c.clone());
230 adjugate_terms.push(m.clone());
231 }
232
233 (coefficients, adjugate_terms)
234}
235
236fn identity(n: usize) -> Matrix<Expression> {
237 let mut result = Matrix::filled(n, n, Expression::zero());
238 for index in 0..n {
239 result[(index, index)] = Expression::one();
240 }
241 result
242}
243
244fn zeros(n: usize) -> Matrix<Expression> {
245 Matrix::filled(n, n, Expression::zero())
246}
247
248fn scalar_multiply(matrix: &Matrix<Expression>, scalar: Expression) -> Matrix<Expression> {
249 matrix.map(|value| value.clone() * scalar.clone())
250}
251
252fn trace(matrix: &Matrix<Expression>) -> Expression {
253 assert_eq!(matrix.rows(), matrix.cols());
254 (0..matrix.rows()).fold(Expression::zero(), |sum, index| {
255 let value = matrix[(index, index)].clone();
256 if value.is_zero() {
257 sum
258 } else {
259 sum + value
260 }
261 })
262}
263
264fn extract(matrix: &Matrix<Expression>, rows: &[usize], cols: &[usize]) -> Matrix<Expression> {
265 let mut result = Matrix::filled(rows.len(), cols.len(), Expression::zero());
266 for (target_row, source_row) in rows.iter().enumerate() {
267 for (target_col, source_col) in cols.iter().enumerate() {
268 result[(target_row, target_col)] = matrix[(*source_row, *source_col)].clone();
269 }
270 }
271 result
272}
273
274fn extract_rows(matrix: &Matrix<Expression>, rows: &[usize]) -> Matrix<Expression> {
275 let columns: Vec<usize> = (0..matrix.cols()).collect();
276 extract(matrix, rows, &columns)
277}
278
279fn multiply(lhs: &Matrix<Expression>, rhs: &Matrix<Expression>) -> Matrix<Expression> {
280 assert_eq!(lhs.cols(), rhs.rows());
281 let mut result = Matrix::filled(lhs.rows(), rhs.cols(), Expression::zero());
282 for row in 0..lhs.rows() {
283 for col in 0..rhs.cols() {
284 let sum = (0..lhs.cols()).fold(Expression::zero(), |sum, inner| {
285 let term = lhs[(row, inner)].clone() * rhs[(inner, col)].clone();
286 if term.is_zero() {
287 sum
288 } else {
289 sum + term
290 }
291 });
292 result[(row, col)] = sum;
293 }
294 }
295 result
296}
297
298fn add(lhs: &Matrix<Expression>, rhs: &Matrix<Expression>) -> Matrix<Expression> {
299 assert_eq!((lhs.rows(), lhs.cols()), (rhs.rows(), rhs.cols()));
300 Matrix::from_vec(
301 lhs.rows(),
302 lhs.cols(),
303 lhs.iter()
304 .zip(rhs.iter())
305 .map(|(left, right)| left.clone() + right.clone())
306 .collect(),
307 )
308 .expect("zipped equal matrices preserve shape")
309}
310
311fn subtract(lhs: &Matrix<Expression>, rhs: &Matrix<Expression>) -> Matrix<Expression> {
312 assert_eq!((lhs.rows(), lhs.cols()), (rhs.rows(), rhs.cols()));
313 Matrix::from_vec(
314 lhs.rows(),
315 lhs.cols(),
316 lhs.iter()
317 .zip(rhs.iter())
318 .map(|(left, right)| left.clone() - right.clone())
319 .collect(),
320 )
321 .expect("zipped equal matrices preserve shape")
322}
323
324fn solve(
330 coefficients: &Matrix<Expression>,
331 rhs: &Matrix<Expression>,
332) -> Result<Matrix<Expression>, usize> {
333 let n = coefficients.rows();
334 if coefficients.cols() != n || rhs.rows() != n {
335 return Err(0);
336 }
337 let rhs_cols = rhs.cols();
338 let mut augmented = Matrix::filled(n, n + rhs_cols, Expression::zero());
339 for row in 0..n {
340 for col in 0..n {
341 augmented[(row, col)] = coefficients[(row, col)].clone();
342 }
343 for col in 0..rhs_cols {
344 augmented[(row, n + col)] = rhs[(row, col)].clone();
345 }
346 }
347
348 for pivot_col in 0..n {
349 let pivot_row = (pivot_col..n).find(|row| !augmented[(*row, pivot_col)].is_zero());
350 let pivot_row = pivot_row.ok_or(pivot_col)?;
351
352 if pivot_row != pivot_col {
353 for col in 0..n + rhs_cols {
354 let temporary = augmented[(pivot_col, col)].clone();
355 augmented[(pivot_col, col)] = augmented[(pivot_row, col)].clone();
356 augmented[(pivot_row, col)] = temporary;
357 }
358 }
359
360 let inverse_pivot = augmented[(pivot_col, pivot_col)].clone().reciprocal();
361 for col in pivot_col..n + rhs_cols {
362 augmented[(pivot_col, col)] =
363 augmented[(pivot_col, col)].clone() * inverse_pivot.clone();
364 }
365 for row in 0..n {
366 if row == pivot_col {
367 continue;
368 }
369 let factor = augmented[(row, pivot_col)].clone();
370 if factor.is_zero() {
371 continue;
372 }
373 for col in pivot_col..n + rhs_cols {
374 let term = factor.clone() * augmented[(pivot_col, col)].clone();
375 augmented[(row, col)] = augmented[(row, col)].clone() - term;
376 }
377 }
378 }
379
380 let mut result = Matrix::filled(n, rhs_cols, Expression::zero());
381 for row in 0..n {
382 for col in 0..rhs_cols {
383 result[(row, col)] = augmented[(row, n + col)].clone();
384 }
385 }
386 Ok(result)
387}
388
389#[cfg(test)]
390mod tests {
391 use super::*;
392 use std::collections::BTreeMap;
393
394 #[test]
395 fn solve_matches_hand_derived_answer_for_voltage_divider_block() {
396 let rs = Expression::symbol("Rs");
408 let g = rs.reciprocal();
409
410 let a_aa = Matrix::from_vec(
411 3,
412 3,
413 vec![
414 g.clone(),
415 -g.clone(),
416 Expression::one(),
417 -g.clone(),
418 g.clone(),
419 Expression::zero(),
420 Expression::one(),
421 Expression::zero(),
422 Expression::zero(),
423 ],
424 )
425 .unwrap();
426 let rhs = Matrix::from_vec(
427 3,
428 1,
429 vec![Expression::zero(), Expression::one(), Expression::zero()],
430 )
431 .unwrap();
432
433 let solved = solve(&a_aa, &rhs).unwrap();
434 let values = BTreeMap::from([("Rs".to_string(), 2.5)]);
435 let x1 = solved[(0, 0)].evaluate(&values).unwrap();
436 let x2 = solved[(1, 0)].evaluate(&values).unwrap();
437 let x3 = solved[(2, 0)].evaluate(&values).unwrap();
438 assert!((x1 - 0.0).abs() < 1e-9, "x1={x1}");
439 assert!((x2 - 2.5).abs() < 1e-9, "x2={x2}");
440 assert!((x3 - 1.0).abs() < 1e-9, "x3={x3}");
441 }
442}