diff --git a/.github/workflows/rust.yml b/.github/workflows/rust.yml index d5b02f5..017402d 100644 --- a/.github/workflows/rust.yml +++ b/.github/workflows/rust.yml @@ -16,8 +16,12 @@ jobs: steps: - uses: actions/checkout@v4 - name: Install fontconfig - run: sudo apt-get install libfontconfig-dev + run: | + sudo apt-get update + sudo apt-get install -y --no-install-recommends libfontconfig1-dev - name: Build run: cargo build --verbose - name: Run tests run: cargo test --verbose + - name: Clippy + run: cargo clippy --all-targets --all-features -- -D warnings diff --git a/Cargo.toml b/Cargo.toml index 8e4b9a5..c2433c9 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -20,10 +20,18 @@ include = [ plotters = "0.3.3" nalgebra = "0.33.2" -[[examples]] +[[example]] name = "step_response" path = "examples/step_response.rs" -[[examples]] +[[example]] name = "controllability" -path = "examples/controllability.rs" \ No newline at end of file +path = "examples/controllability.rs" + +[[example]] +name = "end_to_end" +path = "examples/end_to_end.rs" + +[[example]] +name = "analysis_report" +path = "examples/analysis_report.rs" diff --git a/README.md b/README.md index 3efea11..4ec4a6d 100644 --- a/README.md +++ b/README.md @@ -2,159 +2,55 @@ ![GitHub Actions Workflow Status](https://img.shields.io/github/actions/workflow/status/rdesarz/control-sys-rs/rust.yml) -**control-sys** is a control system library written in Rust. It implements tools to represent and analyze LTI systems using state-space model. +**control-sys** is a Rust library to represent, analyze, simulate, and control LTI systems using state-space models. -## Examples +## Highlights -### Continuous state space model +- Checked model constructors with typed errors (`ModelError`) +- Continuous-to-discrete conversion with explicit methods: + - ZOH +- Time-domain simulation with full output equation `y = Cx + Du` +- Controllability and observability analysis +- Stability checks and rank diagnostics +- Controller helpers: + - SISO pole placement + - Discrete LQR + - Continuous LQR approximation + - Closed-loop assembly (`A-BK`) -A continuous state space model can be defined with `ContinuousStateSpaceModel`. As an example, a state-space model representing a DC motor can be defined this way. We are using `nalgebra` to represent the matrices: +## Example: End-to-End Workflow ```rust +use control_sys::{analysis, model, simulator}; use nalgebra as na; -use control_sys::model; - -// DC motor parameters -let b = 0.1f64; -let j = 0.01f64; -let k = 0.01f64; -let l = 0.5f64; -let r = 1.0f64; - -let mat_ac = na::dmatrix![ - -b / j, k / j; - -k / l, -r / l; - ]; -let mat_bc = na::dmatrix![0.0; - 1.0 / l]; -let mat_cc = na::dmatrix![1.0, 0.0]; - -let cont_model = model::ContinuousStateSpaceModel::from_matrices(&mat_ac, &mat_bc, &mat_cc, &na::dmatrix![]); -``` - -### Continuous to discrete model conversion -A `DiscreteStateSpaceModel` can be built from a continuous one. You then need to specify the sampling step `ts`: - -```rust -use nalgebra as na; -use control_sys::model; - -// DC motor parameters -let b = 0.1f64; -let j = 0.01f64; -let k = 0.01f64; -let l = 0.5f64; -let r = 1.0f64; - -let mat_ac = na::dmatrix![ - -b / j, k / j; - -k / l, -r / l; - ]; -let mat_bc = na::dmatrix![0.0; - 1.0 / l]; -let mat_cc = na::dmatrix![1.0, 0.0]; - -let cont_model = model::ContinuousStateSpaceModel::from_matrices(&mat_ac, &mat_bc, &mat_cc, &na::dmatrix![]); - -let discrete_model = - model::DiscreteStateSpaceModel::from_continuous_ss_forward_euler(&cont_model, 0.05); -``` +let a = na::dmatrix![0.0, 1.0; -2.0, -3.0]; +let b = na::dmatrix![0.0; 1.0]; +let c = na::dmatrix![1.0, 0.0]; +let d = na::dmatrix![0.0]; -### Step response +let cont = model::ContinuousStateSpaceModel::try_from_matrices(&a, &b, &c, &d).unwrap(); +let disc = model::DiscreteStateSpaceModel::from_continuous_zoh(&cont, 0.05).unwrap(); -You can compute the step response of a system. For a discrete system, the simulation steps are given by the sampling step of the discretization: +let (y, u, _x) = simulator::step_for_discrete_ss(&disc, 5.0).unwrap(); +assert_eq!(y.nrows(), 1); +assert_eq!(u.nrows(), 1); -```rust -use nalgebra as na; -use control_sys::{model, simulator}; - -// DC motor parameters -let b = 0.1f64; -let j = 0.01f64; -let k = 0.01f64; -let l = 0.5f64; -let r = 1.0f64; - -let mat_ac = na::dmatrix![ - -b / j, k / j; - -k / l, -r / l; - ]; -let mat_bc = na::dmatrix![0.0; - 1.0 / l]; -let mat_cc = na::dmatrix![1.0, 0.0]; - -let cont_model = model::ContinuousStateSpaceModel::from_matrices(&mat_ac, &mat_bc, &mat_cc, &na::dmatrix![]); - -let discrete_model = - model::DiscreteStateSpaceModel::from_continuous_ss_forward_euler(&cont_model, 0.05); - -// where model implements the traits `StateSpaceModel` and `Discrete` -let duration = 10.0; // [s] -let (response, input, states) = simulator::step_for_discrete_ss( - &discrete_model, - duration, - ); +let (is_ctrb, _ctrb_mat) = analysis::is_ss_controllable(&disc); +assert!(is_ctrb); ``` -You can also compute the step response for a continuous model. You will need to provide the sampling step and the model will be discretized before computing the step response: +## Discretization Methods and Assumptions -```rust -use nalgebra as na; -use control_sys::{model, simulator}; - -// DC motor parameters -let b = 0.1f64; -let j = 0.01f64; -let k = 0.01f64; -let l = 0.5f64; -let r = 1.0f64; - -let mat_ac = na::dmatrix![ - -b / j, k / j; - -k / l, -r / l; - ]; -let mat_bc = na::dmatrix![0.0; - 1.0 / l]; -let mat_cc = na::dmatrix![1.0, 0.0]; - -let cont_model = model::ContinuousStateSpaceModel::from_matrices(&mat_ac, &mat_bc, &mat_cc, &na::dmatrix![]); - -// where model is a continuous state space model -let sampling_dt = 0.05; // [s] -let duration = 10.0; // [s] -let (response, input, states) = simulator::step_for_continuous_ss( - &cont_model, - sampling_dt, - duration, - ); -``` - -### Controllability +`DiscreteStateSpaceModel` has explicit conversion methods: -The controllability of a system can be evaluated using the `is_ss_controllable` method: +- `from_continuous_zoh`: exact ZOH (recommended default) -```rust -use nalgebra as na; -use control_sys::{model, analysis}; - -let ss_model = model::DiscreteStateSpaceModel::from_matrices( - &nalgebra::dmatrix![1.0, -2.0; - 2.0, 1.0], - &nalgebra::dmatrix![1.0; - 2.0], - &nalgebra::dmatrix![], - &nalgebra::dmatrix![], - 0.05, - ); - -let (is_controllable, controllability_matrix) = analysis::is_ss_controllable(&ss_model); - -if is_controllable -{ - println!("The system is controllable"); - println!("Its controllability matrix is: {}", controllability_matrix); -} -``` +Use ZOH by default for sampled-data systems with held inputs. +## Running checks +```bash +cargo test +cargo clippy --all-targets --all-features -- -D warnings +``` diff --git a/examples/analysis_report.rs b/examples/analysis_report.rs new file mode 100644 index 0000000..6a60096 --- /dev/null +++ b/examples/analysis_report.rs @@ -0,0 +1,34 @@ +// LTI analysis-first example: +// Build a model and run a single consolidated analysis pass +// (poles, stability, controllability, observability). +use control_sys::{analysis, model}; +use nalgebra as na; + +fn main() -> Result<(), Box> { + let system = model::DiscreteStateSpaceModel::try_from_matrices( + &na::dmatrix![0.9, 0.1; 0.0, 0.8], + &na::dmatrix![1.0; 0.5], + &na::dmatrix![1.0, 0.0], + &na::dmatrix![0.0], + 0.1, + )?; + + let report = analysis::analyze_lti(&system, analysis::TimeDomain::Discrete, 1e-6)?; + + println!("stable: {}", report.is_stable); + println!("spectral radius: {}", report.spectral_radius); + println!( + "controllable: {} (rank {}/{})", + report.controllability.is_full_rank, + report.controllability.rank, + report.controllability.expected_rank + ); + println!( + "observable: {} (rank {}/{})", + report.observability.is_full_rank, + report.observability.rank, + report.observability.expected_rank + ); + + Ok(()) +} diff --git a/examples/controllability.rs b/examples/controllability.rs index d649b75..53b2a66 100644 --- a/examples/controllability.rs +++ b/examples/controllability.rs @@ -1,18 +1,18 @@ +// Controllability example: +// Build a discrete state-space model and compute its controllability matrix/rank. use control_sys::analysis; use control_sys::model; -extern crate nalgebra as na; - fn main() -> Result<(), Box> { - let ss_model = model::DiscreteStateSpaceModel::from_matrices( - &nalgebra::dmatrix![1.0, -2.0; + let ss_model = model::DiscreteStateSpaceModel::try_from_matrices( + &nalgebra::dmatrix![1.0, -2.0; 2.0, 1.0], &nalgebra::dmatrix![1.0; 2.0], - &nalgebra::dmatrix![], - &nalgebra::dmatrix![], + &nalgebra::dmatrix![1.0, 0.0], + &nalgebra::dmatrix![0.0], 0.05, - ); + )?; let (is_controllable, controllability_matrix) = analysis::is_ss_controllable(&ss_model); diff --git a/examples/end_to_end.rs b/examples/end_to_end.rs new file mode 100644 index 0000000..d74a939 --- /dev/null +++ b/examples/end_to_end.rs @@ -0,0 +1,25 @@ +// End-to-end workflow: +// 1) Build a continuous model. +// 2) Discretize with ZOH. +// 3) Run a step simulation. +// 4) Check controllability. +use control_sys::{analysis, model, simulator}; +use nalgebra as na; + +fn main() -> Result<(), Box> { + let a = na::dmatrix![0.0, 1.0; -2.0, -3.0]; + let b = na::dmatrix![0.0; 1.0]; + let c = na::dmatrix![1.0, 0.0]; + let d = na::dmatrix![0.0]; + + let continuous = model::ContinuousStateSpaceModel::try_from_matrices(&a, &b, &c, &d)?; + let discrete = model::DiscreteStateSpaceModel::from_continuous_zoh(&continuous, 0.05)?; + + let (y, _u, _x) = simulator::step_for_discrete_ss(&discrete, 5.0)?; + let (is_controllable, _ctrb) = analysis::is_ss_controllable(&discrete); + + println!("step samples: {}", y.ncols()); + println!("controllable: {}", is_controllable); + + Ok(()) +} diff --git a/examples/step_response.rs b/examples/step_response.rs index 960a941..89845cd 100644 --- a/examples/step_response.rs +++ b/examples/step_response.rs @@ -1,67 +1,14 @@ -use control_sys::model; +// Step-response plotting example: +// Build a DC motor model, simulate a unit-step input, and export a plot to img/step_response.png. use control_sys::model::Pole; use control_sys::simulator; use plotters::prelude::*; -use std::borrow::Borrow; use std::fs; -use std::rc::Rc; extern crate nalgebra as na; -pub mod two_spring_damper_mass { - extern crate nalgebra as na; - - use std::default::Default; - - use control_sys::model::DiscreteStateSpaceModel; - - pub struct Parameters { - m1: f64, - m2: f64, - k1: f64, - k2: f64, - d1: f64, - d2: f64, - } - - impl Default for Parameters { - fn default() -> Parameters { - Parameters { - m1: 2.0, - m2: 2.0, - k1: 100.0, - k2: 200.0, - d1: 1.0, - d2: 5.0, - } - } - } - - pub fn build_model(params: Parameters, sampling_dt: f64) -> DiscreteStateSpaceModel { - // Define the continuous-time system matrices - let mat_ac = na::dmatrix![ - 0.0, 1.0, 0.0, 0.0; - -(params.k1 + params.k2) / params.m1, -(params.d1 + params.d2) / params.m1, params.k2 / params.m1, params.d2 / params.m1; - 0.0, 0.0, 0.0, 1.0; - params.k2 / params.m2, params.d2 / params.m2, -params.k2 / params.m2, -params.d2 / params.m2 - ]; - let mat_bc = na::dmatrix![0.0; 0.0; 0.0; 1.0 / params.m2]; - let mat_cc = na::dmatrix![1.0, 0.0, 0.0, 0.0]; - let mat_dc = na::dmatrix![0.0]; - - // Model discretization - DiscreteStateSpaceModel::from_continuous_matrix_forward_euler( - &mat_ac, - &mat_bc, - &mat_cc, - &mat_dc, - sampling_dt, - ) - } -} - pub mod dc_motor { use control_sys::model::DiscreteStateSpaceModel; use std::default::Default; @@ -86,8 +33,10 @@ pub mod dc_motor { } } - pub fn build_model(params: Parameters, sampling_dt: f64) -> DiscreteStateSpaceModel { - // Define the continuous-time system matrices + pub fn build_model( + params: Parameters, + sampling_dt: f64, + ) -> Result { let mat_ac = na::dmatrix![ -params.b / params.j, params.k / params.j; -params.k / params.l, -params.r / params.l; @@ -95,8 +44,7 @@ pub mod dc_motor { let mat_bc = na::dmatrix![0.0; 1.0 / params.l]; let mat_cc = na::dmatrix![1.0, 0.0]; - // Model discretization - DiscreteStateSpaceModel::from_continuous_matrix_forward_euler( + DiscreteStateSpaceModel::from_continuous_matrix_zoh( &mat_ac, &mat_bc, &mat_cc, @@ -109,69 +57,59 @@ pub mod dc_motor { fn main() -> Result<(), Box> { let sampling_dt = 0.05; let params = dc_motor::Parameters::default(); - let model = Rc::new(dc_motor::build_model(params, sampling_dt)); + let model = dc_motor::build_model(params, sampling_dt)?; - model.poles(); + let _poles = model.poles(); - let (step_response, step, _) = simulator::step_for_discrete_ss( - as Borrow>::borrow( - &model, - ), - 10.0, - ); + let (step_response, step, _) = simulator::step_for_discrete_ss(&model, 10.0)?; - // Create a folder for results fs::create_dir("img").unwrap_or_else(|_| { println!("The folder img already exists, no need to create it."); }); - // Draw the step response - { - let root = BitMapBackend::new("img/step_response.png", (800, 600)).into_drawing_area(); - root.fill(&WHITE)?; - - let max_y = step_response - .iter() - .cloned() - .fold(f64::NEG_INFINITY, f64::max); - let min_y = step_response.iter().cloned().fold(f64::INFINITY, f64::min); - let mut chart = ChartBuilder::on(&root) - .caption("System Output Y", ("sans-serif", 20)) - .margin(10) - .x_label_area_size(30) - .y_label_area_size(40) - .build_cartesian_2d(0..step_response.len() as i32, min_y..max_y)?; - - chart.configure_mesh().draw()?; - - // Plot input - let series_input: Vec<(i32, f64)> = step - .row(0) - .iter() - .enumerate() - .map(|(i, &val)| (i as i32, val as f64)) - .collect(); - - chart - .draw_series(LineSeries::new(series_input, &Palette99::pick(0)))? - .label(format!("Output {}", 0)) - .legend(move |(x, y)| PathElement::new([(x, y), (x + 20, y)], &Palette99::pick(0))); - - // Plot system response - let series_y: Vec<(i32, f64)> = step_response - .iter() - .enumerate() - .map(|(i, &val)| (i as i32, val as f64)) - .collect(); - - chart.draw_series(LineSeries::new(series_y, &Palette99::pick(1)))?; - - chart - .configure_series_labels() - .background_style(&WHITE) - .border_style(&BLACK) - .draw()?; - } + let root = BitMapBackend::new("img/step_response.png", (800, 600)).into_drawing_area(); + root.fill(&WHITE)?; + + let max_y = step_response + .iter() + .cloned() + .fold(f64::NEG_INFINITY, f64::max); + let min_y = step_response.iter().cloned().fold(f64::INFINITY, f64::min); + let mut chart = ChartBuilder::on(&root) + .caption("System Output Y", ("sans-serif", 20)) + .margin(10) + .x_label_area_size(30) + .y_label_area_size(40) + .build_cartesian_2d(0..step_response.ncols() as i32, min_y..max_y)?; + + chart.configure_mesh().draw()?; + + let series_input: Vec<(i32, f64)> = step + .row(0) + .iter() + .enumerate() + .map(|(i, &val)| (i as i32, val)) + .collect(); + + chart + .draw_series(LineSeries::new(series_input, &Palette99::pick(0)))? + .label("Input") + .legend(move |(x, y)| PathElement::new([(x, y), (x + 20, y)], Palette99::pick(0))); + + let series_y: Vec<(i32, f64)> = step_response + .row(0) + .iter() + .enumerate() + .map(|(i, &val)| (i as i32, val)) + .collect(); + + chart.draw_series(LineSeries::new(series_y, &Palette99::pick(1)))?; + + chart + .configure_series_labels() + .background_style(WHITE) + .border_style(BLACK) + .draw()?; Ok(()) } diff --git a/src/analysis.rs b/src/analysis.rs index 9a70902..bc0a5e4 100644 --- a/src/analysis.rs +++ b/src/analysis.rs @@ -1,55 +1,71 @@ extern crate nalgebra as na; -use crate::model::StateSpaceModel; +use crate::model::{ModelError, Pole, StateSpaceModel}; + +/// Diagnostics for rank-based system properties. +#[derive(Debug, Clone)] +pub struct RankDiagnostics { + /// Numerical rank of the tested matrix. + pub rank: usize, + /// Expected full rank. + pub expected_rank: usize, + /// Condition number estimate using singular values. + pub condition_number: f64, + /// Whether matrix rank matches expected rank. + pub is_full_rank: bool, +} -/// Computes the controllability matrix for a given state-space representation. -/// -/// # Arguments -/// -/// * `mat_a` - The state matrix (A) of the system. -/// * `mat_b` - The input matrix (B) of the system. -/// -/// # Returns -/// -/// * `Result, &'static str>` - The controllability matrix if successful, or an error message if the A matrix is not square. -/// -/// # Errors -/// -/// This function will return an error if the A matrix is not square. -/// -/// # Panics -/// -/// This function does not panic. -/// -/// # Examples -/// -/// ``` -/// use control_sys::analysis; -/// -/// let mat_a = nalgebra::dmatrix![1.0, -2.0; -/// 2.0, 1.0]; -/// let mat_b = nalgebra::dmatrix![1.0; -/// 2.0]; -/// match analysis::compute_controllability_matrix(&mat_a, &mat_b) { -/// Ok(result) => { println!("Controllability matrix is {}", result); }, -/// Err(msg) => { println!("{}", msg)} -/// } -/// ``` -/// +/// Time domain used for stability checks. +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum TimeDomain { + /// Continuous-time model (`Re(lambda_i) < 0` for stability). + Continuous, + /// Discrete-time model (`|lambda_i| < 1` for stability). + Discrete, +} + +/// Consolidated analysis report for an LTI state-space model. +#[derive(Debug, Clone)] +pub struct LtiAnalysisReport { + /// Eigenvalues of the state matrix. + pub poles: Vec>, + /// Spectral radius `max(|lambda_i|)`. + pub spectral_radius: f64, + /// Stability result based on chosen [`TimeDomain`]. + pub is_stable: bool, + /// Controllability diagnostics. + pub controllability: RankDiagnostics, + /// Observability diagnostics. + pub observability: RankDiagnostics, +} + +/// Computes the controllability matrix `[B, AB, ..., A^(n-1)B]`. +#[allow(clippy::toplevel_ref_arg)] pub fn compute_controllability_matrix( mat_a: &na::DMatrix, mat_b: &na::DMatrix, -) -> Result, &'static str> { +) -> Result, ModelError> { if !mat_a.is_square() { - return Err("Error when computing controllability matrix. The A matrix is not square."); + return Err(ModelError::MatrixANotSquare { + rows: mat_a.nrows(), + cols: mat_a.ncols(), + }); + } + + if mat_b.nrows() != mat_a.nrows() { + return Err(ModelError::DimensionMismatch { + a: mat_a.shape(), + b: mat_b.shape(), + c: (0, 0), + d: (0, 0), + }); } - let n = mat_a.nrows(); // Number of states + let n = mat_a.nrows(); let mut controllability_matrix = mat_b.clone_owned(); - // Generate [B, AB, A^2B, ...] for i in 1..n { - let column_block = mat_a.pow(i as u32) * mat_b; // Compute A^i * B + let column_block = mat_a.pow(i as u32) * mat_b; controllability_matrix = na::stack![controllability_matrix, column_block]; } @@ -57,93 +73,41 @@ pub fn compute_controllability_matrix( } /// Checks if a given state-space model is controllable. -/// -/// # Arguments -/// -/// * `model` - A reference to a state-space model that implements the `StateSpaceModel` trait. -/// -/// # Returns -/// -/// * `bool` - `true` if the system is controllable, `false` otherwise. -/// -/// # Panics -/// -/// This function will panic if the computation of the controllability matrix fails. -/// -/// # Examples -/// -/// ``` -/// use control_sys::analysis; -/// use control_sys::model; -/// -/// let ss_model = model::DiscreteStateSpaceModel::from_matrices( -/// &nalgebra::dmatrix![1.0, -2.0; -/// 2.0, 1.0], -/// &nalgebra::dmatrix![1.0; -/// 2.0], -/// &nalgebra::dmatrix![], -/// &nalgebra::dmatrix![], -/// 0.05, -/// ); -/// let (is_controllable, controllability_matrix) = analysis::is_ss_controllable(&ss_model); -/// ``` pub fn is_ss_controllable(model: &T) -> (bool, na::DMatrix) { let mat_a = model.mat_a(); let mat_b = model.mat_b(); - // We know that mat_a is square so we simply unwrap() the result - let mat_contr = compute_controllability_matrix(&mat_a, &mat_b) - .expect("State matrix of the model is not square"); + let mat_contr = compute_controllability_matrix(mat_a, mat_b) + .expect("State-space dimensions are invalid for controllability computation"); - // Since the input is a state space model, we expect A to be square - return (mat_contr.rank(1e-3) == mat_a.nrows(), mat_contr); + (mat_contr.rank(1e-3) == mat_a.nrows(), mat_contr) } -/// Computes the observability matrix for a given state-space representation. -/// -/// # Arguments -/// -/// * `mat_a` - The state matrix (A) of the system. -/// * `mat_c` - The input matrix (B) of the system. -/// -/// # Returns -/// -/// * `Result, &'static str>` - The observability matrix if successful, or an error message if the A matrix is not square. -/// -/// # Errors -/// -/// This function will return an error if the A matrix is not square. -/// -/// # Panics -/// -/// This function does not panic. -/// -/// # Examples -/// -/// ``` -/// use control_sys::analysis; -/// -/// let mat_a = nalgebra::dmatrix![1.0, -2.0; -/// 2.0, 1.0]; -/// let mat_c = nalgebra::dmatrix![1.0, 2.0]; -/// -/// match analysis::compute_observability_matrix(&mat_a, &mat_c) { -/// Ok(result) => { println!("Observability matrix is {}", result); }, -/// Err(msg) => { println!("{}", msg)} -/// } -/// ``` +/// Computes the observability matrix `[C; CA; ...; CA^(n-1)]`. +#[allow(clippy::toplevel_ref_arg)] pub fn compute_observability_matrix( mat_a: &na::DMatrix, mat_c: &na::DMatrix, -) -> Result, &'static str> { +) -> Result, ModelError> { if !mat_a.is_square() { - return Err("Error when computing observability matrix. The A matrix is not square."); + return Err(ModelError::MatrixANotSquare { + rows: mat_a.nrows(), + cols: mat_a.ncols(), + }); + } + + if mat_c.ncols() != mat_a.nrows() { + return Err(ModelError::DimensionMismatch { + a: mat_a.shape(), + b: (0, 0), + c: mat_c.shape(), + d: (0, 0), + }); } - let n = mat_a.nrows(); // Number of states + let n = mat_a.nrows(); let mut observability_mat = mat_c.clone_owned(); - // Generate [C; CA; CA^2;...] for i in 1..n { let column_block = mat_c * mat_a.pow(i as u32); observability_mat = na::stack![observability_mat; column_block]; @@ -153,45 +117,121 @@ pub fn compute_observability_matrix( } /// Checks if a given state-space model is observable. -/// -/// # Arguments -/// -/// * `model` - A reference to a state-space model that implements the `StateSpaceModel` trait. -/// -/// # Returns -/// -/// * A tuple with a `bool` which is `true` if the system is controllable, `false` otherwise and the observability matrix. -/// -/// # Panics -/// -/// This function will panic if the computation of the controllability matrix fails. -/// -/// # Examples -/// -/// ``` -/// use control_sys::analysis; -/// use control_sys::model; -/// -/// let ss_model = model::DiscreteStateSpaceModel::from_matrices( -/// &nalgebra::dmatrix![1.0, -2.0; -/// 2.0, 1.0], -/// &nalgebra::dmatrix![], -/// &nalgebra::dmatrix![1.0, 2.0], -/// &nalgebra::dmatrix![], -/// 0.05, -/// ); -/// let (is_observable, observability_matrix) = analysis::is_ss_observable(&ss_model); -/// ``` pub fn is_ss_observable(model: &T) -> (bool, na::DMatrix) { let mat_a = model.mat_a(); let mat_c = model.mat_c(); - // StateSpaceModel performs check of the matrices so computing observability matrix should not fail - let mat_obs = compute_observability_matrix(&mat_a, &mat_c) - .expect("State matrix of the model is not square"); + let mat_obs = compute_observability_matrix(mat_a, mat_c) + .expect("State-space dimensions are invalid for observability computation"); + + (mat_obs.rank(1e-3) == mat_a.nrows(), mat_obs) +} + +/// Computes controllability rank diagnostics. +pub fn controllability_diagnostics( + model: &T, + rank_tol: f64, +) -> Result { + let mat = compute_controllability_matrix(model.mat_a(), model.mat_b())?; + let rank = mat.rank(rank_tol); + let expected_rank = model.mat_a().nrows(); + let condition_number = condition_number(&mat, 1e-12); + + Ok(RankDiagnostics { + rank, + expected_rank, + condition_number, + is_full_rank: rank == expected_rank, + }) +} + +/// Computes observability rank diagnostics. +pub fn observability_diagnostics( + model: &T, + rank_tol: f64, +) -> Result { + let mat = compute_observability_matrix(model.mat_a(), model.mat_c())?; + let rank = mat.rank(rank_tol); + let expected_rank = model.mat_a().nrows(); + let condition_number = condition_number(&mat, 1e-12); + + Ok(RankDiagnostics { + rank, + expected_rank, + condition_number, + is_full_rank: rank == expected_rank, + }) +} + +/// Returns spectral radius `max(|lambda_i|)`. +pub fn spectral_radius(model: &T) -> f64 { + model + .poles() + .iter() + .map(|pole| pole.norm()) + .fold(0.0, f64::max) +} + +/// Returns true if all discrete poles are strictly inside the unit circle. +pub fn is_discrete_stable(model: &T) -> bool { + model.poles().iter().all(|pole| pole.norm() < 1.0) +} + +/// Returns true if all continuous poles have negative real part. +pub fn is_continuous_stable(model: &T) -> bool { + model.poles().iter().all(|pole| pole.re < 0.0) +} - // Since the input is a state space model, we expect A to be square - return (mat_obs.rank(1e-3) == mat_a.nrows(), mat_obs); +/// Performs a full first-pass LTI analysis. +/// +/// This combines: +/// +/// - Pole extraction +/// - Spectral radius +/// - Stability check for the selected [`TimeDomain`] +/// - Controllability diagnostics +/// - Observability diagnostics +pub fn analyze_lti( + model: &T, + domain: TimeDomain, + rank_tol: f64, +) -> Result { + let poles = model.poles(); + let spectral_radius = poles.iter().map(|pole| pole.norm()).fold(0.0f64, f64::max); + let is_stable = match domain { + TimeDomain::Continuous => poles.iter().all(|pole| pole.re < 0.0), + TimeDomain::Discrete => poles.iter().all(|pole| pole.norm() < 1.0), + }; + + let controllability = controllability_diagnostics(model, rank_tol)?; + let observability = observability_diagnostics(model, rank_tol)?; + + Ok(LtiAnalysisReport { + poles, + spectral_radius, + is_stable, + controllability, + observability, + }) +} + +fn condition_number(mat: &na::DMatrix, min_sv_tol: f64) -> f64 { + let svd = mat.clone().svd(false, false); + let mut max_sv = 0.0f64; + let mut min_sv = f64::INFINITY; + + for sv in svd.singular_values.iter() { + max_sv = max_sv.max(*sv); + if *sv > min_sv_tol { + min_sv = min_sv.min(*sv); + } + } + + if min_sv.is_infinite() || min_sv <= 0.0 { + f64::INFINITY + } else { + max_sv / min_sv + } } #[cfg(test)] @@ -200,43 +240,48 @@ mod tests { use crate::model::DiscreteStateSpaceModel; + // Verifies controllability matrix construction for a nominal 2x2 system. + #[allow(clippy::toplevel_ref_arg)] #[test] fn test_compute_controllability_matrix_2x2() { let mat_a = nalgebra::dmatrix![1.0, -2.0; 2.0, 1.0]; - let mat_b = nalgebra::dmatrix![1.0; + let mat_b = nalgebra::dmatrix![1.0; 2.0]; let result = compute_controllability_matrix(&mat_a, &mat_b).unwrap(); - assert_eq!(result.nrows(), mat_b.ncols() * mat_a.nrows()); - assert_eq!(result.ncols(), mat_a.ncols()); - assert_eq!(result, na::stack![mat_b, mat_a * &mat_b]) + assert_eq!(result.nrows(), mat_b.nrows()); + assert_eq!(result.ncols(), mat_b.ncols() * mat_a.ncols()); + assert_eq!(result, na::stack![mat_b, mat_a * &mat_b]); } + // Verifies a non-square A matrix is rejected for controllability computation. #[test] fn test_compute_controllability_matrix_mat_a_not_square() { let mat_a = nalgebra::dmatrix![1.0, -2.0]; - let mat_b = nalgebra::dmatrix![1.0; + let mat_b = nalgebra::dmatrix![1.0; 2.0]; let result = compute_controllability_matrix(&mat_a, &mat_b); assert_eq!( result, - Err("Error when computing controllability matrix. The A matrix is not square.") + Err(ModelError::MatrixANotSquare { rows: 1, cols: 2 }) ); } + // Verifies a known controllable 2x2 model is detected as controllable. #[test] fn test_controllability_2x2_controllable() { + #[allow(deprecated)] let ss_model = DiscreteStateSpaceModel::from_matrices( - &nalgebra::dmatrix![1.0, -2.0; + &nalgebra::dmatrix![1.0, -2.0; 2.0, 1.0], &nalgebra::dmatrix![1.0; 2.0], - &nalgebra::dmatrix![], - &nalgebra::dmatrix![], + &nalgebra::dmatrix![1.0, 0.0], + &nalgebra::dmatrix![0.0], 0.05, ); @@ -245,8 +290,10 @@ mod tests { assert!(result); } + // Verifies a known 3x3 model is detected as not controllable. #[test] fn test_controllability_3x3_not_controllable() { + #[allow(deprecated)] let ss_model = DiscreteStateSpaceModel::from_matrices( &na::dmatrix![ 0.0, 1.0, 0.0; @@ -256,14 +303,16 @@ mod tests { 1.0; 0.0; 0.0], - &nalgebra::dmatrix![], - &nalgebra::dmatrix![], + &nalgebra::dmatrix![1.0, 0.0, 0.0], + &nalgebra::dmatrix![0.0], 0.05, ); assert!(!is_ss_controllable(&ss_model).0); } + // Verifies observability matrix construction for a nominal 2x2 system. + #[allow(clippy::toplevel_ref_arg)] #[test] fn test_compute_observability_matrix_2x2() { let mat_a = nalgebra::dmatrix![1.0, -2.0; @@ -277,28 +326,31 @@ mod tests { assert_eq!(result, na::stack![&mat_c; &mat_c * mat_a]); } + // Verifies a non-square A matrix is rejected for observability computation. #[test] fn test_compute_observability_matrix_mat_a_not_square() { let mat_a = nalgebra::dmatrix![1.0, -2.0]; - let mat_b = nalgebra::dmatrix![1.0; - 2.0]; + let mat_c = nalgebra::dmatrix![1.0, 2.0]; - let result = compute_observability_matrix(&mat_a, &mat_b); + let result = compute_observability_matrix(&mat_a, &mat_c); assert_eq!( result, - Err("Error when computing observability matrix. The A matrix is not square.") + Err(ModelError::MatrixANotSquare { rows: 1, cols: 2 }) ); } + // Verifies a known observable 2x2 model is detected as observable. #[test] fn test_is_observable_2x2_observable_system() { + #[allow(deprecated)] let ss_model = DiscreteStateSpaceModel::from_matrices( - &nalgebra::dmatrix![1.0, -2.0; + &nalgebra::dmatrix![1.0, -2.0; 2.0, 1.0], - &nalgebra::dmatrix![], + &nalgebra::dmatrix![0.0; + 0.0], &nalgebra::dmatrix![1.0, 2.0], - &nalgebra::dmatrix![], + &nalgebra::dmatrix![0.0], 0.05, ); @@ -307,16 +359,20 @@ mod tests { assert!(result); } + // Verifies a known 3x3 model is detected as not observable. #[test] fn test_observability_3x3_not_observable() { + #[allow(deprecated)] let ss_model = DiscreteStateSpaceModel::from_matrices( &na::dmatrix![ 1.0, 0.0, 0.0; 0.0, 1.0, 0.0; 0.0, 0.0, 0.0], - &na::dmatrix![], + &na::dmatrix![0.0; + 0.0; + 0.0], &nalgebra::dmatrix![1.0, 0.0, 0.0], - &nalgebra::dmatrix![], + &nalgebra::dmatrix![0.0], 0.05, ); @@ -331,4 +387,40 @@ mod tests { 1.0, 0.0, 0.0] ); } + + // Verifies discrete stability helpers on a stable single-state model. + #[test] + fn test_stability_checks() { + #[allow(deprecated)] + let discrete_stable = DiscreteStateSpaceModel::from_matrices( + &na::dmatrix![0.8], + &na::dmatrix![1.0], + &na::dmatrix![1.0], + &na::dmatrix![0.0], + 0.1, + ); + + assert!(is_discrete_stable(&discrete_stable)); + assert!(spectral_radius(&discrete_stable) < 1.0); + } + + // Verifies the consolidated LTI report fields are populated consistently. + #[test] + fn test_analyze_lti_discrete_report() { + #[allow(deprecated)] + let model = DiscreteStateSpaceModel::from_matrices( + &na::dmatrix![0.8, 0.0; 0.0, 0.7], + &na::dmatrix![1.0; 0.0], + &na::dmatrix![1.0, 0.0], + &na::dmatrix![0.0], + 0.1, + ); + + let report = analyze_lti(&model, TimeDomain::Discrete, 1e-6).unwrap(); + + assert!(report.is_stable); + assert!(report.spectral_radius < 1.0); + assert_eq!(report.controllability.expected_rank, 2); + assert_eq!(report.observability.expected_rank, 2); + } } diff --git a/src/lib.rs b/src/lib.rs index 1bd20f9..2830ed0 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -2,173 +2,49 @@ /*! # control-sys -**control-sys** is a control system library written in Rust. It implements tools to represent and analyze LTI systems using state-space model. +`control-sys` is a Rust control systems library focused on LTI state-space models. -## Examples - -### Continuous state space model - -A continuous state space model can be defined with `ContinuousStateSpaceModel`. As an example, a state-space model representing a DC motor can be defined this way. We are using `nalgebra` to represent the matrices: - -```rust -use nalgebra as na; -use control_sys::model; - -// DC motor parameters -let b = 0.1f64; -let j = 0.01f64; -let k = 0.01f64; -let l = 0.5f64; -let r = 1.0f64; - -let mat_ac = na::dmatrix![ - -b / j, k / j; - -k / l, -r / l; - ]; -let mat_bc = na::dmatrix![0.0; - 1.0 / l]; -let mat_cc = na::dmatrix![1.0, 0.0]; - -let cont_model = model::ContinuousStateSpaceModel::from_matrices(&mat_ac, &mat_bc, &mat_cc, &na::dmatrix![]); -``` - -### Continuous to discrete model conversion - -A `DiscreteStateSpaceModel` can be built from a continuous one. You then need to specify the sampling step `ts`: +## Quick Start ```rust use nalgebra as na; -use control_sys::model; +use control_sys::{analysis, model, simulator}; +use control_sys::model::StateSpaceModel; -// DC motor parameters -let b = 0.1f64; -let j = 0.01f64; -let k = 0.01f64; -let l = 0.5f64; -let r = 1.0f64; +let a = na::dmatrix![0.0, 1.0; -2.0, -3.0]; +let b = na::dmatrix![0.0; 1.0]; +let c = na::dmatrix![1.0, 0.0]; +let d = na::dmatrix![0.0]; -let mat_ac = na::dmatrix![ - -b / j, k / j; - -k / l, -r / l; - ]; -let mat_bc = na::dmatrix![0.0; - 1.0 / l]; -let mat_cc = na::dmatrix![1.0, 0.0]; +let continuous = model::ContinuousStateSpaceModel::try_from_matrices(&a, &b, &c, &d).unwrap(); +let discrete = model::DiscreteStateSpaceModel::from_continuous_zoh(&continuous, 0.05).unwrap(); -let cont_model = model::ContinuousStateSpaceModel::from_matrices(&mat_ac, &mat_bc, &mat_cc, &na::dmatrix![]); +let (y, u, x) = simulator::step_for_discrete_ss(&discrete, 2.0).unwrap(); +assert_eq!(u.nrows(), discrete.mat_b().ncols()); +assert_eq!(x.nrows(), discrete.mat_a().nrows()); +assert_eq!(y.nrows(), discrete.mat_c().nrows()); -let discrete_model = - model::DiscreteStateSpaceModel::from_continuous_ss_forward_euler(&cont_model, 0.05); +let (is_controllable, _ctrb) = analysis::is_ss_controllable(&discrete); +assert!(is_controllable); ``` -### Step response - -You can compute the step response of a system. For a discrete system, the simulation steps are given by the sampling step of the discretization: - -```rust -use nalgebra as na; -use control_sys::{model, simulator}; - -// DC motor parameters -let b = 0.1f64; -let j = 0.01f64; -let k = 0.01f64; -let l = 0.5f64; -let r = 1.0f64; - -let mat_ac = na::dmatrix![ - -b / j, k / j; - -k / l, -r / l; - ]; -let mat_bc = na::dmatrix![0.0; - 1.0 / l]; -let mat_cc = na::dmatrix![1.0, 0.0]; - -let cont_model = model::ContinuousStateSpaceModel::from_matrices(&mat_ac, &mat_bc, &mat_cc, &na::dmatrix![]); - -let discrete_model = - model::DiscreteStateSpaceModel::from_continuous_ss_forward_euler(&cont_model, 0.05); - -// where model implements the traits `StateSpaceModel` and `Discrete` -let duration = 10.0; // [s] -let (response, input, states) = simulator::step_for_discrete_ss( - &discrete_model, - duration, - ); -``` - -You can also compute the step response for a continuous model. You will need to provide the sampling step and the model will be discretized before computing the step response: - -```rust -use nalgebra as na; -use control_sys::{model, simulator}; - -// DC motor parameters -let b = 0.1f64; -let j = 0.01f64; -let k = 0.01f64; -let l = 0.5f64; -let r = 1.0f64; - -let mat_ac = na::dmatrix![ - -b / j, k / j; - -k / l, -r / l; - ]; -let mat_bc = na::dmatrix![0.0; - 1.0 / l]; -let mat_cc = na::dmatrix![1.0, 0.0]; - -let cont_model = model::ContinuousStateSpaceModel::from_matrices(&mat_ac, &mat_bc, &mat_cc, &na::dmatrix![]); - -// where model is a continuous state space model -let sampling_dt = 0.05; // [s] -let duration = 10.0; // [s] -let (response, input, states) = simulator::step_for_continuous_ss( - &cont_model, - sampling_dt, - duration, - ); -``` - -### Controllability - -The controllability of a system can be evaluated using the `is_ss_controllable` method: - -```rust -use nalgebra as na; -use control_sys::{model, analysis}; - -let ss_model = model::DiscreteStateSpaceModel::from_matrices( - &nalgebra::dmatrix![1.0, -2.0; - 2.0, 1.0], - &nalgebra::dmatrix![1.0; - 2.0], - &nalgebra::dmatrix![], - &nalgebra::dmatrix![], - 0.05, - ); - -let (is_controllable, controllability_matrix) = analysis::is_ss_controllable(&ss_model); - -if is_controllable -{ - println!("The system is controllable"); - println!("Its controllability matrix is: {}", controllability_matrix); -} -``` +## Discretization Methods and Assumptions +`DiscreteStateSpaceModel` provides explicit conversion APIs: +- `from_continuous_zoh`: exact ZOH using augmented matrix exponential. - */ +Recommended default is ZOH for sampled-data systems driven by held inputs. +*/ -/// Methods to analyze control systems +/// Methods to analyze control systems. pub mod analysis; -/// Structures to represent control systems +/// Structures to represent control systems. pub mod model; -/// Methods to simulate control systems in time-space +/// Methods to simulate control systems in time-space. pub mod simulator; -/// Methods to generate trajectories +/// Methods to generate trajectories. pub mod trajectory; diff --git a/src/model.rs b/src/model.rs index cec916b..5628c9f 100644 --- a/src/model.rs +++ b/src/model.rs @@ -1,5 +1,69 @@ +//! State-space model primitives and discretization utilities. +//! +//! This module defines continuous and discrete LTI state-space models: +//! +//! - Continuous: `x_dot = A x + B u`, `y = C x + D u` +//! - Discrete: `x[k+1] = A x[k] + B u[k]`, `y[k] = C x[k] + D u[k]` +//! +//! It also provides multiple continuous-to-discrete conversion methods: +//! +//! - Exact ZOH (augmented matrix exponential) +//! The library intentionally exposes a small discretization surface for now: +//! exact ZOH. + extern crate nalgebra as na; +use std::error::Error; +use std::fmt; + +/// Error type for state-space model creation and transformations. +#[derive(Debug, Clone, PartialEq)] +pub enum ModelError { + /// State matrix `A` is not square. + MatrixANotSquare { + /// Number of rows in `A`. + rows: usize, + /// Number of columns in `A`. + cols: usize, + }, + /// State-space matrix dimensions are incompatible. + DimensionMismatch { + /// Shape of matrix `A`. + a: (usize, usize), + /// Shape of matrix `B`. + b: (usize, usize), + /// Shape of matrix `C`. + c: (usize, usize), + /// Shape of matrix `D`. + d: (usize, usize), + }, + /// Sampling time is invalid. + InvalidSamplingDt(f64), + /// Matrix inversion failed. + SingularMatrix(&'static str), +} + +impl fmt::Display for ModelError { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + match self { + ModelError::MatrixANotSquare { rows, cols } => { + write!(f, "A must be square, got {rows}x{cols}") + } + ModelError::DimensionMismatch { a, b, c, d } => write!( + f, + "incompatible dimensions: A={:?}, B={:?}, C={:?}, D={:?}", + a, b, c, d + ), + ModelError::InvalidSamplingDt(dt) => { + write!(f, "sampling_dt must be finite and > 0.0, got {dt}") + } + ModelError::SingularMatrix(ctx) => write!(f, "matrix inversion failed: {ctx}"), + } + } +} + +impl Error for ModelError {} + /// A trait representing a state-space model in control systems. /// /// This trait provides methods to access the state-space matrices A, B, C, and D, @@ -91,43 +155,46 @@ pub struct ContinuousStateSpaceModel { /// Represents a continuous state-space model. impl ContinuousStateSpaceModel { - /// Creates a new `ContinuousStateSpaceModel` with the given matrices. - /// - /// # Arguments + /// Creates a new `ContinuousStateSpaceModel` with full dimension checks. /// - /// * `mat_a` - State matrix (A). - /// * `mat_b` - Input matrix (B). - /// * `mat_c` - Output matrix (C). - /// * `mat_d` - Feedthrough matrix (D). + /// The following compatibility constraints are enforced: /// - /// # Returns - /// - /// A new instance of `ContinuousStateSpaceModel`. - pub fn from_matrices( + /// - `A` is square `(n x n)` + /// - `B` is `(n x m)` + /// - `C` is `(p x n)` + /// - `D` is `(p x m)` + pub fn try_from_matrices( mat_a: &na::DMatrix, mat_b: &na::DMatrix, mat_c: &na::DMatrix, mat_d: &na::DMatrix, - ) -> ContinuousStateSpaceModel { - ContinuousStateSpaceModel { + ) -> Result { + validate_state_space_dimensions(mat_a, mat_b, mat_c, mat_d)?; + + Ok(ContinuousStateSpaceModel { mat_a: mat_a.clone(), mat_b: mat_b.clone(), mat_c: mat_c.clone(), mat_d: mat_d.clone(), - } + }) + } + + /// Creates a new model without runtime checks. + #[deprecated(note = "Use try_from_matrices for validated construction")] + pub fn from_matrices( + mat_a: &na::DMatrix, + mat_b: &na::DMatrix, + mat_c: &na::DMatrix, + mat_d: &na::DMatrix, + ) -> ContinuousStateSpaceModel { + Self::try_from_matrices(mat_a, mat_b, mat_c, mat_d).expect("invalid state-space dimensions") } /// Builds a controllable canonical form state-space model from a transfer function. - /// - /// # Arguments - /// - /// * `tf` - The transfer function to convert. - /// - /// # Returns - /// - /// A `ContinuousStateSpaceModel` in controllable canonical form. - fn build_controllable_canonical_form(tf: &TransferFunction) -> ContinuousStateSpaceModel { - // TODO: Still need to normalize coefficients and check for size + #[cfg(test)] + fn build_controllable_canonical_form( + tf: &TransferFunction, + ) -> Result { let n_states = tf.denominator_coeffs.len(); let mut mat_a = na::DMatrix::::zeros(n_states, n_states); @@ -135,52 +202,47 @@ impl ContinuousStateSpaceModel { .view_range_mut(0..n_states - 1, 1..) .copy_from(&na::DMatrix::::identity(n_states - 1, n_states - 1)); for (i, value) in tf.denominator_coeffs.iter().rev().enumerate() { - mat_a[(n_states - 1, i)] = -value.clone(); + mat_a[(n_states - 1, i)] = -*value; } let mut mat_b = na::DMatrix::::zeros(tf.numerator_coeffs.len(), 1); mat_b[(tf.numerator_coeffs.len() - 1, 0)] = 1.0f64; - let mut mat_c = na::DMatrix::::zeros(tf.numerator_coeffs.len(), 1); + let mut mat_c = na::DMatrix::::zeros(1, n_states); for (i, value) in tf.numerator_coeffs.iter().rev().enumerate() { - mat_c[(i, 0)] = value.clone(); + if i < n_states { + mat_c[(0, i)] = *value; + } } let mat_d = na::dmatrix![tf.constant]; - ContinuousStateSpaceModel { - mat_a: mat_a, - mat_b: mat_b, - mat_c: mat_c, - mat_d: mat_d, - } + Self::try_from_matrices(&mat_a, &mat_b, &mat_c, &mat_d) } - /// Returns the size of the state-space model. + /// Returns the number of states `n` of the model. /// - /// # Returns - /// - /// The number of states in the state-space model. + /// This equals the number of columns of `A` (and rows of `A` since `A` is square). pub fn state_space_size(&self) -> usize { - return self.mat_a.ncols(); + self.mat_a.ncols() } } impl StateSpaceModel for ContinuousStateSpaceModel { fn mat_a(&self) -> &na::DMatrix { - return &self.mat_a; + &self.mat_a } fn mat_b(&self) -> &na::DMatrix { - return &self.mat_b; + &self.mat_b } fn mat_c(&self) -> &na::DMatrix { - return &self.mat_c; + &self.mat_c } fn mat_d(&self) -> &na::DMatrix { - return &self.mat_d; + &self.mat_d } } @@ -200,13 +262,6 @@ impl Pole for ContinuousStateSpaceModel { /// - `mat_d`: The feedthrough (or direct transmission) matrix. /// /// Additionally, the model includes a sampling time `sampling_dt` which represents the time interval between each discrete step. -/// -/// # Fields -/// - `mat_a` (`na::DMatrix`): The state transition matrix. -/// - `mat_b` (`na::DMatrix`): The control input matrix. -/// - `mat_c` (`na::DMatrix`): The output matrix. -/// - `mat_d` (`na::DMatrix`): The feedthrough matrix. -/// - `sampling_dt` (f64): The sampling time interval. pub struct DiscreteStateSpaceModel { mat_a: na::DMatrix, mat_b: na::DMatrix, @@ -217,102 +272,109 @@ pub struct DiscreteStateSpaceModel { impl StateSpaceModel for DiscreteStateSpaceModel { fn mat_a(&self) -> &na::DMatrix { - return &self.mat_a; + &self.mat_a } fn mat_b(&self) -> &na::DMatrix { - return &self.mat_b; + &self.mat_b } fn mat_c(&self) -> &na::DMatrix { - return &self.mat_c; + &self.mat_c } fn mat_d(&self) -> &na::DMatrix { - return &self.mat_d; + &self.mat_d } } impl DiscreteStateSpaceModel { - /// Creates a new `DiscreteStateSpaceModel` with the given state-space matrices and sampling time. - /// - /// # Arguments + /// Creates a new `DiscreteStateSpaceModel` with matrix and sampling-time checks. /// - /// * `mat_a` - State transition matrix. - /// * `mat_b` - Control input matrix. - /// * `mat_c` - Observation matrix. - /// * `mat_d` - Feedforward matrix. - /// * `sampling_dt` - Sampling time interval. - /// - /// # Returns - /// - /// A new `DiscreteStateSpaceModel` instance. - pub fn from_matrices( + /// Matrix compatibility is identical to [`ContinuousStateSpaceModel::try_from_matrices`], + /// with the additional constraint that `sampling_dt > 0` and finite. + pub fn try_from_matrices( mat_a: &na::DMatrix, mat_b: &na::DMatrix, mat_c: &na::DMatrix, mat_d: &na::DMatrix, sampling_dt: f64, - ) -> DiscreteStateSpaceModel { - DiscreteStateSpaceModel { + ) -> Result { + validate_state_space_dimensions(mat_a, mat_b, mat_c, mat_d)?; + validate_sampling_dt(sampling_dt)?; + + Ok(DiscreteStateSpaceModel { mat_a: mat_a.clone(), mat_b: mat_b.clone(), mat_c: mat_c.clone(), mat_d: mat_d.clone(), - sampling_dt: sampling_dt, - } + sampling_dt, + }) } - /// Converts a continuous state-space model to a discrete state-space model using the forward Euler method. + /// Creates a new model without runtime checks. + #[deprecated(note = "Use try_from_matrices for validated construction")] + pub fn from_matrices( + mat_a: &na::DMatrix, + mat_b: &na::DMatrix, + mat_c: &na::DMatrix, + mat_d: &na::DMatrix, + sampling_dt: f64, + ) -> DiscreteStateSpaceModel { + Self::try_from_matrices(mat_a, mat_b, mat_c, mat_d, sampling_dt) + .expect("invalid state-space dimensions") + } + + /// Discretizes a continuous model using exact zero-order hold (ZOH). /// - /// # Arguments + /// For continuous dynamics `x_dot = A x + B u` with piecewise-constant input, + /// this computes: /// - /// * `mat_ac` - Continuous state transition matrix. - /// * `mat_bc` - Continuous control input matrix. - /// * `mat_cc` - Continuous observation matrix. - /// * `mat_dc` - Continuous feedforward matrix. - /// * `sampling_dt` - Sampling time interval. + /// - `A_d = exp(A * dt)` + /// - `B_d = integral_0^dt exp(A * tau) d tau * B` /// - /// # Returns + /// The implementation uses the standard augmented matrix exponential: /// - /// A new `DiscreteStateSpaceModel` instance. - pub fn from_continuous_matrix_forward_euler( + /// ```text + /// exp([A*dt B*dt] + /// [ 0 0 ]) = [A_d B_d] + /// [ 0 I ] + /// ``` + pub fn from_continuous_matrix_zoh( mat_ac: &na::DMatrix, mat_bc: &na::DMatrix, mat_cc: &na::DMatrix, mat_dc: &na::DMatrix, sampling_dt: f64, - ) -> DiscreteStateSpaceModel { - let mat_i = na::DMatrix::::identity(mat_ac.nrows(), mat_ac.nrows()); - let mat_a = (mat_i - mat_ac.scale(sampling_dt)).try_inverse().unwrap(); - let mat_b = &mat_a * mat_bc.scale(sampling_dt); - let mat_c = mat_cc.clone(); - let mat_d = mat_dc.clone(); - - DiscreteStateSpaceModel { - mat_a: mat_a, - mat_b: mat_b, - mat_c: mat_c, - mat_d: mat_d, - sampling_dt: sampling_dt, - } + ) -> Result { + validate_state_space_dimensions(mat_ac, mat_bc, mat_cc, mat_dc)?; + validate_sampling_dt(sampling_dt)?; + + let n_states = mat_ac.nrows(); + let n_inputs = mat_bc.ncols(); + let mut aug = na::DMatrix::::zeros(n_states + n_inputs, n_states + n_inputs); + aug.view_mut((0, 0), (n_states, n_states)) + .copy_from(&(mat_ac * sampling_dt)); + aug.view_mut((0, n_states), (n_states, n_inputs)) + .copy_from(&(mat_bc * sampling_dt)); + + let exp_aug = matrix_exponential(&aug); + let mat_a = exp_aug.view((0, 0), (n_states, n_states)).into_owned(); + let mat_b = exp_aug + .view((0, n_states), (n_states, n_inputs)) + .into_owned(); + + Self::try_from_matrices(&mat_a, &mat_b, mat_cc, mat_dc, sampling_dt) } - /// Converts a continuous state-space model to a discrete state-space model using the forward Euler method. - /// - /// # Arguments - /// - /// * `model` - A reference to a `ContinuousStateSpaceModel` instance. - /// * `sampling_dt` - Sampling time interval. + /// Discretizes a continuous model using exact ZOH. /// - /// # Returns - /// - /// A new `DiscreteStateSpaceModel` instance. - pub fn from_continuous_ss_forward_euler( + /// See [`DiscreteStateSpaceModel::from_continuous_matrix_zoh`] for equations. + pub fn from_continuous_zoh( model: &ContinuousStateSpaceModel, sampling_dt: f64, - ) -> DiscreteStateSpaceModel { - Self::from_continuous_matrix_forward_euler( + ) -> Result { + Self::from_continuous_matrix_zoh( model.mat_a(), model.mat_b(), model.mat_c(), @@ -320,6 +382,7 @@ impl DiscreteStateSpaceModel { sampling_dt, ) } + } impl Pole for DiscreteStateSpaceModel { @@ -330,16 +393,18 @@ impl Pole for DiscreteStateSpaceModel { impl Discrete for DiscreteStateSpaceModel { fn sampling_dt(&self) -> f64 { - return self.sampling_dt; + self.sampling_dt } } +#[cfg(test)] struct TransferFunction { numerator_coeffs: Vec, denominator_coeffs: Vec, constant: f64, } +#[cfg(test)] impl TransferFunction { fn new( numerator_coeffs: &[f64], @@ -349,33 +414,171 @@ impl TransferFunction { TransferFunction { numerator_coeffs: numerator_coeffs.to_vec(), denominator_coeffs: denominator_coeffs.to_vec(), - constant: constant, + constant, } } } -#[cfg(test)] -/// This module contains unit tests for the control system models. +fn validate_sampling_dt(sampling_dt: f64) -> Result<(), ModelError> { + if !sampling_dt.is_finite() || sampling_dt <= 0.0 { + return Err(ModelError::InvalidSamplingDt(sampling_dt)); + } + + Ok(()) +} + +fn validate_state_space_dimensions( + mat_a: &na::DMatrix, + mat_b: &na::DMatrix, + mat_c: &na::DMatrix, + mat_d: &na::DMatrix, +) -> Result<(), ModelError> { + if !mat_a.is_square() { + return Err(ModelError::MatrixANotSquare { + rows: mat_a.nrows(), + cols: mat_a.ncols(), + }); + } + + let n_states = mat_a.nrows(); + let n_inputs = mat_b.ncols(); + let n_outputs = mat_c.nrows(); + + if mat_b.nrows() != n_states + || mat_c.ncols() != n_states + || mat_d.nrows() != n_outputs + || mat_d.ncols() != n_inputs + { + return Err(ModelError::DimensionMismatch { + a: mat_a.shape(), + b: mat_b.shape(), + c: mat_c.shape(), + d: mat_d.shape(), + }); + } + + Ok(()) +} + +/// Computes a matrix exponential using a scaling-and-series strategy. +/// +/// Algorithm outline: +/// +/// 1. Compute a matrix norm (`||A||_1`) and choose a power-of-two scale `s` so +/// `A / 2^s` has smaller norm. +/// 2. Approximate `exp(A / 2^s)` by truncating the Taylor series +/// `I + M + M^2/2! + ...`. +/// 3. Recover `exp(A)` via repeated squaring: +/// `exp(A) = (exp(A / 2^s))^(2^s)`. /// -/// # Tests +/// This is a simple implementation intended for moderate matrix sizes in this +/// library context. /// -/// - `test_compute_state_space_model_nominal`: Tests the construction of a continuous state-space model in controllable canonical form from a transfer function and verifies the matrices A, B, C, and D. -/// - `test_compute_poles_pure_real`: Tests the computation of poles for a discrete state-space model with purely real eigenvalues. -/// - `test_compute_poles_pure_im`: Tests the computation of poles for a discrete state-space model with purely imaginary eigenvalues. -/// - `test_compute_poles_real_and_imaginary_part`: Tests the computation of poles for a discrete state-space model with both real and imaginary parts. -/// - `test_compute_controllability_matrix_nominal`: Tests the computation of the controllability matrix for a given state-space model. -/// - `test_controllability_2x2_controllable`: Tests the controllability of a 2x2 discrete state-space model that is controllable. -/// - `test_controllability_3x3_not_controllable`: Tests the controllability of a 3x3 discrete state-space model that is not controllable. +/// References: +/// - C. Moler and C. Van Loan, "Nineteen Dubious Ways to Compute the Exponential of a Matrix, +/// Twenty-Five Years Later", SIAM Review, 45(1), 2003. +fn matrix_exponential(mat: &na::DMatrix) -> na::DMatrix { + let n = mat.nrows(); + if n == 0 { + return na::DMatrix::::zeros(0, 0); + } + + // Step 1: choose a power-of-two scaling factor. + // + // We scale A -> A/2^s so the truncated Taylor series converges rapidly. + // Using powers of two makes the reconstruction step exact in structure: + // repeated squaring s times gives exponentiation by 2^s. + let norm_one = max_column_sum_norm(mat); + let scaling_power = if norm_one <= 0.5 { + 0 + } else { + (norm_one.log2().ceil().max(0.0) as u32) + 1 + }; + + let scale = 2f64.powi(scaling_power as i32); + let scaled = mat / scale; + + let identity = na::DMatrix::::identity(n, n); + let mut result = identity.clone(); + let mut term = identity; + + // Step 2: evaluate exp(M) with truncated Taylor series, where M = A/2^s. + // + // Recurrence: + // term_k = term_{k-1} * M / k, with term_0 = I. + // result = sum_k term_k. + // + // Stop when the latest term is numerically tiny in max-abs norm. + for k in 1..=64 { + term = (&term * &scaled) / (k as f64); + result += &term; + + let max_abs = term.iter().fold(0.0f64, |acc, v| acc.max(v.abs())); + if max_abs < 1e-14 { + break; + } + } + + // Step 3: undo scaling by repeated squaring. + // exp(A) = (exp(A/2^s))^(2^s). + let mut out = result; + for _ in 0..scaling_power { + out = &out * &out; + } + + out +} + +fn max_column_sum_norm(mat: &na::DMatrix) -> f64 { + (0..mat.ncols()) + .map(|col| { + (0..mat.nrows()) + .map(|row| mat[(row, col)].abs()) + .sum::() + }) + .fold(0.0f64, f64::max) +} + +#[cfg(test)] mod tests { use super::*; + fn approx_eq_matrix(a: &na::DMatrix, b: &na::DMatrix, tol: f64) { + assert_eq!(a.shape(), b.shape()); + for i in 0..a.nrows() { + for j in 0..a.ncols() { + assert!( + (a[(i, j)] - b[(i, j)]).abs() <= tol, + "mismatch at ({i},{j}): {} != {}", + a[(i, j)], + b[(i, j)] + ); + } + } + } + + // Verifies checked constructors reject incompatible A/B/C/D dimensions. + #[test] + fn test_try_from_matrices_dimension_validation() { + let err = DiscreteStateSpaceModel::try_from_matrices( + &na::dmatrix![1.0, 0.0; 0.0, 1.0], + &na::dmatrix![1.0; 0.0], + &na::dmatrix![1.0, 0.0], + &na::dmatrix![0.0, 0.0], + 0.1, + ) + .unwrap_err(); + + assert!(matches!(err, ModelError::DimensionMismatch { .. })); + } + + // Verifies controllable canonical form matrices for a nominal transfer function. #[test] fn test_compute_state_space_model_nominal() { let tf = TransferFunction::new(&[1.0, 2.0, 3.0], &[1.0, 4.0, 6.0], 8.0); - let ss_model = ContinuousStateSpaceModel::build_controllable_canonical_form(&tf); + let ss_model = ContinuousStateSpaceModel::build_controllable_canonical_form(&tf).unwrap(); - // Check mat A assert_eq!(ss_model.mat_a().shape(), (3, 3)); assert_eq!(ss_model.mat_a()[(2, 0)], -6.0f64); assert_eq!(ss_model.mat_a()[(2, 1)], -4.0f64); @@ -383,30 +586,48 @@ mod tests { assert_eq!(ss_model.mat_a()[(0, 1)], 1.0f64); assert_eq!(ss_model.mat_a()[(1, 2)], 1.0f64); - // Check mat B assert_eq!(ss_model.mat_b().shape(), (3, 1)); assert_eq!(ss_model.mat_b()[(0, 0)], 0.0f64); assert_eq!(ss_model.mat_b()[(1, 0)], 0.0f64); assert_eq!(ss_model.mat_b()[(2, 0)], 1.0f64); - // Check mat C - assert_eq!(ss_model.mat_c().shape(), (3, 1)); + assert_eq!(ss_model.mat_c().shape(), (1, 3)); assert_eq!(ss_model.mat_c()[(0, 0)], 3.0f64); - assert_eq!(ss_model.mat_c()[(1, 0)], 2.0f64); - assert_eq!(ss_model.mat_c()[(2, 0)], 1.0f64); + assert_eq!(ss_model.mat_c()[(0, 1)], 2.0f64); + assert_eq!(ss_model.mat_c()[(0, 2)], 1.0f64); - // Check mat D assert_eq!(ss_model.mat_d().shape(), (1, 1)); assert_eq!(ss_model.mat_d()[(0, 0)], 8.0f64); } + // Verifies ZOH discretization against first-order exact discretization values. + #[test] + fn test_discretization_zoh_first_order() { + let a = na::dmatrix![-2.0]; + let b = na::dmatrix![1.0]; + let c = na::dmatrix![1.0]; + let d = na::dmatrix![0.0]; + let dt = 0.1; + + let model = + DiscreteStateSpaceModel::from_continuous_matrix_zoh(&a, &b, &c, &d, dt).unwrap(); + + let ad = (-2.0f64 * dt).exp(); + let bd = (1.0 - ad) / 2.0; + + approx_eq_matrix(model.mat_a(), &na::dmatrix![ad], 1e-10); + approx_eq_matrix(model.mat_b(), &na::dmatrix![bd], 1e-10); + } + + // Verifies pole computation for a model with purely real poles. #[test] fn test_compute_poles_pure_real() { + #[allow(deprecated)] let ss_model = DiscreteStateSpaceModel::from_matrices( &nalgebra::dmatrix![2.0, 0.0; 0.0, 1.0], - &nalgebra::dmatrix![], - &nalgebra::dmatrix![], - &nalgebra::dmatrix![], + &nalgebra::dmatrix![0.0; 0.0], + &nalgebra::dmatrix![0.0, 0.0], + &nalgebra::dmatrix![0.0], 0.05, ); @@ -419,13 +640,15 @@ mod tests { assert_eq!(poles[1].im, 0.0); } + // Verifies pole computation for a model with purely imaginary poles. #[test] fn test_compute_poles_pure_im() { + #[allow(deprecated)] let ss_model = DiscreteStateSpaceModel::from_matrices( &nalgebra::dmatrix![0.0, -1.0; 1.0, 0.0], - &nalgebra::dmatrix![], - &nalgebra::dmatrix![], - &nalgebra::dmatrix![], + &nalgebra::dmatrix![0.0; 0.0], + &nalgebra::dmatrix![0.0, 0.0], + &nalgebra::dmatrix![0.0], 0.05, ); @@ -438,13 +661,15 @@ mod tests { assert_eq!(poles[1].im, -1.0); } + // Verifies pole computation for a model with complex-conjugate poles. #[test] fn test_compute_poles_real_and_imaginary_part() { + #[allow(deprecated)] let ss_model = DiscreteStateSpaceModel::from_matrices( &nalgebra::dmatrix![1.0, -2.0; 2.0, 1.0], - &nalgebra::dmatrix![], - &nalgebra::dmatrix![], - &nalgebra::dmatrix![], + &nalgebra::dmatrix![0.0; 0.0], + &nalgebra::dmatrix![0.0, 0.0], + &nalgebra::dmatrix![0.0], 0.05, ); diff --git a/src/simulator.rs b/src/simulator.rs index 12d6637..23f3a59 100644 --- a/src/simulator.rs +++ b/src/simulator.rs @@ -1,100 +1,429 @@ use nalgebra as na; -use crate::model::{ContinuousStateSpaceModel, Discrete, DiscreteStateSpaceModel, StateSpaceModel}; +use crate::model::{ + ContinuousStateSpaceModel, Discrete, DiscreteStateSpaceModel, ModelError, StateSpaceModel, +}; -/// Generates the step response of a discrete state-space model for a given duration. -/// -/// # Arguments -/// -/// * `model` - A reference to an object that implements the `StateSpaceModel` and `Discrete` traits. -/// * `duration` - The duration for which the step response is to be generated. -/// -/// # Returns -/// -/// A tuple containing: -/// * `response` - A matrix containing the output sequence of the step response. -/// * `input` - A matrix containing the input sequence (step input). -/// * `states` - A matrix containing the state sequence during the step response. +use std::error::Error; +use std::fmt; + +/// Simulation-related runtime errors. +#[derive(Debug, Clone, PartialEq)] +pub enum SimulationError { + /// Input dimensions do not match the model dimensions. + DimensionMismatch { + /// Expected number of input rows (`B.ncols()`). + expected_u_rows: usize, + /// Provided number of input rows. + actual_u_rows: usize, + /// Expected number of states (`A.nrows()`). + expected_x0_rows: usize, + /// Provided number of initial-state rows. + actual_x0_rows: usize, + }, + /// Invalid duration. + InvalidDuration(f64), + /// Invalid model state. + Model(ModelError), +} + +impl fmt::Display for SimulationError { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + match self { + SimulationError::DimensionMismatch { + expected_u_rows, + actual_u_rows, + expected_x0_rows, + actual_x0_rows, + } => write!( + f, + "dimension mismatch: u rows expected {expected_u_rows} got {actual_u_rows}, x0 rows expected {expected_x0_rows} got {actual_x0_rows}" + ), + SimulationError::InvalidDuration(duration) => { + write!(f, "duration must be finite and >= 0.0, got {duration}") + } + SimulationError::Model(err) => write!(f, "{err}"), + } + } +} + +impl Error for SimulationError {} + +impl From for SimulationError { + fn from(value: ModelError) -> Self { + SimulationError::Model(value) + } +} + +/// Simulates a discrete state-space model for an arbitrary input sequence. /// +/// The model equations are: +/// `x[k+1] = A x[k] + B u[k]` +/// `y[k] = C x[k] + D u[k]` +pub fn simulate( + model: &(impl StateSpaceModel + Discrete), + mat_u: &na::DMatrix, + x0: &na::DVector, +) -> Result<(na::DMatrix, na::DMatrix), SimulationError> { + let n_state = model.mat_a().nrows(); + let n_input = model.mat_b().ncols(); + let n_output = model.mat_c().nrows(); + let sim_time = mat_u.ncols(); + + if mat_u.nrows() != n_input || x0.nrows() != n_state { + return Err(SimulationError::DimensionMismatch { + expected_u_rows: n_input, + actual_u_rows: mat_u.nrows(), + expected_x0_rows: n_state, + actual_x0_rows: x0.nrows(), + }); + } + + let mut mat_x = na::DMatrix::::zeros(n_state, sim_time + 1); + let mut mat_y = na::DMatrix::::zeros(n_output, sim_time); + mat_x.column_mut(0).copy_from(x0); + + for i in 0..sim_time { + let xk = mat_x.column(i).into_owned(); + let uk = mat_u.column(i).into_owned(); + + let yk = model.mat_c() * &xk + model.mat_d() * &uk; + mat_y.column_mut(i).copy_from(&yk); + + let x_next = model.mat_a() * xk + model.mat_b() * uk; + mat_x.column_mut(i + 1).copy_from(&x_next); + } + + Ok((mat_y, mat_x)) +} + +type ResponseInputState = (na::DMatrix, na::DMatrix, na::DMatrix); + +/// Generates a unit-step response of a discrete state-space model. pub fn step_for_discrete_ss( model: &(impl StateSpaceModel + Discrete), duration: f64, -) -> (na::DMatrix, na::DMatrix, na::DMatrix) { - // Initial state is zero for a step response - let initial_state = na::DVector::::zeros(model.mat_a().nrows()); +) -> Result { + if !duration.is_finite() || duration < 0.0 { + return Err(SimulationError::InvalidDuration(duration)); + } - // Generate step for given duration - let n_samples = (duration / model.sampling_dt()).floor() as usize; - let input = na::DMatrix::from_element(1, n_samples, 1.0f64); + let n_samples = (duration / model.sampling_dt()).floor().max(0.0) as usize; + let n_inputs = model.mat_b().ncols(); + let initial_state = na::DVector::::zeros(model.mat_a().nrows()); + let input = na::DMatrix::from_element(n_inputs, n_samples, 1.0f64); - let (response, states) = simulate_ss_response(model, &input, &initial_state); + let (response, states) = simulate(model, &input, &initial_state)?; - (response, input, states) + Ok((response, input, states)) } -/// Generates the step response of a continuous state-space model for a given duration by converting it to a discrete model using the forward Euler method. -/// -/// # Arguments -/// -/// * `model` - A reference to a `ContinuousStateSpaceModel` object. -/// * `sampling_dt` - The sampling time for the discrete model. -/// * `duration` - The duration for which the step response is to be generated. -/// -/// # Returns -/// -/// A tuple containing: -/// * `response` - A matrix containing the output sequence of the step response. -/// * `input` - A matrix containing the input sequence (step input). -/// * `states` - A matrix containing the state sequence during the step response. -/// +/// Generates a unit-step response for a continuous model after ZOH discretization. pub fn step_for_continuous_ss( model: &ContinuousStateSpaceModel, sampling_dt: f64, duration: f64, -) -> (na::DMatrix, na::DMatrix, na::DMatrix) { - let discrete_model = - DiscreteStateSpaceModel::from_continuous_ss_forward_euler(model, sampling_dt); +) -> Result { + let discrete_model = DiscreteStateSpaceModel::from_continuous_zoh(model, sampling_dt)?; - // Initial state is zero for a step response - let initial_state = na::DVector::::zeros(model.mat_a().nrows()); + step_for_discrete_ss(&discrete_model, duration) +} - // Generate step for given duration - let n_samples = (duration / discrete_model.sampling_dt()).floor() as usize; - let input = na::DMatrix::from_element(1, n_samples, 1.0f64); +/// Generates an impulse response for a discrete model. +pub fn impulse_for_discrete_ss( + model: &(impl StateSpaceModel + Discrete), + duration: f64, +) -> Result { + if !duration.is_finite() || duration < 0.0 { + return Err(SimulationError::InvalidDuration(duration)); + } + + let n_samples = (duration / model.sampling_dt()).floor().max(0.0) as usize; + let n_inputs = model.mat_b().ncols(); + let mut input = na::DMatrix::zeros(n_inputs, n_samples); + if n_samples > 0 { + input.column_mut(0).fill(1.0); + } - let (response, states) = simulate_ss_response(&discrete_model, &input, &initial_state); + let initial_state = na::DVector::::zeros(model.mat_a().nrows()); + let (response, states) = simulate(model, &input, &initial_state)?; - (response, input, states) + Ok((response, input, states)) } -fn simulate_ss_response( +/// Generates a ramp response for a discrete model. +pub fn ramp_for_discrete_ss( model: &(impl StateSpaceModel + Discrete), - mat_u: &na::DMatrix, - x0: &na::DVector, -) -> (na::DMatrix, na::DMatrix) { - let sim_time = mat_u.ncols(); - let n_state = model.mat_a().nrows(); - let n_output = model.mat_c().nrows(); - let mut mat_x = na::DMatrix::::zeros(n_state, sim_time + 1); - let mut mat_y = na::DMatrix::::zeros(n_output, sim_time); - for i in 0..sim_time { - if i == 0 { - mat_x.column_mut(i).copy_from(&x0); - mat_y.column_mut(i).copy_from(&(model.mat_c() * x0)); - mat_x - .column_mut(i + 1) - .copy_from(&(model.mat_a() * x0 + model.mat_b() * mat_u.column(i))); - } else { - mat_y - .column_mut(i) - .copy_from(&(model.mat_c() * mat_x.column(i))); - - let mat_x_slice = mat_x.column(i).into_owned(); - mat_x - .column_mut(i + 1) - .copy_from(&(model.mat_a() * mat_x_slice + model.mat_b() * mat_u.column(i))); + duration: f64, +) -> Result { + if !duration.is_finite() || duration < 0.0 { + return Err(SimulationError::InvalidDuration(duration)); + } + + let n_samples = (duration / model.sampling_dt()).floor().max(0.0) as usize; + let n_inputs = model.mat_b().ncols(); + let mut input = na::DMatrix::zeros(n_inputs, n_samples); + + for i in 0..n_samples { + let value = i as f64 * model.sampling_dt(); + for input_row in 0..n_inputs { + input[(input_row, i)] = value; } } - (mat_y, mat_x) + let initial_state = na::DVector::::zeros(model.mat_a().nrows()); + let (response, states) = simulate(model, &input, &initial_state)?; + + Ok((response, input, states)) +} + +/// Generates a simulation time vector with length `n_samples`. +pub fn time_vector( + sampling_dt: f64, + n_samples: usize, +) -> Result, SimulationError> { + if !sampling_dt.is_finite() || sampling_dt <= 0.0 { + return Err(SimulationError::Model(ModelError::InvalidSamplingDt( + sampling_dt, + ))); + } + + Ok(na::DVector::from_iterator( + n_samples, + (0..n_samples).map(|i| i as f64 * sampling_dt), + )) +} + +#[cfg(test)] +mod tests { + use super::*; + + use crate::model::DiscreteStateSpaceModel; + + fn first_order_siso_model_with_d() -> DiscreteStateSpaceModel { + DiscreteStateSpaceModel::try_from_matrices( + &na::dmatrix![0.5], + &na::dmatrix![1.0], + &na::dmatrix![1.0], + &na::dmatrix![2.0], + 0.1, + ) + .unwrap() + } + + fn integrator_siso_model() -> DiscreteStateSpaceModel { + DiscreteStateSpaceModel::try_from_matrices( + &na::dmatrix![1.0], + &na::dmatrix![1.0], + &na::dmatrix![1.0], + &na::dmatrix![0.0], + 1.0, + ) + .unwrap() + } + + // Verifies simulation output includes direct feedthrough term D*u. + #[test] + fn test_simulate_includes_feedthrough_term() { + let model = first_order_siso_model_with_d(); + let u = na::dmatrix![1.0, 1.0, 1.0]; + let x0 = na::dvector![0.0]; + + let (y, _x) = simulate(&model, &u, &x0).unwrap(); + + assert_eq!(y.ncols(), 3); + assert!((y[0] - 2.0).abs() < 1e-12); + assert!((y[1] - 3.0).abs() < 1e-12); + assert!((y[2] - 3.5).abs() < 1e-12); + } + + // Verifies step generation matches the number of model inputs for MIMO systems. + #[test] + fn test_step_supports_mimo_inputs() { + let model = DiscreteStateSpaceModel::try_from_matrices( + &na::dmatrix![1.0, 0.0; 0.0, 1.0], + &na::dmatrix![1.0, 0.0; 0.0, 1.0], + &na::dmatrix![1.0, 0.0; 0.0, 1.0], + &na::dmatrix![0.0, 0.0; 0.0, 0.0], + 0.1, + ) + .unwrap(); + + let (y, u, x) = step_for_discrete_ss(&model, 1.0).unwrap(); + + assert_eq!(u.nrows(), 2); + assert_eq!(u.ncols(), 10); + assert_eq!(y.shape(), (2, 10)); + assert_eq!(x.shape(), (2, 11)); + + // U should be a 2x10 unit step for each input channel. + for i in 0..u.nrows() { + for k in 0..u.ncols() { + assert!((u[(i, k)] - 1.0).abs() < 1e-12); + } + } + + // With A = I, B = I, x0 = 0 and u_k = 1: + // x_k = [k, k]^T and y_k = x_k (C = I, D = 0). + for k in 0..10 { + let expected_state = k as f64; + assert!((x[(0, k)] - expected_state).abs() < 1e-12); + assert!((x[(1, k)] - expected_state).abs() < 1e-12); + assert!((y[(0, k)] - expected_state).abs() < 1e-12); + assert!((y[(1, k)] - expected_state).abs() < 1e-12); + } + + // Last state sample is after 10 updates. + assert!((x[(0, 10)] - 10.0).abs() < 1e-12); + assert!((x[(1, 10)] - 10.0).abs() < 1e-12); + } + + // Verifies zero-duration simulation returns empty I/O and initial-state-only trajectory. + #[test] + fn test_zero_duration_response() { + let model = first_order_siso_model_with_d(); + let (y, u, x) = step_for_discrete_ss(&model, 0.0).unwrap(); + + assert_eq!(y.shape(), (1, 0)); + assert_eq!(u.shape(), (1, 0)); + assert_eq!(x.shape(), (1, 1)); + } + + // Verifies durations shorter than one sampling period produce zero output samples. + #[test] + fn test_short_duration_response() { + let model = first_order_siso_model_with_d(); + let (y, _u, _x) = step_for_discrete_ss(&model, 0.05).unwrap(); + assert_eq!(y.ncols(), 0); + } + + // Verifies nominal step simulation values for a simple integrator model. x accumulates the input and y is equal to the beginning of x. + #[test] + fn test_step_response_nominal_values() { + let model = integrator_siso_model(); + let (y, u, x) = step_for_discrete_ss(&model, 3.0).unwrap(); + + assert_eq!(u, na::dmatrix![1.0, 1.0, 1.0]); + assert_eq!(y, na::dmatrix![0.0, 1.0, 2.0]); + assert_eq!(x, na::dmatrix![0.0, 1.0, 2.0, 3.0]); + } + + // Verifies nominal impulse simulation values for a simple integrator model. + #[test] + fn test_impulse_response_nominal_values() { + let model = integrator_siso_model(); + let (y, u, x) = impulse_for_discrete_ss(&model, 3.0).unwrap(); + + assert_eq!(u, na::dmatrix![1.0, 0.0, 0.0]); + assert_eq!(y, na::dmatrix![0.0, 1.0, 1.0]); + assert_eq!(x, na::dmatrix![0.0, 1.0, 1.0, 1.0]); + } + + // Verifies nominal ramp simulation values for a simple integrator model. + #[test] + fn test_ramp_response_nominal_values() { + let model = integrator_siso_model(); + let (y, u, x) = ramp_for_discrete_ss(&model, 3.0).unwrap(); + + assert_eq!(u, na::dmatrix![0.0, 1.0, 2.0]); + assert_eq!(y, na::dmatrix![0.0, 0.0, 1.0]); + assert_eq!(x, na::dmatrix![0.0, 0.0, 1.0, 3.0]); + } + + // Verifies simulation fails when input row count does not match B.ncols(). + #[test] + fn test_simulate_input_dimension_mismatch() { + let model = first_order_siso_model_with_d(); + let bad_u = na::dmatrix![1.0, 1.0; 1.0, 1.0]; + let x0 = na::dvector![0.0]; + + let result = simulate(&model, &bad_u, &x0); + assert!(matches!( + result, + Err(SimulationError::DimensionMismatch { + expected_u_rows: 1, + actual_u_rows: 2, + expected_x0_rows: 1, + actual_x0_rows: 1 + }) + )); + } + + // Verifies simulation fails when x0 length does not match A.nrows(). + #[test] + fn test_simulate_state_dimension_mismatch() { + let model = first_order_siso_model_with_d(); + let u = na::dmatrix![1.0, 1.0]; + let bad_x0 = na::dvector![0.0, 0.0]; + + let result = simulate(&model, &u, &bad_x0); + assert!(matches!( + result, + Err(SimulationError::DimensionMismatch { + expected_u_rows: 1, + actual_u_rows: 1, + expected_x0_rows: 1, + actual_x0_rows: 2 + }) + )); + } + + // Verifies negative duration is rejected for step response. + #[test] + fn test_step_negative_duration_error() { + let model = first_order_siso_model_with_d(); + let result = step_for_discrete_ss(&model, -1.0); + assert!(matches!( + result, + Err(SimulationError::InvalidDuration(d)) if (d + 1.0).abs() < 1e-12 + )); + } + + // Verifies negative duration is rejected for impulse response. + #[test] + fn test_impulse_negative_duration_error() { + let model = first_order_siso_model_with_d(); + let result = impulse_for_discrete_ss(&model, -0.5); + assert!(matches!( + result, + Err(SimulationError::InvalidDuration(d)) if (d + 0.5).abs() < 1e-12 + )); + } + + // Verifies negative duration is rejected for ramp response. + #[test] + fn test_ramp_negative_duration_error() { + let model = first_order_siso_model_with_d(); + let result = ramp_for_discrete_ss(&model, -0.25); + assert!(matches!( + result, + Err(SimulationError::InvalidDuration(d)) if (d + 0.25).abs() < 1e-12 + )); + } + + // Verifies invalid sampling periods are rejected in time vector helper. + #[test] + fn test_time_vector_invalid_sampling_dt() { + let zero = time_vector(0.0, 10); + let negative = time_vector(-0.1, 10); + let nan = time_vector(f64::NAN, 10); + let inf = time_vector(f64::INFINITY, 10); + + assert!(matches!( + zero, + Err(SimulationError::Model(ModelError::InvalidSamplingDt(dt))) if dt == 0.0 + )); + assert!(matches!( + negative, + Err(SimulationError::Model(ModelError::InvalidSamplingDt(dt))) if (dt + 0.1).abs() < 1e-12 + )); + assert!(matches!( + nan, + Err(SimulationError::Model(ModelError::InvalidSamplingDt(dt))) if dt.is_nan() + )); + assert!(matches!( + inf, + Err(SimulationError::Model(ModelError::InvalidSamplingDt(dt))) if dt.is_infinite() + )); + } }