control_sys/
simulator.rs

1use nalgebra as na;
2
3use crate::model::{ContinuousStateSpaceModel, Discrete, DiscreteStateSpaceModel, StateSpaceModel};
4
5/// Generates the step response of a discrete state-space model for a given duration.
6///
7/// # Arguments
8///
9/// * `model` - A reference to an object that implements the `StateSpaceModel` and `Discrete` traits.
10/// * `duration` - The duration for which the step response is to be generated.
11///
12/// # Returns
13///
14/// A tuple containing:
15/// * `response` - A matrix containing the output sequence of the step response.
16/// * `input` - A matrix containing the input sequence (step input).
17/// * `states` - A matrix containing the state sequence during the step response.
18///
19pub fn step_for_discrete_ss(
20    model: &(impl StateSpaceModel + Discrete),
21    duration: f64,
22) -> (na::DMatrix<f64>, na::DMatrix<f64>, na::DMatrix<f64>) {
23    // Initial state is zero for a step response
24    let initial_state = na::DVector::<f64>::zeros(model.mat_a().nrows());
25
26    // Generate step for given duration
27    let n_samples = (duration / model.sampling_dt()).floor() as usize;
28    let input = na::DMatrix::from_element(1, n_samples, 1.0f64);
29
30    let (response, states) = simulate_ss_response(model, &input, &initial_state);
31
32    (response, input, states)
33}
34
35/// 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.
36///
37/// # Arguments
38///
39/// * `model` - A reference to a `ContinuousStateSpaceModel` object.
40/// * `sampling_dt` - The sampling time for the discrete model.
41/// * `duration` - The duration for which the step response is to be generated.
42///
43/// # Returns
44///
45/// A tuple containing:
46/// * `response` - A matrix containing the output sequence of the step response.
47/// * `input` - A matrix containing the input sequence (step input).
48/// * `states` - A matrix containing the state sequence during the step response.
49///
50pub fn step_for_continuous_ss(
51    model: &ContinuousStateSpaceModel,
52    sampling_dt: f64,
53    duration: f64,
54) -> (na::DMatrix<f64>, na::DMatrix<f64>, na::DMatrix<f64>) {
55    let discrete_model =
56        DiscreteStateSpaceModel::from_continuous_ss_forward_euler(model, sampling_dt);
57
58    // Initial state is zero for a step response
59    let initial_state = na::DVector::<f64>::zeros(model.mat_a().nrows());
60
61    // Generate step for given duration
62    let n_samples = (duration / discrete_model.sampling_dt()).floor() as usize;
63    let input = na::DMatrix::from_element(1, n_samples, 1.0f64);
64
65    let (response, states) = simulate_ss_response(&discrete_model, &input, &initial_state);
66
67    (response, input, states)
68}
69
70fn simulate_ss_response(
71    model: &(impl StateSpaceModel + Discrete),
72    mat_u: &na::DMatrix<f64>,
73    x0: &na::DVector<f64>,
74) -> (na::DMatrix<f64>, na::DMatrix<f64>) {
75    let sim_time = mat_u.ncols();
76    let n_state = model.mat_a().nrows();
77    let n_output = model.mat_c().nrows();
78    let mut mat_x = na::DMatrix::<f64>::zeros(n_state, sim_time + 1);
79    let mut mat_y = na::DMatrix::<f64>::zeros(n_output, sim_time);
80    for i in 0..sim_time {
81        if i == 0 {
82            mat_x.column_mut(i).copy_from(&x0);
83            mat_y.column_mut(i).copy_from(&(model.mat_c() * x0));
84            mat_x
85                .column_mut(i + 1)
86                .copy_from(&(model.mat_a() * x0 + model.mat_b() * mat_u.column(i)));
87        } else {
88            mat_y
89                .column_mut(i)
90                .copy_from(&(model.mat_c() * mat_x.column(i)));
91
92            let mat_x_slice = mat_x.column(i).into_owned();
93            mat_x
94                .column_mut(i + 1)
95                .copy_from(&(model.mat_a() * mat_x_slice + model.mat_b() * mat_u.column(i)));
96        }
97    }
98
99    (mat_y, mat_x)
100}
control_sys/simulator.rs

control_sys/
simulator.rs
