diff --git a/examples/plotting.rs b/examples/plotting.rs index daa40fd..97f5353 100644 --- a/examples/plotting.rs +++ b/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 = Tf::new(&[1.0], &[0.0, 1.0]); diff --git a/src/analysis/frequency_response.rs b/src/analysis/frequency_response.rs index 296e768..48ee084 100644 --- a/src/analysis/frequency_response.rs +++ b/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 Tf { +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 Tf { /// # 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 Tf { /// # Returns /// - A vector of `[magnitude (dB), phase (degrees), frequency]` tuples for /// each evaluated frequency. - pub fn bode_freqs( - &self, - freqs: impl Iterator, - ) -> Vec<[f64; 3]> { + fn bode_freqs(&self, freqs: impl Iterator) -> 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 Tf { /// /// # Returns /// - A vector of complex numbers representing the Nyquist plot. - pub fn nyquist(&self, min_freq: f64, max_freq: f64) -> Vec { + fn nyquist(&self, min_freq: f64, max_freq: f64) -> Vec { let freqs = log_space(min_freq, max_freq, 1000, 10); self.nyquist_freqs(freqs) } @@ -134,7 +137,7 @@ impl Tf { /// /// # Returns /// - A vector of complex numbers representing the Nyquist plot. - pub fn nyquist_freqs( + fn nyquist_freqs( &self, freqs: impl Iterator, ) -> Vec { @@ -142,11 +145,177 @@ impl Tf { 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 FrequencyResponse for Tf { + fn freq_response(&self, freq: &Complex64) -> Complex64 { + self.eval(freq) + } +} + +impl FrequencyResponse for Ss { + 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, + b: &mut DMatrix, + c: &mut DMatrix, + d: &mut DMatrix, +) -> Result, String> { + // TODO: Should be possible to call it without specifying the time domain. + Ss::::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 = 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); + } + } +} diff --git a/src/lib.rs b/src/lib.rs index e5974bf..21fa75a 100644 --- a/src/lib.rs +++ b/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, diff --git a/src/plot.rs b/src/plot.rs index ef2caa4..cb23cf5 100644 --- a/src/plot.rs +++ b/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] diff --git a/src/slicot_wrapper.rs b/src/slicot_wrapper.rs index 6451298..fe89704 100644 --- a/src/slicot_wrapper.rs +++ b/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