From d33e46383d861a03bebef8c6d39b6d20787e0881 Mon Sep 17 00:00:00 2001 From: Romain Desarzens Date: Mon, 3 Feb 2025 12:51:24 +0000 Subject: [PATCH 01/18] Add a module to create controllers --- src/controller.rs | 0 src/lib.rs | 3 +++ 2 files changed, 3 insertions(+) create mode 100644 src/controller.rs diff --git a/src/controller.rs b/src/controller.rs new file mode 100644 index 0000000..e69de29 diff --git a/src/lib.rs b/src/lib.rs index 1bd20f9..a2c2075 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -172,3 +172,6 @@ pub mod simulator; /// Methods to generate trajectories pub mod trajectory; + +/// Methods and structs to create controllers +pub mod controller; From b3e95fe298ca2e17e4afbaa9759b0c9d05d1a3b3 Mon Sep 17 00:00:00 2001 From: Romain Desarzens Date: Fri, 20 Feb 2026 12:54:39 +0000 Subject: [PATCH 02/18] Fix implementation errors and refactor the code --- .github/workflows/rust.yml | 2 + Cargo.toml | 14 +- README.md | 186 +++-------- examples/controllability.rs | 12 +- examples/end_to_end.rs | 20 ++ examples/lqr.rs | 24 ++ examples/step_response.rs | 166 +++------- src/analysis.rs | 336 ++++++++++---------- src/controller.rs | 316 +++++++++++++++++++ src/lib.rs | 183 ++--------- src/model.rs | 614 +++++++++++++++++++++++++++--------- src/simulator.rs | 348 +++++++++++++++----- 12 files changed, 1421 insertions(+), 800 deletions(-) create mode 100644 examples/end_to_end.rs create mode 100644 examples/lqr.rs diff --git a/.github/workflows/rust.yml b/.github/workflows/rust.yml index d5b02f5..d0bbd2d 100644 --- a/.github/workflows/rust.yml +++ b/.github/workflows/rust.yml @@ -21,3 +21,5 @@ jobs: 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..5573a0b 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 = "lqr" +path = "examples/lqr.rs" + +[[example]] +name = "end_to_end" +path = "examples/end_to_end.rs" diff --git a/README.md b/README.md index 3efea11..8d9727e 100644 --- a/README.md +++ b/README.md @@ -2,159 +2,61 @@ ![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. - -## 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: +**control-sys** is a Rust library to represent, analyze, simulate, and control LTI systems using state-space models. + +## Highlights + +- Checked model constructors with typed errors (`ModelError`) +- Continuous-to-discrete conversion with explicit methods: + - ZOH + - Forward Euler + - Backward Euler + - Tustin +- 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`) + +## 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`: +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]; -```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 cont = model::ContinuousStateSpaceModel::try_from_matrices(&a, &b, &c, &d).unwrap(); +let disc = model::DiscreteStateSpaceModel::from_continuous_zoh(&cont, 0.05).unwrap(); -### Step response +let (y, u, _x) = simulator::step_for_discrete_ss(&disc, 5.0).unwrap(); +assert_eq!(y.nrows(), 1); +assert_eq!(u.nrows(), 1); -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, - ); +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, - ); -``` +`DiscreteStateSpaceModel` has explicit conversion methods: -### Controllability +- `from_continuous_zoh`: exact ZOH (recommended default) +- `from_continuous_forward_euler`: explicit Euler +- `from_continuous_backward_euler`: implicit Euler +- `from_continuous_tustin`: bilinear transform -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); -} -``` +Use ZOH unless you explicitly need the numerical behavior of a different method. +## Running checks +```bash +cargo test +cargo clippy --all-targets --all-features -- -D warnings +``` diff --git a/examples/controllability.rs b/examples/controllability.rs index d649b75..a2c4b1b 100644 --- a/examples/controllability.rs +++ b/examples/controllability.rs @@ -1,18 +1,16 @@ 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..c2c3971 --- /dev/null +++ b/examples/end_to_end.rs @@ -0,0 +1,20 @@ +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/lqr.rs b/examples/lqr.rs new file mode 100644 index 0000000..694ebc4 --- /dev/null +++ b/examples/lqr.rs @@ -0,0 +1,24 @@ +use control_sys::controller; +use control_sys::model; +use control_sys::model::StateSpaceModel; +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 model = model::ContinuousStateSpaceModel::try_from_matrices(&a, &b, &c, &d)?; + + let q = na::dmatrix![10.0, 0.0; 0.0, 1.0]; + let r = na::dmatrix![1.0]; + + let k = controller::continuous_lqr(model.mat_a(), model.mat_b(), &q, &r, 0.01, 5000, 1e-10)?; + let acl = controller::closed_loop(model.mat_a(), model.mat_b(), &k)?; + + println!("LQR gain K = {}", k); + println!("Closed-loop A matrix = {}", acl); + + Ok(()) +} diff --git a/examples/step_response.rs b/examples/step_response.rs index 960a941..732b195 100644 --- a/examples/step_response.rs +++ b/examples/step_response.rs @@ -1,67 +1,12 @@ -use control_sys::model; 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 +31,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 +42,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 +55,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..788cbe7 100644 --- a/src/analysis.rs +++ b/src/analysis.rs @@ -1,55 +1,46 @@ extern crate nalgebra as na; -use crate::model::StateSpaceModel; - -/// 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)} -/// } -/// ``` -/// +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 `[B, AB, ..., A^(n-1)B]`. 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 +48,40 @@ 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)]`. 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(), + }); } - let n = mat_a.nrows(); // Number of states + 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(); 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 +91,88 @@ 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) +} + +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; - // 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); + 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)] @@ -204,39 +185,40 @@ mod tests { 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]); } #[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 }) ); } #[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, ); @@ -247,6 +229,7 @@ mod tests { #[test] fn test_controllability_3x3_not_controllable() { + #[allow(deprecated)] let ss_model = DiscreteStateSpaceModel::from_matrices( &na::dmatrix![ 0.0, 1.0, 0.0; @@ -256,8 +239,8 @@ 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, ); @@ -280,25 +263,26 @@ mod tests { #[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 }) ); } #[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, ); @@ -309,14 +293,17 @@ mod tests { #[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 +318,19 @@ mod tests { 1.0, 0.0, 0.0] ); } + + #[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); + } } diff --git a/src/controller.rs b/src/controller.rs index e69de29..60f747f 100644 --- a/src/controller.rs +++ b/src/controller.rs @@ -0,0 +1,316 @@ +extern crate nalgebra as na; + +use crate::analysis::compute_controllability_matrix; +use crate::model::{ModelError, StateSpaceModel}; + +use std::error::Error; +use std::fmt; + +/// Errors related to controller synthesis utilities. +#[derive(Debug, Clone, PartialEq)] +pub enum ControlError { + /// Model validation error. + Model(ModelError), + /// Matrix dimensions are incompatible with the requested operation. + DimensionMismatch(&'static str), + /// A matrix inversion failed. + SingularMatrix(&'static str), + /// System is not controllable for the requested synthesis. + NotControllable, + /// Requested poles are invalid for the model order. + InvalidDesiredPoles, + /// Riccati solver failed to converge. + RiccatiNoConvergence, +} + +impl fmt::Display for ControlError { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + match self { + ControlError::Model(err) => write!(f, "{err}"), + ControlError::DimensionMismatch(msg) => write!(f, "dimension mismatch: {msg}"), + ControlError::SingularMatrix(msg) => write!(f, "singular matrix: {msg}"), + ControlError::NotControllable => write!(f, "system is not controllable"), + ControlError::InvalidDesiredPoles => write!(f, "invalid desired poles"), + ControlError::RiccatiNoConvergence => write!(f, "DARE solver did not converge"), + } + } +} + +impl Error for ControlError {} + +impl From for ControlError { + fn from(value: ModelError) -> Self { + ControlError::Model(value) + } +} + +/// Builds a closed-loop state matrix `A_cl = A - B*K` for `u = -Kx`. +pub fn closed_loop( + mat_a: &na::DMatrix, + mat_b: &na::DMatrix, + mat_k: &na::DMatrix, +) -> Result, ControlError> { + if !mat_a.is_square() { + return Err(ControlError::Model(ModelError::MatrixANotSquare { + rows: mat_a.nrows(), + cols: mat_a.ncols(), + })); + } + + if mat_b.nrows() != mat_a.nrows() { + return Err(ControlError::DimensionMismatch("B rows must match A rows")); + } + + if mat_k.nrows() != mat_b.ncols() || mat_k.ncols() != mat_a.ncols() { + return Err(ControlError::DimensionMismatch( + "K must have shape (n_inputs, n_states)", + )); + } + + Ok(mat_a - mat_b * mat_k) +} + +/// SISO pole placement via Ackermann's formula. +/// +/// `desired_poles` are real roots for a system of order `n`. +pub fn pole_placement_siso( + mat_a: &na::DMatrix, + mat_b: &na::DMatrix, + desired_poles: &[f64], +) -> Result, ControlError> { + if !mat_a.is_square() { + return Err(ControlError::Model(ModelError::MatrixANotSquare { + rows: mat_a.nrows(), + cols: mat_a.ncols(), + })); + } + + let n = mat_a.nrows(); + if mat_b.shape() != (n, 1) || desired_poles.len() != n { + return Err(ControlError::InvalidDesiredPoles); + } + + let ctrb = compute_controllability_matrix(mat_a, mat_b)?; + let ctrb_inv = ctrb.try_inverse().ok_or(ControlError::NotControllable)?; + + let coeffs = characteristic_polynomial_coeffs_from_roots(desired_poles); + + let mut phi_a = mat_a.pow(n as u32); + for (power, coeff) in coeffs.iter().take(n).enumerate() { + if power == 0 { + phi_a += na::DMatrix::::identity(n, n).scale(*coeff); + } else { + phi_a += mat_a.pow(power as u32).scale(*coeff); + } + } + + let mut e_n_t = na::DMatrix::::zeros(1, n); + e_n_t[(0, n - 1)] = 1.0; + + Ok(e_n_t * ctrb_inv * phi_a) +} + +/// Discrete-time LQR using an iterative DARE solve. +pub fn discrete_lqr( + mat_a: &na::DMatrix, + mat_b: &na::DMatrix, + mat_q: &na::DMatrix, + mat_r: &na::DMatrix, + max_iter: usize, + tol: f64, +) -> Result, ControlError> { + if !mat_a.is_square() { + return Err(ControlError::Model(ModelError::MatrixANotSquare { + rows: mat_a.nrows(), + cols: mat_a.ncols(), + })); + } + + let n = mat_a.nrows(); + let m = mat_b.ncols(); + + if mat_b.nrows() != n + || mat_q.shape() != (n, n) + || mat_r.shape() != (m, m) + || !mat_q.is_square() + || !mat_r.is_square() + { + return Err(ControlError::DimensionMismatch( + "A(n,n), B(n,m), Q(n,n), R(m,m) required", + )); + } + + let mut p = mat_q.clone(); + + for _ in 0..max_iter { + let bt_p = mat_b.transpose() * &p; + let s = mat_r + &bt_p * mat_b; + let s_inv = s + .try_inverse() + .ok_or(ControlError::SingularMatrix("R + B^T P B"))?; + + let p_next = mat_a.transpose() * &p * mat_a + - mat_a.transpose() * &p * mat_b * s_inv * bt_p * mat_a + + mat_q; + + let max_delta = p_next + .iter() + .zip(p.iter()) + .map(|(a, b)| (a - b).abs()) + .fold(0.0, f64::max); + + p = p_next; + if max_delta < tol { + let bt_p = mat_b.transpose() * &p; + let s = mat_r + &bt_p * mat_b; + let s_inv = s + .try_inverse() + .ok_or(ControlError::SingularMatrix("R + B^T P B"))?; + return Ok(s_inv * bt_p * mat_a); + } + } + + Err(ControlError::RiccatiNoConvergence) +} + +/// Continuous-time LQR approximation by Tustin discretization + discrete LQR. +/// +/// This method is pragmatic and does not solve CARE exactly. +pub fn continuous_lqr( + mat_a: &na::DMatrix, + mat_b: &na::DMatrix, + mat_q: &na::DMatrix, + mat_r: &na::DMatrix, + sampling_dt: f64, + max_iter: usize, + tol: f64, +) -> Result, ControlError> { + if !sampling_dt.is_finite() || sampling_dt <= 0.0 { + return Err(ControlError::Model(ModelError::InvalidSamplingDt( + sampling_dt, + ))); + } + + let n = mat_a.nrows(); + if !mat_a.is_square() || mat_b.nrows() != n { + return Err(ControlError::DimensionMismatch("invalid A/B dimensions")); + } + + let i = na::DMatrix::::identity(n, n); + let left = i.clone() - mat_a.scale(0.5 * sampling_dt); + let left_inv = left + .try_inverse() + .ok_or(ControlError::SingularMatrix("I - A*dt/2"))?; + + let ad = &left_inv * (i + mat_a.scale(0.5 * sampling_dt)); + let bd = left_inv * mat_b.scale(sampling_dt); + + let qd = mat_q.scale(sampling_dt); + let rd = mat_r.scale(sampling_dt); + + discrete_lqr(&ad, &bd, &qd, &rd, max_iter, tol) +} + +/// Computes SISO reference prefilter `Nbar` for `u = -Kx + Nbar*r`. +pub fn siso_reference_prefilter( + mat_a: &na::DMatrix, + mat_b: &na::DMatrix, + mat_c: &na::DMatrix, + mat_k: &na::DMatrix, +) -> Result { + if mat_b.ncols() != 1 || mat_c.nrows() != 1 || mat_k.nrows() != 1 { + return Err(ControlError::DimensionMismatch( + "SISO system required for prefilter", + )); + } + + let acl = closed_loop(mat_a, mat_b, mat_k)?; + let inv_acl = acl + .try_inverse() + .ok_or(ControlError::SingularMatrix("A-BK"))?; + + let gain = -(mat_c * inv_acl * mat_b)[(0, 0)]; + if gain.abs() < 1e-12 { + return Err(ControlError::SingularMatrix("dc-gain is near zero")); + } + + Ok(1.0 / gain) +} + +/// Closed-loop helper for `StateSpaceModel` implementors. +pub fn closed_loop_from_model( + model: &T, + mat_k: &na::DMatrix, +) -> Result, ControlError> { + closed_loop(model.mat_a(), model.mat_b(), mat_k) +} + +fn characteristic_polynomial_coeffs_from_roots(roots: &[f64]) -> Vec { + let mut poly = vec![1.0f64]; + + for root in roots { + let mut next = vec![0.0; poly.len() + 1]; + for (i, coeff) in poly.iter().enumerate() { + next[i] += -root * coeff; + next[i + 1] += coeff; + } + poly = next; + } + + poly +} + +#[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)] + ); + } + } + } + + #[test] + fn test_pole_placement_siso() { + let a = na::dmatrix![0.0, 1.0; -2.0, -3.0]; + let b = na::dmatrix![0.0; 1.0]; + + let k = pole_placement_siso(&a, &b, &[-2.0, -4.0]).unwrap(); + let acl = closed_loop(&a, &b, &k).unwrap(); + + let desired_char_poly = na::dmatrix![0.0, 1.0; -8.0, -6.0]; + approx_eq_matrix(&acl, &desired_char_poly, 1e-10); + } + + #[test] + fn test_discrete_lqr_stabilizes_unstable_system() { + let a = na::dmatrix![1.1]; + let b = na::dmatrix![1.0]; + let q = na::dmatrix![1.0]; + let r = na::dmatrix![1.0]; + + let k = discrete_lqr(&a, &b, &q, &r, 5000, 1e-12).unwrap(); + let acl = closed_loop(&a, &b, &k).unwrap(); + + assert!(acl[(0, 0)].abs() < 1.0); + } + + #[test] + fn test_continuous_lqr_returns_gain() { + let a = na::dmatrix![0.0, 1.0; 0.0, 0.0]; + let b = na::dmatrix![0.0; 1.0]; + let q = na::dmatrix![10.0, 0.0; 0.0, 1.0]; + let r = na::dmatrix![1.0]; + + let k = continuous_lqr(&a, &b, &q, &r, 0.01, 5000, 1e-10).unwrap(); + assert_eq!(k.shape(), (1, 2)); + } +} diff --git a/src/lib.rs b/src/lib.rs index a2c2075..9642d97 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -2,176 +2,55 @@ /*! # 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; - -// 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); -``` +use control_sys::{analysis, model, simulator}; +use control_sys::model::StateSpaceModel; -### Step response +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]; -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, - ); -``` +let continuous = model::ContinuousStateSpaceModel::try_from_matrices(&a, &b, &c, &d).unwrap(); +let discrete = model::DiscreteStateSpaceModel::from_continuous_zoh(&continuous, 0.05).unwrap(); -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: +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()); -```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, - ); +let (is_controllable, _ctrb) = analysis::is_ss_controllable(&discrete); +assert!(is_controllable); ``` -### 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. +- `from_continuous_forward_euler`: explicit Euler. +- `from_continuous_backward_euler`: implicit Euler. +- `from_continuous_tustin`: bilinear transform. - */ +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 +/// Methods and structs to create controllers. +pub mod controller; + +/// 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; - -/// Methods and structs to create controllers -pub mod controller; diff --git a/src/model.rs b/src/model.rs index cec916b..f0f0a0e 100644 --- a/src/model.rs +++ b/src/model.rs @@ -1,5 +1,56 @@ 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 +142,39 @@ pub struct ContinuousStateSpaceModel { /// Represents a continuous state-space model. impl ContinuousStateSpaceModel { - /// Creates a new `ContinuousStateSpaceModel` with the given matrices. - /// - /// # Arguments - /// - /// * `mat_a` - State matrix (A). - /// * `mat_b` - Input matrix (B). - /// * `mat_c` - Output matrix (C). - /// * `mat_d` - Feedthrough matrix (D). - /// - /// # Returns - /// - /// A new instance of `ContinuousStateSpaceModel`. - pub fn from_matrices( + /// Creates a new `ContinuousStateSpaceModel` with full dimension checks. + 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 +182,45 @@ 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 in the state-space model. 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 +240,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 +250,195 @@ 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 - /// - /// * `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( + /// Creates a new `DiscreteStateSpaceModel` with matrix and sampling-time checks. + 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. - /// - /// # Arguments - /// - /// * `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. - /// - /// # Returns - /// - /// A new `DiscreteStateSpaceModel` instance. - pub fn from_continuous_matrix_forward_euler( + /// 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). + 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 { + ) -> 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) + } + + /// Discretizes a continuous model using explicit forward Euler. + pub fn from_continuous_matrix_forward_euler_checked( + mat_ac: &na::DMatrix, + mat_bc: &na::DMatrix, + mat_cc: &na::DMatrix, + mat_dc: &na::DMatrix, + sampling_dt: f64, + ) -> Result { + validate_state_space_dimensions(mat_ac, mat_bc, mat_cc, mat_dc)?; + validate_sampling_dt(sampling_dt)?; + 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, - } + let mat_a = mat_i + mat_ac.scale(sampling_dt); + let mat_b = mat_bc.scale(sampling_dt); + + 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. - /// - /// # Returns - /// - /// A new `DiscreteStateSpaceModel` instance. - pub fn from_continuous_ss_forward_euler( + /// Discretizes a continuous model using implicit backward Euler. + pub fn from_continuous_matrix_backward_euler( + mat_ac: &na::DMatrix, + mat_bc: &na::DMatrix, + mat_cc: &na::DMatrix, + mat_dc: &na::DMatrix, + sampling_dt: f64, + ) -> Result { + validate_state_space_dimensions(mat_ac, mat_bc, mat_cc, mat_dc)?; + validate_sampling_dt(sampling_dt)?; + + let mat_i = na::DMatrix::::identity(mat_ac.nrows(), mat_ac.nrows()); + let inv = (mat_i - mat_ac.scale(sampling_dt)) + .try_inverse() + .ok_or(ModelError::SingularMatrix("I - A*dt for backward Euler"))?; + + let mat_a = inv.clone(); + let mat_b = inv * mat_bc.scale(sampling_dt); + + Self::try_from_matrices(&mat_a, &mat_b, mat_cc, mat_dc, sampling_dt) + } + + /// Discretizes a continuous model using the bilinear/Tustin transform. + pub fn from_continuous_matrix_tustin( + mat_ac: &na::DMatrix, + mat_bc: &na::DMatrix, + mat_cc: &na::DMatrix, + mat_dc: &na::DMatrix, + sampling_dt: f64, + ) -> Result { + validate_state_space_dimensions(mat_ac, mat_bc, mat_cc, mat_dc)?; + validate_sampling_dt(sampling_dt)?; + + let mat_i = na::DMatrix::::identity(mat_ac.nrows(), mat_ac.nrows()); + let left = mat_i.clone() - mat_ac.scale(0.5 * sampling_dt); + let inv_left = left + .try_inverse() + .ok_or(ModelError::SingularMatrix("I - A*dt/2 for Tustin"))?; + + let mat_a = &inv_left * (mat_i + mat_ac.scale(0.5 * sampling_dt)); + let mat_b = inv_left * mat_bc.scale(sampling_dt); + + Self::try_from_matrices(&mat_a, &mat_b, mat_cc, mat_dc, sampling_dt) + } + + /// Discretizes a continuous model using exact ZOH. + 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(), + model.mat_d(), + sampling_dt, + ) + } + + /// Discretizes a continuous model using explicit forward Euler. + pub fn from_continuous_forward_euler( + model: &ContinuousStateSpaceModel, + sampling_dt: f64, + ) -> Result { + Self::from_continuous_matrix_forward_euler_checked( + model.mat_a(), + model.mat_b(), + model.mat_c(), + model.mat_d(), + sampling_dt, + ) + } + + /// Discretizes a continuous model using implicit backward Euler. + pub fn from_continuous_backward_euler( + model: &ContinuousStateSpaceModel, + sampling_dt: f64, + ) -> Result { + Self::from_continuous_matrix_backward_euler( + model.mat_a(), + model.mat_b(), + model.mat_c(), + model.mat_d(), + sampling_dt, + ) + } + + /// Discretizes a continuous model using Tustin. + pub fn from_continuous_tustin( + model: &ContinuousStateSpaceModel, + sampling_dt: f64, + ) -> Result { + Self::from_continuous_matrix_tustin( model.mat_a(), model.mat_b(), model.mat_c(), @@ -320,6 +446,35 @@ impl DiscreteStateSpaceModel { sampling_dt, ) } + + /// Converts a continuous state-space model to a discrete model using forward Euler. + #[deprecated(note = "Use from_continuous_forward_euler")] + pub fn from_continuous_ss_forward_euler( + model: &ContinuousStateSpaceModel, + sampling_dt: f64, + ) -> DiscreteStateSpaceModel { + Self::from_continuous_forward_euler(model, sampling_dt) + .expect("forward Euler discretization failed") + } + + /// Legacy matrix-based constructor for forward Euler. + #[deprecated(note = "Use from_continuous_matrix_forward_euler_checked")] + pub fn from_continuous_matrix_forward_euler( + mat_ac: &na::DMatrix, + mat_bc: &na::DMatrix, + mat_cc: &na::DMatrix, + mat_dc: &na::DMatrix, + sampling_dt: f64, + ) -> DiscreteStateSpaceModel { + Self::from_continuous_matrix_forward_euler_checked( + mat_ac, + mat_bc, + mat_cc, + mat_dc, + sampling_dt, + ) + .expect("forward Euler discretization failed") + } } impl Pole for DiscreteStateSpaceModel { @@ -330,16 +485,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 +506,138 @@ impl TransferFunction { TransferFunction { numerator_coeffs: numerator_coeffs.to_vec(), denominator_coeffs: denominator_coeffs.to_vec(), - constant: constant, + constant, + } + } +} + +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(()) +} + +fn matrix_exponential(mat: &na::DMatrix) -> na::DMatrix { + let n = mat.nrows(); + if n == 0 { + return na::DMatrix::::zeros(0, 0); + } + + 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; + + 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; } } + + 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)] -/// This module contains unit tests for the control system models. -/// -/// # Tests -/// -/// - `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. 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)] + ); + } + } + } + + #[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 { .. })); + } + #[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 +645,94 @@ 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); } + #[test] + fn test_discretization_forward_euler_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_forward_euler_checked( + &a, &b, &c, &d, dt, + ) + .unwrap(); + + approx_eq_matrix(model.mat_a(), &na::dmatrix![0.8], 1e-12); + approx_eq_matrix(model.mat_b(), &na::dmatrix![0.1], 1e-12); + } + + #[test] + fn test_discretization_backward_euler_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_backward_euler(&a, &b, &c, &d, dt) + .unwrap(); + + approx_eq_matrix(model.mat_a(), &na::dmatrix![1.0 / 1.2], 1e-12); + approx_eq_matrix(model.mat_b(), &na::dmatrix![dt / 1.2], 1e-12); + } + + #[test] + fn test_discretization_tustin_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_tustin(&a, &b, &c, &d, dt).unwrap(); + + approx_eq_matrix(model.mat_a(), &na::dmatrix![0.8181818181818182], 1e-12); + approx_eq_matrix(model.mat_b(), &na::dmatrix![0.09090909090909091], 1e-12); + } + + #[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); + } + #[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, ); @@ -421,11 +747,12 @@ mod tests { #[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, ); @@ -440,11 +767,12 @@ mod tests { #[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..11accda 100644 --- a/src/simulator.rs +++ b/src/simulator.rs @@ -1,100 +1,306 @@ 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. +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. /// -/// # Returns +/// The model equations are: +/// `x[k+1] = A x[k] + B u[k] + w[k]` +/// `y[k] = C x[k] + D u[k] + v[k]` /// -/// 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. +/// For `simulate`, noise terms are zero. +pub fn simulate( + model: &(impl StateSpaceModel + Discrete), + mat_u: &na::DMatrix, + x0: &na::DVector, +) -> Result<(na::DMatrix, na::DMatrix), SimulationError> { + simulate_with_noise( + model, + mat_u, + x0, + &na::DMatrix::zeros(model.mat_a().nrows(), mat_u.ncols()), + &na::DMatrix::zeros(model.mat_c().nrows(), mat_u.ncols()), + ) +} + +/// Simulates a discrete model with additive process and measurement noise. /// +/// Noise matrices are simulation-only hooks and are not estimator models: +/// `process_noise` has shape `(n_state, n_samples)` and `measurement_noise` has +/// shape `(n_output, n_samples)`. +pub fn simulate_with_noise( + model: &(impl StateSpaceModel + Discrete), + mat_u: &na::DMatrix, + x0: &na::DVector, + process_noise: &na::DMatrix, + measurement_noise: &na::DMatrix, +) -> 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(), + }); + } + + if process_noise.shape() != (n_state, sim_time) + || measurement_noise.shape() != (n_output, sim_time) + { + 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 + measurement_noise.column(i); + mat_y.column_mut(i).copy_from(&yk); + + let x_next = model.mat_a() * xk + model.mat_b() * uk + process_noise.column(i); + mat_x.column_mut(i + 1).copy_from(&x_next); + } + + Ok((mat_y, mat_x)) +} + +/// 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<(na::DMatrix, na::DMatrix, na::DMatrix), SimulationError> { + 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<(na::DMatrix, na::DMatrix, na::DMatrix), SimulationError> { + 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) +} + +/// Generates an impulse response for a discrete model. +pub fn impulse_for_discrete_ss( + model: &(impl StateSpaceModel + Discrete), + duration: f64, +) -> Result<(na::DMatrix, na::DMatrix, na::DMatrix), SimulationError> { + if !duration.is_finite() || duration < 0.0 { + return Err(SimulationError::InvalidDuration(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); + 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<(na::DMatrix, na::DMatrix, na::DMatrix), SimulationError> { + 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() + } + + #[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!((y[(0, 0)] - 2.0).abs() < 1e-12); + assert!((y[(0, 1)] - 3.0).abs() < 1e-12); + } + + #[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); + } + + #[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)); + } + + #[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); + } + + #[test] + fn test_simulate_with_noise() { + let model = first_order_siso_model_with_d(); + let u = na::dmatrix![0.0, 0.0]; + let x0 = na::dvector![0.0]; + let w = na::dmatrix![1.0, 1.0]; + let v = na::dmatrix![0.1, 0.1]; + + let (y, x) = simulate_with_noise(&model, &u, &x0, &w, &v).unwrap(); + + assert_eq!(y.shape(), (1, 2)); + assert_eq!(x.shape(), (1, 3)); + assert!((y[(0, 0)] - 0.1).abs() < 1e-12); + } } From 4f163fd872e109546d05a33e3aa7e7850355507a Mon Sep 17 00:00:00 2001 From: Romain Desarzens Date: Fri, 20 Feb 2026 13:35:25 +0000 Subject: [PATCH 03/18] Add comments to describe examples and remove simulate_with_noise method --- examples/controllability.rs | 2 ++ examples/end_to_end.rs | 5 ++++ examples/lqr.rs | 2 ++ examples/step_response.rs | 2 ++ src/simulator.rs | 56 +++---------------------------------- 5 files changed, 15 insertions(+), 52 deletions(-) diff --git a/examples/controllability.rs b/examples/controllability.rs index a2c4b1b..53b2a66 100644 --- a/examples/controllability.rs +++ b/examples/controllability.rs @@ -1,3 +1,5 @@ +// Controllability example: +// Build a discrete state-space model and compute its controllability matrix/rank. use control_sys::analysis; use control_sys::model; diff --git a/examples/end_to_end.rs b/examples/end_to_end.rs index c2c3971..d74a939 100644 --- a/examples/end_to_end.rs +++ b/examples/end_to_end.rs @@ -1,3 +1,8 @@ +// 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; diff --git a/examples/lqr.rs b/examples/lqr.rs index 694ebc4..40a8ebe 100644 --- a/examples/lqr.rs +++ b/examples/lqr.rs @@ -1,3 +1,5 @@ +// LQR design example: +// Build a continuous model, compute an LQR gain K, and print the closed-loop A-BK matrix. use control_sys::controller; use control_sys::model; use control_sys::model::StateSpaceModel; diff --git a/examples/step_response.rs b/examples/step_response.rs index 732b195..afe7f9c 100644 --- a/examples/step_response.rs +++ b/examples/step_response.rs @@ -1,3 +1,5 @@ +// 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; diff --git a/src/simulator.rs b/src/simulator.rs index 11accda..e898477 100644 --- a/src/simulator.rs +++ b/src/simulator.rs @@ -58,35 +58,12 @@ impl From for SimulationError { /// Simulates a discrete state-space model for an arbitrary input sequence. /// /// The model equations are: -/// `x[k+1] = A x[k] + B u[k] + w[k]` -/// `y[k] = C x[k] + D u[k] + v[k]` -/// -/// For `simulate`, noise terms are zero. +/// `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> { - simulate_with_noise( - model, - mat_u, - x0, - &na::DMatrix::zeros(model.mat_a().nrows(), mat_u.ncols()), - &na::DMatrix::zeros(model.mat_c().nrows(), mat_u.ncols()), - ) -} - -/// Simulates a discrete model with additive process and measurement noise. -/// -/// Noise matrices are simulation-only hooks and are not estimator models: -/// `process_noise` has shape `(n_state, n_samples)` and `measurement_noise` has -/// shape `(n_output, n_samples)`. -pub fn simulate_with_noise( - model: &(impl StateSpaceModel + Discrete), - mat_u: &na::DMatrix, - x0: &na::DVector, - process_noise: &na::DMatrix, - measurement_noise: &na::DMatrix, ) -> Result<(na::DMatrix, na::DMatrix), SimulationError> { let n_state = model.mat_a().nrows(); let n_input = model.mat_b().ncols(); @@ -102,17 +79,6 @@ pub fn simulate_with_noise( }); } - if process_noise.shape() != (n_state, sim_time) - || measurement_noise.shape() != (n_output, sim_time) - { - 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); @@ -121,10 +87,10 @@ pub fn simulate_with_noise( 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 + measurement_noise.column(i); + 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 + process_noise.column(i); + let x_next = model.mat_a() * xk + model.mat_b() * uk; mat_x.column_mut(i + 1).copy_from(&x_next); } @@ -289,18 +255,4 @@ mod tests { assert_eq!(y.ncols(), 0); } - #[test] - fn test_simulate_with_noise() { - let model = first_order_siso_model_with_d(); - let u = na::dmatrix![0.0, 0.0]; - let x0 = na::dvector![0.0]; - let w = na::dmatrix![1.0, 1.0]; - let v = na::dmatrix![0.1, 0.1]; - - let (y, x) = simulate_with_noise(&model, &u, &x0, &w, &v).unwrap(); - - assert_eq!(y.shape(), (1, 2)); - assert_eq!(x.shape(), (1, 3)); - assert!((y[(0, 0)] - 0.1).abs() < 1e-12); - } } From 9489852ca85b115b726a75121d6f2ab0fc432384 Mon Sep 17 00:00:00 2001 From: Romain Desarzens Date: Fri, 20 Feb 2026 14:03:47 +0000 Subject: [PATCH 04/18] Add documentation to the controller module --- src/controller.rs | 70 +++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 70 insertions(+) diff --git a/src/controller.rs b/src/controller.rs index 60f747f..a7a8e3e 100644 --- a/src/controller.rs +++ b/src/controller.rs @@ -1,3 +1,29 @@ +//! Controller synthesis utilities. +//! +//! This module currently provides: +//! +//! - Closed-loop assembly for state feedback (`A_cl = A - B K`) +//! - SISO pole placement (Ackermann formula) +//! - Discrete-time LQR (iterative DARE solve) +//! - Continuous-time LQR approximation (Tustin discretization + discrete LQR) +//! - SISO reference prefilter gain (`Nbar`) +//! +//! # References +//! +//! - Ackermann / pole placement: +//! - +//! - +//! - DARE / discrete LQR: +//! - +//! - +//! - CARE / continuous LQR context: +//! - +//! - Bilinear (Tustin) transform: +//! - +//! - +//! - Reference prefilter scaling (`Nbar` concept): +//! - +//! extern crate nalgebra as na; use crate::analysis::compute_controllability_matrix; @@ -45,6 +71,11 @@ impl From for ControlError { } /// Builds a closed-loop state matrix `A_cl = A - B*K` for `u = -Kx`. +/// +/// The closed-loop dynamics are: +/// +/// - Continuous time: `x_dot = (A - B K) x` +/// - Discrete time: `x[k+1] = (A - B K) x[k]` pub fn closed_loop( mat_a: &na::DMatrix, mat_b: &na::DMatrix, @@ -73,6 +104,16 @@ pub fn closed_loop( /// SISO pole placement via Ackermann's formula. /// /// `desired_poles` are real roots for a system of order `n`. +/// +/// Uses the classic formula: +/// +/// `K = e_n^T * C^{-1} * phi(A)` +/// +/// where: +/// +/// - `C = [B, A B, ..., A^(n-1) B]` is the controllability matrix +/// - `phi(s)` is the desired characteristic polynomial +/// - `e_n^T = [0, ..., 0, 1]` pub fn pole_placement_siso( mat_a: &na::DMatrix, mat_b: &na::DMatrix, @@ -111,6 +152,18 @@ pub fn pole_placement_siso( } /// Discrete-time LQR using an iterative DARE solve. +/// +/// Minimizes: +/// +/// `J = sum_{k=0..inf} (x_k^T Q x_k + u_k^T R u_k)` with `u_k = -K x_k` +/// +/// by iterating: +/// +/// `P_{k+1} = A^T P_k A - A^T P_k B (R + B^T P_k B)^-1 B^T P_k A + Q` +/// +/// and returning: +/// +/// `K = (R + B^T P B)^-1 B^T P A` pub fn discrete_lqr( mat_a: &na::DMatrix, mat_b: &na::DMatrix, @@ -176,6 +229,13 @@ pub fn discrete_lqr( /// Continuous-time LQR approximation by Tustin discretization + discrete LQR. /// /// This method is pragmatic and does not solve CARE exactly. +/// +/// Continuous dynamics are first mapped to discrete form: +/// +/// `A_d = (I - A*dt/2)^-1 (I + A*dt/2)` +/// `B_d = (I - A*dt/2)^-1 (dt * B)` +/// +/// Then discrete LQR is solved on `(A_d, B_d, Q*dt, R*dt)`. pub fn continuous_lqr( mat_a: &na::DMatrix, mat_b: &na::DMatrix, @@ -212,6 +272,14 @@ pub fn continuous_lqr( } /// Computes SISO reference prefilter `Nbar` for `u = -Kx + Nbar*r`. +/// +/// For SISO systems (and the assumptions of this helper), it computes: +/// +/// `gain_dc = -C (A - B K)^-1 B` +/// `Nbar = 1 / gain_dc` +/// +/// so a step reference can be tracked with zero steady-state offset when the +/// closed-loop DC gain is non-zero. pub fn siso_reference_prefilter( mat_a: &na::DMatrix, mat_b: &na::DMatrix, @@ -238,6 +306,8 @@ pub fn siso_reference_prefilter( } /// Closed-loop helper for `StateSpaceModel` implementors. +/// +/// Equivalent to calling [`closed_loop`] with `model.mat_a()` and `model.mat_b()`. pub fn closed_loop_from_model( model: &T, mat_k: &na::DMatrix, From ff87d861ae073f983b2e98c4dc0ee621247a3d0a Mon Sep 17 00:00:00 2001 From: Romain Desarzens Date: Fri, 20 Feb 2026 14:31:47 +0000 Subject: [PATCH 05/18] Add a method to provide an analysis report of an LTI system --- Cargo.toml | 4 ++ examples/analysis_report.rs | 34 +++++++++++++++++ src/analysis.rs | 76 +++++++++++++++++++++++++++++++++++++ 3 files changed, 114 insertions(+) create mode 100644 examples/analysis_report.rs diff --git a/Cargo.toml b/Cargo.toml index 5573a0b..5139826 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -35,3 +35,7 @@ path = "examples/lqr.rs" [[example]] name = "end_to_end" path = "examples/end_to_end.rs" + +[[example]] +name = "analysis_report" +path = "examples/analysis_report.rs" 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/src/analysis.rs b/src/analysis.rs index 788cbe7..0dd6bf5 100644 --- a/src/analysis.rs +++ b/src/analysis.rs @@ -15,6 +15,30 @@ pub struct RankDiagnostics { pub is_full_rank: bool, } +/// 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]`. pub fn compute_controllability_matrix( mat_a: &na::DMatrix, @@ -156,6 +180,39 @@ pub fn is_continuous_stable(model: &T) -> bool { model.poles().iter().all(|pole| pole.re < 0.0) } +/// 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; @@ -333,4 +390,23 @@ mod tests { assert!(is_discrete_stable(&discrete_stable)); assert!(spectral_radius(&discrete_stable) < 1.0); } + + #[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); + } } From bace3f342837d929920f77609ebc33e4cd83b0e1 Mon Sep 17 00:00:00 2001 From: Romain Desarzens Date: Fri, 20 Feb 2026 14:32:33 +0000 Subject: [PATCH 06/18] Remove controllers implementation as they will be integrated in another PR --- src/controller.rs | 386 +--------------------------------------------- 1 file changed, 3 insertions(+), 383 deletions(-) diff --git a/src/controller.rs b/src/controller.rs index a7a8e3e..c3664d4 100644 --- a/src/controller.rs +++ b/src/controller.rs @@ -1,386 +1,6 @@ -//! Controller synthesis utilities. +//! Controller module placeholder. //! -//! This module currently provides: -//! -//! - Closed-loop assembly for state feedback (`A_cl = A - B K`) -//! - SISO pole placement (Ackermann formula) -//! - Discrete-time LQR (iterative DARE solve) -//! - Continuous-time LQR approximation (Tustin discretization + discrete LQR) -//! - SISO reference prefilter gain (`Nbar`) -//! -//! # References -//! -//! - Ackermann / pole placement: -//! - -//! - -//! - DARE / discrete LQR: -//! - -//! - -//! - CARE / continuous LQR context: -//! - -//! - Bilinear (Tustin) transform: -//! - -//! - -//! - Reference prefilter scaling (`Nbar` concept): -//! - -//! -extern crate nalgebra as na; - -use crate::analysis::compute_controllability_matrix; -use crate::model::{ModelError, StateSpaceModel}; - -use std::error::Error; -use std::fmt; - -/// Errors related to controller synthesis utilities. -#[derive(Debug, Clone, PartialEq)] -pub enum ControlError { - /// Model validation error. - Model(ModelError), - /// Matrix dimensions are incompatible with the requested operation. - DimensionMismatch(&'static str), - /// A matrix inversion failed. - SingularMatrix(&'static str), - /// System is not controllable for the requested synthesis. - NotControllable, - /// Requested poles are invalid for the model order. - InvalidDesiredPoles, - /// Riccati solver failed to converge. - RiccatiNoConvergence, -} - -impl fmt::Display for ControlError { - fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { - match self { - ControlError::Model(err) => write!(f, "{err}"), - ControlError::DimensionMismatch(msg) => write!(f, "dimension mismatch: {msg}"), - ControlError::SingularMatrix(msg) => write!(f, "singular matrix: {msg}"), - ControlError::NotControllable => write!(f, "system is not controllable"), - ControlError::InvalidDesiredPoles => write!(f, "invalid desired poles"), - ControlError::RiccatiNoConvergence => write!(f, "DARE solver did not converge"), - } - } -} - -impl Error for ControlError {} - -impl From for ControlError { - fn from(value: ModelError) -> Self { - ControlError::Model(value) - } -} - -/// Builds a closed-loop state matrix `A_cl = A - B*K` for `u = -Kx`. -/// -/// The closed-loop dynamics are: -/// -/// - Continuous time: `x_dot = (A - B K) x` -/// - Discrete time: `x[k+1] = (A - B K) x[k]` -pub fn closed_loop( - mat_a: &na::DMatrix, - mat_b: &na::DMatrix, - mat_k: &na::DMatrix, -) -> Result, ControlError> { - if !mat_a.is_square() { - return Err(ControlError::Model(ModelError::MatrixANotSquare { - rows: mat_a.nrows(), - cols: mat_a.ncols(), - })); - } - - if mat_b.nrows() != mat_a.nrows() { - return Err(ControlError::DimensionMismatch("B rows must match A rows")); - } - - if mat_k.nrows() != mat_b.ncols() || mat_k.ncols() != mat_a.ncols() { - return Err(ControlError::DimensionMismatch( - "K must have shape (n_inputs, n_states)", - )); - } - - Ok(mat_a - mat_b * mat_k) -} - -/// SISO pole placement via Ackermann's formula. -/// -/// `desired_poles` are real roots for a system of order `n`. -/// -/// Uses the classic formula: -/// -/// `K = e_n^T * C^{-1} * phi(A)` -/// -/// where: -/// -/// - `C = [B, A B, ..., A^(n-1) B]` is the controllability matrix -/// - `phi(s)` is the desired characteristic polynomial -/// - `e_n^T = [0, ..., 0, 1]` -pub fn pole_placement_siso( - mat_a: &na::DMatrix, - mat_b: &na::DMatrix, - desired_poles: &[f64], -) -> Result, ControlError> { - if !mat_a.is_square() { - return Err(ControlError::Model(ModelError::MatrixANotSquare { - rows: mat_a.nrows(), - cols: mat_a.ncols(), - })); - } - - let n = mat_a.nrows(); - if mat_b.shape() != (n, 1) || desired_poles.len() != n { - return Err(ControlError::InvalidDesiredPoles); - } - - let ctrb = compute_controllability_matrix(mat_a, mat_b)?; - let ctrb_inv = ctrb.try_inverse().ok_or(ControlError::NotControllable)?; - - let coeffs = characteristic_polynomial_coeffs_from_roots(desired_poles); - - let mut phi_a = mat_a.pow(n as u32); - for (power, coeff) in coeffs.iter().take(n).enumerate() { - if power == 0 { - phi_a += na::DMatrix::::identity(n, n).scale(*coeff); - } else { - phi_a += mat_a.pow(power as u32).scale(*coeff); - } - } - - let mut e_n_t = na::DMatrix::::zeros(1, n); - e_n_t[(0, n - 1)] = 1.0; - - Ok(e_n_t * ctrb_inv * phi_a) -} - -/// Discrete-time LQR using an iterative DARE solve. -/// -/// Minimizes: -/// -/// `J = sum_{k=0..inf} (x_k^T Q x_k + u_k^T R u_k)` with `u_k = -K x_k` -/// -/// by iterating: -/// -/// `P_{k+1} = A^T P_k A - A^T P_k B (R + B^T P_k B)^-1 B^T P_k A + Q` -/// -/// and returning: -/// -/// `K = (R + B^T P B)^-1 B^T P A` -pub fn discrete_lqr( - mat_a: &na::DMatrix, - mat_b: &na::DMatrix, - mat_q: &na::DMatrix, - mat_r: &na::DMatrix, - max_iter: usize, - tol: f64, -) -> Result, ControlError> { - if !mat_a.is_square() { - return Err(ControlError::Model(ModelError::MatrixANotSquare { - rows: mat_a.nrows(), - cols: mat_a.ncols(), - })); - } - - let n = mat_a.nrows(); - let m = mat_b.ncols(); - - if mat_b.nrows() != n - || mat_q.shape() != (n, n) - || mat_r.shape() != (m, m) - || !mat_q.is_square() - || !mat_r.is_square() - { - return Err(ControlError::DimensionMismatch( - "A(n,n), B(n,m), Q(n,n), R(m,m) required", - )); - } - - let mut p = mat_q.clone(); - - for _ in 0..max_iter { - let bt_p = mat_b.transpose() * &p; - let s = mat_r + &bt_p * mat_b; - let s_inv = s - .try_inverse() - .ok_or(ControlError::SingularMatrix("R + B^T P B"))?; +//! Controller synthesis/design APIs will be added in a future iteration. - let p_next = mat_a.transpose() * &p * mat_a - - mat_a.transpose() * &p * mat_b * s_inv * bt_p * mat_a - + mat_q; - - let max_delta = p_next - .iter() - .zip(p.iter()) - .map(|(a, b)| (a - b).abs()) - .fold(0.0, f64::max); - - p = p_next; - if max_delta < tol { - let bt_p = mat_b.transpose() * &p; - let s = mat_r + &bt_p * mat_b; - let s_inv = s - .try_inverse() - .ok_or(ControlError::SingularMatrix("R + B^T P B"))?; - return Ok(s_inv * bt_p * mat_a); - } - } - - Err(ControlError::RiccatiNoConvergence) -} - -/// Continuous-time LQR approximation by Tustin discretization + discrete LQR. -/// -/// This method is pragmatic and does not solve CARE exactly. -/// -/// Continuous dynamics are first mapped to discrete form: -/// -/// `A_d = (I - A*dt/2)^-1 (I + A*dt/2)` -/// `B_d = (I - A*dt/2)^-1 (dt * B)` -/// -/// Then discrete LQR is solved on `(A_d, B_d, Q*dt, R*dt)`. -pub fn continuous_lqr( - mat_a: &na::DMatrix, - mat_b: &na::DMatrix, - mat_q: &na::DMatrix, - mat_r: &na::DMatrix, - sampling_dt: f64, - max_iter: usize, - tol: f64, -) -> Result, ControlError> { - if !sampling_dt.is_finite() || sampling_dt <= 0.0 { - return Err(ControlError::Model(ModelError::InvalidSamplingDt( - sampling_dt, - ))); - } - - let n = mat_a.nrows(); - if !mat_a.is_square() || mat_b.nrows() != n { - return Err(ControlError::DimensionMismatch("invalid A/B dimensions")); - } - - let i = na::DMatrix::::identity(n, n); - let left = i.clone() - mat_a.scale(0.5 * sampling_dt); - let left_inv = left - .try_inverse() - .ok_or(ControlError::SingularMatrix("I - A*dt/2"))?; - - let ad = &left_inv * (i + mat_a.scale(0.5 * sampling_dt)); - let bd = left_inv * mat_b.scale(sampling_dt); - - let qd = mat_q.scale(sampling_dt); - let rd = mat_r.scale(sampling_dt); - - discrete_lqr(&ad, &bd, &qd, &rd, max_iter, tol) -} - -/// Computes SISO reference prefilter `Nbar` for `u = -Kx + Nbar*r`. -/// -/// For SISO systems (and the assumptions of this helper), it computes: -/// -/// `gain_dc = -C (A - B K)^-1 B` -/// `Nbar = 1 / gain_dc` -/// -/// so a step reference can be tracked with zero steady-state offset when the -/// closed-loop DC gain is non-zero. -pub fn siso_reference_prefilter( - mat_a: &na::DMatrix, - mat_b: &na::DMatrix, - mat_c: &na::DMatrix, - mat_k: &na::DMatrix, -) -> Result { - if mat_b.ncols() != 1 || mat_c.nrows() != 1 || mat_k.nrows() != 1 { - return Err(ControlError::DimensionMismatch( - "SISO system required for prefilter", - )); - } - - let acl = closed_loop(mat_a, mat_b, mat_k)?; - let inv_acl = acl - .try_inverse() - .ok_or(ControlError::SingularMatrix("A-BK"))?; - - let gain = -(mat_c * inv_acl * mat_b)[(0, 0)]; - if gain.abs() < 1e-12 { - return Err(ControlError::SingularMatrix("dc-gain is near zero")); - } - - Ok(1.0 / gain) -} - -/// Closed-loop helper for `StateSpaceModel` implementors. -/// -/// Equivalent to calling [`closed_loop`] with `model.mat_a()` and `model.mat_b()`. -pub fn closed_loop_from_model( - model: &T, - mat_k: &na::DMatrix, -) -> Result, ControlError> { - closed_loop(model.mat_a(), model.mat_b(), mat_k) -} - -fn characteristic_polynomial_coeffs_from_roots(roots: &[f64]) -> Vec { - let mut poly = vec![1.0f64]; - - for root in roots { - let mut next = vec![0.0; poly.len() + 1]; - for (i, coeff) in poly.iter().enumerate() { - next[i] += -root * coeff; - next[i + 1] += coeff; - } - poly = next; - } - - poly -} - -#[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)] - ); - } - } - } - - #[test] - fn test_pole_placement_siso() { - let a = na::dmatrix![0.0, 1.0; -2.0, -3.0]; - let b = na::dmatrix![0.0; 1.0]; - - let k = pole_placement_siso(&a, &b, &[-2.0, -4.0]).unwrap(); - let acl = closed_loop(&a, &b, &k).unwrap(); - - let desired_char_poly = na::dmatrix![0.0, 1.0; -8.0, -6.0]; - approx_eq_matrix(&acl, &desired_char_poly, 1e-10); - } - - #[test] - fn test_discrete_lqr_stabilizes_unstable_system() { - let a = na::dmatrix![1.1]; - let b = na::dmatrix![1.0]; - let q = na::dmatrix![1.0]; - let r = na::dmatrix![1.0]; - - let k = discrete_lqr(&a, &b, &q, &r, 5000, 1e-12).unwrap(); - let acl = closed_loop(&a, &b, &k).unwrap(); - - assert!(acl[(0, 0)].abs() < 1.0); - } - - #[test] - fn test_continuous_lqr_returns_gain() { - let a = na::dmatrix![0.0, 1.0; 0.0, 0.0]; - let b = na::dmatrix![0.0; 1.0]; - let q = na::dmatrix![10.0, 0.0; 0.0, 1.0]; - let r = na::dmatrix![1.0]; +extern crate nalgebra as na; - let k = continuous_lqr(&a, &b, &q, &r, 0.01, 5000, 1e-10).unwrap(); - assert_eq!(k.shape(), (1, 2)); - } -} From 77bd26b062b014b5b72e62d3ffaafc6d8f53579c Mon Sep 17 00:00:00 2001 From: Romain Desarzens Date: Fri, 20 Feb 2026 14:32:53 +0000 Subject: [PATCH 07/18] Remove LQR example --- examples/lqr.rs | 26 -------------------------- 1 file changed, 26 deletions(-) delete mode 100644 examples/lqr.rs diff --git a/examples/lqr.rs b/examples/lqr.rs deleted file mode 100644 index 40a8ebe..0000000 --- a/examples/lqr.rs +++ /dev/null @@ -1,26 +0,0 @@ -// LQR design example: -// Build a continuous model, compute an LQR gain K, and print the closed-loop A-BK matrix. -use control_sys::controller; -use control_sys::model; -use control_sys::model::StateSpaceModel; -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 model = model::ContinuousStateSpaceModel::try_from_matrices(&a, &b, &c, &d)?; - - let q = na::dmatrix![10.0, 0.0; 0.0, 1.0]; - let r = na::dmatrix![1.0]; - - let k = controller::continuous_lqr(model.mat_a(), model.mat_b(), &q, &r, 0.01, 5000, 1e-10)?; - let acl = controller::closed_loop(model.mat_a(), model.mat_b(), &k)?; - - println!("LQR gain K = {}", k); - println!("Closed-loop A matrix = {}", acl); - - Ok(()) -} From 4282fdb2ada58e93ff9a2576c9ca468490bae5d4 Mon Sep 17 00:00:00 2001 From: Romain Desarzens Date: Fri, 20 Feb 2026 14:37:51 +0000 Subject: [PATCH 08/18] Remove LQR example from Cargo --- Cargo.toml | 4 ---- 1 file changed, 4 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 5139826..c2433c9 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -28,10 +28,6 @@ path = "examples/step_response.rs" name = "controllability" path = "examples/controllability.rs" -[[example]] -name = "lqr" -path = "examples/lqr.rs" - [[example]] name = "end_to_end" path = "examples/end_to_end.rs" From 0c7332342abee8d5f45a64635eb6370c929f6c95 Mon Sep 17 00:00:00 2001 From: Romain Desarzens Date: Fri, 20 Feb 2026 15:09:01 +0000 Subject: [PATCH 09/18] Add comments to the unit tests --- src/analysis.rs | 10 +++ src/controller.rs | 1 - src/model.rs | 9 +++ src/simulator.rs | 175 +++++++++++++++++++++++++++++++++++++++++++++- 4 files changed, 191 insertions(+), 4 deletions(-) diff --git a/src/analysis.rs b/src/analysis.rs index 0dd6bf5..a6a9041 100644 --- a/src/analysis.rs +++ b/src/analysis.rs @@ -238,6 +238,7 @@ mod tests { use crate::model::DiscreteStateSpaceModel; + // Verifies controllability matrix construction for a nominal 2x2 system. #[test] fn test_compute_controllability_matrix_2x2() { let mat_a = nalgebra::dmatrix![1.0, -2.0; @@ -252,6 +253,7 @@ mod tests { 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]; @@ -266,6 +268,7 @@ mod tests { ); } + // Verifies a known controllable 2x2 model is detected as controllable. #[test] fn test_controllability_2x2_controllable() { #[allow(deprecated)] @@ -284,6 +287,7 @@ mod tests { assert!(result); } + // Verifies a known 3x3 model is detected as not controllable. #[test] fn test_controllability_3x3_not_controllable() { #[allow(deprecated)] @@ -304,6 +308,7 @@ mod tests { assert!(!is_ss_controllable(&ss_model).0); } + // Verifies observability matrix construction for a nominal 2x2 system. #[test] fn test_compute_observability_matrix_2x2() { let mat_a = nalgebra::dmatrix![1.0, -2.0; @@ -317,6 +322,7 @@ 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]; @@ -330,6 +336,7 @@ mod tests { ); } + // Verifies a known observable 2x2 model is detected as observable. #[test] fn test_is_observable_2x2_observable_system() { #[allow(deprecated)] @@ -348,6 +355,7 @@ mod tests { assert!(result); } + // Verifies a known 3x3 model is detected as not observable. #[test] fn test_observability_3x3_not_observable() { #[allow(deprecated)] @@ -376,6 +384,7 @@ mod tests { ); } + // Verifies discrete stability helpers on a stable single-state model. #[test] fn test_stability_checks() { #[allow(deprecated)] @@ -391,6 +400,7 @@ mod tests { 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)] diff --git a/src/controller.rs b/src/controller.rs index c3664d4..e659532 100644 --- a/src/controller.rs +++ b/src/controller.rs @@ -3,4 +3,3 @@ //! Controller synthesis/design APIs will be added in a future iteration. extern crate nalgebra as na; - diff --git a/src/model.rs b/src/model.rs index f0f0a0e..a032f82 100644 --- a/src/model.rs +++ b/src/model.rs @@ -618,6 +618,7 @@ mod tests { } } + // Verifies checked constructors reject incompatible A/B/C/D dimensions. #[test] fn test_try_from_matrices_dimension_validation() { let err = DiscreteStateSpaceModel::try_from_matrices( @@ -632,6 +633,7 @@ mod tests { 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); @@ -659,6 +661,7 @@ mod tests { assert_eq!(ss_model.mat_d()[(0, 0)], 8.0f64); } + // Verifies forward Euler discretization against first-order analytic values. #[test] fn test_discretization_forward_euler_first_order() { let a = na::dmatrix![-2.0]; @@ -676,6 +679,7 @@ mod tests { approx_eq_matrix(model.mat_b(), &na::dmatrix![0.1], 1e-12); } + // Verifies backward Euler discretization against first-order analytic values. #[test] fn test_discretization_backward_euler_first_order() { let a = na::dmatrix![-2.0]; @@ -692,6 +696,7 @@ mod tests { approx_eq_matrix(model.mat_b(), &na::dmatrix![dt / 1.2], 1e-12); } + // Verifies Tustin discretization against first-order analytic values. #[test] fn test_discretization_tustin_first_order() { let a = na::dmatrix![-2.0]; @@ -707,6 +712,7 @@ mod tests { approx_eq_matrix(model.mat_b(), &na::dmatrix![0.09090909090909091], 1e-12); } + // Verifies ZOH discretization against first-order exact discretization values. #[test] fn test_discretization_zoh_first_order() { let a = na::dmatrix![-2.0]; @@ -725,6 +731,7 @@ mod tests { 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)] @@ -745,6 +752,7 @@ 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)] @@ -765,6 +773,7 @@ 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)] diff --git a/src/simulator.rs b/src/simulator.rs index e898477..1b54370 100644 --- a/src/simulator.rs +++ b/src/simulator.rs @@ -209,6 +209,18 @@ mod tests { .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(); @@ -217,10 +229,13 @@ mod tests { let (y, _x) = simulate(&model, &u, &x0).unwrap(); - assert!((y[(0, 0)] - 2.0).abs() < 1e-12); - assert!((y[(0, 1)] - 3.0).abs() < 1e-12); + 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( @@ -232,12 +247,36 @@ mod tests { ) .unwrap(); - let (_y, u, _x) = step_for_discrete_ss(&model, 1.0).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(); @@ -248,6 +287,7 @@ mod tests { 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(); @@ -255,4 +295,133 @@ mod tests { 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() + )); + } } From 8a17d565e8d06fdb4b92e8a84a5abb3e3ec0b24f Mon Sep 17 00:00:00 2001 From: Romain Desarzens Date: Fri, 20 Feb 2026 15:25:57 +0000 Subject: [PATCH 10/18] Remove controllers --- src/controller.rs | 5 ----- 1 file changed, 5 deletions(-) delete mode 100644 src/controller.rs diff --git a/src/controller.rs b/src/controller.rs deleted file mode 100644 index e659532..0000000 --- a/src/controller.rs +++ /dev/null @@ -1,5 +0,0 @@ -//! Controller module placeholder. -//! -//! Controller synthesis/design APIs will be added in a future iteration. - -extern crate nalgebra as na; From b46c35ce8111772101db2791e2c49854e6e294cc Mon Sep 17 00:00:00 2001 From: Romain Desarzens Date: Fri, 20 Feb 2026 19:07:55 +0000 Subject: [PATCH 11/18] Fix CI by installing required dependencies --- .github/workflows/rust.yml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/.github/workflows/rust.yml b/.github/workflows/rust.yml index d0bbd2d..017402d 100644 --- a/.github/workflows/rust.yml +++ b/.github/workflows/rust.yml @@ -16,7 +16,9 @@ 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 From d4780c826db28e0db7934099ba003b0f413e00e6 Mon Sep 17 00:00:00 2001 From: Romain Desarzens Date: Fri, 20 Feb 2026 19:10:12 +0000 Subject: [PATCH 12/18] Remove controller module --- src/lib.rs | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 9642d97..9b5579a 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -43,9 +43,6 @@ Recommended default is ZOH for sampled-data systems driven by held inputs. /// Methods to analyze control systems. pub mod analysis; -/// Methods and structs to create controllers. -pub mod controller; - /// Structures to represent control systems. pub mod model; From b0af7af165d3510df35d372a8420521eb1c255bb Mon Sep 17 00:00:00 2001 From: Romain Desarzens Date: Fri, 20 Feb 2026 19:15:49 +0000 Subject: [PATCH 13/18] Simplify type declaration to avoid Clippy warning --- src/simulator.rs | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/simulator.rs b/src/simulator.rs index 1b54370..23f3a59 100644 --- a/src/simulator.rs +++ b/src/simulator.rs @@ -97,11 +97,13 @@ pub fn simulate( 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, -) -> Result<(na::DMatrix, na::DMatrix, na::DMatrix), SimulationError> { +) -> Result { if !duration.is_finite() || duration < 0.0 { return Err(SimulationError::InvalidDuration(duration)); } @@ -121,7 +123,7 @@ pub fn step_for_continuous_ss( model: &ContinuousStateSpaceModel, sampling_dt: f64, duration: f64, -) -> Result<(na::DMatrix, na::DMatrix, na::DMatrix), SimulationError> { +) -> Result { let discrete_model = DiscreteStateSpaceModel::from_continuous_zoh(model, sampling_dt)?; step_for_discrete_ss(&discrete_model, duration) @@ -131,7 +133,7 @@ pub fn step_for_continuous_ss( pub fn impulse_for_discrete_ss( model: &(impl StateSpaceModel + Discrete), duration: f64, -) -> Result<(na::DMatrix, na::DMatrix, na::DMatrix), SimulationError> { +) -> Result { if !duration.is_finite() || duration < 0.0 { return Err(SimulationError::InvalidDuration(duration)); } @@ -153,7 +155,7 @@ pub fn impulse_for_discrete_ss( pub fn ramp_for_discrete_ss( model: &(impl StateSpaceModel + Discrete), duration: f64, -) -> Result<(na::DMatrix, na::DMatrix, na::DMatrix), SimulationError> { +) -> Result { if !duration.is_finite() || duration < 0.0 { return Err(SimulationError::InvalidDuration(duration)); } From 2fd2853988ab8cb7911cba9cb3ac5519c34d68bb Mon Sep 17 00:00:00 2001 From: Romain Desarzens Date: Fri, 20 Feb 2026 19:24:47 +0000 Subject: [PATCH 14/18] Fix Clippy warnings --- examples/step_response.rs | 6 +++--- src/analysis.rs | 4 ++++ 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/examples/step_response.rs b/examples/step_response.rs index afe7f9c..89845cd 100644 --- a/examples/step_response.rs +++ b/examples/step_response.rs @@ -94,7 +94,7 @@ fn main() -> Result<(), Box> { 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))); + .legend(move |(x, y)| PathElement::new([(x, y), (x + 20, y)], Palette99::pick(0))); let series_y: Vec<(i32, f64)> = step_response .row(0) @@ -107,8 +107,8 @@ fn main() -> Result<(), Box> { chart .configure_series_labels() - .background_style(&WHITE) - .border_style(&BLACK) + .background_style(WHITE) + .border_style(BLACK) .draw()?; Ok(()) diff --git a/src/analysis.rs b/src/analysis.rs index a6a9041..bc0a5e4 100644 --- a/src/analysis.rs +++ b/src/analysis.rs @@ -40,6 +40,7 @@ pub struct LtiAnalysisReport { } /// 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, @@ -83,6 +84,7 @@ pub fn is_ss_controllable(model: &T) -> (bool, na::DMatrix, mat_c: &na::DMatrix, @@ -239,6 +241,7 @@ 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; @@ -309,6 +312,7 @@ mod tests { } // 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; From 3af0fdea564dc7d87fd51a440c7d5d34bc451ea4 Mon Sep 17 00:00:00 2001 From: Romain Desarzens Date: Sat, 21 Feb 2026 07:45:49 +0000 Subject: [PATCH 15/18] Break the API to avoid exposing unsafe methods --- src/model.rs | 23 ++--------------------- 1 file changed, 2 insertions(+), 21 deletions(-) diff --git a/src/model.rs b/src/model.rs index a032f82..561346d 100644 --- a/src/model.rs +++ b/src/model.rs @@ -329,7 +329,7 @@ impl DiscreteStateSpaceModel { } /// Discretizes a continuous model using explicit forward Euler. - pub fn from_continuous_matrix_forward_euler_checked( + pub fn from_continuous_matrix_forward_euler( mat_ac: &na::DMatrix, mat_bc: &na::DMatrix, mat_cc: &na::DMatrix, @@ -410,7 +410,7 @@ impl DiscreteStateSpaceModel { model: &ContinuousStateSpaceModel, sampling_dt: f64, ) -> Result { - Self::from_continuous_matrix_forward_euler_checked( + Self::from_continuous_matrix_forward_euler( model.mat_a(), model.mat_b(), model.mat_c(), @@ -456,25 +456,6 @@ impl DiscreteStateSpaceModel { Self::from_continuous_forward_euler(model, sampling_dt) .expect("forward Euler discretization failed") } - - /// Legacy matrix-based constructor for forward Euler. - #[deprecated(note = "Use from_continuous_matrix_forward_euler_checked")] - pub fn from_continuous_matrix_forward_euler( - mat_ac: &na::DMatrix, - mat_bc: &na::DMatrix, - mat_cc: &na::DMatrix, - mat_dc: &na::DMatrix, - sampling_dt: f64, - ) -> DiscreteStateSpaceModel { - Self::from_continuous_matrix_forward_euler_checked( - mat_ac, - mat_bc, - mat_cc, - mat_dc, - sampling_dt, - ) - .expect("forward Euler discretization failed") - } } impl Pole for DiscreteStateSpaceModel { From 57368e941b36e0331723684c2d6993b2220d95cb Mon Sep 17 00:00:00 2001 From: Romain Desarzens Date: Sat, 21 Feb 2026 10:49:18 +0000 Subject: [PATCH 16/18] Remove forward and backward euler discretization constructors --- README.md | 6 +- src/lib.rs | 2 - src/model.rs | 168 +++++++++++++++++---------------------------------- 3 files changed, 56 insertions(+), 120 deletions(-) diff --git a/README.md b/README.md index 8d9727e..75e690f 100644 --- a/README.md +++ b/README.md @@ -9,8 +9,6 @@ - Checked model constructors with typed errors (`ModelError`) - Continuous-to-discrete conversion with explicit methods: - ZOH - - Forward Euler - - Backward Euler - Tustin - Time-domain simulation with full output equation `y = Cx + Du` - Controllability and observability analysis @@ -48,11 +46,9 @@ assert!(is_ctrb); `DiscreteStateSpaceModel` has explicit conversion methods: - `from_continuous_zoh`: exact ZOH (recommended default) -- `from_continuous_forward_euler`: explicit Euler -- `from_continuous_backward_euler`: implicit Euler - `from_continuous_tustin`: bilinear transform -Use ZOH unless you explicitly need the numerical behavior of a different method. +Use ZOH by default for sampled-data systems with held inputs. ## Running checks diff --git a/src/lib.rs b/src/lib.rs index 9b5579a..c0e4338 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -33,8 +33,6 @@ assert!(is_controllable); `DiscreteStateSpaceModel` provides explicit conversion APIs: - `from_continuous_zoh`: exact ZOH using augmented matrix exponential. -- `from_continuous_forward_euler`: explicit Euler. -- `from_continuous_backward_euler`: implicit Euler. - `from_continuous_tustin`: bilinear transform. Recommended default is ZOH for sampled-data systems driven by held inputs. diff --git a/src/model.rs b/src/model.rs index 561346d..595e2f2 100644 --- a/src/model.rs +++ b/src/model.rs @@ -1,3 +1,15 @@ +//! 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) +//! - Tustin (bilinear transform) + extern crate nalgebra as na; use std::error::Error; @@ -143,6 +155,13 @@ pub struct ContinuousStateSpaceModel { /// Represents a continuous state-space model. impl ContinuousStateSpaceModel { /// Creates a new `ContinuousStateSpaceModel` with full dimension checks. + /// + /// The following compatibility constraints are enforced: + /// + /// - `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, @@ -200,7 +219,9 @@ impl ContinuousStateSpaceModel { 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. + /// + /// This equals the number of columns of `A` (and rows of `A` since `A` is square). pub fn state_space_size(&self) -> usize { self.mat_a.ncols() } @@ -268,6 +289,9 @@ impl StateSpaceModel for DiscreteStateSpaceModel { impl DiscreteStateSpaceModel { /// Creates a new `DiscreteStateSpaceModel` with matrix and sampling-time checks. + /// + /// 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, @@ -301,6 +325,20 @@ impl DiscreteStateSpaceModel { } /// Discretizes a continuous model using exact zero-order hold (ZOH). + /// + /// For continuous dynamics `x_dot = A x + B u` with piecewise-constant input, + /// this computes: + /// + /// - `A_d = exp(A * dt)` + /// - `B_d = integral_0^dt exp(A * tau) d tau * B` + /// + /// The implementation uses the standard augmented matrix exponential: + /// + /// ```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, @@ -328,47 +366,14 @@ impl DiscreteStateSpaceModel { Self::try_from_matrices(&mat_a, &mat_b, mat_cc, mat_dc, sampling_dt) } - /// Discretizes a continuous model using explicit forward Euler. - pub fn from_continuous_matrix_forward_euler( - mat_ac: &na::DMatrix, - mat_bc: &na::DMatrix, - mat_cc: &na::DMatrix, - mat_dc: &na::DMatrix, - sampling_dt: f64, - ) -> Result { - validate_state_space_dimensions(mat_ac, mat_bc, mat_cc, mat_dc)?; - validate_sampling_dt(sampling_dt)?; - - let mat_i = na::DMatrix::::identity(mat_ac.nrows(), mat_ac.nrows()); - let mat_a = mat_i + mat_ac.scale(sampling_dt); - let mat_b = mat_bc.scale(sampling_dt); - - Self::try_from_matrices(&mat_a, &mat_b, mat_cc, mat_dc, sampling_dt) - } - - /// Discretizes a continuous model using implicit backward Euler. - pub fn from_continuous_matrix_backward_euler( - mat_ac: &na::DMatrix, - mat_bc: &na::DMatrix, - mat_cc: &na::DMatrix, - mat_dc: &na::DMatrix, - sampling_dt: f64, - ) -> Result { - validate_state_space_dimensions(mat_ac, mat_bc, mat_cc, mat_dc)?; - validate_sampling_dt(sampling_dt)?; - - let mat_i = na::DMatrix::::identity(mat_ac.nrows(), mat_ac.nrows()); - let inv = (mat_i - mat_ac.scale(sampling_dt)) - .try_inverse() - .ok_or(ModelError::SingularMatrix("I - A*dt for backward Euler"))?; - - let mat_a = inv.clone(); - let mat_b = inv * mat_bc.scale(sampling_dt); - - Self::try_from_matrices(&mat_a, &mat_b, mat_cc, mat_dc, sampling_dt) - } - /// Discretizes a continuous model using the bilinear/Tustin transform. + /// + /// This corresponds to: + /// + /// - `A_d = (I - A*dt/2)^-1 * (I + A*dt/2)` + /// - `B_d = (I - A*dt/2)^-1 * B*dt` + /// + /// It requires inversion of `I - A*dt/2`. pub fn from_continuous_matrix_tustin( mat_ac: &na::DMatrix, mat_bc: &na::DMatrix, @@ -392,6 +397,8 @@ impl DiscreteStateSpaceModel { } /// Discretizes a continuous model using exact ZOH. + /// + /// See [`DiscreteStateSpaceModel::from_continuous_matrix_zoh`] for equations. pub fn from_continuous_zoh( model: &ContinuousStateSpaceModel, sampling_dt: f64, @@ -405,35 +412,9 @@ impl DiscreteStateSpaceModel { ) } - /// Discretizes a continuous model using explicit forward Euler. - pub fn from_continuous_forward_euler( - model: &ContinuousStateSpaceModel, - sampling_dt: f64, - ) -> Result { - Self::from_continuous_matrix_forward_euler( - model.mat_a(), - model.mat_b(), - model.mat_c(), - model.mat_d(), - sampling_dt, - ) - } - - /// Discretizes a continuous model using implicit backward Euler. - pub fn from_continuous_backward_euler( - model: &ContinuousStateSpaceModel, - sampling_dt: f64, - ) -> Result { - Self::from_continuous_matrix_backward_euler( - model.mat_a(), - model.mat_b(), - model.mat_c(), - model.mat_d(), - sampling_dt, - ) - } - /// Discretizes a continuous model using Tustin. + /// + /// See [`DiscreteStateSpaceModel::from_continuous_matrix_tustin`] for equations. pub fn from_continuous_tustin( model: &ContinuousStateSpaceModel, sampling_dt: f64, @@ -447,15 +428,6 @@ impl DiscreteStateSpaceModel { ) } - /// Converts a continuous state-space model to a discrete model using forward Euler. - #[deprecated(note = "Use from_continuous_forward_euler")] - pub fn from_continuous_ss_forward_euler( - model: &ContinuousStateSpaceModel, - sampling_dt: f64, - ) -> DiscreteStateSpaceModel { - Self::from_continuous_forward_euler(model, sampling_dt) - .expect("forward Euler discretization failed") - } } impl Pole for DiscreteStateSpaceModel { @@ -539,6 +511,10 @@ fn matrix_exponential(mat: &na::DMatrix) -> na::DMatrix { return na::DMatrix::::zeros(0, 0); } + // Scaling-and-series approximation: + // 1) choose a power-of-two scale so ||A/scale|| is small, + // 2) evaluate exp(A/scale) by truncated Taylor series, + // 3) square the result repeatedly to recover exp(A). let norm_one = max_column_sum_norm(mat); let scaling_power = if norm_one <= 0.5 { 0 @@ -553,6 +529,7 @@ fn matrix_exponential(mat: &na::DMatrix) -> na::DMatrix { let mut result = identity.clone(); let mut term = identity; + // exp(M) = I + M + M^2/2! + M^3/3! + ... for k in 1..=64 { term = (&term * &scaled) / (k as f64); result += &term; @@ -642,41 +619,6 @@ mod tests { assert_eq!(ss_model.mat_d()[(0, 0)], 8.0f64); } - // Verifies forward Euler discretization against first-order analytic values. - #[test] - fn test_discretization_forward_euler_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_forward_euler_checked( - &a, &b, &c, &d, dt, - ) - .unwrap(); - - approx_eq_matrix(model.mat_a(), &na::dmatrix![0.8], 1e-12); - approx_eq_matrix(model.mat_b(), &na::dmatrix![0.1], 1e-12); - } - - // Verifies backward Euler discretization against first-order analytic values. - #[test] - fn test_discretization_backward_euler_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_backward_euler(&a, &b, &c, &d, dt) - .unwrap(); - - approx_eq_matrix(model.mat_a(), &na::dmatrix![1.0 / 1.2], 1e-12); - approx_eq_matrix(model.mat_b(), &na::dmatrix![dt / 1.2], 1e-12); - } - // Verifies Tustin discretization against first-order analytic values. #[test] fn test_discretization_tustin_first_order() { From 1a6e706276e8fa61186f27d88d7adee2ff141cc8 Mon Sep 17 00:00:00 2001 From: Romain Desarzens Date: Sat, 21 Feb 2026 11:00:33 +0000 Subject: [PATCH 17/18] Remove Tustin discretization --- README.md | 2 -- src/lib.rs | 1 - src/model.rs | 65 ++-------------------------------------------------- 3 files changed, 2 insertions(+), 66 deletions(-) diff --git a/README.md b/README.md index 75e690f..4ec4a6d 100644 --- a/README.md +++ b/README.md @@ -9,7 +9,6 @@ - Checked model constructors with typed errors (`ModelError`) - Continuous-to-discrete conversion with explicit methods: - ZOH - - Tustin - Time-domain simulation with full output equation `y = Cx + Du` - Controllability and observability analysis - Stability checks and rank diagnostics @@ -46,7 +45,6 @@ assert!(is_ctrb); `DiscreteStateSpaceModel` has explicit conversion methods: - `from_continuous_zoh`: exact ZOH (recommended default) -- `from_continuous_tustin`: bilinear transform Use ZOH by default for sampled-data systems with held inputs. diff --git a/src/lib.rs b/src/lib.rs index c0e4338..2830ed0 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -33,7 +33,6 @@ assert!(is_controllable); `DiscreteStateSpaceModel` provides explicit conversion APIs: - `from_continuous_zoh`: exact ZOH using augmented matrix exponential. -- `from_continuous_tustin`: bilinear transform. Recommended default is ZOH for sampled-data systems driven by held inputs. */ diff --git a/src/model.rs b/src/model.rs index 595e2f2..d94b04a 100644 --- a/src/model.rs +++ b/src/model.rs @@ -8,7 +8,8 @@ //! It also provides multiple continuous-to-discrete conversion methods: //! //! - Exact ZOH (augmented matrix exponential) -//! - Tustin (bilinear transform) +//! The library intentionally exposes a small discretization surface for now: +//! exact ZOH. extern crate nalgebra as na; @@ -366,36 +367,6 @@ impl DiscreteStateSpaceModel { Self::try_from_matrices(&mat_a, &mat_b, mat_cc, mat_dc, sampling_dt) } - /// Discretizes a continuous model using the bilinear/Tustin transform. - /// - /// This corresponds to: - /// - /// - `A_d = (I - A*dt/2)^-1 * (I + A*dt/2)` - /// - `B_d = (I - A*dt/2)^-1 * B*dt` - /// - /// It requires inversion of `I - A*dt/2`. - pub fn from_continuous_matrix_tustin( - mat_ac: &na::DMatrix, - mat_bc: &na::DMatrix, - mat_cc: &na::DMatrix, - mat_dc: &na::DMatrix, - sampling_dt: f64, - ) -> Result { - validate_state_space_dimensions(mat_ac, mat_bc, mat_cc, mat_dc)?; - validate_sampling_dt(sampling_dt)?; - - let mat_i = na::DMatrix::::identity(mat_ac.nrows(), mat_ac.nrows()); - let left = mat_i.clone() - mat_ac.scale(0.5 * sampling_dt); - let inv_left = left - .try_inverse() - .ok_or(ModelError::SingularMatrix("I - A*dt/2 for Tustin"))?; - - let mat_a = &inv_left * (mat_i + mat_ac.scale(0.5 * sampling_dt)); - let mat_b = inv_left * mat_bc.scale(sampling_dt); - - Self::try_from_matrices(&mat_a, &mat_b, mat_cc, mat_dc, sampling_dt) - } - /// Discretizes a continuous model using exact ZOH. /// /// See [`DiscreteStateSpaceModel::from_continuous_matrix_zoh`] for equations. @@ -412,22 +383,6 @@ impl DiscreteStateSpaceModel { ) } - /// Discretizes a continuous model using Tustin. - /// - /// See [`DiscreteStateSpaceModel::from_continuous_matrix_tustin`] for equations. - pub fn from_continuous_tustin( - model: &ContinuousStateSpaceModel, - sampling_dt: f64, - ) -> Result { - Self::from_continuous_matrix_tustin( - model.mat_a(), - model.mat_b(), - model.mat_c(), - model.mat_d(), - sampling_dt, - ) - } - } impl Pole for DiscreteStateSpaceModel { @@ -619,22 +574,6 @@ mod tests { assert_eq!(ss_model.mat_d()[(0, 0)], 8.0f64); } - // Verifies Tustin discretization against first-order analytic values. - #[test] - fn test_discretization_tustin_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_tustin(&a, &b, &c, &d, dt).unwrap(); - - approx_eq_matrix(model.mat_a(), &na::dmatrix![0.8181818181818182], 1e-12); - approx_eq_matrix(model.mat_b(), &na::dmatrix![0.09090909090909091], 1e-12); - } - // Verifies ZOH discretization against first-order exact discretization values. #[test] fn test_discretization_zoh_first_order() { From d212a5a58bd99b3d3543041998869e9968866bab Mon Sep 17 00:00:00 2001 From: Romain Desarzens Date: Sat, 21 Feb 2026 11:16:10 +0000 Subject: [PATCH 18/18] Add comments to the matrix exponential computation --- src/model.rs | 36 +++++++++++++++++++++++++++++++----- 1 file changed, 31 insertions(+), 5 deletions(-) diff --git a/src/model.rs b/src/model.rs index d94b04a..5628c9f 100644 --- a/src/model.rs +++ b/src/model.rs @@ -460,16 +460,34 @@ fn validate_state_space_dimensions( 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)`. +/// +/// This is a simple implementation intended for moderate matrix sizes in this +/// library context. +/// +/// 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); } - // Scaling-and-series approximation: - // 1) choose a power-of-two scale so ||A/scale|| is small, - // 2) evaluate exp(A/scale) by truncated Taylor series, - // 3) square the result repeatedly to recover exp(A). + // 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 @@ -484,7 +502,13 @@ fn matrix_exponential(mat: &na::DMatrix) -> na::DMatrix { let mut result = identity.clone(); let mut term = identity; - // exp(M) = I + M + M^2/2! + M^3/3! + ... + // 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; @@ -495,6 +519,8 @@ fn matrix_exponential(mat: &na::DMatrix) -> na::DMatrix { } } + // 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;