Calculate frequency response of State Space system directly

examples/plotting.rs

@@ -1,6 +1,6 @@
 use control_systems_torbox as cst;
 
-use cst::{plot::*, systems::Tf, utils::traits::Continuous};
+use cst::{FrequencyResponse, plot::*, systems::Tf, utils::traits::Continuous};
 
 fn main() {
     let sys: Tf<f64, Continuous> = Tf::new(&[1.0], &[0.0, 1.0]);

src/analysis/frequency_response.rs

@@ -1,6 +1,11 @@
+use std::ffi::{CString, c_int};
+
+use nalgebra::DMatrix;
 use num_complex::{Complex64, c64};
 
 use crate::{
+    Continuous, Ss,
+    slicot_wrapper::tb05ad_,
     systems::Tf,
     utils::traits::{Mag2Db, Rad2Deg, Time},
 };
@@ -65,7 +70,8 @@ pub fn log_space(
     nums.map(move |x| (base as f64).powf(x))
 }
 
-impl<U: Time> Tf<f64, U> {
+pub trait FrequencyResponse {
+    fn freq_response(&self, freq: &Complex64) -> Complex64;
     /// Computes the Bode plot (magnitude and phase) for a transfer function
     /// over a frequency range.
     ///
@@ -77,7 +83,7 @@ impl<U: Time> Tf<f64, U> {
     /// # Returns
     /// - A vector of `[magnitude (dB), phase (degrees), frequency]` tuples for
     ///   each evaluated frequency.
-    pub fn bode(&self, min_freq: f64, max_freq: f64) -> Vec<[f64; 3]> {
+    fn bode(&self, min_freq: f64, max_freq: f64) -> Vec<[f64; 3]> {
         let freqs = log_space(min_freq, max_freq, 1000, 10);
         self.bode_freqs(freqs)
     }
@@ -92,15 +98,12 @@ impl<U: Time> Tf<f64, U> {
     /// # Returns
     /// - A vector of `[magnitude (dB), phase (degrees), frequency]` tuples for
     ///   each evaluated frequency.
-    pub fn bode_freqs(
-        &self,
-        freqs: impl Iterator<Item = f64>,
-    ) -> Vec<[f64; 3]> {
+    fn bode_freqs(&self, freqs: impl Iterator<Item = f64>) -> Vec<[f64; 3]> {
         let mut mag_phase_freq_vec = Vec::with_capacity(freqs.size_hint().0);
 
         for omega in freqs {
             let c = c64(0., omega);
-            let sys_val = self.eval(&c);
+            let sys_val = self.freq_response(&c);
             mag_phase_freq_vec.push([
                 sys_val.norm().mag2db(),
                 sys_val.arg().rad2deg(),
@@ -120,7 +123,7 @@ impl<U: Time> Tf<f64, U> {
     ///
     /// # Returns
     /// - A vector of complex numbers representing the Nyquist plot.
-    pub fn nyquist(&self, min_freq: f64, max_freq: f64) -> Vec<Complex64> {
+    fn nyquist(&self, min_freq: f64, max_freq: f64) -> Vec<Complex64> {
         let freqs = log_space(min_freq, max_freq, 1000, 10);
         self.nyquist_freqs(freqs)
     }
@@ -134,19 +137,185 @@ impl<U: Time> Tf<f64, U> {
     ///
     /// # Returns
     /// - A vector of complex numbers representing the Nyquist plot.
-    pub fn nyquist_freqs(
+    fn nyquist_freqs(
         &self,
         freqs: impl Iterator<Item = f64>,
     ) -> Vec<Complex64> {
         let mut pos_vals = Vec::with_capacity(freqs.size_hint().0);
         let mut neg_vals = Vec::with_capacity(freqs.size_hint().0);
 
         for freq in freqs {
-            pos_vals.push(self.eval(&c64(0., freq)));
-            neg_vals.push(self.eval(&c64(0., -freq)));
+            pos_vals.push(self.freq_response(&c64(0., freq)));
+            neg_vals.push(self.freq_response(&c64(0., -freq)));
         }
 
         pos_vals.extend(neg_vals.iter().rev());
         pos_vals
     }
 }
+
+impl<U: Time> FrequencyResponse for Tf<f64, U> {
+    fn freq_response(&self, freq: &Complex64) -> Complex64 {
+        self.eval(freq)
+    }
+}
+
+impl<U: Time + 'static> FrequencyResponse for Ss<U> {
+    fn freq_response(&self, freq: &Complex64) -> Complex64 {
+        assert_eq!(self.ninputs(), 1);
+        assert_eq!(self.noutputs(), 1);
+
+        let freq_resp = freq_response_ss_mat(
+            *freq,
+            &mut self.a().clone(),
+            &mut self.b().clone(),
+            &mut self.c().clone(),
+            &mut self.d().clone(),
+        )
+        .unwrap();
+        assert_eq!(freq_resp.nrows(), 1);
+        assert_eq!(freq_resp.ncols(), 1);
+
+        freq_resp[(0, 0)]
+    }
+}
+
+fn freq_response_ss_mat(
+    freq: num_complex::Complex64,
+    a: &mut DMatrix<f64>,
+    b: &mut DMatrix<f64>,
+    c: &mut DMatrix<f64>,
+    d: &mut DMatrix<f64>,
+) -> Result<DMatrix<Complex64>, String> {
+    // TODO: Should be possible to call it without specifying the time domain.
+    Ss::<Continuous>::verify_dimensions(a, b, c, d).unwrap();
+    let n = a.nrows();
+    let m = b.ncols();
+    let p = c.nrows();
+    assert_eq!(m, 1, "Function is only tested for SISO systems");
+    assert_eq!(p, 1, "Function is only tested for SISO systems");
+
+    let baleig = CString::new("A").unwrap(); // balance A and compute condition number
+    let inita = CString::new("G").unwrap(); // general A matrix
+
+    let freq_in = crate::slicot_wrapper::Complex64 {
+        re: freq.re,
+        im: freq.im,
+    };
+
+    let mut rank_condition = -1.0;
+    let zero_complex = crate::slicot_wrapper::Complex64 { re: 0.0, im: 0.0 };
+    let mut freq_response = vec![zero_complex; p * m];
+
+    let mut eigen_val_re = vec![0.0; n];
+    let mut eigen_val_im = vec![0.0; n];
+
+    let mut h_inv_times_b = vec![zero_complex; n * m];
+    let l_h_inv_times_b = n;
+
+    let mut iwork = vec![0; n];
+    let ldwork = 10 * 1.max(n + n.max(m - 1).max(p - 1)) + 500;
+    let mut dwork = vec![0.0; ldwork];
+
+    let lzwork = 10 * 1.max(n * n + 2 * n) + 100;
+    let mut zwork = vec![zero_complex; lzwork];
+
+    let mut info = -1;
+
+    unsafe {
+        tb05ad_(
+            baleig.as_ptr(),
+            inita.as_ptr(),
+            &(n as c_int),
+            &(m as c_int),
+            &(p as c_int),
+            &freq_in,
+            a.as_mut_ptr(),
+            &(a.nrows() as c_int),
+            b.as_mut_ptr(),
+            &(b.nrows() as c_int),
+            c.as_mut_ptr(),
+            &(c.nrows() as c_int),
+            &mut rank_condition,
+            freq_response.as_mut_ptr(),
+            &(p as c_int),
+            eigen_val_re.as_mut_ptr(),
+            eigen_val_im.as_mut_ptr(),
+            h_inv_times_b.as_mut_ptr(),
+            &(l_h_inv_times_b as c_int),
+            iwork.as_mut_ptr(),
+            dwork.as_mut_ptr(),
+            &(ldwork as c_int),
+            zwork.as_mut_ptr(),
+            &(lzwork as c_int),
+            &mut info,
+        );
+    }
+
+    if info != 0 {
+        return Err(format!(
+            "Failed to compute frequency response of state space system. Slicot TB05AD failed with error code: {}",
+            info
+        ));
+    }
+
+    let mut response_matrix: DMatrix<Complex64> = DMatrix::zeros(p, m);
+
+    for row in 0..p {
+        for col in 0..m {
+            let resp_no_d = freq_response[row + p * col];
+            response_matrix[(row, col)] =
+                c64(d[(row, col)] + resp_no_d.re, resp_no_d.im);
+        }
+    }
+    Ok(response_matrix)
+}
+
+#[cfg(test)]
+mod tests {
+    use crate::{FrequencyResponse, Tf};
+
+    use super::log_space;
+    use approx::assert_abs_diff_eq;
+    use num_complex::c64;
+
+    #[test]
+    fn tf_and_ss_same_freq_resp() {
+        let sys_tf = (Tf::s() + 2.0) / (1.0 + Tf::s());
+        let sys_ss = sys_tf.to_ss().unwrap();
+
+        for freq in log_space(0.1, 1000.0, 10000, 10) {
+            let freq_complex = c64(0.0, freq);
+            let resp_tf = sys_tf.freq_response(&freq_complex);
+            let resp_ss = sys_ss.freq_response(&freq_complex);
+            assert_abs_diff_eq!(resp_tf.re, resp_ss.re, epsilon = 1e-3);
+            assert_abs_diff_eq!(resp_tf.im, resp_ss.im, epsilon = 1e-3);
+        }
+
+        let sys_tf = sys_tf * 1.0 / Tf::s();
+        let sys_ss = sys_tf.to_ss().unwrap();
+        let bode_tf = sys_tf.bode(0.01, 1000.0);
+        let bode_ss = sys_ss.bode(0.01, 1000.0);
+
+        for (res_tf, res_ss) in bode_tf.iter().zip(bode_ss.iter()) {
+            let mag_tf = res_tf[0];
+            let phase_tf = res_tf[1];
+            let mag_ss = res_ss[0];
+            let phase_ss = res_ss[1];
+
+            assert_abs_diff_eq!(mag_tf, mag_ss, epsilon = 1e-3);
+            assert_abs_diff_eq!(phase_tf, phase_ss, epsilon = 1e-3);
+        }
+
+        let sys_tf = 1.0 / (0.1 + Tf::s()).powi(10);
+        let sys_ss = sys_tf.to_ss().unwrap();
+
+        let nyq_tf = sys_tf.nyquist(0.01, 1000.0);
+        let nyq_ss = sys_ss.nyquist(0.01, 1000.0);
+
+        for (res_tf, res_ss) in nyq_tf.iter().zip(nyq_ss.iter()) {
+            assert_abs_diff_eq!(res_tf.re, res_ss.re, epsilon = 1e-3);
+            assert_abs_diff_eq!(res_tf.im, res_tf.im, epsilon = 1e-3);
+        }
+    }
+}

src/lib.rs

@@ -100,6 +100,7 @@ pub mod systems;
 pub mod transformations;
 pub mod utils;
 
+pub use analysis::frequency_response::FrequencyResponse;
 pub use plot::{
     BodePlot, BodePlotData, BodePlotEgui, BodePlotOptions, NyquistPlot,
     NyquistPlotData, NyquistPlotEgui, NyquistPlotOptions, Plot, RGBAColor,

src/plot.rs

@@ -942,7 +942,7 @@ impl Plot for NyquistPlot {
 mod tests {
 
     use super::*;
-    use crate::{systems::Tf, utils::traits::Continuous};
+    use crate::{FrequencyResponse, systems::Tf, utils::traits::Continuous};
     use egui_kittest::Harness;
 
     #[test]

src/slicot_wrapper.rs

@@ -1,5 +1,12 @@
 use std::ffi::{c_char, c_double, c_int};
 
+#[repr(C)]
+#[derive(Debug, Clone, Copy)]
+pub struct Complex64 {
+    pub re: f64,
+    pub im: f64,
+}
+
 unsafe extern "C" {
     /// Minimal Realization of state space
     pub fn tb01pd_(
@@ -140,6 +147,35 @@ unsafe extern "C" {
         ldwork: *const c_int,
         info: *mut c_int,
     );
+
+    // state space frequency response
+    pub fn tb05ad_(
+        baleig: *const c_char,
+        inita: *const c_char,
+        n: *const c_int,
+        m: *const c_int,
+        p: *const c_int,
+        freq: *const Complex64,
+        a: *mut c_double,
+        lda: *const c_int,
+        b: *mut c_double,
+        ldb: *const c_int,
+        c: *mut c_double,
+        ldc: *const c_int,
+        rcond: *mut c_double,
+        G: *mut Complex64,
+        ldg: *const c_int,
+        evre: *mut c_double,
+        evim: *mut c_double,
+        hinvb: *mut Complex64,
+        ldhinv: *const c_int,
+        iwork: *mut c_int,
+        dwork: *mut c_double,
+        ldwork: *const c_int,
+        zwork: *mut Complex64,
+        lzwork: *const c_int,
+        info: *mut c_int,
+    );
 }
 
 // LAPACK function