use crate::state_space::StateSpace;
#[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,
}
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 })
}
pub fn to_state_space(&self) -> StateSpace {
let leading = self.den[0];
let n = self.den.len() - 1;
let mut a_coeffs = vec![0.0; n];
for (power, coeff) in self.den[1..].iter().rev().enumerate() {
a_coeffs[power] = coeff / leading;
}
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];
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();
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]];
StateSpace::new(a, b, c, d, None).expect("e=None is always a valid descriptor matrix")
}
}
#[cfg(test)]
mod tests {
use super::*;
#[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]]);
}
#[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);
}
}