#[derive(Debug, Clone, PartialEq)]
pub struct StateSpace {
pub a: Vec<Vec<f64>>,
pub b: Vec<Vec<f64>>,
pub c: Vec<Vec<f64>>,
pub d: Vec<Vec<f64>>,
pub e: Option<Vec<Vec<f64>>>,
}
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum StateSpaceError {
NonInvertibleDescriptorMatrix,
}
impl StateSpace {
pub fn new(
a: Vec<Vec<f64>>,
b: Vec<Vec<f64>>,
c: Vec<Vec<f64>>,
d: Vec<Vec<f64>>,
e: Option<Vec<Vec<f64>>>,
) -> Result<Self, StateSpaceError> {
if let Some(e) = &e {
let n = e.len();
if dense_solve(e, &vec![0.0; n]).is_err() {
return Err(StateSpaceError::NonInvertibleDescriptorMatrix);
}
}
Ok(StateSpace { a, b, c, d, e })
}
pub fn states(&self) -> usize {
self.a.len()
}
pub fn inputs(&self) -> usize {
self.b.first().map_or(0, |row| row.len())
}
pub fn outputs(&self) -> usize {
self.c.len()
}
pub fn output(&self, x: &[f64], u: &[f64]) -> Vec<f64> {
(0..self.outputs())
.map(|row| {
let cx: f64 = (0..self.states())
.map(|col| self.c[row][col] * x[col])
.sum();
let du: f64 = (0..self.inputs())
.map(|col| self.d[row][col] * u[col])
.sum();
cx + du
})
.collect()
}
pub fn derivative(&self, x: &[f64], u: &[f64]) -> Vec<f64> {
let rhs: Vec<f64> = (0..self.states())
.map(|row| {
let ax: f64 = (0..self.states())
.map(|col| self.a[row][col] * x[col])
.sum();
let bu: f64 = (0..self.inputs())
.map(|col| self.b[row][col] * u[col])
.sum();
ax + bu
})
.collect();
match &self.e {
None => rhs,
Some(e) => dense_solve(e, &rhs).expect("StateSpace::new guarantees e is invertible"),
}
}
pub fn rk4_step(&self, x: &[f64], u: &[f64], dt: f64) -> Vec<f64> {
let add = |a: &[f64], b: &[f64], scale: f64| -> Vec<f64> {
a.iter().zip(b).map(|(ai, bi)| ai + scale * bi).collect()
};
let k1 = self.derivative(x, u);
let k2 = self.derivative(&add(x, &k1, dt / 2.0), u);
let k3 = self.derivative(&add(x, &k2, dt / 2.0), u);
let k4 = self.derivative(&add(x, &k3, dt), u);
(0..x.len())
.map(|i| x[i] + (dt / 6.0) * (k1[i] + 2.0 * k2[i] + 2.0 * k3[i] + k4[i]))
.collect()
}
pub fn discrete_step(&self, x: &[f64], u: &[f64]) -> Vec<f64> {
(0..self.states())
.map(|row| {
let ax: f64 = (0..self.states())
.map(|col| self.a[row][col] * x[col])
.sum();
let bu: f64 = (0..self.inputs())
.map(|col| self.b[row][col] * u[col])
.sum();
ax + bu
})
.collect()
}
}
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct SingularMatrix;
fn dense_solve(a: &[Vec<f64>], b: &[f64]) -> Result<Vec<f64>, SingularMatrix> {
let n = a.len();
let mut augmented: Vec<Vec<f64>> = a
.iter()
.zip(b)
.map(|(row, &rhs)| {
let mut r = row.clone();
r.push(rhs);
r
})
.collect();
for pivot_col in 0..n {
let pivot_row = (pivot_col..n)
.max_by(|&l, &r| {
augmented[l][pivot_col]
.abs()
.total_cmp(&augmented[r][pivot_col].abs())
})
.expect("range is nonempty");
if augmented[pivot_row][pivot_col].abs() <= 1e-12 {
return Err(SingularMatrix);
}
augmented.swap(pivot_row, pivot_col);
let pivot = augmented[pivot_col][pivot_col];
for v in augmented[pivot_col].iter_mut().skip(pivot_col) {
*v /= pivot;
}
for row in 0..n {
if row == pivot_col {
continue;
}
let factor = augmented[row][pivot_col];
if factor == 0.0 {
continue;
}
#[allow(clippy::needless_range_loop)]
for col in pivot_col..=n {
augmented[row][col] -= factor * augmented[pivot_col][col];
}
}
}
Ok((0..n).map(|row| augmented[row][n]).collect())
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn e_none_is_always_valid() {
let ss = StateSpace::new(
vec![vec![-1.0]],
vec![vec![1.0]],
vec![vec![1.0]],
vec![vec![0.0]],
None,
);
assert!(ss.is_ok());
}
#[test]
fn a_singular_descriptor_matrix_is_rejected_not_a_panic() {
let e = Some(vec![vec![0.0, 0.0], vec![0.0, 0.0]]);
let ss = StateSpace::new(
vec![vec![-1.0, 0.0], vec![0.0, -1.0]],
vec![vec![1.0], vec![1.0]],
vec![vec![1.0, 0.0]],
vec![vec![0.0]],
e,
);
assert_eq!(ss, Err(StateSpaceError::NonInvertibleDescriptorMatrix));
}
#[test]
fn an_invertible_descriptor_matrix_is_accepted() {
let e = Some(vec![vec![2.0, 0.0], vec![0.0, 3.0]]);
let ss = StateSpace::new(
vec![vec![-1.0, 0.0], vec![0.0, -1.0]],
vec![vec![1.0], vec![1.0]],
vec![vec![1.0, 0.0]],
vec![vec![0.0]],
e,
);
assert!(ss.is_ok());
}
#[test]
fn discrete_step_is_a_plain_linear_recursion_no_dt_involved() {
let ss = StateSpace::new(
vec![vec![0.5]],
vec![vec![2.0]],
vec![vec![1.0]],
vec![vec![0.0]],
None,
)
.unwrap();
let mut x = vec![1.0];
x = ss.discrete_step(&x, &[1.0]);
assert_eq!(x, vec![0.5 * 1.0 + 2.0 * 1.0]); x = ss.discrete_step(&x, &[1.0]);
assert_eq!(x, vec![0.5 * 2.5 + 2.0 * 1.0]); assert_eq!(ss.output(&x, &[1.0]), vec![3.25]);
}
}